From 43f758653fcc80ded769fb77680d1a81bfc41e87 Mon Sep 17 00:00:00 2001 From: hav <123@q.com> Date: Wed, 20 Aug 2025 10:49:07 +0800 Subject: [PATCH] Update midesktop --- main.py | 14 +++++--- task_2/task_2.py | 52 +++++++++++++++++++++++++++++- try.py | 2 +- utils/image_raw.py | 79 +++++++++++++++++++++++++++++++++++++++------- 4 files changed, 129 insertions(+), 18 deletions(-) diff --git a/main.py b/main.py index 25abf58..d104ba9 100755 --- a/main.py +++ b/main.py @@ -18,7 +18,7 @@ import cv2 from utils.robot_control_cmd_lcmt import robot_control_cmd_lcmt from utils.robot_control_response_lcmt import robot_control_response_lcmt from utils.localization_lcmt import localization_lcmt -# from utils.image_raw import ImageProcessor +from utils.image_raw import ImageProcessor from utils.base_msg import BaseMsg from utils.speech_demo import speak # from utils.marker_client import MarkerRunner @@ -46,7 +46,7 @@ class TaskType(Enum): UP_AND_DOWN = auto() # TODO 调整其完成从上坡到下坡 STONE_ROAD = auto() # TODO 调整其完成从石板路到石板路 -TASK = TaskType.STONE_ROAD +TASK = TaskType.TASK def main(): rclpy.init() # 新增：在主程序中统一初始化 ROS 2 上下文 @@ -75,6 +75,12 @@ def main(): info("Recovery stand", "info") Ctrl.base_msg.stand_up() Ctrl.base_msg.stop() # BUG 垃圾指令 for eat # INFO 但是正式比赛或许也有用 + + + # image = Ctrl.image_processor.get_current_image('ai') + # timestamp = datetime.now().strftime("%Y%m%d_%H%M%S_%f") + # filename = f"saved_images/rgb_{timestamp}.jpg" + # cv2.imwrite(filename, image) if TASK == TaskType.PASS_BAR: from task_4.pass_bar import pass_bar @@ -170,7 +176,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.image_processor = ImageProcessor() # DEBUG # self.marker_runner = MarkerRunner(pass_flag=pass_marker) self.send_lock = Lock() @@ -191,7 +197,7 @@ class Robot_Ctrl(object): self.send_thread.start() self.rec_thread.start() self.odo_thread.start() - # self.image_processor.run() + self.image_processor.run() # self.marker_runner.run() def msg_handler(self, channel, data): diff --git a/task_2/task_2.py b/task_2/task_2.py index 8e1625b..6e0b702 100755 --- a/task_2/task_2.py +++ b/task_2/task_2.py @@ -551,4 +551,54 @@ def run_task_2_demo(ctrl, msg, xy_flag=False): info(f"箭头检测最终结果: left={left_count}, right={right_count}, 选择={arrow_direction}", "箭头检测结果") # 返回检测到的箭头方向 - return arrow_direction \ No newline at end of file + return arrow_direction + + +# # msg.vel_des = [0.3, 0.02, 0.05] #转向 +# # msg.duration = 3000 # Zero duration means continuous motion until a new command is used. +# # # Continuous motion can interrupt non-zero duration interpolation motion +# # msg.life_count += 1 +# # Ctrl.Send_cmd(msg) +# # time.sleep( 5 ) + +# msg.vel_des = [0.3, 0, 0.42] +# msg.duration = 10300 +# msg.life_count += 1 +# Ctrl.Send_cmd(msg) +# time.sleep( 15 ) + +# # msg.vel_des = [0.3, 0, 0] +# # msg.duration = 200 +# # msg.life_count += 1 +# # Ctrl.Send_cmd(msg) +# # time.sleep( 2 ) + +# msg.vel_des = [0.3, 0, -0.43] +# msg.duration = 12200 +# msg.life_count += 1 +# Ctrl.Send_cmd(msg) +# time.sleep( 15 ) + +# # msg.vel_des = [0.3, 0, 0] +# # msg.duration = 100 +# # msg.life_count += 1 +# # Ctrl.Send_cmd(msg) +# # time.sleep( 2 ) + +# msg.vel_des = [0.3, 0, 0.43] +# msg.duration = 10200 +# msg.life_count += 1 +# Ctrl.Send_cmd(msg) +# time.sleep( 15 ) + +# msg.vel_des = [0.3, 0, -0.5] +# msg.duration = 3600 +# msg.life_count += 1 +# Ctrl.Send_cmd(msg) +# time.sleep( 10 ) + +# msg.vel_des = [0.3, 0, 0] +# msg.duration = 2500 +# msg.life_count += 1 +# Ctrl.Send_cmd(msg) +# time.sleep( 10 ) \ No newline at end of file diff --git a/try.py b/try.py index dba4214..61238aa 100755 --- a/try.py +++ b/try.py @@ -17,7 +17,7 @@ class SimpleStandTest: def handle_response(self, channel, data): """处理状态反馈""" msg = robot_control_response_lcmt().decode(data) - print(f"[状态反馈] 模式:{msg.mode} 进度:{msg.order_process_bar}% 电机状态:{msg.motor_error}") + print(f"[状态反馈] 模式:{msg.mode} 进度:{msg.order_process_bar}% 电机状态:{msg.motor_error} ori:{msg.ori_error} foot:{msg.footpos_error[0]}") def send_stand_command(self): # 1. 退出当前模式 diff --git a/utils/image_raw.py b/utils/image_raw.py index 2e2ba1a..a7b4de6 100755 --- a/utils/image_raw.py +++ b/utils/image_raw.py @@ -1,16 +1,58 @@ #!/usr/bin/env python3 -#ros2 run camera_test camera_server -# //启动stereo_camera -# ros2 run camera_test stereo_camera -# ros2 lifecycle set /stereo_camera configure -# ros2 lifecycle set /stereo_camera activate - -#real +# //real相机：只支持左目的depth #ros2 launch realsense2_camera on_dog.py #ros2 lifecycle set /camera/camera configure #ros2 lifecycle set /camera/camera activate +#/camera/infra1/image_rect_raw（左目图像） +#/camera/infra2/image_rect_raw（右目图像） +#/camera/depth/image_rect_raw（深度图） +#/camera/imu（IMU数据） + +# //启动stereo_camera +# ros2 run camera_test stereo_camera（或者launch） +# ros2 lifecycle set /stereo_camera configure +# ros2 lifecycle set /stereo_camera activate + +#/image_left +#/image_right +#/image_rgb + +# //AI相机：ai相机的图像可以找到real相机对应的深度值 +#ros2 run camera_test camera_server +#/image + +######################################################################################### + +#/camera/camera_align节点启动指令 +#ros2 launch realsense2_camera realsense_align_node.launch.py +#ros2 lifecycle set /camera/camera_align configure +#ros2 lifecycle set /camera/camera_align activate + +#视觉建图 +#1.启动slam +#source /opt/ros2/cyberdog/setup.bash +#ros2 launch laser_slam mapping_launch.py +#ros2 lifecycle set /map_builder 1 +#ros2 lifecycle set /map_builder 3 + +#2.启动建图（另一个终端） +#source /opt/ros2/cyberdog/setup.bash +#ros2 service call /start_mapping std_srvs/srv/SetBool data:\ true + +#保存地图和pose信息 +#ros2 service call /stop_mapping visualization/srv/Stop "{finish: true, map_name: 'map'}" +#雷达修改 +#sudo systemctl disable nvgetty.service + +#重定位程序 +#ros2 launch laser_slam localization_node.py +#ros2 lifecycle set /localization_node 1 +#ros2 lifecycle set /localization_node 3 +#另一个终端 +#ros2 service call /start_location std_srvs/srv/SetBool "{data: true}" + import rclpy from rclpy.node import Node from sensor_msgs.msg import Image @@ -137,7 +179,7 @@ class ImageSubscriber(Node): timestamp = datetime.now().strftime("%Y%m%d_%H%M%S_%f") filename = f"{self.save_dir}/{prefix}_{timestamp}.jpg" cv2.imwrite(filename, image) - self.get_logger().info(f"已保存 {prefix} 图像: {filename}") + # self.get_logger().info(f"已保存 {prefix} 图像: {filename}") return True except Exception as e: self.get_logger().error(f"保存{prefix}图像失败: {str(e)}") @@ -236,14 +278,27 @@ class ImageProcessor: # self.log.error(f"Spin线程错误: {e}", "错误") print('threat wrong') + # def destroy(self): + # self.running = False + # self.stop_async_scan() + # if self.spin_thread: + # self.spin_thread.join() + # # 停止AI相机 + # self.image_subscriber.stop_camera() + # self.image_subscriber.destroy_node() + def destroy(self): - self.running = False - self.stop_async_scan() + if not self._ros_initialized: + return + + self._shutdown_flag = True if self.spin_thread: - self.spin_thread.join() - # 停止AI相机 + self.spin_thread.join(timeout=1.0) + self.image_subscriber.stop_camera() self.image_subscriber.destroy_node() + rclpy.shutdown() + self._ros_initialized = False def get_current_image(self, text = 'rgb'): # INFO 默认返回 rgb 图像