From 63e46639a254a1ad7362dd02517e0721f20ad68e Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Tue, 28 Feb 2012 01:38:33 +0100
Subject: [PATCH] Improve spinner management.
---
include/dynamic_graph_bridge/ros_init.hh | 6 +++-
src/interpreter.cpp | 3 +-
src/ros_export.cpp | 12 ++------
src/ros_export.hh | 3 +-
src/ros_import.cpp | 3 +-
src/ros_import.hh | 2 +-
src/ros_init.cpp | 36 ++++++++++++++++++++----
src/ros_joint_state.cpp | 14 ++-------
src/ros_joint_state.hh | 2 +-
9 files changed, 49 insertions(+), 32 deletions(-)
diff --git a/include/dynamic_graph_bridge/ros_init.hh b/include/dynamic_graph_bridge/ros_init.hh
index e7dea47..996d244 100644
--- a/include/dynamic_graph_bridge/ros_init.hh
+++ b/include/dynamic_graph_bridge/ros_init.hh
@@ -4,7 +4,11 @@
namespace dynamicgraph
{
- ros::NodeHandle& rosInit();
+ ros::NodeHandle& rosInit (bool createAsyncSpinner);
+
+ /// \brief Return spinner or throw an exception if spinner
+ /// creation has been disabled at startup.
+ ros::AsyncSpinner& spinner ();
} // end of namespace dynamicgraph.
diff --git a/src/interpreter.cpp b/src/interpreter.cpp
index a9b4e8a..db9efb6 100644
--- a/src/interpreter.cpp
+++ b/src/interpreter.cpp
@@ -3,7 +3,8 @@
int main ()
{
- ros::NodeHandle& nodeHandle = dynamicgraph::rosInit ();
+ // we spin explicitly so we do not need an async spinner here.
+ ros::NodeHandle& nodeHandle = dynamicgraph::rosInit (false);
dynamicgraph::Interpreter interpreter (nodeHandle);
ros::spin ();
}
diff --git a/src/ros_export.cpp b/src/ros_export.cpp
index ea74ff6..0336ff4 100644
--- a/src/ros_export.cpp
+++ b/src/ros_export.cpp
@@ -119,12 +119,9 @@ namespace dynamicgraph
RosExport::RosExport (const std::string& n)
: dynamicgraph::Entity(n),
- nh_ (rosInit ()),
- bindedSignal_ (),
- spinner_ (1)
+ nh_ (rosInit (true)),
+ bindedSignal_ ()
{
- spinner_.start ();
-
std::string docstring;
addCommand ("add",
new command::rosExport::Add
@@ -141,10 +138,7 @@ namespace dynamicgraph
}
RosExport::~RosExport ()
- {
- spinner_.stop ();
- nh_.shutdown ();
- }
+ {}
void RosExport::display (std::ostream& os) const
{
diff --git a/src/ros_export.hh b/src/ros_export.hh
index 800df9c..564ee64 100644
--- a/src/ros_export.hh
+++ b/src/ros_export.hh
@@ -99,9 +99,8 @@ namespace dynamicgraph
template <typename T>
friend class internal::Add;
private:
- ros::NodeHandle nh_;
+ ros::NodeHandle& nh_;
std::map<std::string, bindedSignal_t> bindedSignal_;
- ros::AsyncSpinner spinner_;
};
} // end of namespace dynamicgraph.
diff --git a/src/ros_import.cpp b/src/ros_import.cpp
index 1f007a4..6fdde8c 100644
--- a/src/ros_import.cpp
+++ b/src/ros_import.cpp
@@ -122,7 +122,8 @@ namespace dynamicgraph
RosImport::RosImport (const std::string& n)
: dynamicgraph::Entity(n),
- nh_ (rosInit ()),
+ // rosImport do not use callback so do not create a useless spinner.
+ nh_ (rosInit (false)),
bindedSignal_ (),
trigger_ (boost::bind (&RosImport::trigger, this, _1, _2),
sotNOSIGNAL,
diff --git a/src/ros_import.hh b/src/ros_import.hh
index aff0cd2..c42723a 100644
--- a/src/ros_import.hh
+++ b/src/ros_import.hh
@@ -87,7 +87,7 @@ namespace dynamicgraph
void add (const std::string& signal, const std::string& topic);
private:
- ros::NodeHandle nh_;
+ ros::NodeHandle& nh_;
std::map<std::string, bindedSignal_t> bindedSignal_;
dynamicgraph::SignalTimeDependent<int,int> trigger_;
ros::Duration rate_;
diff --git a/src/ros_init.cpp b/src/ros_init.cpp
index fc7c83f..8253d10 100644
--- a/src/ros_init.cpp
+++ b/src/ros_init.cpp
@@ -1,3 +1,4 @@
+#include <stdexcept>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
@@ -5,11 +6,24 @@
namespace dynamicgraph
{
- boost::shared_ptr<ros::NodeHandle> nodeHandle;
+ struct GlobalRos
+ {
+ ~GlobalRos ()
+ {
+ if (spinner)
+ spinner->stop ();
+ if (nodeHandle)
+ nodeHandle->shutdown ();
+ }
+
+ boost::shared_ptr<ros::NodeHandle> nodeHandle;
+ boost::shared_ptr<ros::AsyncSpinner> spinner;
+ };
+ GlobalRos ros;
- ros::NodeHandle& rosInit()
+ ros::NodeHandle& rosInit (bool createAsyncSpinner)
{
- if (!nodeHandle)
+ if (!ros.nodeHandle)
{
int argc = 1;
char* arg0 = strdup("dynamic_graph_bridge");
@@ -17,8 +31,20 @@ namespace dynamicgraph
ros::init(argc, argv, "dynamic_graph_bridge");
free (arg0);
- nodeHandle = boost::make_shared<ros::NodeHandle> ("dynamic_graph");
+ ros.nodeHandle = boost::make_shared<ros::NodeHandle> ("dynamic_graph");
+ }
+ if (!ros.spinner && createAsyncSpinner)
+ {
+ ros.spinner = boost::make_shared<ros::AsyncSpinner> (1);
+ ros.spinner->start ();
}
- return *nodeHandle;
+ return *ros.nodeHandle;
+ }
+
+ ros::AsyncSpinner& spinner ()
+ {
+ if (!ros.spinner)
+ throw std::runtime_error ("spinner has not been created");
+ return *ros.spinner;
}
} // end of namespace dynamicgraph.
diff --git a/src/ros_joint_state.cpp b/src/ros_joint_state.cpp
index eeddd68..b35fe82 100644
--- a/src/ros_joint_state.cpp
+++ b/src/ros_joint_state.cpp
@@ -3,6 +3,7 @@
#include <dynamic-graph/factory.h>
+#include "dynamic_graph_bridge/ros_init.hh"
#include "ros_joint_state.hh"
#include "sot_to_ros.hh"
@@ -40,19 +41,10 @@ namespace dynamicgraph
{
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosJointState, "RosJointState");
- static const char* rosInit()
- {
- int argc = 1;
- char* arg0 = strdup("ros_joint_state");
- char* argv[] = {arg0, 0};
- ros::init(argc, argv, "ros_joint_state");
- free (arg0);
- return "dynamic_graph";
- }
-
RosJointState::RosJointState (const std::string& n)
: Entity (n),
- nh_ (rosInit ()),
+ // do not use callbacks, so do not create a useless spinner
+ nh_ (rosInit (false)),
state_ (0, MAKE_SIGNAL_STRING(name, true, "Vector", "state")),
publisher_ (nh_, "joint_states", 5),
jointState_ (),
diff --git a/src/ros_joint_state.hh b/src/ros_joint_state.hh
index ea9a1a6..9d9ff2b 100644
--- a/src/ros_joint_state.hh
+++ b/src/ros_joint_state.hh
@@ -28,7 +28,7 @@ namespace dynamicgraph
int& trigger (int&, int);
private:
- ros::NodeHandle nh_;
+ ros::NodeHandle& nh_;
signalVectorIn_t state_;
realtime_tools::RealtimePublisher<sensor_msgs::JointState> publisher_;
sensor_msgs::JointState jointState_;
--
GitLab