ROS中发布函数内计算的Float32型priority值到自定义话题的实现方法
ROS系统中Float32类型优先级值的发布实现方案
实现卡点梳理
你当前的实现无法正常发布优先级值,核心原因有4个:
- 不熟悉
std_msgs/Float32的消息结构,调用publish()时未传入携带实际优先级值的消息实例,空发会导致下游收到永远为0的默认值 - 里程计权重模型内部计算得到的
priority是局部变量,没有打通从计算逻辑到发布链路的变量传递通道 - 程序结构错误:把
rospy.spin()写在了while死循环的后面,程序永远执行不到spin语句,车辆状态订阅回调根本不会被触发,拿不到计算需要的速度、姿态、转向角参数 - 代码存在多处笔误与逻辑漏洞:话题字符串占位符未格式化、传感器列表变量名写错、循环频率变量名不匹配,会直接触发运行时报错或实际话题路径不符合预期
具体实现步骤
- 正确使用Float32消息类型
std_msgs.msg.Float32是纯浮点型消息封装,仅暴露一个可赋值的data字段,发布时必须将计算得到的原生float值赋值给该字段,不能直接将原始float传入publish()方法,也不能调用空参数的publish(),否则下游订阅端收到的永远是默认值0.0,示例写法:priority_msg = Float32() priority_msg.data = priority # 绑定计算完成的浮点优先级值 pub.publish(priority_msg) - 打通priority值的传递链路
现在priority是在odometry_weight_model函数内部生成的局部变量,发布逻辑根本访问不到这个值,可根据现有代码架构选两种适配方案:- 给
odometry_weight_model增加返回值,计算完成后将priority作为函数返回值传出,在拿到速度、姿态、转向角参数的车辆状态订阅回调中接收返回值,直接构造消息发布 - 将
priority定义为全局变量,在里程计权重模型函数内部更新全局变量值,发布逻辑直接读取该全局变量完成发布
- 给
- 修正原有代码的逻辑漏洞
逐段核对现有代码后,这些问题必须修正:- 初始化Publisher时,话题字符串里的
{role_name}不会被自动替换为参数读到的车辆名,必须手动用字符串格式化替换,否则实际发布的话题名会字面保留大括号占位符,和预期的话题路径完全不一致 - 加载传感器配置时,循环里写的是
camera_loc.append(i["yaw"]),但前面初始化的列表叫CAM_LOC,这里变量名笔误会直接触发NameError - 原结构里的无意义
while循环完全可以移除:ROS Python的订阅回调是在后台线程运行的,只要通过rospy.spin()阻塞主进程等待回调事件即可,不需要在无数据的空循环里轮询发空消息,也避免了rospy.spin()永远执行不到的问题
- 初始化Publisher时,话题字符串里的
修正后的参考代码片段
# 导入依赖的消息类型 from std_msgs.msg import Float32 from carla_msgs.msg import CarlaEgoVehicleStatus import rospy import json import math import numpy as np # 定义全局变量 CAM_LOC = [] priority_pub = None priority = 0.0 def odometry_weight_model(velocity, orientation, steering_angle): """Odometry based camera view prioritization model""" global priority # ... 原有业务计算逻辑 for yaw_ang in CAM_LOC: force_dir = yaw_ang + steering_angle force = np.array([velocity, force_dir]) delta = (np.linalg.norm(force)) / (2 * math.pi) * const priority = prio_orig + delta def vehicle_status_callback(msg: CarlaEgoVehicleStatus): # 从车辆状态消息提取计算参数 velocity = msg.velocity orientation = msg.orientation steering_angle = msg.control.steer # 触发优先级计算 odometry_weight_model(velocity, orientation, steering_angle) # 构造消息完成发布 global priority, priority_pub priority_msg = Float32() priority_msg.data = priority priority_pub.publish(priority_msg) def main(): global priority_pub, CAM_LOC rospy.init_node("view_prioritization", anonymous=True) role_name = rospy.get_param("~role_name", "ego_vehicle") sensor_definition_file = rospy.get_param( "~sensor_definition_file", "$(find telecarla_manual_control)/config/multi_sensors.json", ) # 加载相机位置配置 with open(sensor_definition_file) as sensor_data: multi_sensors = json.load(sensor_data) CAM_LOC = [] for i in multi_sensors["sensors"]: CAM_LOC.append(i["yaw"]) # 修正话题名的占位符替换 priority_topic = "/carla/{}/camera/rgb/view/priority".format(role_name) status_topic = "/carla/{}/vehicle_status".format(role_name) priority_pub = rospy.Publisher(priority_topic, Float32, queue_size=10) rospy.Subscriber(status_topic, CarlaEgoVehicleStatus, vehicle_status_callback) # 阻塞主进程等待回调事件 rospy.spin() if __name__ == "__main__": main()
内容的提问来源于stack exchange,提问作者Siddhant Yeole
相关产品推荐
相关产品推荐

