ros_realtime: Subscriber needs the ability to attach filters before it

[ablasdel]

Migrated from code.ros.org, issue #2960 Reported by: jfaust

https://code.ros.org/trac/ros/ticket/2960

For the rosrt Subscriber to be truly useful in realtime, filters that can (for example) transform a message into the correct frame need to be able to run outside realtime before the Subscriber passes the message along.

[ablasdel]

Changed 2 years ago by jfaust

milestone set to ros_realtime 1.0 Changed 2 years ago by mrinal

cc pastor added We needed this feature in rosrt, so Peter and I quickly hacked up a first version. There is a new rosrt::FilteredSubscriber? class that allows the user to specify a function that transforms the message into a user-defined type. The function is executed in non-realtime. Patch attached, feedback welcome.

Changed 2 years ago by mrinal

attachment rosrt_filters.diff added Patch which adds a rosrt::FilteredSubscriber? class. Apply with patch -p3 in the parent directory of rosrt/

Changed 2 years ago by jfaust

owner changed from jfaust to gerkey Changed 20 months ago by mrinal

owner changed from gerkey to mrinal status changed from new to assigned Changed 16 months ago by kwc

milestone ros_realtime 1.0 deleted Milestone ros_realtime 1.0 deleted

[ablasdel]

patch

diff --git a/ros_extras/ros_realtime/rosrt/CMakeLists.txt b/ros_extras/ros_realtime/rosrt/CMakeLists.txt
index a717b1a..f594db2 100644
--- a/ros_extras/ros_realtime/rosrt/CMakeLists.txt
+++ b/ros_extras/ros_realtime/rosrt/CMakeLists.txt
@@ -58,6 +58,11 @@ target_link_libraries(test_subscriber ${PROJECT_NAME})
 rosbuild_add_gtest_build_flags(test_subscriber)
 rosbuild_add_rostest(test/test_subscriber.xml)

+rosbuild_add_executable(test_filtered_subscriber EXCLUDE_FROM_ALL  test/test_filtered_subscriber.cpp)
+target_link_libraries(test_filtered_subscriber ${PROJECT_NAME})
+rosbuild_add_gtest_build_flags(test_filtered_subscriber)
+rosbuild_add_rostest(test/test_filtered_subscriber.xml)
+
 rosbuild_add_gtest(test_malloc_wrappers test/test_malloc_wrappers.cpp)
 target_link_libraries(test_malloc_wrappers ${PROJECT_NAME})

