You need to enable JavaScript to run this app. ROS中发布函数内计算的Float32型priority值到自定义话题的实现方法
优惠活动
大模型
产品
解决方案
定价
更多

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函数内部生成的局部变量,发布逻辑根本访问不到这个值,可根据现有代码架构选两种适配方案:
    1. 给odometry_weight_model增加返回值,计算完成后将priority作为函数返回值传出,在拿到速度、姿态、转向角参数的车辆状态订阅回调中接收返回值,直接构造消息发布
    2. 将priority定义为全局变量,在里程计权重模型函数内部更新全局变量值,发布逻辑直接读取该全局变量完成发布
  • 修正原有代码的逻辑漏洞
    逐段核对现有代码后,这些问题必须修正:
    1. 初始化Publisher时,话题字符串里的{role_name}不会被自动替换为参数读到的车辆名,必须手动用字符串格式化替换,否则实际发布的话题名会字面保留大括号占位符,和预期的话题路径完全不一致
    2. 加载传感器配置时,循环里写的是camera_loc.append(i["yaw"]),但前面初始化的列表叫CAM_LOC,这里变量名笔误会直接触发NameError
    3. 原结构里的无意义while循环完全可以移除:ROS Python的订阅回调是在后台线程运行的,只要通过rospy.spin()阻塞主进程等待回调事件即可,不需要在无数据的空循环里轮询发空消息,也避免了rospy.spin()永远执行不到的问题

修正后的参考代码片段

# 导入依赖的消息类型
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

相关产品推荐
方舟 Agent Plan

超全模态模型 × Harness 升级,最新支持 Deepseek-V4.1-Flash、GLM-5.3 系列、Doubao-Seedream-5.0-pro、Kimi-K3 (部分), 限时 9.9 元起

最近更新时间:2026.11.15 01:24:03