rtt_roscomm: shortcut ROS spinner callback queue

The patch introduces a new "queue" without a queue to bypass the global CallbackQueue of ROS for the subscriptions of Orocos messages. Instead, the network thread will immediately execute the callback, and will pass it to the global CallbackQueue only in case that it fails to execute with TryAgain status. Helper functions are provided to generate a subscriber with a custom implementation of CallbackQueueInterface. This method is chosen for the subscribers of the message transporter.
orocos · Sep 11, 2020 · 8129810 · 8129810
1 parent c65cf80
commit 8129810
Show file tree

Hide file tree

Showing 4 changed files with 266 additions and 4 deletions.
diff --git a/rtt_roscomm/CMakeLists.txt b/rtt_roscomm/CMakeLists.txt
@@ -17,6 +17,10 @@ orocos_library(rtt_rostopic
   src/rtt_rostopic.cpp)
 target_link_libraries(rtt_rostopic ${catkin_LIBRARIES})
 
+orocos_library(passthrough_callback_queue
+  src/passthrough_callback_queue.cpp)
+target_link_libraries(passthrough_callback_queue ${catkin_LIBRARIES})
+
 orocos_service(rtt_rostopic_service
   src/rtt_rostopic_service.cpp
   src/rtt_rostopic_ros_publish_activity.cpp)

