diff --git a/base_move/go_straight.py b/base_move/go_straight.py index da7c25c..c6d2fc3 100644 --- a/base_move/go_straight.py +++ b/base_move/go_straight.py @@ -422,6 +422,164 @@ def go_straight_with_qrcode(ctrl, msg, distance, speed=0.5, observe=False): return go_success, res +def go_lateral(ctrl, msg, distance, speed=0.5, observe=False, + mode=11, + gait_id=26, + step_height=[0.06, 0.06], + ): + """ + 控制机器人沿y轴方向(侧向)行走指定距离 + + 参数: + ctrl: Robot_Ctrl 对象，包含里程计信息 + msg: robot_control_cmd_lcmt 对象，用于发送命令 + distance: 要行走的距离(米)，正值为向左，负值为向右 + speed: 行走速度(米/秒)，范围0.1~1.0，默认为0.5 + observe: 是否输出中间状态信息，默认为False + + 返回: + bool: 是否成功完成行走 + """ + # 参数验证 + if abs(distance) < 0.01: + info("距离太短，无需移动", "信息") + return True + + # 限制速度范围 + speed = min(max(abs(speed), 0.1), 1.0) + + # 确定左移或右移方向 + leftward = distance > 0 + move_speed = speed if leftward else -speed + abs_distance = abs(distance) + + # 获取起始位置 + start_position = list(ctrl.odo_msg.xyz) + start_yaw = ctrl.odo_msg.rpy[2] # 记录起始朝向，用于保持直线 + + if observe: + debug(f"起始位置: {start_position}", "位置") + info(f"开始{'向左' if leftward 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 = mode # Locomotion模式 + msg.gait_id = gait_id # 自变频步态 + + # 根据需要移动的距离动态调整移动速度 + 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 # 非常接近时用更慢速度 + + # 设置移动速度和方向 - 在y轴方向移动 + msg.vel_des = [0, actual_speed, 0] # [前进速度, 侧向速度, 角速度] + msg.duration = 0 # wait next cmd + msg.step_height = step_height # 抬腿高度 + 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) + + # 计算完成比例 + 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[1]) > 0.05: + info(f"减速: {msg.vel_des[1]:.2f} -> {new_speed:.2f} 米/秒 (完成: {completion_ratio*100:.1f}%)", "移动") + + msg.vel_des[1] = new_speed + msg.life_count += 1 + ctrl.Send_cmd(msg) + + if observe and current_time - start_time > 1 and (current_time % 0.5 < position_check_interval): + debug(f"已移动: {distance_moved:.3f}米, 目标: {abs_distance:.3f}米 (完成: {completion_ratio*100:.1f}%)", "距离") + debug(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: + success(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) + go_success = distance_error < 0.1 # 如果误差小于10厘米，则认为成功 + + if observe: + info(f"目标距离: {abs_distance:.3f}米, 实际距离: {actual_distance:.3f}米, 误差: {distance_error:.3f}米", "距离") + if go_success: + success(f"移动成功", "成功") + else: + warning(f"移动失败，误差过大: {distance_error:.3f}米", "失败") + + return go_success # 用法示例 if __name__ == "__main__": @@ -430,4 +588,8 @@ if __name__ == "__main__": # go_straight(ctrl, msg, 1.0, speed=0.5, observe=True) # 后退0.5米 # go_straight(ctrl, msg, -0.5, speed=0.3, observe=True) + # 向左移动0.5米 + # go_lateral(ctrl, msg, 0.5, speed=0.3, observe=True) + # 向右移动0.8米 + # go_lateral(ctrl, msg, -0.8, speed=0.3, observe=True) pass diff --git a/main.py b/main.py index eb092bf..66a4092 100644 --- a/main.py +++ b/main.py @@ -57,7 +57,6 @@ def main(): # print(f"arrow_direction: {arrow_direction}") # run_task_2_5(Ctrl, msg, direction=arrow_direction) - arrow_direction = 'right' # TEST # if arrow_direction == 'left': diff --git a/task_4/task_4.py b/task_4/task_4.py index 1d91a46..4d13c11 100644 --- a/task_4/task_4.py +++ b/task_4/task_4.py @@ -8,7 +8,7 @@ import numpy as np sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from base_move.turn_degree import turn_degree, turn_degree_v2 -from base_move.go_straight import go_straight +from base_move.go_straight import go_straight, go_lateral from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing from utils.gray_sky_analyzer import analyze_gray_sky_ratio from base_move.move_base_hori_line import go_straight_until_hori_line @@ -48,9 +48,11 @@ def run_task_4_back(ctrl, msg): image_processor: 可选的图像处理器实例 """ turn_degree_v2(ctrl, msg, degree=-90, absolute=True) - center_on_dual_tracks(ctrl, msg, max_time=30, observe=False, stone_path_mode=False) + # center_on_dual_tracks(ctrl, msg, max_time=30, observe=False, stone_path_mode=False) - return + # 向右移动0.5秒 + section('任务4-回程：向右移动', "移动") + go_lateral(ctrl, msg, distance=-0.3, speed=0.1, observe=True) section('任务4-1：移动直到灰色天空比例低于阈值', "天空检测") go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.35, speed=0.5) @@ -58,7 +60,7 @@ def run_task_4_back(ctrl, msg): section('任务4-2：通过栏杆', "移动") pass_bar(ctrl, msg) - section('任务4-3：跟随轨道', "移动") + section('任务4-3：stone', "移动") go_straight(ctrl, msg, distance=1, speed=2) # go_straight_with_visual_track(ctrl, msg, distance=4.5, observe=False, # mode=11, @@ -74,7 +76,7 @@ def run_task_4_back(ctrl, msg): ctrl, msg, target_distance=1, # 目标与黄线的距离(米) - max_distance=5.0, # 最大搜索距离(米) + max_distance=6.0, # 最大搜索距离(米) speed=0.2, # 移动速度(米/秒) mode=11, # 运动模式 gait_id=3, # 步态ID（快步跑） @@ -82,6 +84,8 @@ def run_task_4_back(ctrl, msg): observe=True # 显示调试信息 ) + go_straight(ctrl, msg, distance=0.8, speed=0.2, observe=True, mode=11, gait_id=3, step_height=[0.21, 0.21]) + def go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2,