ros_realtime: add support for Xenomai RTOS

[ablasdel]

Migrated from code.ros.org, issue #3231
Reported by: mrinal

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

When the rosrt::Publisher is used from a Xenomai task, it causes the task to switch to secondary mode when signaling a boost/pthread condition variable.

The attached patch is a first attempt at adding Xenomai support to rosrt. It adds three wrapper classes, rosrt::mutex, rosrt::condition_variable, and rosrt::thread, which wrap xenomai functions or boost classes depending on the platform. Tests have also been modified to work on Xenomai, because a loop without sleeps in a real-time thread can starve the system since there is no preemption.

[ablasdel]

Changed 2 years ago by mrinal

attachment rosrt_xenomai.diff added
First patch for rosrt Xenomai support. (use patch -p3 in the parent dir of rosrt/)

Changed 2 years ago by jfaust

owner changed from jfaust to straszheim
Changed 2 years ago by jfaust

owner changed from straszheim to gerkey
Changed 20 months ago by gerkey

cc straszheim, sglaser added
owner changed from gerkey to mrinal
See comment in #2496.

Changed 20 months ago by mrinal

status changed from new to assigned

[ablasdel]

diff --git a/ros_extras/ros_realtime/rosrt/CMakeLists.txt b/ros_extras/ros_realtime/rosrt/CMakeLists.txt
index 4e64214..a717b1a 100644
--- a/ros_extras/ros_realtime/rosrt/CMakeLists.txt
+++ b/ros_extras/ros_realtime/rosrt/CMakeLists.txt
@@ -11,6 +11,19 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

 rosbuild_init()

+# Check for Xenomai:
+set(XENOMAI_SEARCH_PATH /usr/local/xenomai /usr/xenomai /usr)
+find_path(XENOMAI_DIR bin/xeno-config ${XENOMAI_SEARCH_PATH})
+if (XENOMAI_DIR)
+  set (XENOMAI_CONFIG ${XENOMAI_DIR}/bin/xeno-config)
+  message ("Xenomai found in ${XENOMAI_DIR}")
+  execute_process(COMMAND ${XENOMAI_CONFIG} --skin=native --cflags OUTPUT_VARIABLE XENOMAI_CFLAGS OUTPUT_STRIP_TRAILING_WHITESPACE)
+  execute_process(COMMAND ${XENOMAI_CONFIG} --skin=native --ldflags OUTPUT_VARIABLE XENOMAI_LDFLAGS OUTPUT_STRIP_TRAILING_WHITESPACE)
+  set (ROSRT_PLATFORM_CFLAGS ${XENOMAI_CFLAGS})
+  set (ROSRT_PLATFORM_LDFLAGS ${XENOMAI_LDFLAGS})
+  #add_definitions(${XENOMAI_CFLAGS})
+endif (XENOMAI_DIR)
+
 #set the default path for built executables to the "bin" directory
 set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 #set the default path for built libraries to the "lib" directory
@@ -23,11 +36,17 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

 include_directories(include)

+#add platform-specific compile flags
+add_definitions(${ROSRT_PLATFORM_CFLAGS})
+
 #common commands for building c++ executables and libraries
 rosbuild_add_library(${PROJECT_NAME} src/malloc.cpp src/simple_gc.cpp src/publisher.cpp src/subscriber.cpp src/init.cpp)
 rosbuild_add_boost_directories()
 rosbuild_link_boost(${PROJECT_NAME} thread)

+#add platform-specific linker flags
+target_link_libraries(${PROJECT_NAME} ${ROSRT_PLATFORM_LDFLAGS})
+
 rosbuild_add_executable(test_publisher EXCLUDE_FROM_ALL test/test_publisher.cpp)
 rosbuild_add_gtest_build_flags(test_publisher)
 target_link_libraries(test_publisher ${PROJECT_NAME})
