Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'task-3-2' of ssh://120.27.199.238:222/Havoc420mac/mi-task into task-3-2

Browse Source
This commit is contained in:
Havoc 2025-05-31 20:09:29 +08:00
commit e0961fcdd9
6 changed files with 31 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
base_move/center_on_dual_tracks.py
View File

@ -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:

3
main.py
View File

@ -64,8 +64,6 @@ def main():
else:
run_task_3(Ctrl, msg)
return
turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
run_task_5(Ctrl, msg, direction=arrow_direction)
@ -195,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):

21
task_3/task_3.py
View File

@ -183,17 +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, 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:
@ -253,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] # 记录起始高度
@ -501,17 +494,15 @@ def run_task_3(ctrl, msg):
section('任务3：上下坡', "启动")
info('开始执行任务3...', "启动")
# turn_degree_v2(ctrl, msg, 90, absolute=True)
# go_lateral(ctrl, msg, distance=0.2, speed=0.1, observe=True)
turn_degree_v2(ctrl, msg, 90, absolute=True)
go_lateral(ctrl, msg, distance=0.2, speed=0.1, observe=True)
# section('任务3-1：up', "开始")
# pass_up_down(ctrl, msg)
section('任务3-1：up', "开始")
pass_up_down(ctrl, msg)
turn_degree_v2(ctrl, msg, 90, absolute=True)
center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False)
return
section('任务3-2：yellow stop', "开始")
go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3)

7
task_4/task_4.py
View File

@ -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:

4
task_5/task_5.py
View File

@ -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)

11
utils/detect_dual_track_lines.py
View File

@ -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:
@ -544,11 +545,11 @@ def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True,
# 计算拟合误差
predicted_xs = poly_func(center_ys)
error = np.mean(np.abs(predicted_xs - center_xs))
err = np.mean(np.abs(predicted_xs - center_xs))
# 如果误差更小，更新最佳拟合
if error < best_error:
best_error = error
if err < best_error:
best_error = err
best_poly_coeffs = poly_coeffs
except:
continue