删除当前图像文件，优化 main.py 中的恢复站立逻辑，新增 lie_down 和 stand_up 方法到 BaseMsg 类，更新 task_1.py 以使用新的移动和休息功能。

base_move/go_straight.py

@@ -0,0 +1,198 @@
+import math
+import time
+import sys
+import os
+
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+from utils.localization_lcmt import localization_lcmt
+
+def go_straight(ctrl, msg, distance, speed=0.5, observe=False):
+    """
+    控制机器人沿直线行走指定距离
+    
+    参数:
+    ctrl: Robot_Ctrl 对象，包含里程计信息
+    msg: robot_control_cmd_lcmt 对象，用于发送命令
+    distance: 要行走的距离(米)，正值为前进，负值为后退
+    speed: 行走速度(米/秒)，范围0.1~1.0，默认为0.5
+    observe: 是否输出中间状态信息，默认为False
+    
+    返回:
+    bool: 是否成功完成行走
+    """
+    # 参数验证
+    if abs(distance) < 0.01:
+        print("距离太短，无需移动")
+        return True
+        
+    # 限制速度范围
+    speed = min(max(abs(speed), 0.1), 1.0)
+    
+    # 确定前进或后退方向
+    forward = distance > 0
+    move_speed = speed if forward else -speed
+    abs_distance = abs(distance)
+    
+    # 获取起始位置
+    start_position = list(ctrl.odo_msg.xyz)
+    start_yaw = ctrl.odo_msg.rpy[2]  # 记录起始朝向，用于保持直线
+    
+    if observe:
+        print(f"起始位置: {start_position}")
+        print(f"开始{'前进' if forward else '后退'} {abs_distance:.3f}米，速度: {abs(move_speed):.2f}米/秒")
+        # 在起点放置标记
+        if hasattr(ctrl, 'place_marker'):
+            ctrl.place_marker(start_position[0], start_position[1], 
+                            start_position[2] if len(start_position) > 2 else 0.0, 
+                            'green', observe=True)
+    
+    # 设置移动命令
+    msg.mode = 11  # Locomotion模式
+    msg.gait_id = 26  # 自变频步态
+    
+    # 根据需要移动的距离动态调整移动速度
+    if abs_distance > 1.0:
+        actual_speed = move_speed  # 距离较远时用设定速度
+    elif abs_distance > 0.5:
+        actual_speed = move_speed * 0.8  # 中等距离略微降速
+    elif abs_distance > 0.2:
+        actual_speed = move_speed * 0.6  # 较近距离降低速度
+    else:
+        actual_speed = move_speed * 0.4  # 非常接近时用更慢速度
+    
+    # 设置移动速度和方向
+    msg.vel_des = [actual_speed, 0, 0]  # [前进速度, 侧向速度, 角速度]
+    msg.duration = 0  # wait next cmd
+    msg.step_height = [0.06, 0.06]  # 抬腿高度
+    msg.life_count += 1
+    
+    # 发送命令
+    ctrl.Send_cmd(msg)
+    
+    # 估算移动时间，但实际上会通过里程计控制
+    estimated_time = abs_distance / abs(actual_speed)
+    timeout = estimated_time + 3  # 增加超时时间为预计移动时间加3秒
+    
+    # 使用里程计进行实时监控移动距离
+    distance_moved = 0
+    start_time = time.time()
+    last_position = start_position
+    
+    # 动态调整参数
+    angle_correction_threshold = 0.05  # 角度偏差超过多少弧度开始修正
+    slow_down_ratio = 0.85  # 当移动到目标距离的85%时开始减速
+    completion_threshold = 0.95  # 当移动到目标距离的95%时停止
+    position_check_interval = 0.1  # 位置检查间隔(秒)
+    last_check_time = start_time
+    
+    # 监控移动距离
+    while distance_moved < abs_distance * completion_threshold and time.time() - start_time < timeout:
+        current_time = time.time()
+        
+        # 按固定间隔检查位置，减少计算负担
+        if current_time - last_check_time >= position_check_interval:
+            # 获取当前位置和朝向
+            current_position = ctrl.odo_msg.xyz
+            current_yaw = ctrl.odo_msg.rpy[2]
+            
+            # 计算已移动距离
+            dx = current_position[0] - start_position[0]
+            dy = current_position[1] - start_position[1]
+            distance_moved = math.sqrt(dx*dx + dy*dy)
+            
+            # 计算移动方向与初始朝向的偏差
+            movement_direction = math.atan2(current_position[1] - last_position[1], 
+                                           current_position[0] - last_position[0]) if distance_moved > 0.05 else start_yaw
+            yaw_error = movement_direction - start_yaw
+            # 角度归一化
+            while yaw_error > math.pi:
+                yaw_error -= 2 * math.pi
+            while yaw_error < -math.pi:
+                yaw_error += 2 * math.pi
+                
+            # 计算完成比例
+            completion_ratio = distance_moved / abs_distance
+            
+            # 根据距离完成情况调整速度
+            if completion_ratio > slow_down_ratio:
+                # 计算减速系数
+                slow_factor = 1.0 - (completion_ratio - slow_down_ratio) / (1.0 - slow_down_ratio)
+                # 确保不会减速太多
+                slow_factor = max(0.2, slow_factor)
+                new_speed = actual_speed * slow_factor
+                
+                if observe and abs(new_speed - msg.vel_des[0]) > 0.05:
+                    print(f"减速: {msg.vel_des[0]:.2f} -> {new_speed:.2f} 米/秒 (完成: {completion_ratio*100:.1f}%)")
+                
+                msg.vel_des[0] = new_speed
+                msg.life_count += 1
+                ctrl.Send_cmd(msg)
+            
+            # 如果偏离初始方向，进行角度修正
+            if abs(yaw_error) > angle_correction_threshold:
+                # 计算角速度修正值，偏差越大修正越强
+                angular_correction = -yaw_error * 0.5  # 比例系数可调整
+                # 限制最大角速度修正
+                angular_correction = max(min(angular_correction, 0.2), -0.2)
+                
+                if observe and abs(angular_correction) > 0.05:
+                    print(f"方向修正: 偏差 {math.degrees(yaw_error):.1f}度，应用角速度 {angular_correction:.3f}rad/s")
+                
+                # 应用角速度修正
+                msg.vel_des[2] = angular_correction
+                msg.life_count += 1
+                ctrl.Send_cmd(msg)
+            elif msg.vel_des[2] != 0:
+                # 如果方向已修正，重置角速度
+                msg.vel_des[2] = 0
+                msg.life_count += 1
+                ctrl.Send_cmd(msg)
+            
+            if observe and current_time - start_time > 1 and (current_time % 0.5 < position_check_interval):
+                print(f"已移动: {distance_moved:.3f}米, 目标: {abs_distance:.3f}米 (完成: {completion_ratio*100:.1f}%)")
+                print(f"当前速度: [{msg.vel_des[0]:.2f}, {msg.vel_des[1]:.2f}, {msg.vel_des[2]:.2f}]")
+            
+            # 更新最后检查时间和位置
+            last_check_time = current_time
+            last_position = current_position
+            
+        time.sleep(0.01)  # 小间隔检查位置
+    
+    # 平滑停止
+    if hasattr(ctrl.base_msg, 'stop_smooth'):
+        ctrl.base_msg.stop_smooth()
+    else:
+        ctrl.base_msg.stop()
+    
+    # 获取最终位置和实际移动距离
+    final_position = ctrl.odo_msg.xyz
+    dx = final_position[0] - start_position[0]
+    dy = final_position[1] - start_position[1]
+    actual_distance = math.sqrt(dx*dx + dy*dy)
+    
+    if observe:
+        print(f"移动完成，从里程计计算的移动距离: {actual_distance:.3f}米")
+        # 在终点放置标记
+        if hasattr(ctrl, 'place_marker'):
+            ctrl.place_marker(final_position[0], final_position[1], 
+                           final_position[2] if len(final_position) > 2 else 0.0, 
+                           'red', observe=True)
+    
+    # 判断是否成功完成
+    distance_error = abs(actual_distance - abs_distance)
+    success = distance_error < 0.1  # 如果误差小于10厘米，则认为成功
+    
+    if observe:
+        print(f"目标距离: {abs_distance:.3f}米, 实际距离: {actual_distance:.3f}米, 误差: {distance_error:.3f}米")
+        print(f"移动{'成功' if success else '失败'}")
+    
+    return success
+
+# 用法示例
+if __name__ == "__main__":
+    # 这里是示例代码，实际使用时需要提供合适的ctrl和msg对象
+    # 前进1米
+    # go_straight(ctrl, msg, 1.0, speed=0.5, observe=True)
+    # 后退0.5米
+    # go_straight(ctrl, msg, -0.5, speed=0.3, observe=True)
+    pass