diff --git a/ros_extras/ros_realtime/rosrt/include/rosrt/detail/condition_variable.h b/ros_extras/ros_realtime/rosrt/include/rosrt/detail/condition_variable.h
new file mode 100644
index 0000000..0c7b4b6
--- /dev/null
+++ b/ros_extras/ros_realtime/rosrt/include/rosrt/detail/condition_variable.h
@@ -0,0 +1,145 @@
+/*****************************************************************************
+ * Boost Software License - Version 1.0 - August 17th, 2003
+ *
+ * Permission is hereby granted, free of charge, to any person or organization
+ * obtaining a copy of the software and accompanying documentation covered by
+ * this license (the "Software") to use, reproduce, display, distribute,
+ * execute, and transmit the Software, and to prepare derivative works of the
+ * Software, and to permit third-parties to whom the Software is furnished to
+ * do so, all subject to the following:
+ *
+ * The copyright notices in the Software and this entire statement, including
+ * the above license grant, this restriction and the following disclaimer,
+ * must be included in all copies of the Software, in whole or in part, and
+ * all derivative works of the Software, unless such copies or derivative
+ * works are solely in the form of machine-executable object code generated by
+ * a source language processor.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+ * SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+ * FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * \author Mrinal Kalakrishnan <[email protected]>
+ ******************************************************************************/
+
+#ifndef ROSRT_CONDITION_VARIABLE_H_
+#define ROSRT_CONDITION_VARIABLE_H_
+
+#include <boost/utility.hpp>
+#include <boost/assert.hpp>
+#include <boost/thread/exceptions.hpp>
+#include <boost/thread/locks.hpp>
+#include <rosrt/detail/mutex.h>
+
+#ifdef __XENO__
+#include <native/cond.h>
+#else
+#include <boost/thread/condition_variable.hpp>
+#endif
+
+namespace rosrt
+{
+
+/**
+ * Wrapper for a "real-time" condition variable, implementation differs based on platform.
+ * Falls back to boost::condition_variable on generic platforms.
+ *
+ * Attempts to mimic the boost::condition_variable api, but this is not a complete implementation,
+ * it's only intended for internal rosrt use.
+ */
+class condition_variable: boost::noncopyable
+{
+private:
+
+#ifdef __XENO__
+  RT_COND cond_;
+#else
+  boost::condition_variable cond_;
+#endif
+
+public:
+  condition_variable()
+  {
+#ifdef __XENO__
+    int const res = rt_cond_create(&cond_, NULL);
+    if (res)
+    {
+      throw boost::thread_resource_error();
+    }
+#endif
+  }
+
+  ~condition_variable()
+  {
+#ifdef __XENO__
+    BOOST_VERIFY(!rt_cond_delete(&cond_));
+#endif
+  }
+
+  void wait(boost::unique_lock<mutex>& m)
+  {
+#ifdef __XENO__
+    rosrt::mutex::native_handle_type native_mutex = m.mutex()->native_handle();
+    int const res = rt_cond_wait(&cond_, native_mutex, TM_INFINITE);
+    BOOST_VERIFY(!res);
+#else
+    pthread_cond_t* native_cond = cond_.native_handle();
+    pthread_mutex_t* native_mutex = m.mutex()->native_handle();
+    BOOST_VERIFY(!pthread_cond_wait(native_cond, native_mutex));
+#endif
+  }
+
+  template<typename predicate_type>
+  void wait(boost::unique_lock<mutex>& m, predicate_type pred)
+  {
+    while (!pred())
+      wait(m);
+  }
+
+#ifdef __XENO__
+  typedef RT_COND* native_handle_type;
+#else
+  typedef boost::condition_variable::native_handle_type native_handle_type;
+#endif
+
+  native_handle_type native_handle()
+  {
+#ifdef __XENO__
+    return &cond_;
+#else
+    return cond_.native_handle();
+#endif
+  }
+
+  void notify_one()
+  {
+#ifdef __XENO__
+    int const res = rt_cond_signal(&cond_);
+    if (res)
+    {
+      ROS_ERROR("rt_cond_signal returned %d", res);
+    }
+    BOOST_VERIFY(!res);
+#else
+    cond_.notify_one();
+#endif
+  }
+
+  void notify_all()
+  {
+#ifdef __XENO__
+    BOOST_VERIFY(!rt_cond_broadcast(&cond_));
+#else
+    cond_.notify_all();
+#endif
+  }
+
+};
+
+}
+
+#endif /* ROSRT_CONDITION_VARIABLE_H_ */
diff --git a/ros_extras/ros_realtime/rosrt/include/rosrt/detail/mutex.h b/ros_extras/ros_realtime/rosrt/include/rosrt/detail/mutex.h
new file mode 100644
index 0000000..d68f87e
--- /dev/null
+++ b/ros_extras/ros_realtime/rosrt/include/rosrt/detail/mutex.h
@@ -0,0 +1,135 @@
+/*****************************************************************************
+ * Boost Software License - Version 1.0 - August 17th, 2003
+ *
+ * Permission is hereby granted, free of charge, to any person or organization
+ * obtaining a copy of the software and accompanying documentation covered by
+ * this license (the "Software") to use, reproduce, display, distribute,
+ * execute, and transmit the Software, and to prepare derivative works of the
+ * Software, and to permit third-parties to whom the Software is furnished to
+ * do so, all subject to the following:
+ *
+ * The copyright notices in the Software and this entire statement, including
+ * the above license grant, this restriction and the following disclaimer,
+ * must be included in all copies of the Software, in whole or in part, and
+ * all derivative works of the Software, unless such copies or derivative
+ * works are solely in the form of machine-executable object code generated by
+ * a source language processor.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+ * SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+ * FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * \author Mrinal Kalakrishnan <[email protected]>
+ ******************************************************************************/
+
+#ifndef ROSRT_MUTEX_H_
+#define ROSRT_MUTEX_H_
+
+#include <boost/utility.hpp>
+#include <boost/assert.hpp>
+#include <boost/thread/exceptions.hpp>
+#include <boost/thread/locks.hpp>
+
+#ifdef __XENO__
+#include <native/mutex.h>
+#else
+#include <boost/thread/mutex.hpp>
+#endif
+
+namespace rosrt
+{
+
+/**
+ * Wrapper for a "real-time" mutex, implementation differs based on platform.
+ * Falls back to boost::mutex on generic platforms.
+ *
+ * Attempts to mimic the boost::mutex api, but this is not a complete implementation,
+ * it's only intended for internal rosrt use.
+ */
+class mutex: boost::noncopyable
+{
+private:
+
+#ifdef __XENO__
+  RT_MUTEX mutex_;
+#else
+  boost::mutex mutex_;
+#endif
+
+public:
+
+  mutex()
+  {
+#ifdef __XENO__
+    int const res = rt_mutex_create(&mutex_, NULL);
+    if (res)
+    {
+      throw boost::thread_resource_error();
+    }
+#endif
+  }
+
+  ~mutex()
+  {
+#ifdef __XENO__
+    BOOST_VERIFY(!rt_mutex_delete(&mutex_));
+#endif
+  }
+
+  void lock()
+  {
+#ifdef __XENO__
+    int const res = rt_mutex_acquire(&mutex_, TM_INFINITE);
+    BOOST_VERIFY(!res);
+#else
+    mutex_.lock();
+#endif
+  }
+
+  void unlock()
+  {
+#ifdef __XENO__
+    BOOST_VERIFY(!rt_mutex_release(&mutex_));
+#else
+    mutex_.unlock();
+#endif
+  }
+
+  bool try_lock()
+  {
+#ifdef __XENO__
+    int const res = rt_mutex_acquire(&mutex_, TM_NONBLOCK);
+    BOOST_VERIFY(!res || res==EWOULDBLOCK);
+    return !res;
+#else
+    return mutex_.try_lock();
+#endif
+  }
+
+#ifdef __XENO__
+  typedef RT_MUTEX* native_handle_type;
+#else
+  typedef boost::mutex::native_handle_type native_handle_type;
+#endif
+
+  native_handle_type native_handle()
+  {
+#ifdef __XENO__
+    return &mutex_;
+#else
+    return mutex_.native_handle();
+#endif
+  }
+
+  typedef boost::unique_lock<mutex> scoped_lock;
+  typedef boost::detail::try_lock_wrapper<mutex> scoped_try_lock;
+
+};
+
+}
+
+#endif /* ROSRT_MUTEX_H_ */
diff --git a/ros_extras/ros_realtime/rosrt/include/rosrt/detail/publisher_manager.h b/ros_extras/ros_realtime/rosrt/include/rosrt/detail/publisher_manager.h
index 8c75518..b41e17b 100644
--- a/ros_extras/ros_realtime/rosrt/include/rosrt/detail/publisher_manager.h
+++ b/ros_extras/ros_realtime/rosrt/include/rosrt/detail/publisher_manager.h
@@ -41,7 +41,9 @@
 #include <ros/publisher.h>
 #include <rosrt/publisher.h>
 #include <lockfree/object_pool.h>
