Add Xenomai support (#6)

rosrt/CMakeLists.txt

@@ -11,6 +11,18 @@ 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})
+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 +35,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})

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_ */

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_ */

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,9 +83,9 @@ class PublisherManager
   void publishThread();
 
   PublishQueue queue_;
-  boost::condition_variable cond_;
-  boost::mutex cond_mutex_;
-  boost::thread pub_thread_;
+  rosrt::condition_variable cond_;
+  rosrt::mutex cond_mutex_;
+  rosrt::thread pub_thread_;
   ros::atomic<uint32_t> pub_count_;
   volatile bool running_;
 };