base_move/turn_degree.py

@@ -110,4 +110,4 @@ def turn_degree(ctrl, msg, degree=90, absolute=False):
             # 判断是否成功达到目标
             return final_error <= limit
     
-    return True
+    return True

main.py

@@ -31,11 +31,7 @@ def main():
 
     try:
         print("Recovery stand")
-        msg.mode = 12  # Recovery stand
-        msg.gait_id = 0
-        msg.life_count += 1
-        Ctrl.Send_cmd(msg)
-        Ctrl.Wait_finish(12, 0)
+        Ctrl.base_msg.stand_up()
         
         # time.sleep(100) # TEST 
 

task_1/task_1.py

@@ -8,6 +8,7 @@ sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
 from base_move.move_base_hori_line import move_to_hori_line, arc_turn_around_hori_line, align_to_horizontal_line
 from utils.detect_track import detect_horizontal_track_edge
 from base_move.move_base_hori_line import calculate_distance_to_line
+from base_move.go_straight import go_straight
 
 observe = False
 
@@ -38,7 +39,7 @@ def run_task_1(ctrl, msg):
 
     print('😺 task 1 - 3')
     # direction = True if qr_result == 'A-1' else False
-    # TODO 
+    # TODO
     turn_success = arc_turn_around_hori_line(
         ctrl=ctrl,
         msg=msg,
@@ -47,13 +48,21 @@ def run_task_1(ctrl, msg):
         left=False, # direction,
         pass_align=True,
         observe=observe,
-
         # TODO clear
         bad_big_angle_corret=True
     )
 
     print('😺 task 1 - 4')