-#include <boost/thread.hpp>
+#include <rosrt/detail/thread.h>
+#include <rosrt/detail/mutex.h>
+#include <rosrt/detail/condition_variable.h>

 namespace rosrt
 {
@@ -81,11 +83,11 @@ private:
   void publishThread();

   PublishQueue queue_;
-  boost::condition_variable cond_;
-  boost::mutex cond_mutex_;
-  boost::thread pub_thread_;
+  rosrt::condition_variable cond_;
+  rosrt::mutex cond_mutex_;
   ros::atomic<uint32_t> pub_count_;
   volatile bool running_;
+  rosrt::thread pub_thread_;
 };

 } // namespace detail
diff --git a/ros_extras/ros_realtime/rosrt/include/rosrt/detail/thread.h b/ros_extras/ros_realtime/rosrt/include/rosrt/detail/thread.h
new file mode 100644
index 0000000..f192ef3
--- /dev/null
+++ b/ros_extras/ros_realtime/rosrt/include/rosrt/detail/thread.h
@@ -0,0 +1,104 @@
+/*********************************************************************
+* 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_THREAD_H_
+#define ROSRT_THREAD_H_
+
+#include <boost/utility.hpp>
+
+#ifdef __XENO__
+#include <native/task.h>
+#else
+#include <boost/thread.hpp>
+#endif
+
+extern "C"
+{
+static void thread_proxy(void* arg);
+
+}
+
+namespace rosrt
+{
+
+/**
+ * Thin wrapper for a "real-time" thread, implementation differs based on platform.
+ * Falls back to boost::thread on generic platforms.
+ *
+ */
+class thread: boost::noncopyable
+{
+private:
+#ifdef __XENO__
+  RT_TASK thread_;
+  boost::function<void ()> thread_fn_;
+#else
+  boost::thread thread_;
+#endif
+
+public:
+  explicit thread(boost::function<void ()> thread_fn)
+  {
+#ifdef __XENO__
+    thread_fn_ = thread_fn;
+    rt_task_spawn(&thread_, NULL, 0, 0, T_FPU | T_JOINABLE, thread_proxy, &thread_fn_);
+    //thread_proxy(&thread_fn_);
+#else
+    thread_ = boost::thread(thread_fn);
+#endif
+  }
+
+  ~thread()
+  {
+  }
+
+  void join()
+  {
+#ifdef __XENO__
+    rt_task_join(&thread_);
+#else
+    thread_.join();
+#endif
+  }
+};
+
+}
+
+static void thread_proxy(void* arg)
+{
+  boost::function<void ()>* fn = static_cast<boost::function<void ()>*>(arg);
+  (*fn)();
+}
+
+#endif /* ROSRT_THREAD_H_ */
diff --git a/ros_extras/ros_realtime/rosrt/src/publisher.cpp b/ros_extras/ros_realtime/rosrt/src/publisher.cpp
index b9357d6..8bd960e 100644
--- a/ros_extras/ros_realtime/rosrt/src/publisher.cpp
+++ b/ros_extras/ros_realtime/rosrt/src/publisher.cpp
@@ -95,8 +95,8 @@ PublisherManager::PublisherManager(const InitOptions& ops)
 : queue_(ops.pubmanager_queue_size)
 , pub_count_(0)
 , running_(true)
