Permalink
Browse files

Imported rosrt changes from ARM - thread naming and mode switching for

xenomai
  • Loading branch information...
kalakris committed Dec 9, 2011
1 parent 13e396d commit 5d1b83bdcbf4e5b5779047831c481e9aed00be45
@@ -7,7 +7,7 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
+set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
@@ -68,11 +68,15 @@ class thread: boost::noncopyable
#endif
public:
- explicit thread(boost::function<void ()> thread_fn)
+ explicit thread(boost::function<void ()> thread_fn, const char* name="")
{
#ifdef __XENO__
thread_fn_ = thread_fn;
- rt_task_spawn(&thread_, NULL, 0, 0, T_FPU | T_JOINABLE, thread_proxy, &thread_fn_);
+ int error_code;
+ if (error_code = rt_task_spawn(&thread_, name, 0, 0, T_FPU | T_JOINABLE, 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);
@@ -42,6 +42,10 @@
#include <boost/thread.hpp>
+#ifdef __XENO__
+#include <native/task.h>
+#endif
+
namespace rosrt
{
namespace detail
@@ -95,14 +99,16 @@ 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::bind(&PublisherManager::publishThread, this), "rosrt_publisher")
{
}
PublisherManager::~PublisherManager()
{
+ cond_mutex_.lock();
running_ = false;
cond_.notify_one();
+ cond_mutex_.unlock();
pub_thread_.join();
}
@@ -122,6 +128,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);
}

0 comments on commit 5d1b83b

Please sign in to comment.