+    move_distance = 0.4
+    go_straight(ctrl, msg, distance=move_distance, speed=0.5, observe=observe)
     # move_to_hori_line(ctrl, msg, target_distance=0.6, observe=observe)
 
     print('😺 task 1 - 5 休眠，模拟装货')
+    ctrl.base_msg.lie_down(wait_time=5000)
+
+    # 站起来
+    ctrl.base_msg.stand_up()
+
+    print('😺 task 1 - 6 back')
+    go_straight(ctrl, msg, distance=-move_distance + 0.1, speed=0.5, observe=observe)
 

utils/base_msg.py

@@ -61,4 +61,26 @@ class BaseMsg:
         self.msg.life_count += 1
         self.ctrl.Send_cmd(self.msg)
         time.sleep(step_time / 1000)
+
+    def lie_down(self, wait_time=500):
+        """发送趴下指令
+        
+        参数:
+            wait_time: 等待时间(毫秒)，默认为500ms
+        """
+        assert wait_time != 0 or wait_time > 3000, "wait_time 必须在 0 or > 3000"
+        self.msg.mode = 7
+        self.msg.gait_id = 1
+        self.msg.duration = wait_time
+        self.msg.life_count += 1
+        self.ctrl.Send_cmd(self.msg)
+        if wait_time:
+            time.sleep(wait_time / 1000)
+
+    def stand_up(self):
+        self.msg.mode = 12
+        self.msg.gait_id = 0
+        self.msg.life_count += 1
+        self.ctrl.Send_cmd(self.msg)
+        self.ctrl.Wait_finish(12, 0)