524 lines
22 KiB
Python
524 lines
22 KiB
Python
import time
|
||
import sys
|
||
import os
|
||
import toml
|
||
import copy
|
||
import math
|
||
import lcm
|
||
import numpy as np
|
||
import cv2
|
||
import tempfile
|
||
|
||
# 添加父目录到路径，以便能够导入utils
|
||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||
# 添加当前目录到路径，确保可以找到local文件
|
||
sys.path.append(os.path.dirname(os.path.abspath(__file__)))
|
||
|
||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||
from base_move.turn_degree import turn_degree, turn_degree_v2
|
||
from base_move.go_straight import go_straight, go_lateral
|
||
from base_move.go_to_xy import go_to_x_v2
|
||
from base_move.center_on_dual_tracks import center_on_dual_tracks
|
||
from file_send_lcmt import file_send_lcmt
|
||
from utils.yellow_area_analyzer import analyze_yellow_area_ratio
|
||
|
||
# 创建本模块特定的日志记录器
|
||
logger = get_logger("任务3")
|
||
|
||
observe = True
|
||
|
||
robot_cmd = {
|
||
'mode':0, 'gait_id':0, 'contact':0, 'life_count':0,
|
||
'vel_des':[0.0, 0.0, 0.0],
|
||
'rpy_des':[0.0, 0.0, 0.0],
|
||
'pos_des':[0.0, 0.0, 0.0],
|
||
'acc_des':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
'ctrl_point':[0.0, 0.0, 0.0],
|
||
'foot_pose':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
'step_height':[0.0, 0.0],
|
||
'value':0, 'duration':0
|
||
}
|
||
|
||
|
||
def pass_up_down(ctrl, msg):
|
||
usergait_msg = file_send_lcmt()
|
||
# lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
|
||
|
||
try:
|
||
steps = toml.load("./task_3/Gait_Params_up.toml")
|
||
full_steps = {'step':[robot_cmd]}
|
||
k =0
|
||
for i in steps['step']:
|
||
cmd = copy.deepcopy(robot_cmd)
|
||
cmd['duration'] = i['duration']
|
||
if i['type'] == 'usergait':
|
||
cmd['mode'] = 11 # LOCOMOTION
|
||
cmd['gait_id'] = 110 # USERGAIT
|
||
cmd['vel_des'] = i['body_vel_des']
|
||
cmd['rpy_des'] = i['body_pos_des'][0:3]
|
||
cmd['pos_des'] = i['body_pos_des'][3:6]
|
||
cmd['foot_pose'][0:2] = i['landing_pos_des'][0:2]
|
||
cmd['foot_pose'][2:4] = i['landing_pos_des'][3:5]
|
||
cmd['foot_pose'][4:6] = i['landing_pos_des'][6:8]
|
||
cmd['ctrl_point'][0:2] = i['landing_pos_des'][9:11]
|
||
cmd['step_height'][0] = math.ceil(i['step_height'][0] * 1e3) + math.ceil(i['step_height'][1] * 1e3) * 1e3
|
||
cmd['step_height'][1] = math.ceil(i['step_height'][2] * 1e3) + math.ceil(i['step_height'][3] * 1e3) * 1e3
|
||
cmd['acc_des'] = i['weight']
|
||
cmd['value'] = i['use_mpc_traj']
|
||
cmd['contact'] = math.floor(i['landing_gain'] * 1e1)
|
||
cmd['ctrl_point'][2] = i['mu']
|
||
if k == 0:
|
||
full_steps['step'] = [cmd]
|
||
else:
|
||
full_steps['step'].append(cmd)
|
||
k=k+1
|
||
f = open("./task_3/Gait_Params_up_full.toml", 'w')
|
||
f.write("# Gait Params\n")
|
||
f.writelines(toml.dumps(full_steps))
|
||
f.close()
|
||
|
||
# pre
|
||
file_obj_gait_def = open("./task_3/Gait_Def_up.toml",'r')
|
||
file_obj_gait_params = open("./task_3/Gait_Params_up_full.toml",'r')
|
||
usergait_msg.data = file_obj_gait_def.read()
|
||
ctrl.lc_s.publish("user_gait_file", usergait_msg.encode())
|
||
time.sleep(0.5)
|
||
usergait_msg.data = file_obj_gait_params.read()
|
||
ctrl.lc_s.publish("user_gait_file", usergait_msg.encode())
|
||
time.sleep(0.1)
|
||
file_obj_gait_def.close()
|
||
file_obj_gait_params.close()
|
||
|
||
msg.mode = 62
|
||
msg.value = 0
|
||
msg.contact = 15
|
||
msg.gait_id = 110
|
||
msg.duration = 1000
|
||
msg.life_count += 1
|
||
|
||
# 参数设置
|
||
stable_count = 0 # 用于计数z轴稳定的次数
|
||
stable_threshold = 8 # 连续15次检测z轴不再增加则认为已经停止
|
||
z_speed_threshold = 0.01 # z轴速度阈值，小于这个值认为已经停止爬升
|
||
climb_speed_threshold = 0.05 # 检测到开始爬坡的速度阈值
|
||
max_iterations = 230 # 最大循环次数，作为安全保障
|
||
min_iterations = 170 # 最小循环次数，作为安全保障
|
||
|
||
# 姿态判断参数
|
||
pitch_threshold = 0.05 # 俯仰角阈值（弧度）
|
||
angular_rate_threshold = 0.03 # 角速度阈值（弧度/秒）
|
||
|
||
# 阶段控制
|
||
climbing_detected = False # 是否检测到正在爬坡
|
||
|
||
# 高度变化记录
|
||
height_window = []
|
||
pitch_window = []
|
||
window_size = 8
|
||
|
||
# 记录起始姿态和高度
|
||
start_height = ctrl.odo_msg.xyz[2]
|
||
info(f"开始监测爬坡过程，初始高度: {start_height:.3f}", "监测")
|
||
|
||
for i in range(max_iterations):
|
||
# 发送控制命令维持心跳
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 获取当前状态数据
|
||
vz = ctrl.odo_msg.vxyz[2] # Z轴速度
|
||
current_height = ctrl.odo_msg.xyz[2] # 当前高度
|
||
current_pitch = ctrl.odo_msg.rpy[1] # 当前俯仰角
|
||
pitch_rate = ctrl.odo_msg.omegaBody[1] # 俯仰角速度
|
||
vbody_z = ctrl.odo_msg.vBody[2] # 机体坐标系Z速度
|
||
|
||
# 更新滑动窗口数据
|
||
height_window.append(current_height)
|
||
pitch_window.append(current_pitch)
|
||
if len(height_window) > window_size:
|
||
height_window.pop(0)
|
||
pitch_window.pop(0)
|
||
|
||
# 每10次迭代打印一次当前信息
|
||
if i % 10 == 0:
|
||
info(f"Step:{i} 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
|
||
|
||
# 检测是否开始爬坡阶段
|
||
if not climbing_detected and vz > climb_speed_threshold:
|
||
climbing_detected = True
|
||
info(f"检测到开始爬坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}, 俯仰角: {current_pitch:.3f}", "监测")
|
||
|
||
# 多条件判断是否完成爬坡
|
||
if i > min_iterations and climbing_detected and len(height_window) == window_size:
|
||
# 计算高度和俯仰角的稳定性
|
||
height_std = np.std(height_window) # 高度标准差
|
||
pitch_std = np.std(pitch_window) # 俯仰角标准差
|
||
|
||
# 多条件综合判断
|
||
position_stable = abs(vz) < z_speed_threshold # 垂直速度稳定
|
||
attitude_stable = abs(current_pitch) < pitch_threshold # 俯仰角接近水平
|
||
angular_rate_stable = abs(pitch_rate) < angular_rate_threshold # 角速度稳定
|
||
height_stable = height_std < 0.01 # 高度变化小
|
||
pitch_stable = pitch_std < 0.01 # 俯仰角变化小
|
||
vbody_stable = abs(vbody_z) < 0.01 # 机体Z方向速度稳定
|
||
|
||
# 综合判断条件
|
||
if (position_stable and attitude_stable and angular_rate_stable) or \
|
||
(position_stable and height_stable and pitch_stable) or \
|
||
(vbody_stable and attitude_stable and height_stable):
|
||
stable_count += 1
|
||
if stable_count >= stable_threshold:
|
||
info(f"检测到已完成爬坡：\n - Z轴速度: {vz:.3f}\n - 俯仰角: {current_pitch:.3f}\n - 角速度: {pitch_rate:.3f}\n - 当前高度: {current_height:.3f}\n - 上升了: {current_height - start_height:.3f}米", "监测")
|
||
break
|
||
else:
|
||
# 重置稳定计数
|
||
stable_count = 0
|
||
|
||
time.sleep(0.2)
|
||
except KeyboardInterrupt:
|
||
msg.mode = 7 #PureDamper before KeyboardInterrupt:
|
||
msg.gait_id = 0
|
||
msg.duration = 0
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
pass
|
||
|
||
section('任务3-2：x = 2', "开始")
|
||
turn_degree_v2(ctrl, msg, 90, absolute=True)
|
||
center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False, detect_height=0.3)
|
||
go_straight(ctrl, msg, distance=0.5, speed=0.5, observe=True)
|
||
time.sleep(1)
|
||
|
||
section('任务3-3：down', "完成")
|
||
try:
|
||
steps = toml.load("./task_3/Gait_Params_down.toml")
|
||
full_steps = {'step':[robot_cmd]}
|
||
k = 0
|
||
for i in steps['step']:
|
||
cmd = copy.deepcopy(robot_cmd)
|
||
cmd['duration'] = i['duration']
|
||
if i['type'] == 'usergait':
|
||
cmd['mode'] = 11 # LOCOMOTION
|
||
cmd['gait_id'] = 110 # USERGAIT
|
||
cmd['vel_des'] = i['body_vel_des']
|
||
cmd['rpy_des'] = i['body_pos_des'][0:3]
|
||
cmd['pos_des'] = i['body_pos_des'][3:6]
|
||
cmd['foot_pose'][0:2] = i['landing_pos_des'][0:2]
|
||
cmd['foot_pose'][2:4] = i['landing_pos_des'][3:5]
|
||
cmd['foot_pose'][4:6] = i['landing_pos_des'][6:8]
|
||
cmd['ctrl_point'][0:2] = i['landing_pos_des'][9:11]
|
||
cmd['step_height'][0] = math.ceil(i['step_height'][0] * 1e3) + math.ceil(i['step_height'][1] * 1e3) * 1e3
|
||
cmd['step_height'][1] = math.ceil(i['step_height'][2] * 1e3) + math.ceil(i['step_height'][3] * 1e3) * 1e3
|
||
cmd['acc_des'] = i['weight']
|
||
cmd['value'] = i['use_mpc_traj']
|
||
cmd['contact'] = math.floor(i['landing_gain'] * 1e1)
|
||
cmd['ctrl_point'][2] = i['mu']
|
||
if k == 0:
|
||
full_steps['step'] = [cmd]
|
||
else:
|
||
full_steps['step'].append(cmd)
|
||
k=k+1
|
||
f = open("./task_3/Gait_Params_down_full.toml", 'w')
|
||
f.write("# Gait Params\n")
|
||
f.writelines(toml.dumps(full_steps))
|
||
f.close()
|
||
|
||
# pre
|
||
file_obj_gait_def = open("./task_3/Gait_Def_up.toml",'r')
|
||
file_obj_gait_params = open("./task_3/Gait_Params_down_full.toml",'r')
|
||
usergait_msg.data = file_obj_gait_def.read()
|
||
ctrl.lc_s.publish("user_gait_file", usergait_msg.encode())
|
||
time.sleep(0.5)
|
||
usergait_msg.data = file_obj_gait_params.read()
|
||
ctrl.lc_s.publish("user_gait_file", usergait_msg.encode())
|
||
time.sleep(0.1)
|
||
file_obj_gait_def.close()
|
||
file_obj_gait_params.close()
|
||
|
||
msg.mode = 62
|
||
msg.value = 0
|
||
msg.contact = 15
|
||
msg.gait_id = 110
|
||
msg.duration = 1000
|
||
msg.life_count += 1
|
||
|
||
# 参数设置
|
||
stable_count = 0 # 用于计数z轴稳定的次数
|
||
stable_threshold = 10 # 连续10次检测z轴速度接近零则认为已经到达平地
|
||
z_speed_threshold = 0.005 # z轴速度阈值，小于这个值认为已经停止下降
|
||
descent_speed_threshold = -0.05 # 检测到开始下坡的速度阈值（负值表示下降）
|
||
max_iterations = 110 # 最大循环次数，作为安全保障
|
||
min_iterations = 80 # 最小循环次数，确保有足够的时间开始动作
|
||
start_height = ctrl.odo_msg.xyz[2] # 记录起始高度
|
||
|
||
# 姿态判断参数
|
||
pitch_threshold = 0.05 # 俯仰角阈值（弧度）
|
||
angular_rate_threshold = 0.03 # 角速度阈值（弧度/秒）
|
||
|
||
# 阶段控制
|
||
descending_detected = False # 是否检测到正在下坡
|
||
flat_ground_detected = False # 是否检测到已到达平地
|
||
|
||
# 高度变化记录
|
||
height_window = []
|
||
pitch_window = []
|
||
window_size = 8
|
||
|
||
info(f"开始监测下坡过程，初始高度: {start_height}", "监测")
|
||
|
||
for i in range(max_iterations):
|
||
# 发送控制命令维持心跳
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 获取当前状态数据
|
||
vz = ctrl.odo_msg.vxyz[2] # Z轴速度
|
||
current_height = ctrl.odo_msg.xyz[2] # 当前高度
|
||
current_pitch = ctrl.odo_msg.rpy[1] # 当前俯仰角
|
||
pitch_rate = ctrl.odo_msg.omegaBody[1] # 俯仰角速度
|
||
vbody_z = ctrl.odo_msg.vBody[2] # 机体坐标系Z速度
|
||
|
||
# 更新滑动窗口数据
|
||
height_window.append(current_height)
|
||
pitch_window.append(current_pitch)
|
||
if len(height_window) > window_size:
|
||
height_window.pop(0)
|
||
pitch_window.pop(0)
|
||
|
||
# 每10次迭代打印一次当前信息
|
||
if observe and i % 10 == 0:
|
||
info(f"Step:{i} 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
|
||
|
||
# 检测是否开始下坡阶段
|
||
if not descending_detected and vz < descent_speed_threshold:
|
||
descending_detected = True
|
||
info(f"检测到开始下坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}, 俯仰角: {current_pitch:.3f}", "监测")
|
||
|
||
# 多条件判断是否到达平地
|
||
if i > min_iterations and descending_detected and len(height_window) == window_size:
|
||
# 计算高度和俯仰角的稳定性
|
||
height_std = np.std(height_window) # 高度标准差
|
||
pitch_std = np.std(pitch_window) # 俯仰角标准差
|
||
|
||
# 多条件综合判断
|
||
position_stable = abs(vz) < z_speed_threshold # 垂直速度稳定
|
||
attitude_stable = abs(current_pitch) < pitch_threshold # 俯仰角接近水平
|
||
angular_rate_stable = abs(pitch_rate) < angular_rate_threshold # 角速度稳定
|
||
height_stable = height_std < 0.01 # 高度变化小
|
||
pitch_stable = pitch_std < 0.01 # 俯仰角变化小
|
||
vbody_stable = abs(vbody_z) < 0.01 # 机体Z方向速度稳定
|
||
|
||
# 综合判断条件
|
||
if (position_stable and attitude_stable and angular_rate_stable) or \
|
||
(position_stable and height_stable and pitch_stable) or \
|
||
(vbody_stable and attitude_stable and height_stable):
|
||
stable_count += 1
|
||
if stable_count >= stable_threshold:
|
||
info(f"检测到已到达平地：\n - Z轴速度: {vz:.3f}\n - 俯仰角: {current_pitch:.3f}\n - 角速度: {pitch_rate:.3f}\n - 高度: {current_height:.3f}\n - 下降了: {start_height - current_height:.3f}米", "监测")
|
||
flat_ground_detected = True
|
||
break
|
||
else:
|
||
# 重置稳定计数
|
||
stable_count = 0
|
||
|
||
time.sleep(0.2)
|
||
|
||
if not flat_ground_detected:
|
||
info(f"达到最大循环次数，未能明确检测到到达平地。当前高度: {ctrl.odo_msg.xyz[2]:.3f}", "警告")
|
||
except KeyboardInterrupt:
|
||
msg.mode = 7 #PureDamper before KeyboardInterrupt:
|
||
msg.gait_id = 0
|
||
msg.duration = 0
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
pass
|
||
|
||
|
||
def go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3, max_time=30, observe=True):
|
||
"""
|
||
控制机器人直走，直到摄像头检测到黄色区域比例超过指定阈值后开始下降时停止
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
yellow_ratio_threshold: 黄色区域占比阈值(0-1之间的浮点数)，默认为0.15
|
||
speed: 前进速度(米/秒)，默认为0.3米/秒
|
||
max_time: 最大行走时间(秒)，默认为30秒
|
||
observe: 是否输出中间状态信息和可视化结果，默认为True
|
||
|
||
返回:
|
||
bool: 是否成功检测到黄色区域并停止
|
||
"""
|
||
section("开始直行寻找黄色区域", "黄色检测")
|
||
|
||
# 设置移动命令基本参数
|
||
msg.mode = 11 # Locomotion模式
|
||
msg.gait_id = 26 # 自变频步态
|
||
msg.duration = 0 # wait next cmd
|
||
msg.step_height = [0.06, 0.06] # 抬腿高度
|
||
msg.vel_des = [speed, 0, 0] # [前进速度, 侧向速度, 角速度]
|
||
|
||
# 记录起始时间和位置
|
||
start_time = time.time()
|
||
start_position = list(ctrl.odo_msg.xyz)
|
||
|
||
if observe:
|
||
info(f"开始寻找黄色区域，阈值: {yellow_ratio_threshold:.2%}", "启动")
|
||
debug(f"起始位置: {start_position}", "位置")
|
||
|
||
# 检测间隔
|
||
check_interval = 0.3 # 秒
|
||
last_check_time = 0
|
||
|
||
# 黄色区域监测变量
|
||
yellow_peak_detected = False # 是否检测到峰值
|
||
yellow_decreasing = False # 是否开始下降
|
||
max_yellow_ratio = 0.0 # 记录最大黄色区域占比
|
||
yellow_ratio_history = [] # 记录黄色区域占比历史
|
||
history_window_size = 5 # 历史窗口大小，用于平滑处理
|
||
|
||
try:
|
||
# 开始移动
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 持续检测黄色区域
|
||
while time.time() - start_time < max_time and not yellow_decreasing:
|
||
current_time = time.time()
|
||
|
||
# 定期发送移动命令保持移动状态
|
||
if current_time - last_check_time >= check_interval:
|
||
# 获取当前图像并保存到临时文件
|
||
current_image = ctrl.image_processor.get_current_image()
|
||
|
||
# 创建临时文件保存图像
|
||
with tempfile.NamedTemporaryFile(suffix='.jpg', delete=False) as temp_file:
|
||
temp_filename = temp_file.name
|
||
cv2.imwrite(temp_filename, current_image)
|
||
|
||
try:
|
||
# 分析图像中的黄色区域
|
||
yellow_ratio = analyze_yellow_area_ratio(temp_filename, debug=False, save_result=False)
|
||
|
||
# 添加到历史记录
|
||
yellow_ratio_history.append(yellow_ratio)
|
||
if len(yellow_ratio_history) > history_window_size:
|
||
yellow_ratio_history.pop(0)
|
||
|
||
# 计算平滑后的当前黄色占比（使用最近几次的平均值以减少噪声）
|
||
current_smooth_ratio = sum(yellow_ratio_history) / len(yellow_ratio_history)
|
||
|
||
# 计算已移动距离（仅用于显示）
|
||
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)
|
||
|
||
if observe:
|
||
info(f"当前黄色区域占比: {yellow_ratio:.2%}, 平滑值: {current_smooth_ratio:.2%}, 已移动: {distance_moved:.2f}米", "检测")
|
||
|
||
# 检测是否达到阈值（开始监测峰值）
|
||
if current_smooth_ratio >= yellow_ratio_threshold:
|
||
# 更新最大值
|
||
if current_smooth_ratio > max_yellow_ratio:
|
||
max_yellow_ratio = current_smooth_ratio
|
||
if not yellow_peak_detected:
|
||
yellow_peak_detected = True
|
||
if observe:
|
||
info(f"黄色区域占比超过阈值，开始监测峰值", "检测")
|
||
# 检测是否开始下降
|
||
elif yellow_peak_detected and current_smooth_ratio < max_yellow_ratio * 0.9: # 下降到峰值的90%以下认为开始下降
|
||
yellow_decreasing = True
|
||
if observe:
|
||
success(f"检测到黄色区域占比开始下降，峰值: {max_yellow_ratio:.2%}, 当前: {current_smooth_ratio:.2%}", "检测")
|
||
|
||
finally:
|
||
# 删除临时文件
|
||
try:
|
||
os.unlink(temp_filename)
|
||
except:
|
||
pass
|
||
|
||
# 更新心跳
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
last_check_time = current_time
|
||
|
||
# 小间隔
|
||
time.sleep(0.05)
|
||
|
||
# 平滑停止
|
||
if yellow_decreasing:
|
||
if observe:
|
||
info("开始平滑停止", "停止")
|
||
|
||
# 先降低速度再停止，实现平滑停止
|
||
slowdown_steps = 5
|
||
for i in range(slowdown_steps, 0, -1):
|
||
slowdown_factor = i / slowdown_steps
|
||
msg.vel_des = [speed * slowdown_factor, 0, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(0.1)
|
||
|
||
# 完全停止
|
||
ctrl.base_msg.stop()
|
||
|
||
# 计算最终移动距离
|
||
final_position = ctrl.odo_msg.xyz
|
||
dx = final_position[0] - start_position[0]
|
||
dy = final_position[1] - start_position[1]
|
||
final_distance = math.sqrt(dx*dx + dy*dy)
|
||
|
||
if observe:
|
||
if yellow_decreasing:
|
||
success(f"成功检测到黄色区域峰值并停止，峰值占比: {max_yellow_ratio:.2%}, 总移动距离: {final_distance:.2f}米", "完成")
|
||
else:
|
||
warning(f"未能在限定时间内检测到黄色区域峰值，总移动距离: {final_distance:.2f}米", "超时")
|
||
|
||
return yellow_decreasing
|
||
|
||
except KeyboardInterrupt:
|
||
# 处理键盘中断
|
||
ctrl.base_msg.stop()
|
||
if observe:
|
||
warning("操作被用户中断", "中断")
|
||
return False
|
||
except Exception as e:
|
||
# 处理其他异常
|
||
ctrl.base_msg.stop()
|
||
if observe:
|
||
error(f"发生错误: {str(e)}", "错误")
|
||
return False
|
||
|
||
|
||
def run_task_3(ctrl, msg, time_sleep=5000):
|
||
section('任务3：上下坡', "启动")
|
||
info('开始执行任务3...', "启动")
|
||
|
||
turn_degree_v2(ctrl, msg, 90, absolute=True)
|
||
|
||
section('任务3-1：up', "开始")
|
||
pass_up_down(ctrl, msg)
|
||
|
||
turn_degree_v2(ctrl, msg, 90, absolute=True)
|
||
center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False)
|
||
|
||
section('任务3-2：yellow stop', "开始")
|
||
go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3)
|
||
|
||
# 原地站立3秒
|
||
section("原地站立3秒", "站立")
|
||
msg.mode = 12
|
||
msg.gait_id = 0
|
||
msg.duration = 0
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.vel_des = [0, 0, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
info("开始原地站立3秒", "站立")
|
||
time.sleep(time_sleep / 1000)
|
||
info("完成原地站立", "站立")
|
||
|
||
|
||
def run_task_3_back(ctrl, msg):
|
||
return |