From f77cada464e843ea78a80c6aed7e2d50a5f134fe Mon Sep 17 00:00:00 2001 From: Havoc <2993167370@qq.com> Date: Thu, 15 May 2025 11:19:24 +0800 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20=E5=A2=9E=E5=8A=A0=20MarkerRunner?= =?UTF-8?q?=20=E6=94=AF=E6=8C=81=EF=BC=8C=E4=BC=98=E5=8C=96=E6=A0=87?= =?UTF-8?q?=E8=AE=B0=E6=94=BE=E7=BD=AE=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 在 Robot_Ctrl 类中初始化 MarkerRunner，并在运行时调用其方法 - 在移动到横向线的函数中添加标记放置功能，分别在起点和终点放置绿色和红色标记 - 增强错误处理，确保 MarkerRunner 正确销毁 --- base_move/move_base_hori_line.py | 7 ++++ main.py | 26 ++++++++++++++- utils/marker_client.py | 55 ++++++++++++++++++++++++++++++++ 3 files changed, 87 insertions(+), 1 deletion(-) create mode 100755 utils/marker_client.py diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 873096a..2dce2f6 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -302,6 +302,9 @@ def move_to_hori_line(ctrl, msg, target_distance=0.1, observe=False): if observe: print(f"起始位置: {start_position}") print(f"开始移动: {'前进' if forward else '后退'} {abs(distance_to_move):.3f}米") + # 在起点放置绿色标记 + if hasattr(ctrl, 'place_marker'): + ctrl.place_marker(start_position[0], start_position[1], start_position[2] if len(start_position) > 2 else 0.0, 'green', observe=True) # 估算移动时间，但实际上会通过里程计控制 move_time = abs(distance_to_move) / move_speed @@ -340,6 +343,10 @@ def move_to_hori_line(ctrl, msg, target_distance=0.1, observe=False): if observe: print(f"移动完成，通过里程计计算的移动距离: {distance_moved:.3f}米") + # 在终点放置红色标记 + end_position = list(ctrl.odo_msg.xyz) + if hasattr(ctrl, 'place_marker'): + ctrl.place_marker(end_position[0], end_position[1], end_position[2] if len(end_position) > 2 else 0.0, 'red', observe=True) # 如果没有提供图像处理器或图像验证失败，则使用里程计数据判断 return abs(distance_moved - abs(distance_to_move)) < 0.1 # 如果误差小于10厘米，则认为成功 diff --git a/main.py b/main.py index 74badf8..7a01c28 100644 --- a/main.py +++ b/main.py @@ -15,6 +15,7 @@ from utils.robot_control_cmd_lcmt import robot_control_cmd_lcmt from utils.robot_control_response_lcmt import robot_control_response_lcmt from utils.localization_lcmt import localization_lcmt from utils.image_raw import ImageProcessor +from utils.marker_client import MarkerRunner from task_1.task_1 import run_task_1 from task_5.task_5 import run_task_5 @@ -63,11 +64,14 @@ class Robot_Ctrl(object): self.rec_msg = robot_control_response_lcmt() self.odo_msg = localization_lcmt() self.image_processor = ImageProcessor() + # DEBUG + self.marker_runner = MarkerRunner() self.send_lock = Lock() self.delay_cnt = 0 self.mode_ok = 0 self.gait_ok = 0 self.runing = 1 + # 初始化 MarkerRunner def run(self): self.lc_r.subscribe("robot_control_response", self.msg_handler) @@ -76,6 +80,7 @@ class Robot_Ctrl(object): self.rec_thread.start() self.odo_thread.start() self.image_processor.run() + self.marker_runner.run() def msg_handler(self, channel, data): self.rec_msg = robot_control_response_lcmt().decode(data) @@ -126,12 +131,31 @@ class Robot_Ctrl(object): self.cmd_msg = msg self.send_lock.release() + def place_marker(self, x, y, z, color, observe=False): + """调用 MarkerRunner 放置标志物""" + if self.marker_runner is None or self.marker_runner.marker_client is None: + print("MarkerRunner 未初始化，无法放置标志物") + return None + try: + response = self.marker_runner.send_request(x, y, z, color, observe=observe) + print(f"放置标志物结果: {response.success}, 消息: {response.message}") + return response + except Exception as e: + print(f"放置标志物时发生异常: {e}") + return None + def quit(self): self.runing = 0 self.rec_thread.join() self.send_thread.join() self.image_processor.destroy() - + # 销毁 MarkerRunner + if hasattr(self, 'marker_runner') and self.marker_runner is not None: + try: + self.marker_runner.destroy() + except Exception as e: + print(f"MarkerRunner 销毁失败: {e}") + # Main function if __name__ == '__main__': main() \ No newline at end of file diff --git a/utils/marker_client.py b/utils/marker_client.py new file mode 100755 index 0000000..4e0ce5b --- /dev/null +++ b/utils/marker_client.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from cyberdog_marker.srv import PlaceMarker + +class MarkerClient(Node): + def __init__(self): + super().__init__('marker_client') + self.client = self.create_client(PlaceMarker, 'place_marker') + while not self.client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('服务不可用，等待...') + self.req = PlaceMarker.Request() + + def send_request(self, x, y, z, color): + self.req.x = x + self.req.y = y + self.req.z = z + self.req.color = color + self.future = self.client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + + +class MarkerRunner: + def __init__(self): + self.marker_client = None + + def run(self): + rclpy.init() + self.marker_client = MarkerClient() + + def destroy(self): + self.marker_client.destroy_node() + rclpy.shutdown() + + def send_request(self, x, y, z, color, observe=False): + response = self.marker_client.send_request(x, y, z, color) + if observe: + self.marker_client.get_logger().info( + f'结果: {response.success}, 消息: {response.message}') + return response + + +def main(args=None): + rclpy.init(args=args) + client = MarkerClient() + response = client.send_request(1.0, 1.0, 0.0, 'blue') + client.get_logger().info( + f'结果: {response.success}, 消息: {response.message}') + client.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file