diff --git a/.DS_Store b/.DS_Store index aa13957..ed91dd6 100644 Binary files a/.DS_Store and b/.DS_Store differ 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: