diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index a3f9ad2..73c2d42 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -288,9 +288,10 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): # 使用里程计进行实时监控移动距离 distance_moved = 0 start_time = time.time() - timeout = move_time + 1 # 超时时间设置为预计移动时间加5秒 + timeout = move_time + 1 # 超时时间设置为预计移动时间加1秒 - while distance_moved < abs(distance_to_move) and time.time() - start_time < timeout: + # 监控移动距离,但不执行减速(改用stop_smooth) + while distance_moved < abs(distance_to_move) * 0.95 and time.time() - start_time < timeout: # 计算已移动距离 current_position = ctrl.odo_msg.xyz dx = current_position[0] - start_position[0] @@ -299,18 +300,20 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 print(f"已移动: {distance_moved:.3f}米, 目标: {abs(distance_to_move):.3f}米") - - # 如果已经接近目标距离,准备停止 - if distance_moved >= abs(distance_to_move) * 0.95: - break time.sleep(0.05) # 小间隔检查位置 - # 发送停止命令 - ctrl.base_msg.stop_force() + # 使用平滑停止方法 + if forward: + current_vel = [move_speed, 0, 0] + else: + current_vel = [-move_speed, 0, 0] + + # 使用平滑停止 + ctrl.base_msg.stop_smooth(current_vel=current_vel) if observe: - print(f"移动完成,通过里程计计算的移动距离: {distance_moved:.3f}米") + print(f"移动完成,平稳停止,通过里程计计算的移动距离: {distance_moved:.3f}米") # 在终点放置红色标记 end_position = list(ctrl.odo_msg.xyz) if hasattr(ctrl, 'place_marker'): @@ -325,6 +328,7 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left 参数: ctrl: Robot_Ctrl对象 msg: robot_control_cmd_lcmt对象 + target_distance: 横线前目标保持距离,默认0.5米 angle_deg: 旋转角度,支持90或180度 left: True为左转,False为右转 observe: 是否打印调试信息 @@ -375,7 +379,7 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left msg.mode = 11 msg.gait_id = 26 msg.vel_des = [v, 0, w] # [前进速度, 侧向速度, 角速度] - msg.duration = int((t+1) * 1000) # 加1秒余量 + msg.duration = int((t + 2) * 1000) # 加2秒余量 msg.step_height = [0.06, 0.06] msg.life_count += 1 @@ -394,12 +398,14 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left 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: + # 监控旋转进度,但不执行减速(改用stop_smooth) + while distance_moved < arc_length * 0.95 and abs(angle_turned) < abs(angle_rad) * 0.95 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.rpy[2] angle_turned = current_yaw - start_yaw @@ -408,13 +414,17 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left angle_turned -= 2 * math.pi while angle_turned < -math.pi: angle_turned += 2 * math.pi + + if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 + print(f"已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度") + time.sleep(0.05) - # 5. 发送停止指令 - ctrl.base_msg.stop_force() - + # 使用平滑停止 + ctrl.base_msg.stop_smooth(current_vel=[v, 0, w]) + if observe: - print("圆弧运动完成") + print("圆弧运动完成,平稳停止") return True diff --git a/utils/base_msg.py b/utils/base_msg.py index 6b32d49..2ee2146 100644 --- a/utils/base_msg.py +++ b/utils/base_msg.py @@ -1,3 +1,4 @@ +import time class BaseMsg: def __init__(self, ctrl, msg): @@ -10,4 +11,60 @@ class BaseMsg: 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 + self.ctrl.Wait_finish(0, 0) + + def stop_smooth(self, current_vel=None, steps=3, delay=0.2): + """ + 平滑停止移动,通过逐步减小速度而不是强制停止 + + 参数: + current_vel: 当前速度列表 [x, y, z],如果为None则使用当前消息中的速度 + steps: 减速步骤数,默认为3步 + delay: 每步之间的延迟时间(秒),默认为0.2秒 + """ + + # 如果没有提供当前速度,使用消息中的当前速度 + if current_vel is None: + current_vel = self.msg.vel_des.copy() if hasattr(self.msg, 'vel_des') else [0, 0, 0] + + # 保存当前消息的一些参数 + current_mode = self.msg.mode + current_gait_id = self.msg.gait_id + + # 检查是否有速度需要减小 + if all(abs(v) < 0.01 for v in current_vel): + # 速度已经很小,直接停止 + self.stop_force() + return + + # 逐步减小速度 + for i in range(1, steps + 1): + # 计算这一步的减速比例 + ratio = (steps - i) / steps + + # 计算新的速度 + new_vel = [v * ratio for v in current_vel] + + # 更新消息 + self.msg.mode = current_mode + self.msg.gait_id = current_gait_id + self.msg.vel_des = new_vel + self.msg.duration = int(delay * 1000) # 毫秒 + self.msg.life_count += 1 + + # 发送命令 + self.ctrl.Send_cmd(self.msg) + + # 等待这一步完成 + time.sleep(delay) + + # 最后发送一个零速度命令,确保完全停止 + self.msg.mode = current_mode + self.msg.gait_id = current_gait_id + self.msg.vel_des = [0, 0, 0] + self.msg.duration = int(delay * 1000) + self.msg.life_count += 1 + self.ctrl.Send_cmd(self.msg) + + # 等待最后的命令完成 + time.sleep(delay) \ No newline at end of file