From 85212f2077040bdc7769ad80cc27b3e07a719f63 Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Thu, 10 Nov 2011 18:37:45 +0100
Subject: [PATCH] Make sure we do not publish faster than 100hz.
---
src/ros_import.cpp | 8 +++++++-
src/ros_import.hh | 4 ++++
src/ros_joint_state.cpp | 8 ++++++--
src/ros_joint_state.hh | 4 ++++
4 files changed, 21 insertions(+), 3 deletions(-)
diff --git a/src/ros_import.cpp b/src/ros_import.cpp
index 8600331..7998fc0 100644
--- a/src/ros_import.cpp
+++ b/src/ros_import.cpp
@@ -123,7 +123,9 @@ namespace dynamicgraph
bindedSignal_ (),
trigger_ (boost::bind (&RosImport::trigger, this, _1, _2),
sotNOSIGNAL,
- MAKE_SIGNAL_STRING(name, true, "int", "trigger"))
+ MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
+ rate_ (ROS_JOINT_STATE_PUBLISHER_RATE),
+ lastPublicated_ (ros::Time::now () - rate_ - rate_)
{
signalRegistration (trigger_);
trigger_.setNeedUpdateFromAllChildren (true);
@@ -171,6 +173,10 @@ namespace dynamicgraph
{
typedef std::map<std::string, bindedSignal_t>::iterator iterator_t;
+ ros::Duration dt = ros::Time::now () - lastPublicated_;
+ if (dt < rate_)
+ return dummy;
+
for (iterator_t it = bindedSignal_.begin ();
it != bindedSignal_.end (); ++it)
{
diff --git a/src/ros_import.hh b/src/ros_import.hh
index 2041c14..a7de9a5 100644
--- a/src/ros_import.hh
+++ b/src/ros_import.hh
@@ -60,6 +60,8 @@ namespace dynamicgraph
callback_t>
bindedSignal_t;
+ static const double ROS_JOINT_STATE_PUBLISHER_RATE = 1. / 100.;
+
RosImport (const std::string& n);
virtual ~RosImport ();
@@ -88,6 +90,8 @@ namespace dynamicgraph
ros::NodeHandle nh_;
std::map<std::string, bindedSignal_t> bindedSignal_;
dynamicgraph::SignalTimeDependent<int,int> trigger_;
+ ros::Duration rate_;
+ ros::Time lastPublicated_;
};
} // end of namespace dynamicgraph.
diff --git a/src/ros_joint_state.cpp b/src/ros_joint_state.cpp
index fd6fa8a..8a019d0 100644
--- a/src/ros_joint_state.cpp
+++ b/src/ros_joint_state.cpp
@@ -28,7 +28,9 @@ namespace dynamicgraph
jointState_ (),
trigger_ (boost::bind (&RosJointState::trigger, this, _1, _2),
sotNOSIGNAL,
- MAKE_SIGNAL_STRING(name, true, "int", "trigger"))
+ MAKE_SIGNAL_STRING(name, true, "int", "trigger")),
+ rate_ (ROS_JOINT_STATE_PUBLISHER_RATE),
+ lastPublicated_ (ros::Time::now () - rate_ - rate_)
{
signalRegistration (state_ << trigger_);
trigger_.setNeedUpdateFromAllChildren (true);
@@ -46,8 +48,10 @@ namespace dynamicgraph
int&
RosJointState::trigger (int& dummy, int t)
{
- if (publisher_.trylock ())
+ ros::Duration dt = ros::Time::now () - lastPublicated_;
+ if (dt > rate_ && publisher_.trylock ())
{
+ lastPublicated_ = ros::Time::now ();
std::size_t s = state_.access (t).size ();
// Update header.
diff --git a/src/ros_joint_state.hh b/src/ros_joint_state.hh
index 5da4632..ea9a1a6 100644
--- a/src/ros_joint_state.hh
+++ b/src/ros_joint_state.hh
@@ -21,6 +21,8 @@ namespace dynamicgraph
/// \brief Vector input signal.
typedef SignalPtr<ml::Vector, int> signalVectorIn_t;
+ static const double ROS_JOINT_STATE_PUBLISHER_RATE = 1. / 100.;
+
RosJointState (const std::string& n);
virtual ~RosJointState ();
@@ -31,6 +33,8 @@ namespace dynamicgraph
realtime_tools::RealtimePublisher<sensor_msgs::JointState> publisher_;
sensor_msgs::JointState jointState_;
dynamicgraph::SignalTimeDependent<int,int> trigger_;
+ ros::Duration rate_;
+ ros::Time lastPublicated_;
};
} // end of namespace dynamicgraph.
--
GitLab