更新main.py中的LCM注释以增强可读性,移除move_base_hori_line.py中的多余空行,并在image_raw.py中重新组织异步扫描相关代码,提升代码结构和可维护性。
This commit is contained in:
parent
f8980287e2
commit
5d1b8e0d41
@ -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):
|
||||
"""
|
||||
根据相机参数和图像中横线位置计算相机到横线的实际距离
|
||||
|
6
main.py
6
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()
|
||||
|
62
struct.cpp
Normal file
62
struct.cpp
Normal file
@ -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
|
||||
};
|
@ -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:
|
||||
|
Loading…
x
Reference in New Issue
Block a user