更新任务4，添加直线前进至黄线的功能
- 在task_4.py中引入go_straight_until_hori_line函数，控制机器人直线前进直到检测到黄线并停在指定距离。 - 调整任务4的执行流程，确保机器人能够顺利完成新的移动任务。 - 优化了相关参数设置，以提高任务执行的准确性和稳定性。
This commit is contained in:
parent
6c55201f73
commit
8e172759ac
@ -1335,3 +1335,196 @@ def follow_left_side_track(ctrl, msg, min_distance=600, max_distance=650, speed=
|
||||
warning("无法检测最终位置的左侧轨迹线", "警告")
|
||||
return False
|
||||
|
||||
def go_straight_until_hori_line(ctrl, msg, target_distance=0.5, max_distance=5.0,
|
||||
check_interval=0.5, speed=0.3, detect_func_version=1,
|
||||
mode=11, gait_id=26, step_height=[0.06, 0.06],
|
||||
observe=False):
|
||||
"""
|
||||
控制机器人直线前进，直到检测到前方的横向黄线并到达指定距离
|
||||
|
||||
参数:
|
||||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||||
target_distance: 目标与横向线的距离(米)，默认为0.5米
|
||||
max_distance: 最大前进距离(米)，防止无限前进，默认为5.0米
|
||||
check_interval: 检查横向线的时间间隔(秒)，默认为0.5秒
|
||||
speed: 移动速度(米/秒)，默认为0.3米/秒
|
||||
detect_func_version: 使用的横向线检测函数版本，默认为1
|
||||
mode: 机器人运动模式，默认为11(Locomotion模式)
|
||||
gait_id: 步态ID，默认为26(自变频步态)
|
||||
step_height: 抬腿高度，默认为[0.06, 0.06]
|
||||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||||
|
||||
返回:
|
||||
bool: 是否成功找到横向线并到达目标距离
|
||||
"""
|
||||
section("开始直线前进寻找横向线", "寻线")
|
||||
|
||||
# 设置移动命令基本参数
|
||||
msg.mode = mode
|
||||
msg.gait_id = gait_id
|
||||
msg.step_height = step_height
|
||||
msg.vel_des = [speed, 0, 0] # 前进速度
|
||||
msg.duration = 0 # wait next cmd
|
||||
msg.life_count += 1
|
||||
|
||||
# 记录起始位置
|
||||
start_position = list(ctrl.odo_msg.xyz)
|
||||
if observe:
|
||||
debug(f"起始位置: {start_position}", "位置")
|
||||
# 在起点放置绿色标记
|
||||
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)
|
||||
|
||||
# 发送命令开始移动
|
||||
ctrl.Send_cmd(msg)
|
||||
|
||||
# 初始化变量
|
||||
total_distance = 0
|
||||
line_detected = False
|
||||
last_check_time = time.time()
|
||||
start_time = time.time()
|
||||
|
||||
# 设置相机高度
|
||||
camera_height = 0.355 # 单位: 米
|
||||
|
||||
while total_distance < max_distance:
|
||||
# 计算已移动距离
|
||||
current_position = ctrl.odo_msg.xyz
|
||||
dx = current_position[0] - start_position[0]
|
||||
dy = current_position[1] - start_position[1]
|
||||
total_distance = math.sqrt(dx*dx + dy*dy)
|
||||
|
||||
# 每隔指定时间检查一次
|
||||
current_time = time.time()
|
||||
if current_time - last_check_time >= check_interval:
|
||||
last_check_time = current_time
|
||||
|
||||
# 获取当前图像
|
||||
image = ctrl.image_processor.get_current_image()
|
||||
|
||||
# 根据指定版本检测横向线
|
||||
if detect_func_version == 1:
|
||||
edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True)
|
||||
elif detect_func_version == 2:
|
||||
edge_point, edge_info = detect_horizontal_track_edge_v2(image, observe=observe, save_log=True)
|
||||
elif detect_func_version == 3:
|
||||
edge_point, edge_info = detect_horizontal_track_edge_v3(image, observe=observe, save_log=True)
|
||||
else:
|
||||
error("未指定检测版本，请检查参数", "失败")
|
||||
edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True)
|
||||
|
||||
if edge_point is not None and edge_info is not None:
|
||||
line_detected = True
|
||||
|
||||
# 计算当前距离
|
||||
current_distance = calculate_distance_to_line(edge_info, camera_height, observe=observe)
|
||||
|
||||
if current_distance is None:
|
||||
warning("无法计算到横向线的距离，继续前进", "警告")
|
||||
continue
|
||||
|
||||
if observe:
|
||||
info(f"检测到横向线，当前距离: {current_distance:.3f}米, 目标距离: {target_distance:.3f}米", "距离")
|
||||
|
||||
# 判断是否达到目标距离
|
||||
if current_distance <= target_distance + 0.1: # 允许0.1米的误差
|
||||
success(f"已达到目标距离，当前距离: {current_distance:.3f}米", "完成")
|
||||
break
|
||||
|
||||
# 如果距离比目标距离小，需要后退
|
||||
if current_distance < target_distance - 0.1:
|
||||
warning(f"过度接近横向线，当前距离: {current_distance:.3f}米，需要后退", "警告")
|
||||
# 平滑停止当前移动
|
||||
ctrl.base_msg.stop()
|
||||
time.sleep(0.5)
|
||||
|
||||
# 计算需要后退的距离
|
||||
back_distance = target_distance - current_distance
|
||||
|
||||
# 设置后退命令
|
||||
msg.vel_des = [-speed/2, 0, 0] # 降低后退速度
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
|
||||
# 后退指定时间
|
||||
back_time = back_distance / (speed/2)
|
||||
time.sleep(back_time)
|
||||
|
||||
# 停止后退
|
||||
ctrl.base_msg.stop()
|
||||
|
||||
if observe:
|
||||
info(f"后退完成，预计后退距离: {back_distance:.3f}米", "后退")
|
||||
|
||||
# 重新检查距离
|
||||
time.sleep(0.5) # 等待稳定
|
||||
image = ctrl.image_processor.get_current_image()
|
||||
if detect_func_version == 1:
|
||||
edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True)
|
||||
elif detect_func_version == 2:
|
||||
edge_point, edge_info = detect_horizontal_track_edge_v2(image, observe=observe, save_log=True)
|
||||
elif detect_func_version == 3:
|
||||
edge_point, edge_info = detect_horizontal_track_edge_v3(image, observe=observe, save_log=True)
|
||||
|
||||
if edge_point is not None and edge_info is not None:
|
||||
current_distance = calculate_distance_to_line(edge_info, camera_height, observe=observe)
|
||||
if current_distance is not None:
|
||||
if observe:
|
||||
info(f"后退后的距离: {current_distance:.3f}米", "距离")
|
||||
|
||||
break
|
||||
else:
|
||||
if observe and time.time() % 2 < 0.05: # 每2秒左右打印一次
|
||||
info(f"未检测到横向线，继续前进，已移动: {total_distance:.3f}米", "搜索")
|
||||
|
||||
# 定期显示移动距离
|
||||
if observe and time.time() % 1 < 0.05: # 每1秒左右打印一次
|
||||
debug(f"已移动: {total_distance:.3f}米, 最大距离: {max_distance:.3f}米", "移动")
|
||||
|
||||
# 短暂延时以降低CPU使用率
|
||||
time.sleep(0.05)
|
||||
|
||||
# 停止移动
|
||||
ctrl.base_msg.stop()
|
||||
|
||||
# 移动完成后再次检查
|
||||
if line_detected:
|
||||
# 等待稳定
|
||||
time.sleep(0.5)
|
||||
|
||||
# 最终检查
|
||||
image = ctrl.image_processor.get_current_image()
|
||||
if detect_func_version == 1:
|
||||
edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True)
|
||||
elif detect_func_version == 2:
|
||||
edge_point, edge_info = detect_horizontal_track_edge_v2(image, observe=observe, save_log=True)
|
||||
elif detect_func_version == 3:
|
||||
edge_point, edge_info = detect_horizontal_track_edge_v3(image, observe=observe, save_log=True)
|
||||
|
||||
if edge_point is not None and edge_info is not None:
|
||||
final_distance = calculate_distance_to_line(edge_info, camera_height, observe=observe)
|
||||
|
||||
if final_distance is not None:
|
||||
if observe:
|
||||
success(f"最终距离: {final_distance:.3f}米, 目标距离: {target_distance:.3f}米", "结果")
|
||||
|
||||
# 判断是否达到目标
|
||||
is_success = abs(final_distance - target_distance) < 0.2 # 误差在20cm内算成功
|
||||
|
||||
if observe:
|
||||
if is_success:
|
||||
success(f"成功达到目标距离，误差: {abs(final_distance - target_distance):.2f}米", "成功")
|
||||
else:
|
||||
warning(f"未能精确达到目标距离，误差: {abs(final_distance - target_distance):.2f}米", "警告")
|
||||
|
||||
return is_success
|
||||
|
||||
if not line_detected:
|
||||
if observe:
|
||||
error(f"在最大距离({max_distance}米)内未检测到横向线", "失败")
|
||||
return False
|
||||
|
||||
# 如果没有进行最终检查，但检测到过横向线，默认返回成功
|
||||
return True
|
||||
|
||||
|
||||
7
main.py
7
main.py
@ -43,10 +43,11 @@ def main():
|
||||
Ctrl.base_msg.stop() # BUG 垃圾指令 for eat
|
||||
|
||||
# time.sleep(100) # TEST
|
||||
# run_task_1(Ctrl, msg)
|
||||
run_task_1(Ctrl, msg)
|
||||
|
||||
run_task_2(Ctrl, msg)
|
||||
# run_task_2_5(Ctrl, msg)
|
||||
res = run_task_2(Ctrl, msg)
|
||||
|
||||
run_task_2_5(Ctrl, msg)
|
||||
|
||||
# run_task_4(Ctrl, msg)
|
||||
|
||||
|
||||
@ -11,6 +11,7 @@ from base_move.turn_degree import turn_degree
|
||||
from base_move.go_straight import go_straight
|
||||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||||
from utils.gray_sky_analyzer import analyze_gray_sky_ratio
|
||||
from base_move.move_base_hori_line import go_straight_until_hori_line
|
||||
|
||||
from task_4.pass_bar import pass_bar
|
||||
|
||||
@ -30,17 +31,19 @@ def run_task_4(ctrl, msg):
|
||||
section('任务4-2：通过栏杆', "移动")
|
||||
pass_bar(ctrl, msg)
|
||||
|
||||
section('任务4-3：直线移动 - 石板路', "移动")
|
||||
# 设置机器人运动模式为快步跑
|
||||
msg.mode = 11 # 运动模式
|
||||
msg.gait_id = 3 # 步态ID（快步跑）
|
||||
msg.vel_des = [0.35, 0, 0] # 期望速度
|
||||
msg.pos_des = [ 0, 0, 0]
|
||||
msg.duration = 0 # 零时长表示持续运动，直到接收到新命令
|
||||
msg.step_height = [0.21, 0.21] # 持续运动时摆动腿的离地高度
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
time.sleep(5000) # 持续5秒钟
|
||||
section('任务4-3：前进直到遇到黄线 - 石板路', "移动")
|
||||
# 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处
|
||||
go_straight_until_hori_line(
|
||||
ctrl,
|
||||
msg,
|
||||
target_distance=0.5, # 目标与黄线的距离(米)
|
||||
max_distance=8.0, # 最大搜索距离(米)
|
||||
speed=0.35, # 移动速度(米/秒)
|
||||
mode=11, # 运动模式
|
||||
gait_id=3, # 步态ID（快步跑）
|
||||
step_height=[0.21, 0.21], # 摆动腿离地高度
|
||||
observe=True # 显示调试信息
|
||||
)
|
||||
|
||||
|
||||
def go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2, step_distance=0.5, max_distance=10, speed=0.3):
|
||||
@ -134,7 +137,7 @@ def run_task_4_back(ctrl, msg):
|
||||
msg.step_height = [0.21, 0.21] # 持续运动时摆动腿的离地高度
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
time.sleep(5000) # 持续5秒钟
|
||||
time.sleep(5) # 持续5秒钟
|
||||
|
||||
section('任务4-2：移动直到灰色天空比例小于阈值', "天空检测")
|
||||
go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user