Make sure we do not publish faster than 100hz.

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)
       {

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.
 

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.

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.