优化移动控制逻辑，改进平滑停止方法
- 修改 move_to_hori_line 和 arc_turn_around_hori_line 函数，使用平滑停止代替强制停止 - 调整超时时间和移动监控逻辑，确保机器人在接近目标时平稳减速 - 在 BaseMsg 类中新增 stop_smooth 方法，实现逐步减小速度的功能
This commit is contained in:
parent
c6f236ca92
commit
cb84ca79ac
@ -318,9 +318,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]
|
||||
@ -329,18 +330,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'):
|
||||
@ -355,6 +358,7 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left
|
||||
参数:
|
||||
ctrl: Robot_Ctrl对象
|
||||
msg: robot_control_cmd_lcmt对象
|
||||
target_distance: 横线前目标保持距离，默认0.5米
|
||||
angle_deg: 旋转角度，支持90或180度
|
||||
left: True为左转，False为右转
|
||||
observe: 是否打印调试信息
|
||||
@ -402,7 +406,7 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left
|
||||
msg.mode = 11
|
||||
msg.gait_id = 26
|
||||
msg.vel_des = [v, 0, w] # [前进速度, 侧向速度, 角速度]
|
||||
msg.duration = int((t + 2) * 1000) # 加1秒余量
|
||||
msg.duration = int((t + 2) * 1000) # 加2秒余量
|
||||
msg.step_height = [0.06, 0.06]
|
||||
msg.life_count += 1
|
||||
|
||||
@ -421,12 +425,14 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, 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
|
||||
@ -435,13 +441,17 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, 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
|
||||
|
||||
|
||||
@ -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)
|
||||
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)
|
||||
Loading…
x
Reference in New Issue
Block a user