删除当前图像文件,优化 main.py 中的恢复站立逻辑,新增 lie_down 和 stand_up 方法到 BaseMsg 类,更新 task_1.py 以使用新的移动和休息功能。

This commit is contained in:
Havoc 2025-05-17 11:27:39 +08:00
parent 35e8cc9858
commit 18248d7ad9
6 changed files with 233 additions and 8 deletions

198
base_move/go_straight.py Normal file
View File

@ -0,0 +1,198 @@
import math
import time
import sys
import os
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from utils.localization_lcmt import localization_lcmt
def go_straight(ctrl, msg, distance, speed=0.5, observe=False):
"""
控制机器人沿直线行走指定距离
参数:
ctrl: Robot_Ctrl 对象包含里程计信息
msg: robot_control_cmd_lcmt 对象用于发送命令
distance: 要行走的距离()正值为前进负值为后退
speed: 行走速度(/)范围0.1~1.0默认为0.5
observe: 是否输出中间状态信息默认为False
返回:
bool: 是否成功完成行走
"""
# 参数验证
if abs(distance) < 0.01:
print("距离太短,无需移动")
return True
# 限制速度范围
speed = min(max(abs(speed), 0.1), 1.0)
# 确定前进或后退方向
forward = distance > 0
move_speed = speed if forward else -speed
abs_distance = abs(distance)
# 获取起始位置
start_position = list(ctrl.odo_msg.xyz)
start_yaw = ctrl.odo_msg.rpy[2] # 记录起始朝向,用于保持直线
if observe:
print(f"起始位置: {start_position}")
print(f"开始{'前进' if forward 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 = 11 # Locomotion模式
msg.gait_id = 26 # 自变频步态
# 根据需要移动的距离动态调整移动速度
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 # 非常接近时用更慢速度
# 设置移动速度和方向
msg.vel_des = [actual_speed, 0, 0] # [前进速度, 侧向速度, 角速度]
msg.duration = 0 # wait next cmd
msg.step_height = [0.06, 0.06] # 抬腿高度
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)
# 计算移动方向与初始朝向的偏差
movement_direction = math.atan2(current_position[1] - last_position[1],
current_position[0] - last_position[0]) if distance_moved > 0.05 else start_yaw
yaw_error = movement_direction - start_yaw
# 角度归一化
while yaw_error > math.pi:
yaw_error -= 2 * math.pi
while yaw_error < -math.pi:
yaw_error += 2 * math.pi
# 计算完成比例
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[0]) > 0.05:
print(f"减速: {msg.vel_des[0]:.2f} -> {new_speed:.2f} 米/秒 (完成: {completion_ratio*100:.1f}%)")
msg.vel_des[0] = new_speed
msg.life_count += 1
ctrl.Send_cmd(msg)
# 如果偏离初始方向,进行角度修正
if abs(yaw_error) > angle_correction_threshold:
# 计算角速度修正值,偏差越大修正越强
angular_correction = -yaw_error * 0.5 # 比例系数可调整
# 限制最大角速度修正
angular_correction = max(min(angular_correction, 0.2), -0.2)
if observe and abs(angular_correction) > 0.05:
print(f"方向修正: 偏差 {math.degrees(yaw_error):.1f}度,应用角速度 {angular_correction:.3f}rad/s")
# 应用角速度修正
msg.vel_des[2] = angular_correction
msg.life_count += 1
ctrl.Send_cmd(msg)
elif msg.vel_des[2] != 0:
# 如果方向已修正,重置角速度
msg.vel_des[2] = 0
msg.life_count += 1
ctrl.Send_cmd(msg)
if observe and current_time - start_time > 1 and (current_time % 0.5 < position_check_interval):
print(f"已移动: {distance_moved:.3f}米, 目标: {abs_distance:.3f}米 (完成: {completion_ratio*100:.1f}%)")
print(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:
print(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)
success = distance_error < 0.1 # 如果误差小于10厘米则认为成功
if observe:
print(f"目标距离: {abs_distance:.3f}米, 实际距离: {actual_distance:.3f}米, 误差: {distance_error:.3f}")
print(f"移动{'成功' if success else '失败'}")
return success
# 用法示例
if __name__ == "__main__":
# 这里是示例代码实际使用时需要提供合适的ctrl和msg对象
# 前进1米
# go_straight(ctrl, msg, 1.0, speed=0.5, observe=True)
# 后退0.5米
# go_straight(ctrl, msg, -0.5, speed=0.3, observe=True)
pass

View File

@ -110,4 +110,4 @@ def turn_degree(ctrl, msg, degree=90, absolute=False):
# 判断是否成功达到目标
return final_error <= limit
return True
return True

Binary file not shown.

Before

Width:  |  Height:  |  Size: 76 KiB

View File

@ -31,11 +31,7 @@ def main():
try:
print("Recovery stand")
msg.mode = 12 # Recovery stand
msg.gait_id = 0
msg.life_count += 1
Ctrl.Send_cmd(msg)
Ctrl.Wait_finish(12, 0)
Ctrl.base_msg.stand_up()
# time.sleep(100) # TEST

View File

@ -8,6 +8,7 @@ sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from base_move.move_base_hori_line import move_to_hori_line, arc_turn_around_hori_line, align_to_horizontal_line
from utils.detect_track import detect_horizontal_track_edge
from base_move.move_base_hori_line import calculate_distance_to_line
from base_move.go_straight import go_straight
observe = False
@ -38,7 +39,7 @@ def run_task_1(ctrl, msg):
print('😺 task 1 - 3')
# direction = True if qr_result == 'A-1' else False
# TODO
# TODO
turn_success = arc_turn_around_hori_line(
ctrl=ctrl,
msg=msg,
@ -47,13 +48,21 @@ def run_task_1(ctrl, msg):
left=False, # direction,
pass_align=True,
observe=observe,
# TODO clear
bad_big_angle_corret=True
)
print('😺 task 1 - 4')
move_distance = 0.4
go_straight(ctrl, msg, distance=move_distance, speed=0.5, observe=observe)
# move_to_hori_line(ctrl, msg, target_distance=0.6, observe=observe)
print('😺 task 1 - 5 休眠,模拟装货')
ctrl.base_msg.lie_down(wait_time=5000)
# 站起来
ctrl.base_msg.stand_up()
print('😺 task 1 - 6 back')
go_straight(ctrl, msg, distance=-move_distance + 0.1, speed=0.5, observe=observe)

View File

@ -61,4 +61,26 @@ class BaseMsg:
self.msg.life_count += 1
self.ctrl.Send_cmd(self.msg)
time.sleep(step_time / 1000)
def lie_down(self, wait_time=500):
"""发送趴下指令
参数:
wait_time: 等待时间(毫秒)默认为500ms
"""
assert wait_time != 0 or wait_time > 3000, "wait_time 必须在 0 or > 3000"
self.msg.mode = 7
self.msg.gait_id = 1
self.msg.duration = wait_time
self.msg.life_count += 1
self.ctrl.Send_cmd(self.msg)
if wait_time:
time.sleep(wait_time / 1000)
def stand_up(self):
self.msg.mode = 12
self.msg.gait_id = 0
self.msg.life_count += 1
self.ctrl.Send_cmd(self.msg)
self.ctrl.Wait_finish(12, 0)