Update midesktop

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):

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
+    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 )

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. 退出当前模式

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 图像