diff --git a/rtt_roscomm/include/rtt_roscomm/passthrough_callback_queue.hpp b/rtt_roscomm/include/rtt_roscomm/passthrough_callback_queue.hpp
@@ -0,0 +1,232 @@
+
+#ifndef RTT_ROSCOMM__PASSTHROUGH_CALLBACK_QUEUE_HPP_
+#define RTT_ROSCOMM__PASSTHROUGH_CALLBACK_QUEUE_HPP_
+
+#include <ros/ros.h>
+#include <ros/callback_queue_interface.h>
+#include <ros/subscription_queue.h>
+#include <ros/callback_queue.h>
+
+namespace rtt_roscomm {
+  class PassthroughCallbackQueue: public ros::CallbackQueueInterface
+  {
+    public:
+      PassthroughCallbackQueue();
+
+      /** 
+       * Implementation of ros::CallbackQueueInterface::addCallback()
+       * 
+       * @param callback aaa
+       * @param owner_id aaa
+       * 
+       * @return void
+       */
+      virtual void addCallback(
+          const ros::CallbackInterfacePtr &callback,
+          uint64_t owner_id=0);
+
+      /** 
+       * Implementation of ros::CallbackQueueInterface::removeByID()
+       * 
+       * @param owner_id aaa
+       * 
+       * @return void
+       */
+      virtual void removeByID(uint64_t owner_id);
+
+    private:
+
+      /**
+       * Port where to write the message
+       */
+      // base::PortInterface* output_port_ptr_;
+
+  }; // class PassthroughCallbackQueue
+
+  /**
+   * Helpers to generate subscribers with a custom ros::CallbackQueueInterface
+   */
+
+      // template<class M, class T>
+    // ros::Subscriber subscribe(const std::string& topic, uint32_t queue_size, 
+    //                     void(T::*fp)(const boost::shared_ptr<M const>&) const, T* obj,
+    //                     ros::CallbackQueueInterface* cq, ros::NodeHandle& nh,
+    //                     const TransportHints& transport_hints = ros::TransportHints())
+    // {
+    //   ros::SubscribeOptions ops;
+    //   ops.template init<M>(topic, queue_size, boost::bind(fp, obj, _1));
+    //   ops.transport_hints = transport_hints;
+    //   ops.callback_queue = cq;
+    //   return nh.subscribe(ops);
+    // }
+
+  template<class M, class T>
+  ros::Subscriber subscribe(
+      ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
+      const std::string& topic, uint32_t queue_size, void(T::*fp)(M), T* obj, 
+      const ros::TransportHints& transport_hints = ros::TransportHints())
+  {
+    ros::SubscribeOptions ops;
+    ops.template initByFullCallbackType<M>(topic, queue_size, boost::bind(fp, obj, _1));
+    ops.transport_hints = transport_hints;
+    ops.callback_queue = cq;
+    return nh.subscribe(ops);
+  }
+
+  /// and the const version
+  template<class M, class T>
+  ros::Subscriber subscribe(
+      ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
+      const std::string& topic, uint32_t queue_size, void(T::*fp)(M) const, T* obj, 
+      const ros::TransportHints& transport_hints = ros::TransportHints())
+  {
+    ros::SubscribeOptions ops;
+    ops.template initByFullCallbackType<M>(topic, queue_size, boost::bind(fp, obj, _1));
+    ops.transport_hints = transport_hints;
+    ops.callback_queue = cq;
+    return nh.subscribe(ops);
+  }
+  template<class M, class T>
+  ros::Subscriber subscribe(
+      ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
+      const std::string& topic, uint32_t queue_size, 
+      void(T::*fp)(const boost::shared_ptr<M const>&), T* obj, 
+      const ros::TransportHints& transport_hints = ros::TransportHints())
+  {
+    ros::SubscribeOptions ops;
+    ops.template init<M>(topic, queue_size, boost::bind(fp, obj, _1));
+    ops.transport_hints = transport_hints;
+    ops.callback_queue = cq;
+    return nh.subscribe(ops);
+  }
+  template<class M, class T>
+  ros::Subscriber subscribe(
+      ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
+      const std::string& topic, uint32_t queue_size, 
+      void(T::*fp)(const boost::shared_ptr<M const>&) const, T* obj, 
+      const ros::TransportHints& transport_hints = ros::TransportHints())
+  {
+    ros::SubscribeOptions ops;
+    ops.template init<M>(topic, queue_size, boost::bind(fp, obj, _1));
+    ops.transport_hints = transport_hints;
+    ops.callback_queue = cq;
+    return nh.subscribe(ops);
+  }
+  template<class M, class T>
+  ros::Subscriber subscribe(
+      ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
+      const std::string& topic, uint32_t queue_size, void(T::*fp)(M), 
+      const boost::shared_ptr<T>& obj,
+      const ros::TransportHints& transport_hints = ros::TransportHints())
+  {
+    ros::SubscribeOptions ops;
+    ops.template initByFullCallbackType<M>(topic, queue_size, boost::bind(fp, obj.get(), _1));
+    ops.tracked_object = obj;
+    ops.transport_hints = transport_hints;
+    ops.callback_queue = cq;
+    return nh.subscribe(ops);
+  }
+
+  template<class M, class T>
+  ros::Subscriber subscribe(
+      ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
+      const std::string& topic, uint32_t queue_size, void(T::*fp)(M) const, 
+      const boost::shared_ptr<T>& obj,
+      const ros::TransportHints& transport_hints = ros::TransportHints())
+  {
+    ros::SubscribeOptions ops;
+    ops.template initByFullCallbackType<M>(topic, queue_size, boost::bind(fp, obj.get(), _1));
+    ops.tracked_object = obj;
+    ops.transport_hints = transport_hints;
+    ops.callback_queue = cq;
+    return nh.subscribe(ops);
+  }
+  template<class M, class T>
+  ros::Subscriber subscribe(
+      ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
+      const std::string& topic, uint32_t queue_size, 
+      void(T::*fp)(const boost::shared_ptr<M const>&), 
+      const boost::shared_ptr<T>& obj,
+      const ros::TransportHints& transport_hints = ros::TransportHints())
+  {
+    ros::SubscribeOptions ops;
+    ops.template init<M>(topic, queue_size, boost::bind(fp, obj.get(), _1));
+    ops.tracked_object = obj;
+    ops.transport_hints = transport_hints;
+    ops.callback_queue = cq;
+    return nh.subscribe(ops);
+  }
+  template<class M, class T>
+  ros::Subscriber subscribe(
+      ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
+      const std::string& topic, uint32_t queue_size, 
+      void(T::*fp)(const boost::shared_ptr<M const>&) const, 
+      const boost::shared_ptr<T>& obj,
+      const ros::TransportHints& transport_hints = ros::TransportHints())
+  {
+    ros::SubscribeOptions ops;
+    ops.template init<M>(topic, queue_size, boost::bind(fp, obj.get(), _1));
+    ops.tracked_object = obj;
+    ops.transport_hints = transport_hints;
+    ops.callback_queue = cq;
+    return nh.subscribe(ops);
+  }
+  template<class M>
+  ros::Subscriber subscribe(
+      ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
+      const std::string& topic, uint32_t queue_size, void(*fp)(M),
+      const ros::TransportHints& transport_hints = ros::TransportHints())
+  {
+    ros::SubscribeOptions ops;
+    ops.template initByFullCallbackType<M>(topic, queue_size, fp);
+    ops.transport_hints = transport_hints;
+    ops.callback_queue = cq;
+    return nh.subscribe(ops);
+  }
+  template<class M>
+  ros::Subscriber subscribe(
+      ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
+      const std::string& topic, uint32_t queue_size,
+      void(*fp)(const boost::shared_ptr<M const>&),
+      const ros::TransportHints& transport_hints = ros::TransportHints())
+  {
+    ros::SubscribeOptions ops;
+    ops.template init<M>(topic, queue_size, fp);
+    ops.transport_hints = transport_hints;
+    ops.callback_queue = cq;
+    return nh.subscribe(ops);
+  }
+  template<class M>
+  ros::Subscriber subscribe(
+      ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
+      const std::string& topic, uint32_t queue_size,
+      const boost::function<void (const boost::shared_ptr<M const>&)>& callback,
+      const ros::VoidConstPtr& tracked_object = ros::VoidConstPtr(),
+      const ros::TransportHints& transport_hints = ros::TransportHints())
+  {
+    ros::SubscribeOptions ops;
+    ops.template init<M>(topic, queue_size, callback);
+    ops.tracked_object = tracked_object;
+    ops.transport_hints = transport_hints;
+    ops.callback_queue = cq;
+    return nh.subscribe(ops);
+  }
+  template<class M, class C>
+  ros::Subscriber subscribe(
+      ros::NodeHandle& nh, ros::CallbackQueueInterface* cq,
+      const std::string& topic, uint32_t queue_size,
+      const boost::function<void (C)>& callback,
+      const ros::VoidConstPtr& tracked_object = ros::VoidConstPtr(),
+      const ros::TransportHints& transport_hints = ros::TransportHints())
+  {
+    ros::SubscribeOptions ops;
+    ops.template initByFullCallbackType<C>(topic, queue_size, callback);
+    ops.tracked_object = tracked_object;
+    ops.transport_hints = transport_hints;
+    ops.callback_queue = cq;
+    return nh.subscribe(ops);
+  }
+
+} // namespace rtt_roscomm
+
+#endif // RTT_ROSCOMM__PASSTHROUGH_CALLBACK_QUEUE_HPP_
diff --git a/rtt_roscomm/include/rtt_roscomm/rtt_rostopic_ros_msg_transporter.hpp b/rtt_roscomm/include/rtt_roscomm/rtt_rostopic_ros_msg_transporter.hpp
@@ -56,6 +56,7 @@
 #include <rtt/internal/ConnFactory.hpp>
 #include <ros/ros.h>
 
