97 lines
2.6 KiB
Python
97 lines
2.6 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.move_base_hori_line import (
|
|||
|
|
# arc_turn_around_hori_line,
|
|||
|
|
# go_straight_by_hori_line,
|
|||
|
|
# move_to_hori_line,
|
|||
|
|
# align_to_horizontal_line
|
|||
|
|
# )
|
|||
|
|
# from base_move.go_straight import go_straight
|
|||
|
|
# from base_move.turn_degree import turn_degree
|
|||
|
|
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
|||
|
|
|
|||
|
|
# 创建本模块特定的日志记录器
|
|||
|
|
logger = get_logger("任务1")
|
|||
|
|
|
|||
|
|
observe = True
|
|||
|
|
|
|||
|
|
def run_task_2(ctrl, msg):
|
|||
|
|
|
|||
|
|
# print("Recovery stand")
|
|||
|
|
# Ctrl.base_msg.stand_up()
|
|||
|
|
|
|||
|
|
# # time.sleep(100) # TEST
|
|||
|
|
|
|||
|
|
# run_task_1(Ctrl, msg)
|
|||
|
|
|
|||
|
|
# x,y=image_processor.get_current_x()
|
|||
|
|
# print('分别是',x,y)
|
|||
|
|
# # 初始恢复站立
|
|||
|
|
# msg.mode = 12 # Recovery stand
|
|||
|
|
# msg.gait_id = 0
|
|||
|
|
# msg.life_count += 1
|
|||
|
|
# Ctrl.Send_cmd(msg)
|
|||
|
|
# Ctrl.Wait_finish(12, 0)
|
|||
|
|
# print('1')
|
|||
|
|
# x,y=image_processor.get_current_x()
|
|||
|
|
# print('分别是',x,y)
|
|||
|
|
|
|||
|
|
# # 初始恢复站立
|
|||
|
|
# msg.mode = 11 # Recovery stand
|
|||
|
|
# msg.gait_id = 27
|
|||
|
|
# msg.vel_des = [0.3,0,0]
|
|||
|
|
# msg.pos_des[2]=0.24
|
|||
|
|
# msg.duration = 0
|
|||
|
|
# msg.step_height = [0.01, 0.01]
|
|||
|
|
# msg.life_count += 1
|
|||
|
|
# Ctrl.Send_cmd(msg)
|
|||
|
|
# time.sleep(1)
|
|||
|
|
|
|||
|
|
#获取微调角度
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 连续三重弯道行走
|
|||
|
|
directions = [
|
|||
|
|
#直线走一段
|
|||
|
|
(0.4,0,0,0,0,0,0.5),
|
|||
|
|
#第一个圆弧
|
|||
|
|
(0.45, 0, 0.77,0,0,0,7.3),
|
|||
|
|
#微调路径
|
|||
|
|
(0.4,0,0,0,0,0,0.8),
|
|||
|
|
#第二个圆弧
|
|||
|
|
(0.4,0,-0.72,0,0,0,7.7),
|
|||
|
|
#第三个圆弧
|
|||
|
|
(0.4,0.05,-0.75,0,0,0,1.2),
|
|||
|
|
#微调路径
|
|||
|
|
(0.45, 0, 0.8,0,0,0,7.2),
|
|||
|
|
#第四个圆弧
|
|||
|
|
(0.4,0,-0.72,0,0,0,4)] # [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"开始第 {i+1} 次转弯")
|
|||
|
|
|
|||
|
|
# 根据转弯方向调整持续时间（假设半圆需要3秒）
|
|||
|
|
time.sleep(t) # 持续3秒
|
|||
|
|
|
|||
|
|
# # 停止
|
|||
|
|
# msg.mode = 7 # PureDamper
|
|||
|
|
# msg.gait_id = 0
|
|||
|
|
# msg.life_count += 1
|
|||
|
|
# Ctrl.Send_cmd(msg)
|
|||
|
|
# Ctrl.Wait_finish(7, 0)
|