更新 .DS_Store 文件以反映最新的文件状态。

base-move-demo.py

@@ -0,0 +1,187 @@
+'''
+This demo show the communication interface of MR813 motion control board based on Lcm.
+Dependency: 
+- robot_control_cmd_lcmt.py
+- robot_control_response_lcmt.py
+'''
+import lcm
+from threading import Thread, Lock
+import sys
+import os
+import time
+import sys
+
+import rclpy
+
+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.base_msg import BaseMsg
+
+from base_move.turn_degree import turn_degree_v2
+from base_move.go_straight import go_straight
+
+from utils.log_helper import info
+
+pass_marker = True
+TIME_SLEEP = 3000
+
+def main():
+    rclpy.init()  # 新增：在主程序中统一初始化 ROS 2 上下文
+    Ctrl = Robot_Ctrl()
+    Ctrl.run()
+    msg = robot_control_cmd_lcmt()
+
+    try:
+        info("Recovery stand", "info")
+        Ctrl.base_msg.stand_up()
+        Ctrl.base_msg.stop()    
+
+        # GO STRAIGHT TEST
+        go_straight(Ctrl, msg, distance=0.2, absolute=True)
+
+                # TURN TEST
+        turn_degree_v2(Ctrl, msg, degree=90, absolute=True)    
+
+        
+    except KeyboardInterrupt:
+        print("\n程序被用户中断")
+    except Exception as e:
+        print(f"发生错误: {e}")
+    finally:
+        print("正在清理资源...")
+        Ctrl.quit()
+        rclpy.shutdown()  # 新增：在主程序中统一销毁 ROS 2 上下文
+        print("程序已退出")
+        sys.exit()
+
+
+class Robot_Ctrl(object):
+    def __init__(self):
+        self.rec_thread = Thread(target=self.rec_responce)
+        self.send_thread = Thread(target=self.send_publish)
+        self.odo_thread = Thread(target=self.rec_responce_o)
+        self.lc_r = lcm.LCM("udpm://239.255.76.67:7670?ttl=255")
+        self.lc_s = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
+        self.lc_o = lcm.LCM("udpm://239.255.76.67:7667?ttl=255")
+        self.cmd_msg = robot_control_cmd_lcmt()
+        self.rec_msg = robot_control_response_lcmt()
+        self.odo_msg = localization_lcmt()
+        # self.image_processor = ImageProcessor()
+        # DEBUG
+        # self.marker_runner = MarkerRunner(pass_flag=pass_marker)
+        self.send_lock = Lock()
+        self.delay_cnt = 0
+        self.mode_ok = 0
+        self.gait_ok = 0
+        self.runing = 1
+        # 新增: 校准相关变量
+        self.is_calibrated = False  # 是否已完成校准
+        self.calibration_offset = [0, 0, 0]  # 校准偏移量
+
+        # 新增: 基础消息
+        self.base_msg = BaseMsg(self, self.cmd_msg)
+
+    def run(self):
+        self.lc_r.subscribe("robot_control_response", self.msg_handler)
+        self.lc_o.subscribe("global_to_robot", self.msg_handler_o)
+        self.send_thread.start()
+        self.rec_thread.start()
+        self.odo_thread.start()
+        # self.image_processor.run()
+        # self.marker_runner.run()
+
+    def msg_handler(self, channel, data):
+        self.rec_msg = robot_control_response_lcmt().decode(data)
+        if self.rec_msg.order_process_bar >= 95:
+            self.mode_ok = self.rec_msg.mode
+        else:
+            self.mode_ok = 0
+
+    def msg_handler_o(self, channel, data):
+        self.odo_msg = localization_lcmt().decode(data)
+        # 如果尚未校准，记录第一帧数据作为校准基准
+        if not self.is_calibrated:
+            self.calibration_offset = self.odo_msg.xyz
+            self.is_calibrated = True
+            print(f"校准完成，基准值: {self.calibration_offset}")
+        # 将接收到的 odo 数据减去校准基准值
+        self.odo_msg.xyz = [
+            self.odo_msg.xyz[0] - self.calibration_offset[0],
+            self.odo_msg.xyz[1] - self.calibration_offset[1],
+            self.odo_msg.xyz[2] - self.calibration_offset[2]
+        ]
+        if self.odo_msg.timestamp % 100 == 0:
+            print(self.odo_msg.xyz, self.odo_msg.rpy, self.odo_msg.vxyz, self.odo_msg.omegaBody, self.odo_msg.vBody)
+
+    def odo_reset(self):
+        self.calibration_offset = self.odo_msg.xyz
+
+    def rec_responce(self):
+        while self.runing:
+            self.lc_r.handle()
+            time.sleep(0.002)
+
+    def rec_responce_o(self):
+        while self.runing:
+            self.lc_o.handle()
+            time.sleep(0.002)
+
+    def Wait_finish(self, mode, gait_id):
+        count = 0
+        while self.runing and count < 2000:  # 10s
+            if self.mode_ok == mode and self.gait_ok == gait_id:
+                return True
+            else:
+                time.sleep(0.005)
+                count += 1
+        print("Wait_finish timeout.")
+        return False
+
+    def send_publish(self):
+        while self.runing:
+            self.send_lock.acquire()
+            if self.delay_cnt > 20:  # Heartbeat signal 10HZ
+                self.lc_s.publish("robot_control_cmd", self.cmd_msg.encode())
+                self.delay_cnt = 0
+            self.delay_cnt += 1
+            self.send_lock.release()
+            time.sleep(0.005)
+
+    def Send_cmd(self, msg):
+        self.send_lock.acquire()
+        self.delay_cnt = 50
+        self.cmd_msg = msg
+        self.cmd_msg.life_count %= 127
+        self.send_lock.release()
+
+    def place_marker(self, x, y, z, color, observe=False):
+        return None
+        """调用 MarkerRunner 放置标志物"""
+        if self.marker_runner is None or self.marker_runner.marker_client is None:
+            print("MarkerRunner 未初始化，无法放置标志物")
+            return None
+        try:
+            response = self.marker_runner.send_request(x, y, z, color, observe=observe)
+            print(f"放置标志物结果: {response.success}, 消息: {response.message}")
+            return response
+        except Exception as e:
+            print(f"放置标志物时发生异常: {e}")
+            return None
+
+    def quit(self):
+        self.runing = 0
+        self.rec_thread.join()
+        self.send_thread.join()
+        # self.image_processor.destroy()
+        # 销毁 MarkerRunner
+        if hasattr(self, 'marker_runner') and self.marker_runner is not None:
+            try:
+                self.marker_runner.destroy()
+            except Exception as e:
+                print(f"MarkerRunner 销毁失败: {e}")
+
+# Main function
+if __name__ == '__main__':
+    main()

