1 |
|
|
#include "dynamic_graph_bridge/ros_init.hh" |
2 |
|
|
|
3 |
|
|
#include <boost/make_shared.hpp> |
4 |
|
|
#include <boost/shared_ptr.hpp> |
5 |
|
|
#include <stdexcept> |
6 |
|
|
|
7 |
|
|
namespace dynamicgraph { |
8 |
|
|
struct GlobalRos { |
9 |
|
|
~GlobalRos() { |
10 |
|
|
if (spinner) spinner->stop(); |
11 |
|
|
if (nodeHandle) nodeHandle->shutdown(); |
12 |
|
|
} |
13 |
|
|
|
14 |
|
|
boost::shared_ptr<ros::NodeHandle> nodeHandle; |
15 |
|
|
boost::shared_ptr<ros::AsyncSpinner> spinner; |
16 |
|
|
boost::shared_ptr<ros::MultiThreadedSpinner> mtSpinner; |
17 |
|
|
}; |
18 |
|
|
GlobalRos ros; |
19 |
|
|
|
20 |
|
|
ros::NodeHandle& rosInit(bool createAsyncSpinner, |
21 |
|
|
bool createMultiThreadedSpinner) { |
22 |
|
|
if (!ros.nodeHandle) { |
23 |
|
|
int argc = 1; |
24 |
|
|
char* arg0 = strdup("dynamic_graph_bridge"); |
25 |
|
|
char* argv[] = {arg0, 0}; |
26 |
|
|
ros::init(argc, argv, "dynamic_graph_bridge"); |
27 |
|
|
free(arg0); |
28 |
|
|
|
29 |
|
|
ros.nodeHandle = boost::make_shared<ros::NodeHandle>(""); |
30 |
|
|
} |
31 |
|
|
if (!ros.spinner && createAsyncSpinner) { |
32 |
|
|
ros.spinner = boost::make_shared<ros::AsyncSpinner>(4); |
33 |
|
|
|
34 |
|
|
// Change the thread's scheduler from real-time to normal and reduce its |
35 |
|
|
// priority |
36 |
|
|
int oldThreadPolicy, newThreadPolicy; |
37 |
|
|
struct sched_param oldThreadParam, newThreadParam; |
38 |
|
|
if (pthread_getschedparam(pthread_self(), &oldThreadPolicy, |
39 |
|
|
&oldThreadParam) == 0) { |
40 |
|
|
newThreadPolicy = SCHED_OTHER; |
41 |
|
|
newThreadParam = oldThreadParam; |
42 |
|
|
newThreadParam.sched_priority -= |
43 |
|
|
5; // Arbitrary number, TODO: choose via param/file? |
44 |
|
|
if (newThreadParam.sched_priority < |
45 |
|
|
sched_get_priority_min(newThreadPolicy)) |
46 |
|
|
newThreadParam.sched_priority = sched_get_priority_min(newThreadPolicy); |
47 |
|
|
|
48 |
|
|
pthread_setschedparam(pthread_self(), newThreadPolicy, &newThreadParam); |
49 |
|
|
} |
50 |
|
|
|
51 |
|
|
// AsyncSpinners are created with the reduced priority |
52 |
|
|
ros.spinner->start(); |
53 |
|
|
|
54 |
|
|
// Switch the priority of the parent thread (this thread) back to real-time. |
55 |
|
|
pthread_setschedparam(pthread_self(), oldThreadPolicy, &oldThreadParam); |
56 |
|
|
} else { |
57 |
|
|
if (!ros.mtSpinner && createMultiThreadedSpinner) { |
58 |
|
|
// Seems not to be used. |
59 |
|
|
// If we need to reduce its threads priority, it needs to be done before |
60 |
|
|
// calling the MultiThreadedSpinner::spin() method |
61 |
|
|
ros.mtSpinner = boost::make_shared<ros::MultiThreadedSpinner>(4); |
62 |
|
|
} |
63 |
|
|
} |
64 |
|
|
return *ros.nodeHandle; |
65 |
|
|
} |
66 |
|
|
|
67 |
|
|
ros::AsyncSpinner& spinner() { |
68 |
|
|
if (!ros.spinner) throw std::runtime_error("spinner has not been created"); |
69 |
|
|
return *ros.spinner; |
70 |
|
|
} |
71 |
|
|
|
72 |
|
|
ros::MultiThreadedSpinner& mtSpinner() { |
73 |
|
|
if (!ros.mtSpinner) throw std::runtime_error("spinner has not been created"); |
74 |
|
|
return *ros.mtSpinner; |
75 |
|
|
} |
76 |
|
|
|
77 |
|
|
} // end of namespace dynamicgraph. |