diff --git a/ros_extras/ros_realtime/rosrt/include/rosrt/filtered_subscriber.h b/ros_extras/ros_realtime/rosrt/include/rosrt/filtered_subscriber.h
new file mode 100644
index 0000000..89cb95f
--- /dev/null
+++ b/ros_extras/ros_realtime/rosrt/include/rosrt/filtered_subscriber.h
@@ -0,0 +1,239 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2010, CLMC Lab, University of Southern California
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * 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.
+*   * Neither the name of the Willow Garage 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 OWNER 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 ROSRT_FILTERED_SUBSCRIBER_H
+#define ROSRT_FILTERED_SUBSCRIBER_H
+
+#include <boost/function.hpp>
+
+#include <lockfree/object_pool.h>
+#include "detail/pool_gc.h"
+
+#include <ros/atomic.h>
+#include <ros/ros.h>
+#include <rosrt/subscriber.h>
+
+namespace rosrt
+{
+
+/**
+ * \brief A lock-free, filtered subscriber.  Allows you to receive ROS messages inside a realtime thread.
+ *
+ * This subscriber will pass the message through a user-defined "filter", which will convert the message
+ * into another user-defined type. The filter function will be called outside of realtime.
+ *
+ * This subscriber works in a polling manner rather than the usual callback-based mechanism, e.g.:
+\verbatim
+bool filter(const MsgConstPtr& msg, const FilteredPtr filtered)
+{
+   // filtered->data = msg->data;
+}
+
+FilteredSubscriber<Msg,Filtered> sub(2, nh, "my_topic", filter);
+while (true)
+{
+  FilteredConstPtr filtered_msg = sub.poll();
+  if (filtered_msg)
+  {
+    // do something with filtered_msg
+    ...
+  }
+}
+\endverbatim
+ */
+template<typename M, typename Filtered>
+class FilteredSubscriber
+{
+public:
+  /**
+   * \brief Default constructor.  You must call initialize() before doing anything else if you use this constructor.
+   */
+  FilteredSubscriber()
+  : filtered_pool_(0)
+  {
+  }
+
+  /**
+   * \brief Constructor with initialization.  Call subscribe() to subscribe to a topic.
+   * \param message_pool_size The size of the message pool to use.  If this pool fills up no more messages
+   * will be received until some messages are freed.
+   */
+  FilteredSubscriber(uint32_t message_pool_size)
+  : filtered_pool_(0)
+  {
+    initialize(message_pool_size);
+  }
+
+  /**
+   * \brief Constructor with initialization and subscription
+   * \param message_pool_size The size of the message pool to use.  If this pool fills up no more messages
+   * will be received until some messages are freed.
+   * \param nh The ros::NodeHandle to use to subscribe
+   * \param topic The topic to subscribe on
+   * \param filter The filter function to convert the message into the type Filtered
+   * \param [optional] transport_hints the transport hints to use
+   */
+  FilteredSubscriber(uint32_t message_pool_size, ros::NodeHandle& nh, const std::string& topic,
+                     boost::function<bool (const boost::shared_ptr<M const>& msg, const boost::shared_ptr<Filtered> filtered)> filter,
+                     const ros::TransportHints& transport_hints = ros::TransportHints())
+  : filtered_pool_(0)
+  {
+    initialize(message_pool_size);
+    subscribe(nh, topic, filter, transport_hints);
+  }
+
+  ~FilteredSubscriber()
+  {
+    Filtered const* latest = latest_.exchange(0);
+    if (latest)
+    {
+      filtered_pool_->free(latest);
+    }
+
+    detail::addPoolToGC((void*)filtered_pool_, detail::deletePool<Filtered>, detail::poolIsDeletable<Filtered>);
+  }
+
+  /**
+   * \brief Initialize this subscribe.  Only use with the default constructor.
+   * \param message_pool_size The size of the message pool to use.  If this pool fills up no more messages
+   * will be received until some messages are freed.
+   */
+  void initialize(uint32_t message_pool_size)
+  {
+    ROS_ASSERT(message_pool_size > 1);
+    ROS_ASSERT(!filtered_pool_);
+    filtered_pool_ = new lockfree::ObjectPool<Filtered>();
+    filtered_pool_->initialize(message_pool_size, Filtered());
+    latest_.store(0);
+  }
+
+  /**
+   * \brief Initialize this subscribe.  Only use with the default constructor.
+   * \param message_pool_size The size of the message pool to use.  If this pool fills up no more messages
+   * will be received until some messages are freed.
+   * \param nh The ros::NodeHandle to use to subscribe
+   * \param topic The topic to subscribe on
+   * \param filter The filter function to convert the message into the type Filtered
+   * \param [optional] transport_hints the transport hints to use
+   * \return Whether or not we successfully subscribed
+   */
+  bool initialize(uint32_t message_pool_size, ros::NodeHandle& nh, const std::string& topic,
+                  boost::function<bool (const boost::shared_ptr<M const>& msg, const boost::shared_ptr<Filtered> filtered)> filter,
+                  const ros::TransportHints& transport_hints = ros::TransportHints())
+  {
+    initialize(message_pool_size);
+    return subscribe(nh, topic, filter, transport_hints);
+  }
+
+  /**
+   * \brief Subscribe to a topic
+   * \param nh The ros::NodeHandle to use to subscribe
+   * \param topic The topic to subscribe on
+   * \param filter The filter function to convert the message into the type Filtered
+   * \param [optional] transport_hints the transport hints to use
+   * \return Whether or not we successfully subscribed
+   */
+  bool subscribe(ros::NodeHandle& nh, const std::string& topic,
+                 boost::function<bool (const boost::shared_ptr<M const>& msg, const boost::shared_ptr<Filtered> filtered)> filter,
+                 const ros::TransportHints& transport_hints = ros::TransportHints())
+  {
+    ros::SubscribeOptions ops;
+    ops.template init<M>(topic, 1, boost::bind(&FilteredSubscriber::callback, this, _1));
+    ops.callback_queue = detail::getSubscriberCallbackQueue();
+    sub_ = nh.subscribe(ops);
+    filter_ = filter;
+    return (bool)sub_;
+  }
+
+  /**
+   * \brief Retrieve the newest message received.
+   *
+   * The same message will only be returned once, i.e:
+\verbatim
+<msg received>
+msg = poll(); // Returns a valid message
+msg = poll(); // Returns NULL
+\endverbatim
+   */
+  boost::shared_ptr<Filtered const> poll()
+  {
+    Filtered const* latest = latest_.exchange(0);
+    if (!latest)
+    {
+      return boost::shared_ptr<Filtered const>();
+    }
+
+    boost::shared_ptr<Filtered const> ptr = filtered_pool_->makeShared(latest);
+    if (!ptr)
+    {
+      filtered_pool_->free(latest);
+      return boost::shared_ptr<Filtered const>();
+    }
+
+    return ptr;
+  }
+
+private:
+  void callback(const boost::shared_ptr<M const>& msg)
+  {
+    boost::shared_ptr<Filtered> filtered = filtered_pool_->allocateShared();
+    if (!filtered)
+    {
+      ROS_ERROR("FilteredSubscriber: could not allocate filtered object.");
+      return;
+    }
+    if (!filter_(msg, filtered))
+    {
+      ROS_ERROR("FilteredSubscriber: filter function failed.");
+      return;
+    }
+
+    Filtered const* latest = filtered_pool_->removeShared(filtered);
+    Filtered const* old = latest_.exchange(latest);
+    if (old)
+    {
+      filtered_pool_->free(old);
+    }
+  }
+
+  ros::atomic<Filtered const*> latest_;
+
+  lockfree::ObjectPool<Filtered>* filtered_pool_;
+  ros::Subscriber sub_;
+  boost::function<bool (const boost::shared_ptr<M const>& msg, const boost::shared_ptr<Filtered> filtered)> filter_;
+};
+
+} // namespace rosrt
+
+#endif // ROSRT_FILTERED_SUBSCRIBER_H
diff --git a/ros_extras/ros_realtime/rosrt/include/rosrt/rosrt.h b/ros_extras/ros_realtime/rosrt/include/rosrt/rosrt.h
index a640667..f35423a 100644
--- a/ros_extras/ros_realtime/rosrt/include/rosrt/rosrt.h
+++ b/ros_extras/ros_realtime/rosrt/include/rosrt/rosrt.h
@@ -37,6 +37,7 @@

 #include "publisher.h"
 #include "subscriber.h"