+, pub_thread_(boost::bind(&PublisherManager::publishThread, this))
 {
-  pub_thread_ = boost::thread(&PublisherManager::publishThread, this);
 }

 PublisherManager::~PublisherManager()
@@ -111,7 +111,7 @@ void PublisherManager::publishThread()
   while (running_)
   {
     {
-      boost::mutex::scoped_lock lock(cond_mutex_);
+      rosrt::mutex::scoped_lock lock(cond_mutex_);
       while (running_ && pub_count_.load() == 0)
       {
         cond_.wait(lock);
@@ -122,7 +122,6 @@ void PublisherManager::publishThread()
         return;
       }
     }
-
     uint32_t count = queue_.publishAll();
     pub_count_.fetch_sub(count);
   }
diff --git a/ros_extras/ros_realtime/rosrt/test/test_publisher.cpp b/ros_extras/ros_realtime/rosrt/test/test_publisher.cpp
index f632031..8fd2d90 100644
--- a/ros_extras/ros_realtime/rosrt/test/test_publisher.cpp
+++ b/ros_extras/ros_realtime/rosrt/test/test_publisher.cpp
@@ -41,6 +41,12 @@
 #include <std_msgs/UInt32.h>

 #include <boost/thread.hpp>
+#include <rosrt/detail/thread.h>
+
+#ifdef __XENO__
+#include <native/task.h>
+#include <sys/mman.h>
+#endif

 using namespace rosrt;

