如何將 ROS 主題延遲一定時間

Question

實際問題：

我有一個訂閱 2 個主題並發布到 1 個主題的 controller 節點。 盡管在模擬中一切似乎都按預期工作，但在實際硬件中，性能會下降。 我懷疑問題在於兩個輸入主題之一在相當長的時間內落后於另一個。

問題：

我想在模擬中重新創建這種行為，以測試 controller 的穩健性。 因此，我需要將其中一個主題延遲一定時間 - 理想情況下，這應該是可配置的參數。 我可以編寫一個具有 FIFO memory 緩沖區的節點，並通過監控主題的頻率來調整延遲時間。 在我這樣做之前，是否有一個命令行工具或任何其他我可以使用的快速實現方法？

Ps 我正在使用 Ubuntu 16.04 和 ROS-Kinetic。

Answer 1

我不知道有什么開箱即用的解決方案可以完全按照您在此處描述的方式進行。

對於快速破解，如果您的主題沒有時間戳並且節點只是在消息到達時接收消息，最簡單的做法是記錄一個包並從兩個不同的rosbag play實例播放兩個主題。 像這樣的東西：

first terminal
rosbag play mybag.bag --clock --topics /my/topic
second terminal started some amount of time later
rosbag play mybag.bag --topics /my/other_topic

不確定--clock標志，是否需要它主要取決於模擬的含義。 如果您想控制時差而不是在兩個不同的終端中按 Enter 鍵，您可以編寫一個小的 bash 腳本來啟動它們。

---

另一個仍然涉及包的選項，但可以讓您更好地控制消息延遲的確切時間，即編輯包以使包中的消息已經具有正確的延遲。 通過修改rosbag 食譜中的第一個示例可以相對容易地完成：

import rosbag

with rosbag.Bag('output.bag', 'w') as outbag:
    for topic, msg, t in rosbag.Bag('input.bag').read_messages():
        # This also replaces tf timestamps under the assumption 
        # that all transforms in the message share the same timestamp
        if topic == "/tf" and msg.transforms:
            outbag.write(topic, msg, msg.transforms[0].header.stamp)
        else:
            outbag.write(topic, msg, msg.header.stamp if msg._has_header else t)

將 if else 替換為：

...
import rospy
...

if topic == "/my/other_topic":
    outbag.write(topic, msg, t + rospy.Duration.from_sec(0.5))
else:
    outbag.write(topic, msg, t)

應該能讓你大部分時間到達那里。

---

除此之外，如果您認為該節點將來會有用，或者您希望它也可以處理實時數據，那么您需要使用一些隊列來實現您描述的節點。 您可以從主題工具中尋找靈感，git 是主題工具源。