refactor(base_move): 优化移动控制逻辑和校准流程

- 修改 move_to_hori_line 函数中的超时设置和停止命令发送方式
- 在主程序中初始化和销毁 ROS 2 上下文
- 增加校准相关变量和逻辑，提高定位精度
- 优化 ImageProcessor 和 MarkerRunner 类的实现

base_move/move_base_hori_line.py

@@ -247,7 +247,7 @@ def move_to_hori_line(ctrl, msg, target_distance=0.1, observe=False):
     """
     # 首先校准到水平
     print("校准到横向线水平")
-    aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
+    aligned = align_to_horizontal_line(ctrl, msg, observe=False)
     
     if not aligned:
         print("无法校准到横向线水平，停止移动")
@@ -318,7 +318,7 @@ def move_to_hori_line(ctrl, msg, target_distance=0.1, observe=False):
     # 使用里程计进行实时监控移动距离
     distance_moved = 0
     start_time = time.time()
-    timeout = move_time + 5  # 超时时间设置为预计移动时间加5秒
+    timeout = move_time + 1  # 超时时间设置为预计移动时间加5秒
     
     while distance_moved < abs(distance_to_move) and time.time() - start_time < timeout:
         # 计算已移动距离
@@ -337,8 +337,9 @@ def move_to_hori_line(ctrl, msg, target_distance=0.1, observe=False):
         time.sleep(0.05)  # 小间隔检查位置
     
     # 发送停止命令
-    msg.vel_des = [0, 0, 0]
-    msg.life_count += 1
+    msg.mode = 0
+    msg.gait_id = 0
+    msg.duration = 0
     ctrl.Send_cmd(msg)
     
     if observe:

main.py

@@ -11,6 +11,8 @@ import time
 from threading import Thread, Lock
 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
@@ -21,6 +23,7 @@ from task_1.task_1 import run_task_1
 from task_5.task_5 import run_task_5
 
 def main():
+    rclpy.init()  # 新增：在主程序中统一初始化 ROS 2 上下文
     Ctrl = Robot_Ctrl()
     Ctrl.run()
     msg = robot_control_cmd_lcmt()
@@ -48,6 +51,7 @@ def main():
     finally:
         print("正在清理资源...")
         Ctrl.quit()
+        rclpy.shutdown()  # 新增：在主程序中统一销毁 ROS 2 上下文
         print("程序已退出")
         sys.exit()
 
@@ -71,6 +75,9 @@ class Robot_Ctrl(object):
         self.mode_ok = 0
         self.gait_ok = 0
         self.runing = 1
+        # 新增: 校准相关变量
+        self.is_calibrated = False  # 是否已完成校准
+        self.calibration_offset = [0, 0, 0]  # 校准偏移量
         # 初始化 MarkerRunner
 
     def run(self):
@@ -91,8 +98,19 @@ class Robot_Ctrl(object):
 
     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.rpy)
+        #     print(self.odo_msg.xyz)
 
     def rec_responce(self):
         while self.runing:

setup.bash

@@ -0,0 +1,15 @@
+#!/bin/bash
+
+# 检查是否通过 source 命令执行
+if [ -n "$BASH_VERSION" ]; then
+    # 如果是通过 source 命令执行，则直接执行 setup.bash
+    source ../cyberdog_sim/install/setup.bash
+else
+    # 如果是直接执行，则通过 source 命令加载 setup.bash
+    echo "Warning: This script should be sourced, not executed directly."
+    echo "Sourcing setup.bash..."
+    source ../cyberdog_sim/install/setup.bash
+fi
+
+# 确保 cyberdog_marker 的路径已添加到 PYTHONPATH
+export PYTHONPATH=$PYTHONPATH:$(pwd)/../cyberdog_sim/install/lib/python3/dist-packages

utils/image_raw.py

@@ -41,7 +41,6 @@ class ImageSubscriber(Node):
 
 class ImageProcessor:
     def __init__(self):
-        rclpy.init()
         self.image_subscriber = ImageSubscriber()
         self.qreader = QReader()
         self.spin_thread = None
@@ -63,7 +62,6 @@ class ImageProcessor:
         if self.spin_thread:
             self.spin_thread.join()
         self.image_subscriber.destroy_node()
-        rclpy.shutdown()
 
     def get_current_image(self):
         return self.image_subscriber.cv_image
@@ -88,8 +86,7 @@ def main(args=None):
     
     # 清理
     image_subscriber.destroy_node()
-    rclpy.shutdown()
 
 
 if __name__ == '__main__':
-    main()
+    main()

utils/marker_client.py

@@ -15,7 +15,7 @@ class MarkerClient(Node):
     def send_request(self, x, y, z, color):
         self.req.x = x
         self.req.y = y
-        self.req.z = z
+        self.req.z = 1.0  # INFO 作为标志，防止阻碍 Robot
         self.req.color = color
         self.future = self.client.call_async(self.req)
         rclpy.spin_until_future_complete(self, self.future)
@@ -27,12 +27,11 @@ class MarkerRunner:
         self.marker_client = None
 
     def run(self):
-        rclpy.init()
         self.marker_client = MarkerClient()
     
     def destroy(self):
-        self.marker_client.destroy_node()
-        rclpy.shutdown()
+        if self.marker_client:
+            self.marker_client.destroy_node()
     
     def send_request(self, x, y, z, color, observe=False):
         response = self.marker_client.send_request(x, y, z, color)
@@ -49,7 +48,6 @@ def main(args=None):
     client.get_logger().info(
         f'结果: {response.success}, 消息: {response.message}')
     client.destroy_node()
-    rclpy.shutdown()
 
 if __name__ == '__main__':
-    main() 
+    main()