如何为ROS话题添加延迟发布？车辆仿真场景需求

一、现成命令行工具：topic_tools/delay #

ROS官方提供的topic_tools包包含delay节点，可直接给指定话题添加固定延迟，无需编写代码。

使用步骤 #

1. 确认topic_tools包已安装：
   若未安装，执行对应ROS版本的安装命令（以Noetic为例）：

   sudo apt install ros-noetic-topic-tools
   

2. 启动延迟节点：

   rosrun topic_tools delay /original_steer_topic /delayed_steer_topic 2.0
   

   参数说明：

   • /original_steer_topic：原始转向信号话题名称
   • /delayed_steer_topic：延迟后输出的话题名称
   • 2.0：需要添加的延迟时长（单位：秒）

3. 集成到launch文件（可选）：

   <node name="steer_delay_node" pkg="topic_tools" type="delay" args="/original_steer_topic /delayed_steer_topic 2.0" />
   

二、自定义实现方案（适用于需扩展逻辑的场景） #

如果需要动态调整延迟、结合其他业务逻辑，可自行编写ROS节点实现：

Python节点示例 #

import rospy
from std_msgs.msg import Float64  # 根据实际转向信号的消息类型修改

class TopicDelay:
    def __init__(self):
        self.delay = rospy.get_param('~delay', 2.0)
        self.msg_queue = []
        
        self.sub = rospy.Subscriber('/original_steer_topic', Float64, self.msg_callback)
        self.pub = rospy.Publisher('/delayed_steer_topic', Float64, queue_size=10)
        
        # 定时检查队列，发送达到延迟要求的消息
        self.timer = rospy.Timer(rospy.Duration(0.01), self.process_queue)

    def msg_callback(self, msg):
        # 存储消息及其接收时间
        self.msg_queue.append((rospy.Time.now(), msg))

    def process_queue(self, event):
        current_time = rospy.Time.now()
        # 筛选出已满足延迟要求的消息并发送
        pending_indices = []
        for idx, (recv_time, msg) in enumerate(self.msg_queue):
            if (current_time - recv_time).to_sec() >= self.delay:
                self.pub.publish(msg)
                pending_indices.append(idx)
        # 从队列中移除已发送的消息（倒序删除避免索引混乱）
        for idx in reversed(pending_indices):
            del self.msg_queue[idx]

if __name__ == '__main__':
    rospy.init_node('steer_delay_node')
    TopicDelay()
    rospy.spin()

C++节点示例 #

#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <queue>

struct TimedMessage {
    ros::Time receive_time;
    std_msgs::Float64 message;
};

class TopicDelayNode {
private:
    ros::NodeHandle nh_;
    ros::Subscriber sub_;
    ros::Publisher pub_;
    ros::Timer timer_;
    double delay_;
    std::queue<TimedMessage> msg_queue_;

    void callback(const std_msgs::Float64::ConstPtr& msg) {
        TimedMessage timed_msg;
        timed_msg.receive_time = ros::Time::now();
        timed_msg.message = *msg;
        msg_queue_.push(timed_msg);
    }

    void processQueue(const ros::TimerEvent& event) {
        ros::Time current_time = ros::Time::now();
        // 遍历队列，发送满足延迟要求的消息
        while (!msg_queue_.empty()) {
            TimedMessage front_msg = msg_queue_.front();
            if ((current_time - front_msg.receive_time).toSec() >= delay_) {
                pub_.publish(front_msg.message);
                msg_queue_.pop();
            } else {
                break;
            }
        }
    }

public:
    TopicDelayNode() : delay_(2.0) {
        nh_.param("delay", delay_, 2.0);
        sub_ = nh_.subscribe("/original_steer_topic", 10, &TopicDelayNode::callback, this);
        pub_ = nh_.advertise<std_msgs::Float64>("/delayed_steer_topic", 10);
        timer_ = nh_.createTimer(ros::Duration(0.01), &TopicDelayNode::processQueue, this);
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "steer_delay_node");
    TopicDelayNode node;
    ros::spin();
    return 0;
}

注意：代码中的消息类型（std_msgs/Float64）需替换为你实际使用的转向信号话题类型（如geometry_msgs/Twist）。

内容的提问来源于stack exchange，提问作者gxnse