Update centering functionality in dual track control by adding new parameters for detection height and interval. Adjust message life count handling and improve variable management in task execution. Refactor related functions for enhanced clarity and performance.
This commit is contained in:
parent
87be6b8328
commit
1ce0f9e7bf
@ -9,7 +9,9 @@ from utils.detect_dual_track_lines import detect_dual_track_lines
|
||||
|
||||
def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=False,
|
||||
mode=11, gait_id=26, step_height=[0.06, 0.06],
|
||||
estimated_track_distance=1.0):
|
||||
estimated_track_distance=1.0,
|
||||
detect_height=0.4,
|
||||
detection_interval=0.2):
|
||||
"""
|
||||
控制机器狗仅使用Y轴移动调整到双轨道线的中间位置
|
||||
|
||||
@ -23,6 +25,8 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal
|
||||
gait_id: 步态ID，默认为26
|
||||
step_height: 抬腿高度，默认为[0.06, 0.06]
|
||||
estimated_track_distance: 估计的机器人到轨道的距离(米)，用于计算实际偏差，默认为1.0米
|
||||
detect_height: 检测高度，默认为0.4
|
||||
detection_interval: 检测间隔时间(秒)，默认为0.2秒
|
||||
|
||||
返回:
|
||||
bool: 是否成功调整到中心位置
|
||||
@ -37,6 +41,7 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal
|
||||
|
||||
# 记录起始时间
|
||||
start_time = time.time()
|
||||
last_detection_time = 0 # 记录上次检测时间
|
||||
|
||||
# 记录起始位置
|
||||
start_position = list(ctrl.odo_msg.xyz)
|
||||
@ -73,7 +78,7 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal
|
||||
|
||||
# 检测双轨道线
|
||||
detection_total_count += 1
|
||||
|
||||
|
||||
center_info, left_info, right_info = detect_dual_track_lines(image, observe=observe, save_log=True)
|
||||
|
||||
if center_info is not None:
|
||||
@ -104,7 +109,6 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal
|
||||
if stable_count < required_stable_count:
|
||||
# 直接根据实际偏差移动
|
||||
msg.vel_des = [0, actual_deviation, 0]
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
|
||||
if stable_count >= required_stable_count:
|
||||
@ -211,7 +215,8 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=Fal
|
||||
|
||||
return success
|
||||
def center_and_follow_dual_tracks(ctrl, msg, distance, speed=0.5, max_centering_time=15, observe=False,
|
||||
mode=11, gait_id=26, step_height=[0.06, 0.06], estimated_track_distance=1.0):
|
||||
mode=11, gait_id=26, step_height=[0.06, 0.06], estimated_track_distance=1.0,
|
||||
detection_interval=0.2):
|
||||
"""
|
||||
先居中到双轨道线中间，然后沿轨道线行走指定距离
|
||||
|
||||
@ -226,6 +231,7 @@ def center_and_follow_dual_tracks(ctrl, msg, distance, speed=0.5, max_centering_
|
||||
gait_id: 步态ID，默认为26
|
||||
step_height: 抬腿高度，默认为[0.06, 0.06]
|
||||
estimated_track_distance: 估计的机器人到轨道的距离(米)，用于计算实际偏差，默认为1.0米
|
||||
detection_interval: 检测间隔时间(秒)，默认为0.2秒
|
||||
|
||||
返回:
|
||||
bool: 是否成功完成居中和轨道跟随
|
||||
@ -243,7 +249,8 @@ def center_and_follow_dual_tracks(ctrl, msg, distance, speed=0.5, max_centering_
|
||||
mode=mode,
|
||||
gait_id=gait_id,
|
||||
step_height=step_height,
|
||||
estimated_track_distance=estimated_track_distance
|
||||
estimated_track_distance=estimated_track_distance,
|
||||
detection_interval=detection_interval
|
||||
)
|
||||
|
||||
if not centering_success:
|
||||
|
||||
1
main.py
1
main.py
@ -193,6 +193,7 @@ class Robot_Ctrl(object):
|
||||
self.send_lock.acquire()
|
||||
self.delay_cnt = 50
|
||||
self.cmd_msg = msg
|
||||
self.cmd_msg.life_count %= 127
|
||||
self.send_lock.release()
|
||||
|
||||
def place_marker(self, x, y, z, color, observe=False):
|
||||
|
||||
@ -183,18 +183,10 @@ def pass_up_down(ctrl, msg):
|
||||
pass
|
||||
|
||||
section('任务3-2：x = 2', "开始")
|
||||
# msg.mode = 11 # Locomotion模式 # DEBUG
|
||||
# msg.gait_id = 26 # 自变频步态
|
||||
# msg.duration = 0 # wait next cmd
|
||||
# msg.step_height = [0.06, 0.06] # 抬腿高度
|
||||
# msg.vel_des = [0, 0.2, 0] # [前进速度, 侧向速度, 角速度]
|
||||
# msg.life_count += 1
|
||||
# ctrl.Send_cmd(msg)
|
||||
turn_degree_v2(ctrl, msg, 90, absolute=True)
|
||||
center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False)
|
||||
center_on_dual_tracks(ctrl, msg, max_time=15, 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)
|
||||
# go_to_x_v2(ctrl, msg, 2, speed=0.5, precision=True, observe=True)
|
||||
|
||||
section('任务3-3：down', "完成")
|
||||
try:
|
||||
@ -254,7 +246,7 @@ def pass_up_down(ctrl, msg):
|
||||
stable_threshold = 10 # 连续10次检测z轴速度接近零则认为已经到达平地
|
||||
z_speed_threshold = 0.005 # z轴速度阈值，小于这个值认为已经停止下降
|
||||
descent_speed_threshold = -0.05 # 检测到开始下坡的速度阈值（负值表示下降）
|
||||
max_iterations = 100 # 最大循环次数，作为安全保障
|
||||
max_iterations = 110 # 最大循环次数，作为安全保障
|
||||
min_iterations = 80 # 最小循环次数，确保有足够的时间开始动作
|
||||
start_height = ctrl.odo_msg.xyz[2] # 记录起始高度
|
||||
|
||||
|
||||
@ -65,8 +65,9 @@ def run_task_4_back(ctrl, msg):
|
||||
go_straight(ctrl, msg, distance=1, speed=2)
|
||||
|
||||
# Use enhanced calibration for better Y-axis correction on stone path
|
||||
go_straight_with_enhanced_calibration(ctrl, msg, distance=4.5, speed=0.35,
|
||||
mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True)
|
||||
go_straight(ctrl, msg, distance=4.5, speed=0.35, mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True)
|
||||
# go_straight_with_enhanced_calibration(ctrl, msg, distance=4.5, speed=0.35,
|
||||
# mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True)
|
||||
|
||||
section('任务4-3：前进直到遇到黄线 - 石板路', "移动")
|
||||
# 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处
|
||||
@ -366,7 +367,7 @@ def go_straight_with_enhanced_calibration(ctrl, msg, distance, speed=0.5, observ
|
||||
if frame is not None:
|
||||
# 检测轨道线 - 使用特定的石板路模式
|
||||
center_info, _, _ = detect_dual_track_lines(
|
||||
frame, observe=False, save_log=False, stone_path_mode=True)
|
||||
frame, observe=False, save_log=False)
|
||||
|
||||
# 如果成功检测到轨道线，使用它进行修正
|
||||
if center_info is not None:
|
||||
|
||||
@ -211,7 +211,7 @@ def run_task_5(ctrl, msg, direction='left', observe=False):
|
||||
走向卸货
|
||||
"""
|
||||
|
||||
center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False, stone_path_mode=False)
|
||||
center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False)
|
||||
section('任务5-1：直线移动并扫描二维码', "移动")
|
||||
|
||||
# 最大移动距离为8米
|
||||
@ -231,7 +231,7 @@ def run_task_5(ctrl, msg, direction='left', observe=False):
|
||||
error("未能成功到达横线前指定距离", "失败")
|
||||
|
||||
section('任务5-2：移动到卸货点', "移动")
|
||||
center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False, stone_path_mode=False)
|
||||
center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False)
|
||||
if direction == 'right' and res['qr_result'] == 'B-2' or direction == 'left' and res['qr_result'] == 'B-1':
|
||||
# 直走
|
||||
move_to_hori_line(ctrl, msg, target_distance=1.4, observe=observe, detect_func_version=4)
|
||||
|
||||
@ -9,7 +9,8 @@ from utils.log_helper import LogHelper, get_logger, section, info, debug, warnin
|
||||
def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True,
|
||||
min_slope_threshold=0.4,
|
||||
min_line_length=0.05,
|
||||
max_line_gap=40):
|
||||
max_line_gap=40,
|
||||
detect_height=0.4):
|
||||
"""
|
||||
检测左右两条平行的黄色轨道线，优化后能够更准确处理各种路况
|
||||
|
||||
@ -72,7 +73,7 @@ def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True,
|
||||
cv2.waitKey(delay)
|
||||
|
||||
# 裁剪底部区域重点关注近处的黄线
|
||||
bottom_roi_height = int(height * 0.4) # 增加关注区域到图像底部60%
|
||||
bottom_roi_height = int(height * detect_height) # 增加关注区域到图像底部60%
|
||||
bottom_roi = mask[height-bottom_roi_height:, :]
|
||||
|
||||
if observe:
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user