rtt_roscomm: Shortcut ROS spinner callback queue for rtt subscribers

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)

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_

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

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
+