+#include <rtt_roscomm/passthrough_callback_queue.hpp>
 #include <rtt_roscomm/rtt_rostopic_ros_publish_activity.hpp>
 
 #ifndef RTT_VERSION_GTE
@@ -241,6 +242,7 @@ namespace rtt_roscomm {
     ros::NodeHandle ros_node;
     ros::NodeHandle ros_node_private;
     ros::Subscriber ros_sub;
+    PassthroughCallbackQueue prassthrough_callback_queue;
 
   public:
     /** 
@@ -264,9 +266,9 @@ namespace rtt_roscomm {
         RTT::log(RTT::Debug)<<"Creating ROS subscriber for port "<<port->getName()<<" on topic "<<policy.name_id<<RTT::endlog();
       }
       if(topicname.length() > 1 && topicname.at(0) == '~') {
-        ros_sub = ros_node_private.subscribe(policy.name_id.substr(1), policy.size > 0 ? policy.size : 1, &RosSubChannelElement::newData, this); // minimum queue_size 1
+        ros_sub = subscribe(ros_node_private, &prassthrough_callback_queue, policy.name_id.substr(1), policy.size > 0 ? policy.size : 1, &RosSubChannelElement::newData, this); // minimum queue_size 1
       } else {
-        ros_sub = ros_node.subscribe(policy.name_id, policy.size > 0 ? policy.size : 1, &RosSubChannelElement::newData, this); // minimum queue_size 1
+        ros_sub = subscribe(ros_node, &prassthrough_callback_queue, policy.name_id, policy.size > 0 ? policy.size : 1, &RosSubChannelElement::newData, this); // minimum queue_size 1
       }
     }
 
@@ -350,6 +352,6 @@ namespace rtt_roscomm {
 
       return channel;
     }
-  };
-} 
+  }; // class RosMsgTransporter
+} // namespace rtt_roscomm
 #endif
diff --git a/rtt_roscomm/src/passthrough_callback_queue.cpp b/rtt_roscomm/src/passthrough_callback_queue.cpp
@@ -0,0 +1,24 @@
+
+#include "rtt_roscomm/passthrough_callback_queue.hpp"
+
+namespace rtt_roscomm {
+
+PassthroughCallbackQueue::PassthroughCallbackQueue()
+        {}
+
+void PassthroughCallbackQueue::addCallback(
+    const ros::CallbackInterfacePtr &callback,
+    uint64_t owner_id)
+{
+  // Call CallbackInterface::CallResult SubscriptionQueue::call()
+  if (ros::CallbackInterface::TryAgain == callback->call()) {
+    ros::getGlobalCallbackQueue()->addCallback(callback, owner_id);
+  }
+}
+
+void PassthroughCallbackQueue::removeByID(uint64_t owner_id)
+{
+}
+
+} // namespace rtt_roscomm
+