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] 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(