tmp/img-demo.py

@@ -0,0 +1,109 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
+from datetime import datetime
+import os
+
+class SimpleImageSubscriber(Node):
+    def __init__(self):
+        super().__init__('simple_image_subscriber')
+        
+        # Define QoS Profile
+        qos_profile = QoSProfile(
+            reliability=QoSReliabilityPolicy.BEST_EFFORT,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            depth=10
+        )
+
+        self.subscription = self.create_subscription(
+            Image,
+            '/rgb_camera/image_raw',
+            self.image_callback,
+            qos_profile=qos_profile
+        )
+        self.subscription  # Prevent unused variable warning
+        self.image_received = False  # Flag to track if image is received
+        self.image_count = 0  # Count received images
+
+    def image_callback(self, msg):
+        if self.image_received:
+            return  # Skip processing if already processed
+
+        try:
+            # Get image metadata
+            height = msg.height
+            width = msg.width
+            encoding = msg.encoding
+            step = msg.step
+            data_size = len(msg.data)
+            
+            # Log image information
+            self.get_logger().info(f"收到图像消息!")
+            self.get_logger().info(f"图像尺寸: {width} x {height}")
+            self.get_logger().info(f"编码格式: {encoding}")
+            self.get_logger().info(f"步长: {step}")
+            self.get_logger().info(f"数据大小: {data_size} 字节")
+            
+            # Save image data to a simple text file for verification
+            self.save_image_info(height, width, encoding, step, data_size)
+            
+            self.image_received = True
+            self.image_count += 1
+            
+            # Log success and continue listening for more images
+            self.get_logger().info(f"成功处理图像 #{self.image_count}")
+            
+            # Optionally shutdown after receiving a few images
+            if self.image_count >= 3:
+                self.get_logger().info("已收到3张图像，测试完成!")
+                self.destroy_node()
+                rclpy.shutdown()
+                exit(0)
+
+        except Exception as e:
+            self.get_logger().error(f'处理图像时出错: {str(e)}')
+
+    def save_image_info(self, height, width, encoding, step, data_size):
+        # Create output directory if it doesn't exist
+        output_dir = "./captured_images/dual_path"
+        os.makedirs(output_dir, exist_ok=True)
+        
+        # Generate a timestamped filename
+        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+        filename = f"{output_dir}/image_info_{timestamp}.txt"
+        
+        try:
+            with open(filename, 'w', encoding='utf-8') as f:
+                f.write(f"图像信息记录 - {timestamp}\n")
+                f.write(f"图像尺寸: {width} x {height}\n")
+                f.write(f"编码格式: {encoding}\n")
+                f.write(f"步长: {step}\n")
+                f.write(f"数据大小: {data_size} 字节\n")
+                f.write(f"ROS节点: {self.get_name()}\n")
+                f.write(f"话题: /rgb_camera/image_raw\n")
+                f.write("图像数据已成功接收，ROS模块工作正常!\n")
+            
+            self.get_logger().info(f"图像信息已保存到: {filename}")
+        except Exception as e:
+            self.get_logger().error(f"保存图像信息时出错: {str(e)}")
+
+def main(args=None):
+    rclpy.init(args=args)
+    simple_subscriber = SimpleImageSubscriber()
+    
+    print("启动简化的ROS图像订阅测试...")
+    print("正在监听话题: /rgb_camera/image_raw")
+    print("按 Ctrl+C 停止测试")
+    
+    try:
+        rclpy.spin(simple_subscriber)
+    except KeyboardInterrupt:
+        print("\n测试被用户中断")
+    finally:
+        print("测试完成，清理资源...")
+        simple_subscriber.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()