havoc420ubuntu
bffcd973e0
- Updated task_1.py to replace turn_degree_twice calls with turn_degree_v2 for improved rotation accuracy. - Modified task_2.py to utilize turn_degree_v2 for enhanced turning capabilities and adjusted movement parameters for better path execution. - Refined go_to_xy parameters in task_2 to optimize navigation and added additional logging for debugging purposes.
148 lines
4.8 KiB
Python
148 lines
4.8 KiB
Python
import time
|
||
import sys
|
||
import os
|
||
import math
|
||
|
||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||
from base_move.go_to_xy import go_to_xy
|
||
from base_move.turn_degree import turn_degree, turn_degree_twice, turn_degree_v2
|
||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||
|
||
from base_move.move_base_hori_line import (
|
||
arc_turn_around_hori_line,
|
||
go_straight_by_hori_line,
|
||
move_to_hori_line,
|
||
align_to_horizontal_line
|
||
)
|
||
|
||
# 创建本模块特定的日志记录器
|
||
logger = get_logger("任务1")
|
||
|
||
observe = True
|
||
|
||
def run_task_2(ctrl, msg):
|
||
# 微调 xy 和角度
|
||
go_to_xy(ctrl, msg, 0.9, 0.25, speed=0.5, observe=True)
|
||
turn_degree(ctrl, msg, 0.8, absolute=True)
|
||
|
||
print('角度为',ctrl.odo_msg.rpy[2])
|
||
#获取微调角度
|
||
text=["直线走一段","第一个圆弧","微调路径","第二个圆弧","微调路径","第三个圆弧","微调路径","第四个圆弧","绕出来","前走一点"]
|
||
# 连续三重弯道行走
|
||
directions = [
|
||
#直线走一段
|
||
(0.4,0,0,0,0,0,1.2),
|
||
#第一个圆弧
|
||
(0.45, 0, 0.77,0,0,0,6.5),
|
||
#微调路径
|
||
(0.3,0,0,0,0,0,0.4),
|
||
#第二个圆弧
|
||
(0.4,0,-0.73,0,0,0,7.9),
|
||
#微调路径
|
||
(0.3,0,0,0,0,0,1),
|
||
# 第三个圆弧
|
||
(0.4,0,0.73,0,0,0,6.4),
|
||
#微调路径
|
||
(0.3,0,0,0,0,0,1.3),
|
||
#第四个圆弧
|
||
(0.4,0,-0.72,0,0,0,3.6),
|
||
#绕出来
|
||
(0.3,0,0.6,0,0,0,1),
|
||
# #前走一点
|
||
# (0.3,0,0,0,0,0,0.5)
|
||
] # [vel_x, vel_z] 对应 [左转, 右转, 左转]
|
||
|
||
gotoxy=[
|
||
(0.87,0.23),
|
||
(1.19,0.22),
|
||
(0.91,1.55),
|
||
(0.81,1.46),
|
||
(0.67,2.59),
|
||
(0.92,2.43),
|
||
(1.53,3.62),
|
||
(1.15,3.70),
|
||
(0.63,4.73),
|
||
]
|
||
|
||
for i, (vel_x, vel_y, vel_z ,rpy_x,rpy_y,rpy_z,t) in enumerate(directions):
|
||
# 设置转向命令
|
||
msg.mode = 11 # Locomotion
|
||
msg.gait_id = 26 # TROT_FAST
|
||
msg.vel_des = [vel_x, vel_y, vel_z] # [x, y, z]，y=0表示不侧移
|
||
msg.rpy_des = [rpy_x, rpy_y, rpy_z]
|
||
msg.duration = 0 # 连续运动
|
||
msg.step_height = [0.03, 0.03]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 根据转弯方向调整持续时间（假设半圆需要3秒）
|
||
go_to_xy(ctrl, msg, gotoxy[i][0], gotoxy[i][1], speed=0.5, observe=True)
|
||
# 打印当前方向
|
||
print(f"开始 {text[i]} ")
|
||
print(f"第{i}次",'角度为',ctrl.odo_msg.rpy[2])
|
||
print(f"第{i}次",'x为',ctrl.odo_msg.xyz[0])
|
||
print(f"第{i}次",'y为',ctrl.odo_msg.xyz[1])
|
||
print(f"第{i}次",'z为',ctrl.odo_msg.xyz[2])
|
||
time.sleep(t) # 持续3秒
|
||
|
||
print('角度为',ctrl.odo_msg.rpy[2])
|
||
print('x为',ctrl.odo_msg.xyz[0])
|
||
print('y为',ctrl.odo_msg.xyz[1])
|
||
print('z为',ctrl.odo_msg.xyz[2])
|
||
# # 停止
|
||
# msg.mode = 7 # PureDamper
|
||
# msg.gait_id = 0
|
||
# msg.life_count += 1
|
||
# Ctrl.Send_cmd(msg)
|
||
# Ctrl.Wait_finish(7, 0)
|
||
|
||
def run_task_2_back(ctrl, msg):
|
||
# go_to_xy(ctrl, msg, 0.2, 1, speed=0.5, observe=True)
|
||
go_to_xy(ctrl, msg, 0.7, 5, speed=0.5, observe=True)
|
||
turn_degree_v2(ctrl, msg, -106.5, absolute=True)
|
||
|
||
print('角度为',ctrl.odo_msg.rpy[2])
|
||
print('x为',ctrl.odo_msg.xyz[0])
|
||
print('y为',ctrl.odo_msg.xyz[1])
|
||
print('z为',ctrl.odo_msg.xyz[2])
|
||
|
||
text=["直线走一段","第一个圆弧","微调路径","第二个圆弧","微调路径","第三个圆弧","微调路径","第四个圆弧","绕出来","前走一点"]
|
||
|
||
# 连续三重弯道行走
|
||
directions = [
|
||
(0.3,0,0,0,0,0,0.6),
|
||
#第四个圆弧
|
||
(0.4,0,0.69,0,0,0,3),
|
||
#微调路径
|
||
(0.3,0,0,0,0,0,1.3),
|
||
# 第三个圆弧
|
||
(0.4,0,-0.73,0,0,0,6),
|
||
#微调路径
|
||
(0.3,0,0,0,0,0,1.4),
|
||
#第二个圆弧
|
||
(0.4,0,0.74,0,0,0,7.4),
|
||
#微调路径
|
||
(0.3,0,0,0,0,0,1.2),
|
||
#第一个圆弧
|
||
(0.45, 0, -0.78,0,0,0,5.8),
|
||
#直线走一段
|
||
(0.4,0.05,0,0,0,0,0.3),
|
||
(0.4,0,0,0,0,0,1.5),
|
||
] # [vel_x, vel_z] 对应 [左转, 右转, 左转]
|
||
|
||
for i, (vel_x, vel_y, vel_z ,rpy_x,rpy_y,rpy_z,t) in enumerate(directions):
|
||
# 设置转向命令
|
||
msg.mode = 11 # Locomotion
|
||
msg.gait_id = 26 # TROT_FAST
|
||
msg.vel_des = [vel_x, vel_y, vel_z] # [x, y, z]，y=0表示不侧移
|
||
msg.rpy_des = [rpy_x, rpy_y, rpy_z]
|
||
msg.duration = 0 # 连续运动
|
||
msg.step_height = [0.03, 0.03]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 打印当前方向
|
||
print(f"开始 {text[i]} ")
|
||
|
||
# 根据转弯方向调整持续时间（假设半圆需要3秒）
|
||
time.sleep(t) # 持续3秒 |