diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 2fd8512..82dddd5 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -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: 是否成功到达目标位置 diff --git a/main.py b/main.py index 0d64d1b..43fb809 100644 --- a/main.py +++ b/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() \ No newline at end of file diff --git a/task_1/task_1.py b/task_1/task_1.py index d1df4ff..4e74462 100644 --- a/task_1/task_1.py +++ b/task_1/task_1.py @@ -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) diff --git a/test/ros2/rgb-camera/location-1.py b/test/ros2/rgb-camera/location-1.py index b6f92bd..7dcc1b9 100644 --- a/test/ros2/rgb-camera/location-1.py +++ b/test/ros2/rgb-camera/location-1.py @@ -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 \ No newline at end of file + main() \ No newline at end of file diff --git a/test/ros2/rgb-camera/location-2.py b/test/ros2/rgb-camera/location-2.py deleted file mode 100644 index 65b0258..0000000 --- a/test/ros2/rgb-camera/location-2.py +++ /dev/null @@ -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 \ No newline at end of file