+#include "filtered_subscriber.h"
 #include "malloc_wrappers.h"
 #include "init.h"

diff --git a/ros_extras/ros_realtime/rosrt/test/test_filtered_subscriber.cpp b/ros_extras/ros_realtime/rosrt/test/test_filtered_subscriber.cpp
new file mode 100644
index 0000000..f885499
--- /dev/null
+++ b/ros_extras/ros_realtime/rosrt/test/test_filtered_subscriber.cpp
@@ -0,0 +1,128 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2010, CLMC Lab, University of Southern California
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * 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.
+*   * Neither the name of the Willow Garage 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 OWNER 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 <gtest/gtest.h>
+
+#include "rosrt/rosrt.h"
+
+#include <ros/ros.h>
+
+#include <std_msgs/UInt32.h>
+#include <std_msgs/UInt64.h>
+
+#include <boost/thread.hpp>
+
+#ifdef __XENO__
+#include <native/task.h>
+#include <sys/mman.h>
+#endif
+
+using namespace rosrt;
+
+void publishThread(ros::Publisher& pub, bool& done)
+{
+  uint32_t i = 0;
+  std_msgs::UInt32 msg;
+  while (!done)
+  {
+    msg.data = i;
+    pub.publish(msg);
+    ros::WallDuration(0.0001).sleep();
+    ++i;
+  }
+}
+
+bool filter(const boost::shared_ptr<std_msgs::UInt32 const>& msg, const boost::shared_ptr<std_msgs::UInt64> filtered)
+{
+  filtered->data = msg->data;
+
+  // some dummy memory allocations
+  int* x = new int();
+  *x=0;
+  delete x;
+
+  return true;
+}
+
+TEST(FilteredSubscriber, singleSubscriber)
+{
+  ros::NodeHandle nh;
+  ros::Publisher pub = nh.advertise<std_msgs::UInt32>("test", 1);
+  bool done = false;
+  boost::thread t(boost::bind(publishThread, boost::ref(pub), boost::ref(done)));
+
+  FilteredSubscriber<std_msgs::UInt32, std_msgs::UInt64> sub;
+  sub.initialize(2, nh, "test", filter);
+  //FilteredSubscriber<std_msgs::UInt32, std_msgs::UInt64> sub(2, nh, filter, "test");
+
+  resetThreadAllocInfo();
+
+  uint32_t count = 0;
+  int32_t last = -1;
+  while (count < 10000)
+  {
+    std_msgs::UInt64ConstPtr msg = sub.poll();
+    if (msg)
+    {
+      ASSERT_GT((int32_t)msg->data, last);
+      last = msg->data;
+      ++count;
+      //printf("inc\n");
+    }
+#ifdef  __XENO__
+    rt_task_sleep(1000000);
+#endif
+  }
+
+  ASSERT_EQ(getThreadAllocInfo().total_ops, 0UL);
+
+  done = true;
+  t.join();
+}
+
+int main(int argc, char** argv)
+{
+#ifdef __XENO__
+  mlockall(MCL_CURRENT | MCL_FUTURE);
+  rt_task_shadow(NULL, "test_rt_filtered_subscriber", 0, 0);
+#endif
+
+  ros::init(argc, argv, "test_rt_filtered_subscriber");
+  testing::InitGoogleTest(&argc, argv);
+
+  ros::NodeHandle nh;
+  rosrt::init();
+
+  return RUN_ALL_TESTS();
+}
diff --git a/ros_extras/ros_realtime/rosrt/test/test_filtered_subscriber.xml b/ros_extras/ros_realtime/rosrt/test/test_filtered_subscriber.xml
new file mode 100644
index 0000000..558f937
--- /dev/null
+++ b/ros_extras/ros_realtime/rosrt/test/test_filtered_subscriber.xml
@@ -0,0 +1,6 @@
+<launch>
+  <test test-name="test_filtered_subscriber" pkg="rosrt" type="test_filtered_subscriber" time-limit="1000"/>
+</launch>
+
+
+