From 2b5ef9be6a8d8d216e02f621677384e900170448 Mon Sep 17 00:00:00 2001 From: Mrinal Kalakrishnan Date: Mon, 18 Feb 2013 17:26:07 -0800 Subject: [PATCH 1/3] rosrt: added Xenomai support (fixes #6) Xenomai support added using the native Xenomai API. Added 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 updated to work on Xenomai, because a loop without sleeps in a Xenomai real-time thread can starve the system. --- rosrt/CMakeLists.txt | 19 +++ .../include/rosrt/detail/condition_variable.h | 145 ++++++++++++++++++ rosrt/include/rosrt/detail/mutex.h | 135 ++++++++++++++++ .../include/rosrt/detail/publisher_manager.h | 10 +- rosrt/include/rosrt/detail/thread.h | 108 +++++++++++++ rosrt/include/rosrt/init.h | 4 + rosrt/src/publisher.cpp | 18 ++- rosrt/test/test_publisher.cpp | 34 +++- rosrt/test/test_publisher_no_initialize.cpp | 9 ++ rosrt/test/test_subscriber.cpp | 19 +++ 10 files changed, 490 insertions(+), 11 deletions(-) create mode 100644 rosrt/include/rosrt/detail/condition_variable.h create mode 100644 rosrt/include/rosrt/detail/mutex.h create mode 100644 rosrt/include/rosrt/detail/thread.h diff --git a/rosrt/CMakeLists.txt b/rosrt/CMakeLists.txt index e09b4ca..754cc0a 100644 --- a/rosrt/CMakeLists.txt +++ b/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) +set(PROJECT_NAME ${PROJECT_NAME}) +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 +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/rosrt/include/rosrt/detail/condition_variable.h b/rosrt/include/rosrt/detail/condition_variable.h new file mode 100644 index 0000000..0c7b4b6 --- /dev/null +++ b/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 + ******************************************************************************/ + +#ifndef ROSRT_CONDITION_VARIABLE_H_ +#define ROSRT_CONDITION_VARIABLE_H_ + +#include +#include +#include +#include +#include + +#ifdef __XENO__ +#include +#else +#include +#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& 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 + void wait(boost::unique_lock& 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/rosrt/include/rosrt/detail/mutex.h b/rosrt/include/rosrt/detail/mutex.h new file mode 100644 index 0000000..d68f87e --- /dev/null +++ b/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 + ******************************************************************************/ + +#ifndef ROSRT_MUTEX_H_ +#define ROSRT_MUTEX_H_ + +#include +#include +#include +#include + +#ifdef __XENO__ +#include +#else +#include +#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 scoped_lock; + typedef boost::detail::try_lock_wrapper scoped_try_lock; + +}; + +} + +#endif /* ROSRT_MUTEX_H_ */ diff --git a/rosrt/include/rosrt/detail/publisher_manager.h b/rosrt/include/rosrt/detail/publisher_manager.h index 8c75518..5e81022 100644 --- a/rosrt/include/rosrt/detail/publisher_manager.h +++ b/rosrt/include/rosrt/detail/publisher_manager.h @@ -41,7 +41,9 @@ #include #include #include -#include +#include +#include +#include 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 pub_count_; volatile bool running_; }; diff --git a/rosrt/include/rosrt/detail/thread.h b/rosrt/include/rosrt/detail/thread.h new file mode 100644 index 0000000..99b8fb9 --- /dev/null +++ b/rosrt/include/rosrt/detail/thread.h @@ -0,0 +1,108 @@ +/********************************************************************* +* 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 + +#ifdef __XENO__ +#include +#else +#include +#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 thread_fn_; +#else + boost::thread thread_; +#endif + +public: + explicit thread(boost::function thread_fn, const char* name="", const int cpu_id=0) + { +#ifdef __XENO__ + thread_fn_ = thread_fn; + int error_code; + if (error_code = rt_task_spawn(&thread_, name, 0, 0, T_FPU | T_JOINABLE | T_CPU(cpu_id), thread_proxy, &thread_fn_)) + { + ROS_ERROR("rosrt::thread - Couldn't spawn xenomai thread %s, error code = %d", name, error_code); + } + //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* fn = static_cast*>(arg); + (*fn)(); +} + +#endif /* ROSRT_THREAD_H_ */ diff --git a/rosrt/include/rosrt/init.h b/rosrt/include/rosrt/init.h index 411819d..e3ccef0 100644 --- a/rosrt/include/rosrt/init.h +++ b/rosrt/include/rosrt/init.h @@ -46,11 +46,15 @@ struct InitOptions { InitOptions() : pubmanager_queue_size(10000) + , pubmanager_thread_name("rosrt_pubmanager") + , pubmanager_thread_cpu(0) , gc_queue_size(1000) , gc_period(0.1) {} uint32_t pubmanager_queue_size; + std::string pubmanager_thread_name; + uint32_t pubmanager_thread_cpu; uint32_t gc_queue_size; ros::WallDuration gc_period; }; diff --git a/rosrt/src/publisher.cpp b/rosrt/src/publisher.cpp index b9357d6..ed382df 100644 --- a/rosrt/src/publisher.cpp +++ b/rosrt/src/publisher.cpp @@ -42,6 +42,10 @@ #include +#ifdef __XENO__ +#include +#endif + namespace rosrt { namespace detail @@ -95,14 +99,18 @@ PublisherManager::PublisherManager(const InitOptions& ops) : queue_(ops.pubmanager_queue_size) , pub_count_(0) , running_(true) +, pub_thread_(boost::bind(&PublisherManager::publishThread, this), + ops.pubmanager_thread_name.c_str(), + ops.pubmanager_thread_cpu) { - pub_thread_ = boost::thread(&PublisherManager::publishThread, this); } PublisherManager::~PublisherManager() { + cond_mutex_.lock(); running_ = false; cond_.notify_one(); + cond_mutex_.unlock(); pub_thread_.join(); } @@ -111,7 +119,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 +130,11 @@ void PublisherManager::publishThread() return; } } - +#ifdef __XENO__ + // in Xenomai, force a switch to secondary mode here so that + // publishing doesn't interfere with real-time tasks + rt_task_set_mode(T_PRIMARY, 0, NULL); +#endif uint32_t count = queue_.publishAll(); pub_count_.fetch_sub(count); } diff --git a/rosrt/test/test_publisher.cpp b/rosrt/test/test_publisher.cpp index f632031..e6a29eb 100644 --- a/rosrt/test/test_publisher.cpp +++ b/rosrt/test/test_publisher.cpp @@ -41,6 +41,12 @@ #include #include +#include + +#ifdef __XENO__ +#include +#include +#endif using namespace rosrt; @@ -152,10 +158,18 @@ void publishThread(Publisher& 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 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(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; ijoin(); ASSERT_GE(recv_count, count * 10000); @@ -210,6 +231,11 @@ 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); diff --git a/rosrt/test/test_publisher_no_initialize.cpp b/rosrt/test/test_publisher_no_initialize.cpp index 30ef1d7..f1fbbf1 100644 --- a/rosrt/test/test_publisher_no_initialize.cpp +++ b/rosrt/test/test_publisher_no_initialize.cpp @@ -36,6 +36,10 @@ #include #include +#ifdef __XENO__ +#include +#include +#endif /* * This little program will segfault if uninitialized pool resources @@ -43,6 +47,11 @@ */ int main(int argc, char** argv) { +#ifdef __XENO__ + mlockall(MCL_CURRENT | MCL_FUTURE); + rt_task_shadow(NULL, "test_publisher_no_initialize", 0, 0); +#endif + ros::init(argc, argv, "test_rt_publisher"); ros::NodeHandle nh; rosrt::init(); diff --git a/rosrt/test/test_subscriber.cpp b/rosrt/test/test_subscriber.cpp index fa1e320..28e1c2b 100644 --- a/rosrt/test/test_subscriber.cpp +++ b/rosrt/test/test_subscriber.cpp @@ -42,6 +42,11 @@ #include +#ifdef __XENO__ +#include +#include +#endif + using namespace rosrt; void publishThread(ros::Publisher& pub, bool& done) @@ -79,6 +84,9 @@ TEST(Subscriber, singleSubscriber) last = msg->data; ++count; } +#ifdef __XENO__ + rt_task_sleep(1000000); +#endif } ASSERT_EQ(getThreadAllocInfo().total_ops, 0UL); @@ -128,6 +136,9 @@ TEST(Subscriber, multipleSubscribersSameTopic) all_done = false; } } +#ifdef __XENO__ + rt_task_sleep(1000000); +#endif } ASSERT_EQ(getThreadAllocInfo().total_ops, 0UL); @@ -182,6 +193,9 @@ TEST(Subscriber, multipleSubscribersMultipleTopics) all_done = false; } } +#ifdef __XENO__ + rt_task_sleep(1000000); +#endif } done = true; @@ -260,6 +274,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); From 55ea456787d6cc481c8f4827c4b982ddfc253216 Mon Sep 17 00:00:00 2001 From: Mrinal Kalakrishnan Date: Mon, 18 Feb 2013 21:10:48 -0800 Subject: [PATCH 2/3] rosrt: get rid of some compiler warnings --- rosrt/include/rosrt/detail/thread.h | 10 +++++----- rosrt/src/publisher.cpp | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/rosrt/include/rosrt/detail/thread.h b/rosrt/include/rosrt/detail/thread.h index 99b8fb9..ffef438 100644 --- a/rosrt/include/rosrt/detail/thread.h +++ b/rosrt/include/rosrt/detail/thread.h @@ -62,24 +62,24 @@ class thread: boost::noncopyable private: #ifdef __XENO__ RT_TASK thread_; - boost::function thread_fn_; #else boost::thread thread_; #endif + boost::function thread_fn_; public: explicit thread(boost::function thread_fn, const char* name="", const int cpu_id=0) { -#ifdef __XENO__ thread_fn_ = thread_fn; +#ifdef __XENO__ int error_code; - if (error_code = rt_task_spawn(&thread_, name, 0, 0, T_FPU | T_JOINABLE | T_CPU(cpu_id), thread_proxy, &thread_fn_)) + error_code = rt_task_spawn(&thread_, name, 0, 0, T_FPU | T_JOINABLE | T_CPU(cpu_id), thread_proxy, &thread_fn_); + if (error_code) { ROS_ERROR("rosrt::thread - Couldn't spawn xenomai thread %s, error code = %d", name, error_code); } - //thread_proxy(&thread_fn_); #else - thread_ = boost::thread(thread_fn); + thread_ = boost::thread(boost::bind(&thread_proxy, &thread_fn_)); #endif } diff --git a/rosrt/src/publisher.cpp b/rosrt/src/publisher.cpp index ed382df..bb751e4 100644 --- a/rosrt/src/publisher.cpp +++ b/rosrt/src/publisher.cpp @@ -97,11 +97,11 @@ uint32_t PublishQueue::publishAll() PublisherManager::PublisherManager(const InitOptions& ops) : queue_(ops.pubmanager_queue_size) -, pub_count_(0) -, running_(true) , pub_thread_(boost::bind(&PublisherManager::publishThread, this), ops.pubmanager_thread_name.c_str(), ops.pubmanager_thread_cpu) +, pub_count_(0) +, running_(true) { } From caea5b5f344805e362af5a3de4334a5d9f1e2e84 Mon Sep 17 00:00:00 2001 From: Mrinal Kalakrishnan Date: Mon, 18 Feb 2013 21:19:52 -0800 Subject: [PATCH 3/3] rosrt: removed redundant line in CMakeLists.txt --- rosrt/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/rosrt/CMakeLists.txt b/rosrt/CMakeLists.txt index 754cc0a..7b7e1d0 100644 --- a/rosrt/CMakeLists.txt +++ b/rosrt/CMakeLists.txt @@ -13,7 +13,6 @@ rosbuild_init() # Check for Xenomai: set(XENOMAI_SEARCH_PATH /usr/local/xenomai /usr/xenomai /usr) -set(PROJECT_NAME ${PROJECT_NAME}) find_path(XENOMAI_DIR bin/xeno-config ${XENOMAI_SEARCH_PATH}) if (XENOMAI_DIR) set (XENOMAI_CONFIG ${XENOMAI_DIR}/bin/xeno-config)