task-1 #5

base_move/move_base_hori_line.py

@@ -337,10 +337,7 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False):
         time.sleep(0.05)  # 小间隔检查位置
     
     # 发送停止命令
-    msg.mode = 0
+    ctrl.base_msg.stop_force()
-    msg.gait_id = 0
-    msg.duration = 0
-    ctrl.Send_cmd(msg)
     
     if observe:
         print(f"移动完成，通过里程计计算的移动距离: {distance_moved:.3f}米")
@@ -395,8 +392,8 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left
     angle_rad = math.radians(angle_deg)
     # 设定角速度（rad/s），可根据实际调整
     w = 0.4 if left else -0.4  # 左转为正，右转为负
-    v = abs(w * r)  # 线速度
+    v = w * r  # 线速度，正负号与角速度方向一致
-    t = abs(angle_rad / w)  # 运动时间
+    t = abs(angle_rad / w)  # 运动时间，取绝对值保证为正
 
     if observe:
         print(f"圆弧半径: {r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s, 运动时间: {t:.2f}s")
@@ -410,13 +407,35 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left
     msg.life_count += 1
 
     ctrl.Send_cmd(msg)
-    time.sleep(t)  # 等待运动完成
+
+    # 记录起始角度和位置
+    start_yaw = ctrl.odo_msg.yaw
+    start_position = list(ctrl.odo_msg.xyz)
+
+    # 计算圆弧长度
+    arc_length = abs(angle_rad * r)
+
+    # 进入循环，实时判断
+    distance_moved = 0
+    angle_turned = 0
+    start_time = time.time()
+    timeout = t + 2  # 给个超时保护
+
+    while distance_moved < arc_length * 0.98 and abs(angle_turned) < abs(angle_deg) * 0.98 and time.time() - start_time < timeout:
+        # 计算已移动距离
+        current_position = ctrl.odo_msg.xyz
+        dx = current_position[0] - start_position[0]
+        dy = current_position[1] - start_position[1]
+        distance_moved = math.sqrt(dx*dx + dy*dy)
+        # 计算已旋转角度
+        current_yaw = ctrl.odo_msg.yaw
+        angle_turned = current_yaw - start_yaw
+        # 角度归一化处理
+        # ...
+        time.sleep(0.05)
 
     # 5. 发送停止指令
-    msg.mode = 0
+    ctrl.base_msg.stop_force()
-    msg.gait_id = 0
-    msg.duration = 0
-    ctrl.Send_cmd(msg)
 
     if observe:
         print("圆弧运动完成")

main.py

@@ -18,6 +18,7 @@ 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.marker_client import MarkerRunner
+from utils.base_msg import BaseMsg
 
 from task_1.task_1 import run_task_1
 from task_5.task_5 import run_task_5
@@ -78,7 +79,9 @@ class Robot_Ctrl(object):
         # 新增: 校准相关变量
         self.is_calibrated = False  # 是否已完成校准
         self.calibration_offset = [0, 0, 0]  # 校准偏移量
-        # 初始化 MarkerRunner
+
+        # 新增: 基础消息
+        self.base_msg = BaseMsg(self, self.cmd_msg)
 
     def run(self):
         self.lc_r.subscribe("robot_control_response", self.msg_handler)

utils/base_msg.py

@@ -0,0 +1,13 @@
+
+class BaseMsg:
+    def __init__(self, ctrl, msg):
+        self.ctrl = ctrl
+        self.msg = msg
+
+    def stop_force(self):
+        self.msg.mode = 0
+        self.msg.gait_id = 0
+        self.msg.duration = 0
+        self.msg.life_count += 1
+        self.ctrl.Send_cmd(self.msg)
+        self.ctrl.Wait_finish(0, 0)