重构主程序，移除图像处理器实例化，简化任务1函数参数，删除无用的测试文件，优化代码结构。
This commit is contained in:
parent
aa4621ed55
commit
6be89617e7
@ -160,7 +160,7 @@ def calculate_distance_to_line(edge_info, camera_height):
|
||||
|
||||
return max(0.0, distance) # 确保距离是正数
|
||||
|
||||
def move_to_hori_line(ctrl, msg, image, target_distance=0.1, observe=False, image_processor=None):
|
||||
def move_to_hori_line(ctrl, msg, image, target_distance=0.1, observe=False):
|
||||
"""
|
||||
控制机器人校准并移动到横向线前的指定距离
|
||||
|
||||
@ -170,7 +170,6 @@ def move_to_hori_line(ctrl, msg, image, target_distance=0.1, observe=False, imag
|
||||
image: 输入图像，可以是文件路径或者已加载的图像数组
|
||||
target_distance: 目标与横向线的距离(米)，默认为0.1米
|
||||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||||
image_processor: 图像处理器，用于获取实时图像，如果提供则在移动完成后进行验证
|
||||
|
||||
返回:
|
||||
bool: 是否成功到达目标位置
|
||||
|
||||
10
main.py
10
main.py
@ -23,8 +23,6 @@ def main():
|
||||
Ctrl = Robot_Ctrl()
|
||||
Ctrl.run()
|
||||
msg = robot_control_cmd_lcmt()
|
||||
image_processor = ImageProcessor()
|
||||
image_processor.run()
|
||||
|
||||
try:
|
||||
print("Recovery stand")
|
||||
@ -36,7 +34,7 @@ def main():
|
||||
|
||||
# time.sleep(100) # TEST
|
||||
|
||||
run_task_1(Ctrl, msg, image_processor)
|
||||
run_task_1(Ctrl, msg)
|
||||
|
||||
# run_task_5(Ctrl, msg)
|
||||
|
||||
@ -49,7 +47,6 @@ def main():
|
||||
finally:
|
||||
print("正在清理资源...")
|
||||
Ctrl.quit()
|
||||
image_processor.destroy()
|
||||
print("程序已退出")
|
||||
sys.exit()
|
||||
|
||||
@ -65,6 +62,7 @@ class Robot_Ctrl(object):
|
||||
self.cmd_msg = robot_control_cmd_lcmt()
|
||||
self.rec_msg = robot_control_response_lcmt()
|
||||
self.odo_msg = localization_lcmt()
|
||||
self.image_processor = ImageProcessor()
|
||||
self.send_lock = Lock()
|
||||
self.delay_cnt = 0
|
||||
self.mode_ok = 0
|
||||
@ -77,6 +75,7 @@ class Robot_Ctrl(object):
|
||||
self.send_thread.start()
|
||||
self.rec_thread.start()
|
||||
self.odo_thread.start()
|
||||
self.image_processor.run()
|
||||
|
||||
def msg_handler(self, channel, data):
|
||||
self.rec_msg = robot_control_response_lcmt().decode(data)
|
||||
@ -131,7 +130,8 @@ class Robot_Ctrl(object):
|
||||
self.runing = 0
|
||||
self.rec_thread.join()
|
||||
self.send_thread.join()
|
||||
|
||||
self.image_processor.destroy()
|
||||
|
||||
# Main function
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -8,13 +8,11 @@ sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
|
||||
from base_move.move_base_hori_line import align_to_horizontal_line, move_to_hori_line
|
||||
|
||||
def run_task_1(ctrl, msg, image_processor):
|
||||
def run_task_1(ctrl, msg):
|
||||
print('Running task 1...')
|
||||
|
||||
# v2
|
||||
# align_to_horizontal_line(ctrl, msg, image_processor.get_current_image())
|
||||
|
||||
move_to_hori_line(ctrl, msg, image_processor.get_current_image(), image_processor=image_processor)
|
||||
move_to_hori_line(ctrl, msg)
|
||||
|
||||
|
||||
|
||||
|
||||
@ -1,34 +1,70 @@
|
||||
#!/usr/bin/env python3
|
||||
import rospy
|
||||
import tf2_ros
|
||||
from geometry_msgs.msg import TransformStamped
|
||||
|
||||
def get_camera_height():
|
||||
rospy.init_node('camera_height_monitor')
|
||||
|
||||
# 创建TF2监听器
|
||||
tf_buffer = tf2_ros.Buffer()
|
||||
tf_listener = tf2_ros.TransformListener(tf_buffer)
|
||||
|
||||
rate = rospy.Rate(10.0) # 10Hz
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from tf2_ros.buffer import Buffer
|
||||
from tf2_ros.transform_listener import TransformListener
|
||||
from std_msgs.msg import Float64
|
||||
import math
|
||||
|
||||
class CameraHeightMonitor(Node):
|
||||
"""节点用于监控RGB相机相对于世界坐标系的高度"""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('camera_height_monitor')
|
||||
|
||||
# 创建TF2缓冲区和监听器
|
||||
self.tf_buffer = Buffer()
|
||||
self.tf_listener = TransformListener(self.tf_buffer, self)
|
||||
|
||||
# 创建发布器，发布高度信息到专门的话题
|
||||
self.height_publisher = self.create_publisher(
|
||||
Float64, 'rgb_camera_height', 10)
|
||||
|
||||
# 创建定时器，定期查询转换
|
||||
timer_period = 0.1 # 10 Hz
|
||||
self.timer = self.create_timer(timer_period, self.timer_callback)
|
||||
|
||||
self.get_logger().info('RGB相机高度监测节点已启动')
|
||||
|
||||
def timer_callback(self):
|
||||
"""定时器回调函数，查询并发布相机高度"""
|
||||
try:
|
||||
# 查询从world到RGB_camera_link的变换
|
||||
# 如果您的Gazebo使用其他世界坐标系名称，请相应调整
|
||||
trans = tf_buffer.lookup_transform('world', 'RGB_camera_link', rospy.Time())
|
||||
# 查询从world坐标系到RGB_camera_link的变换
|
||||
# 根据您的系统，可能需要修改坐标系名称
|
||||
transform = self.tf_buffer.lookup_transform(
|
||||
'world', # 目标坐标系（通常是世界坐标系）
|
||||
'RGB_camera_link', # 源坐标系（相机坐标系）
|
||||
rclpy.time.Time()) # 获取最新的变换
|
||||
|
||||
# 提取z轴值（高度）
|
||||
camera_height = trans.transform.translation.z
|
||||
rospy.loginfo("RGB相机当前高度: %.3f 米", camera_height)
|
||||
# 提取z轴值，即高度
|
||||
camera_height = transform.transform.translation.z
|
||||
|
||||
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
|
||||
rospy.logwarn("无法获取变换: %s", e)
|
||||
# 创建Float64消息并发布
|
||||
height_msg = Float64()
|
||||
height_msg.data = camera_height
|
||||
self.height_publisher.publish(height_msg)
|
||||
|
||||
rate.sleep()
|
||||
# 打印高度信息
|
||||
self.get_logger().info(f'RGB相机当前高度: {camera_height:.3f} 米')
|
||||
|
||||
except Exception as e:
|
||||
# 如果找不到变换，记录错误但不中断执行
|
||||
self.get_logger().warning(f'无法获取RGB相机的高度: {str(e)}')
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
camera_height_monitor = CameraHeightMonitor()
|
||||
|
||||
try:
|
||||
rclpy.spin(camera_height_monitor)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
# 清理并关闭节点
|
||||
camera_height_monitor.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
get_camera_height()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
main()
|
||||
@ -1,36 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import rospy
|
||||
from gazebo_msgs.srv import GetModelState
|
||||
|
||||
def get_camera_height_from_gazebo():
|
||||
rospy.init_node('camera_height_monitor_gazebo')
|
||||
|
||||
# 等待Gazebo的GetModelState服务可用
|
||||
rospy.wait_for_service('/gazebo/get_model_state')
|
||||
get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
|
||||
|
||||
rate = rospy.Rate(10.0) # 10Hz
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
try:
|
||||
# 获取机器人模型状态
|
||||
# 注意：需要替换为您的机器人模型名称，通常是"robot"或类似名称
|
||||
model_state = get_model_state("robot", "world")
|
||||
|
||||
# 计算相机高度 (机器人z坐标 + 相机相对于机器人的z偏移)
|
||||
# 根据您之前查找的数据：机器人高度一半(54.5mm) + 相机高度(125.794mm)
|
||||
robot_z = model_state.pose.position.z
|
||||
camera_height = robot_z + 0.180294 # 根据之前计算的相机高度
|
||||
|
||||
rospy.loginfo("RGB相机当前高度: %.3f 米", camera_height)
|
||||
|
||||
except rospy.ServiceException as e:
|
||||
rospy.logerr("服务调用失败: %s", e)
|
||||
|
||||
rate.sleep()
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
get_camera_height_from_gazebo()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
Loading…
x
Reference in New Issue
Block a user