更新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
|
return aligned
|
||||||
|
|
||||||
|
|
||||||
def calculate_distance_to_line(edge_info, camera_height, camera_tilt_angle_deg=0, observe=False):
|
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.rec_thread = Thread(target=self.rec_responce)
|
||||||
self.send_thread = Thread(target=self.send_publish)
|
self.send_thread = Thread(target=self.send_publish)
|
||||||
self.odo_thread = Thread(target=self.rec_responce_o)
|
self.odo_thread = Thread(target=self.rec_responce_o)
|
||||||
self.lc_r = lcm.LCM("udpm://239.255.76.67:7670?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_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_o = lcm.LCM("udpm://239.255.76.67:7667?ttl=255") # 里程计
|
||||||
self.cmd_msg = robot_control_cmd_lcmt()
|
self.cmd_msg = robot_control_cmd_lcmt()
|
||||||
self.rec_msg = robot_control_response_lcmt()
|
self.rec_msg = robot_control_response_lcmt()
|
||||||
self.odo_msg = localization_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):
|
class ImageSubscriber(Node):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
|
# 初始化 rgb 图像订阅
|
||||||
super().__init__('image_subscriber')
|
super().__init__('image_subscriber')
|
||||||
# 定义 QoS 配置(匹配发布者的可靠性策略)
|
|
||||||
qos_profile = QoSProfile(
|
qos_profile = QoSProfile(
|
||||||
reliability=QoSReliabilityPolicy.BEST_EFFORT, # 或 BEST_EFFORT,取决于发布者
|
reliability=QoSReliabilityPolicy.BEST_EFFORT,
|
||||||
history=QoSHistoryPolicy.KEEP_LAST,
|
history=QoSHistoryPolicy.KEEP_LAST,
|
||||||
depth=10
|
depth=10
|
||||||
)
|
)
|
||||||
@ -106,13 +106,6 @@ class ImageProcessor:
|
|||||||
self.scan_thread.start()
|
self.scan_thread.start()
|
||||||
self.log.info("启动异步 QR 码扫描线程", "启动")
|
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):
|
def _async_scan_worker(self, interval):
|
||||||
"""异步扫描工作线程"""
|
"""异步扫描工作线程"""
|
||||||
last_scan_time = 0
|
last_scan_time = 0
|
||||||
@ -143,6 +136,13 @@ class ImageProcessor:
|
|||||||
# 短暂休眠避免占用过多 CPU
|
# 短暂休眠避免占用过多 CPU
|
||||||
time.sleep(0.05)
|
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):
|
def get_last_qr_result(self):
|
||||||
"""获取最后一次成功扫描的 QR 码结果"""
|
"""获取最后一次成功扫描的 QR 码结果"""
|
||||||
with self.scan_lock:
|
with self.scan_lock:
|
||||||
|
Loading…
x
Reference in New Issue
Block a user