From 234e11eadc29bbe916c4ad575c66a335f9aaab4b Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Mon, 19 May 2025 12:53:12 +0000 Subject: [PATCH 1/3] add save ori --- utils/detect_track.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/utils/detect_track.py b/utils/detect_track.py index 360bb6b..8f788ec 100644 --- a/utils/detect_track.py +++ b/utils/detect_track.py @@ -310,6 +310,12 @@ def detect_horizontal_track_edge(image, observe=False, delay=1000, save_log=True timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S_%f") log_dir = "logs/image" os.makedirs(log_dir, exist_ok=True) + + # 保存原图 + origin_image_path = os.path.join(log_dir, f"origin_horizontal_edge_{timestamp}.jpg") + cv2.imwrite(origin_image_path, img) + info(f"保存原始图像到: {origin_image_path}", "日志") + img_path = os.path.join(log_dir, f"horizontal_edge_{timestamp}.jpg") cv2.imwrite(img_path, result_img) info(f"保存横向边缘检测结果图像到: {img_path}", "日志") @@ -819,6 +825,12 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True): timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S_%f") log_dir = "logs/image" os.makedirs(log_dir, exist_ok=True) + + # 保存原图 + original_img_path = os.path.join(log_dir, f"original_{timestamp}.jpg") + cv2.imwrite(original_img_path, img) + info(f"保存原始图像到: {original_img_path}", "日志") + img_path = os.path.join(log_dir, f"left_track_{timestamp}.jpg") cv2.imwrite(img_path, result_img) info(f"保存左侧轨迹线检测结果图像到: {img_path}", "日志") From a781b77b425a599a8fa1832cd8c1a1d2ee782153 Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Tue, 20 May 2025 10:31:14 +0000 Subject: [PATCH 2/3] pre merge --- utils/detect_track.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utils/detect_track.py b/utils/detect_track.py index ed423d1..15f0058 100644 --- a/utils/detect_track.py +++ b/utils/detect_track.py @@ -19,7 +19,7 @@ def detect_horizontal_track_edge(image, observe=False, delay=1000, save_log=True edge_point: 赛道前方边缘点的坐标 (x, y) edge_info: 边缘信息字典 """ - # observe = False # TSET + observe = False # TEST # 如果输入是字符串（文件路径），则加载图像 if isinstance(image, str): img = cv2.imread(image) From 1d18ec1e5638091de171ecf8eddccc513380c1e0 Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Thu, 22 May 2025 04:27:22 +0000 Subject: [PATCH 3/3] test(task_1): adjust go_straight parameters and comment out angle correction - Comment out angle correction logic in go_straight function - Adjust go_straight parameters in task_1 - Add success message for horizontal line calibration - Update main function to include task_test - Modify marker request response format --- base_move/go_straight.py | 32 ++++++++++++++++---------------- base_move/move_base_hori_line.py | 2 ++ main.py | 12 ++++++++---- task_1/task_1.py | 17 +++++++++-------- task_test/task_test.py | 15 +++++++++++++++ utils/marker_client.py | 5 ++++- 6 files changed, 54 insertions(+), 29 deletions(-) create mode 100644 task_test/task_test.py diff --git a/base_move/go_straight.py b/base_move/go_straight.py index d51919f..660d939 100644 --- a/base_move/go_straight.py +++ b/base_move/go_straight.py @@ -134,24 +134,24 @@ def go_straight(ctrl, msg, distance, speed=0.5, observe=False): ctrl.Send_cmd(msg) # 如果偏离初始方向，进行角度修正 - if abs(yaw_error) > angle_correction_threshold: - # 计算角速度修正值，偏差越大修正越强 - angular_correction = -yaw_error * 0.5 # 比例系数可调整 - # 限制最大角速度修正 - angular_correction = max(min(angular_correction, 0.2), -0.2) + # if abs(yaw_error) > angle_correction_threshold: + # # 计算角速度修正值，偏差越大修正越强 + # angular_correction = -yaw_error * 0.5 # 比例系数可调整 + # # 限制最大角速度修正 + # angular_correction = max(min(angular_correction, 0.2), -0.2) - if observe and abs(angular_correction) > 0.05: - warning(f"方向修正: 偏差 {math.degrees(yaw_error):.1f}度，应用角速度 {angular_correction:.3f}rad/s", "角度") + # if observe and abs(angular_correction) > 0.05: + # warning(f"方向修正: 偏差 {math.degrees(yaw_error):.1f}度，应用角速度 {angular_correction:.3f}rad/s", "角度") - # 应用角速度修正 - msg.vel_des[2] = angular_correction - msg.life_count += 1 - ctrl.Send_cmd(msg) - elif msg.vel_des[2] != 0: - # 如果方向已修正，重置角速度 - msg.vel_des[2] = 0 - msg.life_count += 1 - ctrl.Send_cmd(msg) + # # 应用角速度修正 + # msg.vel_des[2] = angular_correction + # msg.life_count += 1 + # ctrl.Send_cmd(msg) + # elif msg.vel_des[2] != 0: + # # 如果方向已修正，重置角速度 + # msg.vel_des[2] = 0 + # msg.life_count += 1 + # ctrl.Send_cmd(msg) if observe and current_time - start_time > 1 and (current_time % 0.5 < position_check_interval): debug(f"已移动: {distance_moved:.3f}米, 目标: {abs_distance:.3f}米 (完成: {completion_ratio*100:.1f}%)", "距离") diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 325c836..9adb1f0 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -296,6 +296,8 @@ def go_straight_by_hori_line(ctrl, msg, distance=0.5, speed=0.5, observe=False): if not aligned: error("无法校准到横向线水平，停止移动", "停止") return False + else: + success("校准完成", "完成") go_straight(ctrl, msg, distance=distance, speed=speed, observe=observe) diff --git a/main.py b/main.py index c129f48..cf32a5b 100644 --- a/main.py +++ b/main.py @@ -23,6 +23,8 @@ from utils.base_msg import BaseMsg from task_1.task_1 import run_task_1 from task_5.task_5 import run_task_5 +from task_test.task_test import run_task_test + pass_marker = True def main(): @@ -34,13 +36,15 @@ def main(): try: print("Recovery stand") Ctrl.base_msg.stand_up() + Ctrl.base_msg.stop() # BUG 垃圾指令 for eat # time.sleep(100) # TEST - run_task_1(Ctrl, msg) # run_task_5(Ctrl, msg) + # run_task_test(Ctrl, msg) + # time.sleep(100) except KeyboardInterrupt: @@ -104,14 +108,14 @@ class Robot_Ctrl(object): self.calibration_offset = self.odo_msg.xyz self.is_calibrated = True print(f"校准完成，基准值: {self.calibration_offset}") - # 将接收到的 odo 数据减去校准基准值 + # # 将接收到的 odo 数据减去校准基准值 self.odo_msg.xyz = [ self.odo_msg.xyz[0] - self.calibration_offset[0], self.odo_msg.xyz[1] - self.calibration_offset[1], self.odo_msg.xyz[2] - self.calibration_offset[2] ] - # if self.odo_msg.timestamp % 100 == 0: - # print(self.odo_msg.xyz) + # if self.odo_msg.timestamp % 50 == 0: + # print(self.odo_msg.rpy) def odo_reset(self): self.calibration_offset = self.odo_msg.xyz diff --git a/task_1/task_1.py b/task_1/task_1.py index 2909714..4a79069 100644 --- a/task_1/task_1.py +++ b/task_1/task_1.py @@ -25,9 +25,10 @@ def run_task_1(ctrl, msg): info('开始执行任务1...', "启动") # TEST better align - aligned = align_to_horizontal_line(ctrl, msg, observe=observe) - print(aligned) - return + # aligned = align_to_horizontal_line(ctrl, msg, observe=observe) + # print(aligned) + # go_straight(ctrl, msg, -10, observe=observe) + # return # v2 section('任务1-1：转弯并扫描QR码', "移动") @@ -57,7 +58,7 @@ def run_task_1(ctrl, msg): ctrl=ctrl, msg=msg, angle_deg=-170, # 负值表示右转 - target_distance=0.5, + target_distance=0.8, # pass_align=True, observe=observe, @@ -72,11 +73,11 @@ def run_task_1(ctrl, msg): return section('任务1-4：直线移动', "移动") - move_distance = 0.3 + move_distance = 0.5 move_speed = 0.5 if not direction: # TODO to check - go_straight_by_hori_line(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe) + go_straight_by_hori_line(ctrl, msg, distance=move_distance, speed=move_distance, observe=observe) else: go_straight(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe) @@ -92,7 +93,7 @@ def run_task_1(ctrl, msg): ctrl.base_msg.stand_up() section('任务1-6：返回', "移动") - go_straight(ctrl, msg, distance=-(move_distance + res['radius'] * 2), speed=0.3, observe=observe) + go_straight(ctrl, msg, distance=-(move_distance + res['radius'] + 0.2), speed=0.3, observe=observe) # turn and back turn_degree(ctrl, msg, degree=-90) @@ -102,7 +103,7 @@ def run_task_1(ctrl, msg): ctrl=ctrl, msg=msg, radius=res['radius'] * 2, - angle_deg=90, + angle_deg=85, # pass_align=True, observe=observe diff --git a/task_test/task_test.py b/task_test/task_test.py new file mode 100644 index 0000000..eec073a --- /dev/null +++ b/task_test/task_test.py @@ -0,0 +1,15 @@ +import time +import sys +import os + +# 添加父目录到路径，以便能够导入utils +sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +from task_5.detect_arrow_direction import ArrowDetector +from base_move.turn_degree import turn_degree + +def run_task_test(ctrl, msg): + ctrl.base_msg.stop() + turn_degree(ctrl, msg, 90) + turn_degree(ctrl, msg, 90) + diff --git a/utils/marker_client.py b/utils/marker_client.py index aca9690..41f4f74 100755 --- a/utils/marker_client.py +++ b/utils/marker_client.py @@ -36,7 +36,10 @@ class MarkerRunner: def send_request(self, x, y, z, color, observe=False): if self.pass_flag: - return "pass flag." + return { + 'success': True, + 'message': "Pass" + } response = self.marker_client.send_request(x, y, z, color) if observe: self.marker_client.get_logger().info(