From 8f7dd59e5a5327917da9e0748d418b48a285c0be Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Thu, 10 Nov 2011 17:57:27 +0100
Subject: [PATCH] Switch to realtime publisher.
---
manifest.xml | 2 ++
src/ros_import.cpp | 2 +-
src/ros_import.hh | 13 +++++++----
src/ros_import.hxx | 28 +++++++++++++++-------
src/ros_joint_state.cpp | 52 +++++++++++++++++++++++------------------
src/ros_joint_state.hh | 3 ++-
6 files changed, 62 insertions(+), 38 deletions(-)
diff --git a/manifest.xml b/manifest.xml
index cacb368..e579f57 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -14,4 +14,6 @@
<depend package="geometry_msgs"/>
<depend package="sensor_msgs"/>
<depend package="bullet"/>
+
+ <depend package="realtime_tools"/>
</package>
diff --git a/src/ros_import.cpp b/src/ros_import.cpp
index e709bac..8600331 100644
--- a/src/ros_import.cpp
+++ b/src/ros_import.cpp
@@ -174,7 +174,7 @@ namespace dynamicgraph
for (iterator_t it = bindedSignal_.begin ();
it != bindedSignal_.end (); ++it)
{
- boost::get<2>(it->second) (t);
+ boost::get<1>(it->second) (t);
}
return dummy;
}
diff --git a/src/ros_import.hh b/src/ros_import.hh
index c7f5025..2041c14 100644
--- a/src/ros_import.hh
+++ b/src/ros_import.hh
@@ -12,6 +12,8 @@
# include <ros/ros.h>
+# include <realtime_tools/realtime_publisher.h>
+
# include "converter.hh"
# include "sot_to_ros.hh"
@@ -55,7 +57,6 @@ namespace dynamicgraph
typedef boost::tuple<
boost::shared_ptr<dynamicgraph::SignalBase<int> >,
- boost::shared_ptr<ros::Publisher>,
callback_t>
bindedSignal_t;
@@ -72,9 +73,13 @@ namespace dynamicgraph
int& trigger (int&, int);
template <typename T>
- void sendData (boost::shared_ptr<ros::Publisher> publisher,
- boost::shared_ptr<typename SotToRos<T>::signal_t> signal,
- int time);
+ void
+ sendData
+ (boost::shared_ptr
+ <realtime_tools::RealtimePublisher
+ <typename SotToRos<T>::ros_t> > publisher,
+ boost::shared_ptr<typename SotToRos<T>::signal_t> signal,
+ int time);
template <typename T>
void add (const std::string& signal, const std::string& topic);
diff --git a/src/ros_import.hxx b/src/ros_import.hxx
index 5b7b8cf..146714f 100644
--- a/src/ros_import.hxx
+++ b/src/ros_import.hxx
@@ -2,6 +2,7 @@
# define DYNAMIC_GRAPH_ROS_IMPORT_HXX
# include <vector>
# include <std_msgs/Float64.h>
+
# include "dynamic_graph_bridge/Matrix.h"
# include "dynamic_graph_bridge/Vector.h"
@@ -13,13 +14,19 @@ namespace dynamicgraph
{
template <typename T>
void
- RosImport::sendData (boost::shared_ptr<ros::Publisher> publisher,
- boost::shared_ptr<typename SotToRos<T>::signal_t> signal,
- int time)
+ RosImport::sendData
+ (boost::shared_ptr
+ <realtime_tools::RealtimePublisher
+ <typename SotToRos<T>::ros_t> > publisher,
+ boost::shared_ptr<typename SotToRos<T>::signal_t> signal,
+ int time)
{
typename SotToRos<T>::ros_t result;
- converter (result, signal->access (time));
- publisher->publish (result);
+ if (publisher->trylock ())
+ {
+ converter (publisher->msg_, signal->access (time));
+ publisher->unlockAndPublish ();
+ }
}
template <typename T>
@@ -33,8 +40,11 @@ namespace dynamicgraph
bindedSignal_t bindedSignal;
// Initialize the publisher.
- boost::get<1> (bindedSignal) =
- boost::make_shared<ros::Publisher> (nh_.advertise<ros_t>(topic, 1));
+ boost::shared_ptr
+ <realtime_tools::RealtimePublisher<ros_t> >
+ pubPtr =
+ boost::make_shared<realtime_tools::RealtimePublisher<ros_t> >
+ (nh_, topic, 1);
// Initialize the signal.
boost::shared_ptr<signal_t> signalPtr =
@@ -47,10 +57,10 @@ namespace dynamicgraph
callback_t callback = boost::bind
(&RosImport::sendData<T>,
this,
- boost::get<1> (bindedSignal),
+ pubPtr,
signalPtr,
_1);
- boost::get<2> (bindedSignal) = callback;
+ boost::get<1> (bindedSignal) = callback;
bindedSignal_[signal] = bindedSignal;
}
diff --git a/src/ros_joint_state.cpp b/src/ros_joint_state.cpp
index 5f2af2c..fd6fa8a 100644
--- a/src/ros_joint_state.cpp
+++ b/src/ros_joint_state.cpp
@@ -24,7 +24,7 @@ namespace dynamicgraph
: Entity (n),
nh_ (rosInit ()),
state_ (0, MAKE_SIGNAL_STRING(name, true, "Vector", "state")),
- publisher_ (nh_.advertise<sensor_msgs::JointState>("jointState", 5)),
+ publisher_ (nh_, "jointState", 5),
jointState_ (),
trigger_ (boost::bind (&RosJointState::trigger, this, _1, _2),
sotNOSIGNAL,
@@ -46,32 +46,38 @@ namespace dynamicgraph
int&
RosJointState::trigger (int& dummy, int t)
{
- std::size_t s = state_.access (t).size ();
+ if (publisher_.trylock ())
+ {
+ std::size_t s = state_.access (t).size ();
- // Update header.
- ++jointState_.header.seq;
- jointState_.header.stamp.sec = 0;
- jointState_.header.stamp.nsec = 0;
+ // Update header.
+ ++jointState_.header.seq;
- // Fill names if needed.
- if (jointState_.name.size () != s)
- {
- boost::format fmtFreeFlyer ("free-flyer-%s");
- boost::format fmtJoint ("joint-%s");
- jointState_.name.resize (s);
- for (std::size_t i = 0; i < s; ++i)
- if (i < 6)
- jointState_.name[i] = (fmtFreeFlyer % i).str ();
- else
- jointState_.name[i] = (fmtJoint % (i - 6)).str ();
- }
+ ros::Time now = ros::Time::now ();
+ jointState_.header.stamp.sec = now.sec;
+ jointState_.header.stamp.nsec = now.nsec;
+
+ // Fill names if needed.
+ if (jointState_.name.size () != s)
+ {
+ boost::format fmtFreeFlyer ("free-flyer-%s");
+ boost::format fmtJoint ("joint-%s");
+ jointState_.name.resize (s);
+ for (std::size_t i = 0; i < s; ++i)
+ if (i < 6)
+ jointState_.name[i] = (fmtFreeFlyer % i).str ();
+ else
+ jointState_.name[i] = (fmtJoint % (i - 6)).str ();
+ }
- // Fill position.
- jointState_.position.resize (s);
- for (std::size_t i = 0; i < s; ++i)
- jointState_.position[i] = state_.access (t) (i);
+ // Fill position.
+ jointState_.position.resize (s);
+ for (std::size_t i = 0; i < s; ++i)
+ jointState_.position[i] = state_.access (t) (i);
- publisher_.publish (jointState_);
+ publisher_.msg_ = jointState_;
+ publisher_.unlockAndPublish ();
+ }
return dummy;
}
} // end of namespace dynamicgraph.
diff --git a/src/ros_joint_state.hh b/src/ros_joint_state.hh
index 402081c..5da4632 100644
--- a/src/ros_joint_state.hh
+++ b/src/ros_joint_state.hh
@@ -5,6 +5,7 @@
# include <dynamic-graph/signal-time-dependent.h>
# include <ros/ros.h>
+# include <realtime_tools/realtime_publisher.h>
# include <sensor_msgs/JointState.h>
# include "converter.hh"
@@ -27,7 +28,7 @@ namespace dynamicgraph
private:
ros::NodeHandle nh_;
signalVectorIn_t state_;
- ros::Publisher publisher_;
+ realtime_tools::RealtimePublisher<sensor_msgs::JointState> publisher_;
sensor_msgs::JointState jointState_;
dynamicgraph::SignalTimeDependent<int,int> trigger_;
};
--
GitLab