Enhance navigation and movement functions in tasks

- Updated main.py to adjust task execution flow and added print statements for debugging.
- Refactored task_2.py to utilize go_to_xy_v2 for improved navigation accuracy and added an xy_flag parameter to run_task_2.
- Introduced go_to_xy_with_correction_v2 and go_to_xy_v2 in go_to_xy.py for enhanced movement control and precision.
- Modified task_2_5.py to incorporate turn_degree_v2 and added go_straight function for better maneuverability.

base_move/go_to_xy.py

@@ -258,11 +258,368 @@ def go_to_xy_with_correction(ctrl, msg, target_x, target_y, speed=0.5, precision
     
     return False
 
-# 用法示例
-if __name__ == "__main__":
-    # 这里是示例代码，实际使用时需要提供合适的ctrl和msg对象
-    # 移动到坐标(1.0, 2.0)
-    # go_to_xy(ctrl, msg, 1.0, 2.0, speed=0.5, observe=True)
-    # 移动到坐标(0.0, 0.0)并自动修正
-    # go_to_xy_with_correction(ctrl, msg, 0.0, 0.0, speed=0.4, observe=True)
-    pass 
+def go_to_xy_v2(ctrl, msg, target_x, target_y, speed=0.5, precision=True, observe=False):
+    """
+    控制机器人移动到指定的(x,y)坐标位置，使用增强的里程计监控和校正机制
+    
+    参数:
+    ctrl: Robot_Ctrl 对象，包含里程计信息
+    msg: robot_control_cmd_lcmt 对象，用于发送命令
+    target_x: 目标X坐标(米)
+    target_y: 目标Y坐标(米)
+    speed: 行走速度(米/秒)，范围0.1~1.0，默认为0.5
+    precision: 是否使用高精度模式（更慢速度），默认为True
+    observe: 是否输出中间状态信息，默认为False
+    
+    返回:
+    bool: 是否成功完成移动到目标位置
+    """
+    # 获取当前位置
+    current_position = ctrl.odo_msg.xyz
+    current_x, current_y = current_position[0], current_position[1]
+    
+    if observe:
+        section('目标点导航V2', "开始")
+        info(f"当前位置: ({current_x:.3f}, {current_y:.3f})", "位置")
+        info(f"目标位置: ({target_x:.3f}, {target_y:.3f})", "目标")
+        # 在起点放置标记
+        if hasattr(ctrl, 'place_marker'):
+            ctrl.place_marker(current_x, current_y, 
+                            current_position[2] if len(current_position) > 2 else 0.0, 
+                            'blue', observe=True)
+        # 在目标点放置标记
+        if hasattr(ctrl, 'place_marker'):
+            ctrl.place_marker(target_x, target_y, 0.0, 'green', observe=True)
+    
+    # 计算与目标点的距离
+    dx = target_x - current_x
+    dy = target_y - current_y
+    distance = math.sqrt(dx*dx + dy*dy)
+    
+    # 如果已经非常接近目标，无需移动
+    if distance < 0.05:  # 5厘米以内视为已达到
+        if observe:
+            success("已处于目标位置附近，无需移动", "完成")
+        return True
+    
+    if observe:
+        info(f"到目标点的距离: {distance:.3f}米", "路径")
+    
+    # 限制速度范围
+    max_speed = min(max(abs(speed), 0.1), 1.0)
+    if precision:
+        max_speed = min(max_speed, 0.5)  # 高精度模式下限制最大速度
+    
+    # 根据距离动态调整速度
+    if distance > 1.0:
+        actual_speed = max_speed
+    elif distance > 0.5:
+        actual_speed = max_speed * 0.8
+    elif distance > 0.2:
+        actual_speed = max_speed * 0.6
+    else:
+        actual_speed = max_speed * 0.4
+    
+    # 设置移动命令基本参数
+    msg.mode = 11  # Locomotion模式
+    msg.gait_id = 26  # 自变频步态
+    msg.step_height = [0.06, 0.06]  # 抬腿高度
+    
+    # 估算移动时间，但实际上会通过里程计控制
+    estimated_time = distance / actual_speed
+    timeout = estimated_time + 5  # 增加超时时间为预计移动时间加5秒
+    
+    # 增强的监控变量
+    start_time = time.time()
+    position_check_interval = 0.05  # 位置检查间隔(秒)
+    last_check_time = start_time
+    last_position_x, last_position_y = current_x, current_y
+    last_distance = distance
+    stall_count = 0
+    position_history = []  # 用于存储历史位置，检测轨迹偏移
+    
+    # 轨迹偏移检测阈值
+    trajectory_check_interval = 0.2  # 每0.2秒检查一次轨迹
+    last_trajectory_check = start_time
+    max_trajectory_deviation = 0.15  # 最大允许的轨迹偏移（米）
+    
+    if observe:
+        info(f"开始移动，预计用时 {estimated_time:.2f}秒", "移动")
+    
+    # 监控移动到达目标点
+    while 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_x, current_y = current_position[0], current_position[1]
+            
+            # 记录位置历史用于轨迹分析
+            position_history.append((current_x, current_y, current_time))
+            # 只保留最近的位置历史
+            if len(position_history) > 50:
+                position_history = position_history[-50:]
+            
+            # 计算新的位移向量和距离
+            dx = target_x - current_x
+            dy = target_y - current_y
+            current_distance = math.sqrt(dx*dx + dy*dy)
+            
+            # 判断是否已经足够接近目标
+            if current_distance < 0.05:  # 5厘米以内视为已达到
+                break
+            
+            # 计算移动进度
+            progress = 1.0 - (current_distance / distance)
+            
+            # 检测是否卡住（距离变化很小）
+            distance_change = last_distance - current_distance
+            position_change = math.sqrt((current_x - last_position_x)**2 + (current_y - last_position_y)**2)
+            
+            if position_change < 0.005:  # 几乎没有移动（5毫米以内）
+                stall_count += 1
+                if stall_count > 10:  # 连续多次检测到几乎没有移动
+                    if observe:
+                        warning("检测到机器人可能卡住，调整策略", "移动")
+                    # 尝试短暂后退再前进
+                    msg.vel_des = [-0.1, 0, 0]  # 小幅后退
+                    msg.life_count = (msg.life_count + 1) % 127
+                    ctrl.Send_cmd(msg)
+                    time.sleep(0.5)
+                    stall_count = 0
+                    continue
+            else:
+                stall_count = 0
+            
+            # 动态调整速度，接近目标时减速
+            if current_distance < 0.2:  # 接近目标时减速
+                target_speed = max_speed * 0.3
+            elif current_distance < 0.5:
+                target_speed = max_speed * 0.5
+            else:
+                target_speed = actual_speed
+            
+            # 计算速度分量，归一化后乘以目标速度
+            speed_factor = target_speed / current_distance if current_distance > 0 else 0
+            vel_x = dx * speed_factor
+            vel_y = dy * speed_factor
+            
+            # 限制速度大小
+            vel_magnitude = math.sqrt(vel_x*vel_x + vel_y*vel_y)
+            if vel_magnitude > target_speed:
+                scale_factor = target_speed / vel_magnitude
+                vel_x *= scale_factor
+                vel_y *= scale_factor
+            
+            # 更新速度命令
+            msg.vel_des = [vel_x, vel_y, 0]  # [前进速度, 侧向速度, 角速度]
+            msg.duration = 0  # 持续执行直到下一个命令
+            msg.life_count = (msg.life_count + 1) % 127  # 防止溢出，确保life_count在int8_t范围内
+            ctrl.Send_cmd(msg)
+            
+            if observe and (current_time - start_time) % 1.0 < position_check_interval:
+                debug(f"当前位置: ({current_x:.3f}, {current_y:.3f}), 剩余距离: {current_distance:.3f}米, 完成: {progress*100:.1f}%", "移动")
+                debug(f"速度: x={vel_x:.2f}, y={vel_y:.2f}", "速度")
+            
+            # 保存当前状态用于下次比较
+            last_check_time = current_time
+            last_distance = current_distance
+            last_position_x, last_position_y = current_x, current_y
+        
+        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
+    final_x, final_y = final_position[0], final_position[1]
+    final_dx = target_x - final_x
+    final_dy = target_y - final_y
+    final_distance = math.sqrt(final_dx*final_dx + final_dy*final_dy)
+    
+    if observe:
+        info(f"最终位置: ({final_x:.3f}, {final_y:.3f})", "位置")
+        info(f"与目标的距离误差: {final_distance:.3f}米", "误差")
+        # 在终点放置标记
+        if hasattr(ctrl, 'place_marker'):
+            ctrl.place_marker(final_x, final_y, 
+                           final_position[2] if len(final_position) > 2 else 0.0, 
+                           'red', observe=True)
+    
+    # 判断是否成功到达目标点（误差在10厘米以内）
+    nav_success = final_distance < 0.1
+    
+    # 如果误差较大但还在可接受范围内，尝试精确调整
+    if not nav_success and final_distance < 0.3:
+        if observe:
+            warning(f"第一阶段导航误差为 {final_distance:.3f}米，尝试精确调整", "调整")
+        
+        # 使用直线模式进行精确调整
+        msg.mode = 11
+        msg.gait_id = 1  # 使用步行步态提高精度
+        
+        # 计算新的速度分量，使用更低的速度
+        dx = target_x - final_x
+        dy = target_y - final_y
+        adjust_distance = math.sqrt(dx*dx + dy*dy)
+        adjust_speed = min(0.2, max_speed * 0.4)  # 降低速度提高精度
+        
+        # 计算速度分量
+        speed_factor = adjust_speed / adjust_distance if adjust_distance > 0 else 0
+        vel_x = dx * speed_factor
+        vel_y = dy * speed_factor
+        
+        # 短暂精确移动
+        adjust_time = min(adjust_distance / adjust_speed * 1.5, 3.0)  # 最多3秒的调整时间
+        adjust_start = time.time()
+        
+        while time.time() - adjust_start < adjust_time:
+            # 更新当前位置
+            current_position = ctrl.odo_msg.xyz
+            current_x, current_y = current_position[0], current_position[1]
+            
+            # 重新计算距离和方向
+            dx = target_x - current_x
+            dy = target_y - current_y
+            current_distance = math.sqrt(dx*dx + dy*dy)
+            
+            # 如果已经足够接近，提前结束
+            if current_distance < 0.05:
+                break
+            
+            # 动态调整速度
+            adjust_speed = min(0.2, current_distance * 0.5)
+            speed_factor = adjust_speed / current_distance if current_distance > 0 else 0
+            vel_x = dx * speed_factor
+            vel_y = dy * speed_factor
+            
+            # 发送命令
+            msg.vel_des = [vel_x, vel_y, 0]
+            msg.life_count = (msg.life_count + 1) % 127
+            ctrl.Send_cmd(msg)
+            
+            if observe and (time.time() - adjust_start) % 0.5 < 0.05:
+                debug(f"精确调整中: 位置 ({current_x:.3f}, {current_y:.3f}), 距离: {current_distance:.3f}米", "调整")
+            
+            time.sleep(0.05)
+        
+        # 停止
+        if hasattr(ctrl.base_msg, 'stop_smooth'):
+            ctrl.base_msg.stop_smooth()
+        else:
+            ctrl.base_msg.stop()
+        
+        # 获取最终位置并重新计算误差
+        final_position = ctrl.odo_msg.xyz
+        final_x, final_y = final_position[0], final_position[1]
+        final_dx = target_x - final_x
+        final_dy = target_y - final_y
+        final_distance = math.sqrt(final_dx*final_dx + final_dy*final_dy)
+        
+        if observe:
+            info(f"调整后最终位置: ({final_x:.3f}, {final_y:.3f})", "位置")
+            info(f"调整后误差: {final_distance:.3f}米", "误差")
+            # 更新终点标记
+            if hasattr(ctrl, 'place_marker'):
+                ctrl.place_marker(final_x, final_y, 
+                               final_position[2] if len(final_position) > 2 else 0.0, 
+                               'purple', observe=True)
+        
+        # 重新判断是否成功
+        nav_success = final_distance < 0.1
+    
+    if observe:
+        if nav_success:
+            success(f"成功到达目标点，误差: {final_distance:.3f}米", "成功")
+        else:
+            warning(f"未能精确到达目标点，误差: {final_distance:.3f}米", "警告")
+    
+    return nav_success
+def go_to_xy_with_correction_v2(ctrl, msg, target_x, target_y, speed=0.5, precision=True, 
+                              max_attempts=2, observe=False):
+    """
+    控制机器人移动到指定的(x,y)坐标位置，使用增强的里程计监控和校正机制，并在必要时进行多次尝试
+    
+    参数:
+    ctrl: Robot_Ctrl 对象，包含里程计信息
+    msg: robot_control_cmd_lcmt 对象，用于发送命令
+    target_x: 目标X坐标(米)
+    target_y: 目标Y坐标(米)
+    speed: 行走速度(米/秒)，范围0.1~1.0，默认为0.5
+    precision: 是否使用高精度模式，默认为True
+    max_attempts: 最大尝试次数，默认为2
+    observe: 是否输出中间状态信息，默认为False
+    
+    返回:
+    bool: 是否成功完成移动到目标位置
+    """
+    attempt = 1
+    
+    while attempt <= max_attempts:
+        if observe:
+            section(f'导航尝试V2 {attempt}/{max_attempts}', "开始")
+        
+        # 执行增强版导航
+        success = go_to_xy_v2(ctrl, msg, target_x, target_y, speed, precision, observe)
+        
+        if success:
+            return True
+        
+        # 如果导航失败且还有尝试次数，进行修正
+        if attempt < max_attempts:
+            if observe:
+                warning(f"第{attempt}次导航未达到预期精度，尝试不同策略", "修正")
+            
+            # 获取当前位置
+            current_position = ctrl.odo_msg.xyz
+            current_x, current_y = current_position[0], current_position[1]
+            
+            # 计算与目标点的距离和角度
+            dx = target_x - current_x
+            dy = target_y - current_y
+            distance = math.sqrt(dx*dx + dy*dy)
+            
+            # 距离较近时使用更精确的方式
+            if distance < 0.3:  # 30厘米以内
+                if observe:
+                    info(f"距离目标较近 ({distance:.3f}米)，使用精确模式移动", "策略")
+                speed = min(speed * 0.6, 0.3)  # 降低速度
+                precision = True
+            else:
+                # 距离较远时尝试用分段路径
+                if observe:
+                    info(f"距离目标较远 ({distance:.3f}米)，尝试分段导航", "策略")
+                # 计算中间点
+                mid_x = current_x + dx * 0.5
+                mid_y = current_y + dy * 0.5
+                
+                # 先移动到中间点
+                if observe:
+                    info(f"先移动到中间点 ({mid_x:.3f}, {mid_y:.3f})", "中间点")
+                mid_success = go_to_xy_v2(ctrl, msg, mid_x, mid_y, speed, precision, observe)
+                
+                if not mid_success and observe:
+                    warning("到达中间点失败，直接尝试目标点", "策略调整")
+        
+        attempt += 1
+    
+    # 获取最终位置并计算与目标的误差
+    final_position = ctrl.odo_msg.xyz
+    final_x, final_y = final_position[0], final_position[1]
+    final_dx = target_x - final_x
+    final_dy = target_y - final_y
+    final_distance = math.sqrt(final_dx*final_dx + final_dy*final_dy)
+    
+    if observe:
+        if final_distance < 0.2:
+            warning(f"虽未达到精确目标，但已在目标附近 ({final_distance:.3f}米)", "部分成功")
+        else:
+            error(f"多次尝试后仍未能达到目标点，最终误差: {final_distance:.3f}米", "失败")
+    
+    return False

main.py

@@ -28,6 +28,7 @@ from task_test.task_left_line import run_task_test
 from task_5.task_5 import run_task_5
 # from task_test.go_to_xy_example import run_go_to_xy_example
 
+
 from base_move.turn_degree import turn_degree
 
 pass_marker = True
@@ -44,10 +45,11 @@ def main():
         Ctrl.base_msg.stop()  # BUG 垃圾指令 for eat
         
         # time.sleep(100) # TEST 
-        run_task_1(Ctrl, msg)
+        # run_task_1(Ctrl, msg)
 
-        arrow_direction = run_task_2(Ctrl, msg)
+        arrow_direction = run_task_2(Ctrl, msg, xy_flag=False)
 
+        print(f"arrow_direction: {arrow_direction}")
         run_task_2_5(Ctrl, msg, direction=arrow_direction)
 
         if arrow_direction == 'left':

task_2/task_2.py

@@ -8,7 +8,7 @@ import queue
 from threading import Thread, Lock
 
 sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
-from base_move.go_to_xy import go_to_xy
+from base_move.go_to_xy import go_to_xy, go_to_xy_v2
 from base_move.turn_degree import turn_degree, turn_degree_twice, turn_degree_v2
 from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
 from utils.decode_arrow import detect_arrow_direction, visualize_arrow_detection
@@ -100,7 +100,7 @@ class AsyncArrowDetector:
                             # 保存检测结果的可视化图像
                             timestamp = time.strftime("%Y%m%d_%H%M%S")
                             save_path = f"{self.save_dir}/arrow_detection_{timestamp}.jpg"
-                            visualize_arrow_detection(img, save_path=save_path)
+                            # visualize_arrow_detection(img, save_path=save_path)
                             info(f"箭头检测可视化结果已保存至: {save_path}", "箭头检测")
                     except Exception as e:
                         error(f"异步箭头检测出错: {str(e)}", "错误")
@@ -115,9 +115,9 @@ class AsyncArrowDetector:
         with self.lock:
             return self.arrow_result, self.result_time, self.last_processed_image
 
-def run_task_2(ctrl, msg):
+def run_task_2(ctrl, msg, xy_flag=False):
     # 微调 xy 和角度
-    go_to_xy(ctrl, msg, 0.9, 0.25, speed=0.5, observe=True)
+    go_to_xy_v2(ctrl, msg, 0.9, 0.25, speed=0.5, observe=True)
     turn_degree(ctrl, msg, 0.8, absolute=True)
 
     print('角度为',ctrl.odo_msg.rpy[2])
@@ -145,7 +145,7 @@ def run_task_2(ctrl, msg):
         (0.3,0,0.6,0,0,0,1),
         # #前走一点
         # (0.3,0,0,0,0,0,0.5)
-        ]  # [vel_x, vel_z] 对应 [左转, 右转, 左转]
+    ]  # [vel_x, vel_z] 对应 [左转, 右转, 左转]
     
     gotoxy=[
         (0.87,0.23),
@@ -184,8 +184,9 @@ def run_task_2(ctrl, msg):
         msg.life_count += 1
         ctrl.Send_cmd(msg)
         
-        # 根据转弯方向调整持续时间（假设半圆需要3秒）
-        go_to_xy(ctrl, msg, gotoxy[i][0], gotoxy[i][1], speed=0.5, observe=True)
+        # 根据转弯方向调整持续时间（假设半圆需要3秒
+        if xy_flag:
+            go_to_xy(ctrl, msg, gotoxy[i][0], gotoxy[i][1], speed=0.5, observe=True)
         # 打印当前方向
         print(f"开始 {text[i]} ")
         print(f"第{i}次",'角度为',ctrl.odo_msg.rpy[2])
@@ -222,14 +223,14 @@ def run_task_2(ctrl, msg):
                 image = ctrl.image_processor.get_current_image()
                 if image is not None:
                     # 直接在当前图像上检测
-                    arrow_direction = detect_arrow_direction(image, observe=True)
+                    arrow_direction = detect_arrow_direction(image, observe=False)
                     info(f"直接检测到箭头方向: {arrow_direction}", "箭头检测")
                     
                     # 保存检测结果的可视化图像
                     timestamp = time.strftime("%Y%m%d_%H%M%S")
                     save_path = f"logs/image/arrow_detection_final_{timestamp}.jpg"
                     os.makedirs(os.path.dirname(save_path), exist_ok=True)
-                    visualize_arrow_detection(image, save_path=save_path)
+                    # visualize_arrow_detection(image, save_path=save_path)
                     info(f"最终箭头检测可视化结果已保存至: {save_path}", "箭头检测")
                 else:
                     warning("无法获取当前图像，箭头方向检测失败", "箭头检测")

task_2_5/task_2_5.py

@@ -5,7 +5,8 @@ import os
 # 添加父目录到路径，以便能够导入utils
 sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
 
-from base_move.turn_degree import turn_degree
+from base_move.turn_degree import turn_degree, turn_degree_v2
+from base_move.go_to_xy import go_straight
 from base_move.move_base_hori_line import arc_turn_around_hori_line, align_to_horizontal_line
 from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
 
@@ -14,6 +15,8 @@ observe = True
 def run_task_2_5(Ctrl, msg, direction='left'):
     section('任务2.5：预备进入任务3', "启动")
 
+    go_straight(Ctrl, msg, distance=-0.1, speed=0.5, observe=observe)
+
     # TEST
     turn_degree(Ctrl, msg, 90, absolute=observe)
 
@@ -28,9 +31,11 @@ def run_task_2_5(Ctrl, msg, direction='left'):
         observe=observe,
     )
 
+    go_straight(Ctrl, msg, distance=0.2, speed=0.5, observe=observe)
+
     section('任务2.5-2：第二次旋转', "移动")
 
-    turn_degree(Ctrl, msg, degree=90, absolute=True)
+    turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
 
     # arc_turn_around_hori_line(
     #     Ctrl,