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

task-1 #1

Browse Source
This commit is contained in:
havoc420ubuntu 2025-05-14 11:25:44 +00:00
parent df7c98f09a
commit aa4621ed55
4 changed files with 13 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
base_move/move_base_hori_line.py
View File

@ -268,35 +268,6 @@ def move_to_hori_line(ctrl, msg, image, target_distance=0.1, observe=False, imag
if observe:
print(f"移动完成，通过里程计计算的移动距离: {distance_moved:.3f}米")
# 如果提供了图像处理器，则使用视觉进行最终验证
if image_processor is not None:
# 等待机器人完全停止
time.sleep(0.5)
# 获取新图像并重新检测横向线
new_image = image_processor.get_image()
if new_image is not None:
edge_point, edge_info = detect_horizontal_track_edge(new_image, observe=observe)
if edge_point is not None and edge_info is not None:
final_distance = calculate_distance_to_line(edge_info, camera_height)
if final_distance is not None:
if observe:
print(f"最终到横向线的视觉距离: {final_distance:.3f}米, 目标距离: {target_distance:.3f}米")
distance_error = abs(final_distance - target_distance)
# 如果距离误差较大且需要精确定位，可以考虑进行小幅度的微调
if distance_error > 0.05 and distance_error < 0.2:
if observe:
print(f"进行微调以达到精确距离")
# 递归调用自身进行微调，但不再使用图像验证以避免无限循环
return move_to_hori_line(ctrl, msg, new_image, target_distance, observe, None)
return distance_error < 0.1 # 如果误差小于10厘米，则认为成功
# 如果没有提供图像处理器或图像验证失败，则使用里程计数据判断
return abs(distance_moved - abs(distance_to_move)) < 0.1 # 如果误差小于10厘米，则认为成功

7
main.py
View File

@ -33,11 +33,12 @@ def main():
msg.life_count += 1
Ctrl.Send_cmd(msg)
Ctrl.Wait_finish(12, 0)
# time.sleep(1)
# time.sleep(100) # TEST
# run_task_1(Ctrl, msg, image_processor)
run_task_1(Ctrl, msg, image_processor)
run_task_5(Ctrl, msg)
# run_task_5(Ctrl, msg)
# time.sleep(100)

BIN
res/path/test/edge_img.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 58 KiB

After

Width:  |  Height:  |  Size: 57 KiB

78
task_1/task_1.py
View File

@ -1,82 +1,22 @@
import time
import cv2
import sys
import os
def turn_90(ctrl, msg, direction="right"):
msg.mode = 11
msg.gait_id = 10
msg.vel_des = [1.1, 0, -1.5 if direction == "right" else 1.5]
msg.duration = 1500
msg.step_height = [0.06, 0.06]
msg.life_count += 1
ctrl.Send_cmd(msg)
time.sleep(1.5)
# 添加父目录到路径，以便能够导入utils
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
def turn_right_180(ctrl, msg):
msg.mode = 11
msg.gait_id = 3
msg.vel_des = [0.55, 0, -1.0]
msg.duration = 3500
msg.step_height = [0.06, 0.06]
msg.life_count += 1
ctrl.Send_cmd(msg)
time.sleep(3.5)
def go_straight(ctrl, msg, time_ms, fb=True):
msg.mode = 11
msg.gait_id = 26
msg.vel_des = [1 if fb else -1, 0, 0]
msg.duration = time_ms
msg.life_count += 1
ctrl.Send_cmd(msg)
time.sleep(time_ms / 1000)
def stand_up(ctrl, msg):
msg.mode = 12 # Recovery stand
msg.gait_id = 0
msg.life_count += 1
ctrl.Send_cmd(msg)
time.sleep(1.5)
def down_and_back(ctrl, msg):
# # TAG 趴下
msg.mode = 7
msg.gait_id = 1
msg.duration = 2000
msg.life_count += 1
ctrl.Send_cmd(msg)
ctrl.Wait_finish(7, 1)
stand_up(ctrl, msg)
go_straight(ctrl, msg, 1500, False)
def circle_90(ctrl, msg, direction="right"):
msg.mode = 11
msg.gait_id = 10
msg.duration = 1000
msg.vel_des = [0, 0, -2 if direction == "right" else 2]
msg.life_count += 1
ctrl.Send_cmd(msg)
time.sleep(1)
from base_move.move_base_hori_line import align_to_horizontal_line, move_to_hori_line
def run_task_1(ctrl, msg, image_processor):
print('Running task 1...')
# v2
# align_to_horizontal_line(ctrl, msg, image_processor.get_current_image())
# 右前方
turn_90(ctrl, msg)
move_to_hori_line(ctrl, msg, image_processor.get_current_image(), image_processor=image_processor)
# TAG take photo
qrcode_info = image_processor.decode_qrcode()
print(qrcode_info)
# TODO 不同的走向
turn_right_180(ctrl, msg)
go_straight(ctrl, msg, 200)
down_and_back(ctrl, msg)
circle_90(ctrl, msg)
# turn_90(ctrl, msg, 'left')