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,69 @@
+/*
+ * (C) 2020, Intermodalics BVBA
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#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:
+    /** 
+     * Implementation of ros::CallbackQueueInterface::addCallback()
+     * 
+     * This method is executing the callback received instead of adding
+     * it to a queue. In this way, the queue is bypassed and the callback is
+     * immediately executed.
+     * 
+     * @param callback callback to execute, instead of queueing
+     * @param owner_id Not used
+     */
+    virtual void addCallback(
+        const ros::CallbackInterfacePtr &callback,
+        uint64_t owner_id=0);
+
+    /** 
+     * Implementation of ros::CallbackQueueInterface::removeByID()
+     * 
+     * No-op
+     * 
+     * @param owner_id Not used.
+     */
+    virtual void removeByID(uint64_t owner_id) {}
+
+}; // class PassthroughCallbackQueue
+
+} // 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
@@ -147,8 +148,6 @@ namespace rtt_roscomm {
     }
 
     ~RosPubChannelElement() {
-      RTT::Logger::In in(topicname);
-//      RTT::log(RTT::Debug) << "Destroying RosPubChannelElement" << RTT::endlog();
       act->removePublisher( this );
     }
 
@@ -240,6 +239,7 @@ namespace rtt_roscomm {
     std::string topicname;
     ros::NodeHandle ros_node;
     ros::NodeHandle ros_node_private;
+    PassthroughCallbackQueue passthrough_callback_queue;
     ros::Subscriber ros_sub;
 
   public:
@@ -256,23 +256,30 @@ namespace rtt_roscomm {
       ros_node(),
       ros_node_private("~")
     {
-      topicname=policy.name_id;
+      topicname = policy.name_id;
+      ros::SubscribeOptions ops;
+      ops.template initByFullCallbackType<const RosType&>(
+          topicname,
+          1u,  // Always 1, since data can be buffered in RTT buffer
+          boost::bind(&RosSubChannelElement::newData, this, _1));
+      ops.callback_queue = &passthrough_callback_queue;
+
       RTT::Logger::In in(topicname);
       if (port->getInterface() && port->getInterface()->getOwner()) {
         RTT::log(RTT::Debug)<<"Creating ROS subscriber for port "<<port->getInterface()->getOwner()->getName()<<"."<<port->getName()<<" on topic "<<policy.name_id<<RTT::endlog();
       } else {
         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
+        ops.topic = ops.topic.substr(1);
+        ros_sub = ros_node_private.subscribe(ops);
       } else {
-        ros_sub = ros_node.subscribe(policy.name_id, policy.size > 0 ? policy.size : 1, &RosSubChannelElement::newData, this); // minimum queue_size 1
+        ros_sub = ros_node.subscribe(ops);
       }
     }
 
     ~RosSubChannelElement() {
-      RTT::Logger::In in(topicname);
-//      RTT::log(RTT::Debug)<<"Destroying RosSubChannelElement"<<RTT::endlog();
+      ros_sub.shutdown();
     }
 
     virtual bool inputReady() {
@@ -350,6 +357,6 @@ namespace rtt_roscomm {
 
       return channel;
     }
-  };
-} 
+  }; // class RosMsgTransporter
+} // namespace rtt_roscomm
 #endif

rtt_roscomm/src/passthrough_callback_queue.cpp

@@ -0,0 +1,44 @@
+/*
+ * (C) 2020, Intermodalics BVBA
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "rtt_roscomm/passthrough_callback_queue.hpp"
+
+namespace rtt_roscomm {
+
+void PassthroughCallbackQueue::addCallback(
+    const ros::CallbackInterfacePtr &callback,
+    uint64_t owner_id)
+{
+  ros::CallbackInterface::CallResult result = callback->call();
+  if (ros::CallbackInterface::TryAgain == result) {
+    ros::getGlobalCallbackQueue()->addCallback(callback, owner_id);
+  }
+}
+
+} // namespace rtt_roscomm