@@ -152,10 +158,18 @@ void publishThread(Publisher<std_msgs::UInt32>& pub, bool& done)
     if (msg)
     {
       pub.publish(msg);
+#ifdef __XENO__
+      rt_task_yield();
+#endif
     }
     else
     {
+#ifdef __XENO__
+      rt_task_yield();
+      rt_task_sleep(1000000);
+#else
       ros::WallDuration(0.0001).sleep();
+#endif
     }
   }
 }
@@ -170,7 +184,8 @@ TEST(Publisher, multipleThreads)
   Helper helpers[count];
   ros::Subscriber subs[count];

-  boost::thread_group tg;
+  boost::shared_ptr<rosrt::thread> threads[count];
+
   bool done = false;
   for (uint32_t i = 0; i < count; ++i)
   {
@@ -178,8 +193,7 @@ TEST(Publisher, multipleThreads)
     topic << "test" << i;
     pubs[i].initialize(nh.advertise<std_msgs::UInt32>(topic.str(), 0), 100, std_msgs::UInt32());
     subs[i] = nh.subscribe(topic.str(), 0, &Helper::cb, &helpers[i]);
-
-    tg.create_thread(boost::bind(publishThread, boost::ref(pubs[i]), boost::ref(done)));
+    threads[i].reset(new rosrt::thread(boost::bind(publishThread, boost::ref(pubs[i]), boost::ref(done))));
   }

   uint32_t recv_count = 0;
@@ -194,11 +208,18 @@ TEST(Publisher, multipleThreads)
       recv_count += helpers[i].count;
     }

+#ifdef __XENO__
+    rt_task_yield();
+    rt_task_sleep(1000000);
+#else
     ros::WallDuration(0.01).sleep();
+#endif
   }

   done = true;
-  tg.join_all();
+
+  for (uint32_t i=0; i<count; ++i)
+    threads[i]->join();

   ASSERT_GE(recv_count, count * 10000);

@@ -210,11 +231,17 @@ TEST(Publisher, multipleThreads)

 int main(int argc, char** argv)
 {
+#ifdef __XENO__
+  mlockall(MCL_CURRENT | MCL_FUTURE);
+  rt_task_shadow(NULL, "test_rt_publisher", 1, 0);
+#endif
+
   ros::init(argc, argv, "test_rt_publisher");
   testing::InitGoogleTest(&argc, argv);

   ros::NodeHandle nh;
   rosrt::init();

+
   return RUN_ALL_TESTS();
 }
diff --git a/ros_extras/ros_realtime/rosrt/test/test_subscriber.cpp b/ros_extras/ros_realtime/rosrt/test/test_subscriber.cpp
index fa1e320..58c6463 100644
--- a/ros_extras/ros_realtime/rosrt/test/test_subscriber.cpp
+++ b/ros_extras/ros_realtime/rosrt/test/test_subscriber.cpp
@@ -42,6 +42,11 @@

 #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)
@@ -78,7 +83,11 @@ TEST(Subscriber, singleSubscriber)
       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);
@@ -128,6 +137,9 @@ TEST(Subscriber, multipleSubscribersSameTopic)
         all_done = false;
       }
     }
+#ifdef  __XENO__
+    rt_task_sleep(1000000);
+#endif
   }

   ASSERT_EQ(getThreadAllocInfo().total_ops, 0UL);
@@ -182,6 +194,9 @@ TEST(Subscriber, multipleSubscribersMultipleTopics)
         all_done = false;
       }
     }
+#ifdef  __XENO__
+    rt_task_sleep(1000000);
+#endif
   }

   done = true;
@@ -260,6 +275,11 @@ TEST(Subscriber, multipleThreadsSameTopic)

 int main(int argc, char** argv)
 {
+#ifdef __XENO__
+  mlockall(MCL_CURRENT | MCL_FUTURE);
+  rt_task_shadow(NULL, "test_rt_subscriber", 0, 0);
+#endif
+
   ros::init(argc, argv, "test_rt_subscriber");
   testing::InitGoogleTest(&argc, argv);