From 5d1b8e0d418e59560d71c9509e2ba169cdd4479d Mon Sep 17 00:00:00 2001 From: Havoc <2993167370@qq.com> Date: Sun, 1 Jun 2025 17:33:50 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0main.py=E4=B8=AD=E7=9A=84LCM?= =?UTF-8?q?=E6=B3=A8=E9=87=8A=E4=BB=A5=E5=A2=9E=E5=BC=BA=E5=8F=AF=E8=AF=BB?= =?UTF-8?q?=E6=80=A7=EF=BC=8C=E7=A7=BB=E9=99=A4move=5Fbase=5Fhori=5Fline.p?= =?UTF-8?q?y=E4=B8=AD=E7=9A=84=E5=A4=9A=E4=BD=99=E7=A9=BA=E8=A1=8C?= =?UTF-8?q?=EF=BC=8C=E5=B9=B6=E5=9C=A8image=5Fraw.py=E4=B8=AD=E9=87=8D?= =?UTF-8?q?=E6=96=B0=E7=BB=84=E7=BB=87=E5=BC=82=E6=AD=A5=E6=89=AB=E6=8F=8F?= =?UTF-8?q?=E7=9B=B8=E5=85=B3=E4=BB=A3=E7=A0=81=EF=BC=8C=E6=8F=90=E5=8D=87?= =?UTF-8?q?=E4=BB=A3=E7=A0=81=E7=BB=93=E6=9E=84=E5=92=8C=E5=8F=AF=E7=BB=B4?= =?UTF-8?q?=E6=8A=A4=E6=80=A7=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .DS_Store | Bin 6148 -> 6148 bytes base_move/move_base_hori_line.py | 1 - main.py | 6 +-- struct.cpp | 62 +++++++++++++++++++++++++++++++ utils/image_raw.py | 18 ++++----- 5 files changed, 74 insertions(+), 13 deletions(-) create mode 100644 struct.cpp diff --git a/.DS_Store b/.DS_Store index aa1395735049acc0ff1c9f83469c9e2c89c064c7..ed91dd62cc3ebbd2a3c73102f8419a0145ed1a10 100644 GIT binary patch delta 31 ncmZoMXfc@J&&azmU^g=(?_?g9{>>*@?3gDu=xt`__{$Ffplk{w delta 70 zcmZoMXfc@J&&aniU^g=(-(((^e$7;d3Wh|6T!sRM9EMcSoc!dZoctsP1_l8J2F9O2 VTJJv?FfeSM#^TJpnVsV=KLF8579{`x diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 69355ca..f4e90a7 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -204,7 +204,6 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3, detect_fu return aligned - def calculate_distance_to_line(edge_info, camera_height, camera_tilt_angle_deg=0, observe=False): """ 根据相机参数和图像中横线位置计算相机到横线的实际距离 diff --git a/main.py b/main.py index dc335c4..75f3632 100644 --- a/main.py +++ b/main.py @@ -94,9 +94,9 @@ class Robot_Ctrl(object): self.rec_thread = Thread(target=self.rec_responce) self.send_thread = Thread(target=self.send_publish) self.odo_thread = Thread(target=self.rec_responce_o) - self.lc_r = lcm.LCM("udpm://239.255.76.67:7670?ttl=255") - self.lc_s = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") - self.lc_o = lcm.LCM("udpm://239.255.76.67:7667?ttl=255") + self.lc_r = lcm.LCM("udpm://239.255.76.67:7670?ttl=255") # 控制指令 + self.lc_s = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") # 状态反馈 + self.lc_o = lcm.LCM("udpm://239.255.76.67:7667?ttl=255") # 里程计 self.cmd_msg = robot_control_cmd_lcmt() self.rec_msg = robot_control_response_lcmt() self.odo_msg = localization_lcmt() diff --git a/struct.cpp b/struct.cpp new file mode 100644 index 0000000..8af1b80 --- /dev/null +++ b/struct.cpp @@ -0,0 +1,62 @@ +#define int8_t int +#define int16_t int +#define int32_t int +#define int64_t int + +/** + * 基本控制 + * URL: udpm://239.255.76.67:7671?ttl=255 + * 通道: robot_control_cmd + * 频率: 2~500Hz，超时500ms触发趴下保护 + */ + +struct robot_control_cmd_lcmt { + int8_t mode; // 控制模式 + int8_t gait_id; // 步态ID + int8_t contact; // 四足着地状态 + int8_t life_count; // 生命计数，计数增加时命令生效 + + float vel_des[3]; // 期望速度 [x, y(1.6), yaw(2.5)] m/s + float rpy_des[3]; // 期望姿态 [roll, pitch, yaw(0.45)] rad + float pos_des[3]; // 期望位置 [x, y, z(0.1-0.32)] m + float acc_des[6]; // 跳跃加速度 m^2/s + float ctrl_point[3]; // 姿态控制点 m + float foot_pose[6]; // 前后足位置 [x,y,z] m + float step_height[2]; // 小跑步态步高 0~0.08m + + int32_t value; // 控制标志位 + // bit0: 舞蹈模式下是否使用MPC轨迹 + // bit1: 0-内八节能步态 1-垂直步态 + int32_t duration; // 命令执行时间 +}; + + +/** 状态反馈 + * URL: "udpm://239.255.76.67:7670?ttl=255" + * channel: "robot_control_response" + * frequncy: 50HZ + */ +struct robot_control_response_lcmt { + int8_t mode; // 控制模式 + int8_t gait_id; // 步态ID + int8_t contact; // 四足着地状态 + int8_t order_process_bar; // 进度条 order process, 1 == 1 % + int8_t switch_status; // 0:Done, 1:TRANSITIONING, 2:ESTOP, 3:EDAMP, 4:LIFTED 5:BAN_TRANS 6:OVER_HEAT 7:LOW_BAT + int8_t ori_error; // 姿态误差 + int16_t footpos_error; // 足端位置误差 + int32_t motor_error[12]; // 电机误差 +}; + +/** 里程计数据 + * URL: "udpm://239.255.76.67:7667?ttl=255" + * channel: "global_to_robot" + * frequncy: 50HZ + */ +struct localization_lcmt { + float xyz[3]; // 表达在腿式里程计坐标系的机身位置 + float vxyz[3]; // 表达在腿式里程计坐标系的机身线速度 + float rpy[3]; // 表达在腿式里程计坐标系的机身欧拉角 + float omegaBody[3]; // 表达在机身坐标系的机身角速度 + float vBody[3]; // 表达在机身坐标系的机身线速度 + int64_t timestamp; // 时间戳，单位 nanoseconds +}; diff --git a/utils/image_raw.py b/utils/image_raw.py index baa90dd..882844d 100644 --- a/utils/image_raw.py +++ b/utils/image_raw.py @@ -15,10 +15,10 @@ from utils.log_helper import get_logger class ImageSubscriber(Node): def __init__(self): + # 初始化 rgb 图像订阅 super().__init__('image_subscriber') - # 定义 QoS 配置（匹配发布者的可靠性策略） qos_profile = QoSProfile( - reliability=QoSReliabilityPolicy.BEST_EFFORT, # 或 BEST_EFFORT，取决于发布者 + reliability=QoSReliabilityPolicy.BEST_EFFORT, history=QoSHistoryPolicy.KEEP_LAST, depth=10 ) @@ -106,13 +106,6 @@ class ImageProcessor: self.scan_thread.start() self.log.info("启动异步 QR 码扫描线程", "启动") - def stop_async_scan(self): - """停止异步 QR 码扫描""" - self.enable_async_scan = False - if self.scan_thread and self.scan_thread.is_alive(): - self.scan_thread.join(timeout=1.0) - self.log.info("异步 QR 码扫描线程已停止", "停止") - def _async_scan_worker(self, interval): """异步扫描工作线程""" last_scan_time = 0 @@ -143,6 +136,13 @@ class ImageProcessor: # 短暂休眠避免占用过多 CPU time.sleep(0.05) + def stop_async_scan(self): + """停止异步 QR 码扫描""" + self.enable_async_scan = False + if self.scan_thread and self.scan_thread.is_alive(): + self.scan_thread.join(timeout=1.0) + self.log.info("异步 QR 码扫描线程已停止", "停止") + def get_last_qr_result(self): """获取最后一次成功扫描的 QR 码结果""" with self.scan_lock: