diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 9bff0b6..3087286 100644 --- a/base_move/move_base_hori_line.py +++ b/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 - msg.gait_id = 0 - msg.duration = 0 - ctrl.Send_cmd(msg) + ctrl.base_msg.stop_force() 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) # 线速度 - t = abs(angle_rad / w) # 运动时间 + v = w * r # 线速度,正负号与角速度方向一致 + 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 - msg.gait_id = 0 - msg.duration = 0 - ctrl.Send_cmd(msg) + ctrl.base_msg.stop_force() if observe: print("圆弧运动完成") diff --git a/main.py b/main.py index e0f834f..d29a450 100644 --- a/main.py +++ b/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) diff --git a/utils/base_msg.py b/utils/base_msg.py new file mode 100644 index 0000000..6b32d49 --- /dev/null +++ b/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) \ No newline at end of file