diff --git a/src/ros_import.cpp b/src/ros_import.cpp index 8600331ab13426e7b79ec0ab555daebe20dba129..7998fc0726b8b731668fb599320c33f7b1ba0202 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 2041c1464b1feafa128b5be160036b67b51847c8..a7de9a5c175ea1afa0d688dcaa35aa829d135536 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 fd6fa8a4a590c0019d0e1c8214ca64aed5ac5b29..8a019d0af75cf3ae2d024efaa519dae6acfb6dc4 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 5da46323f561e7a200ddec000229036387ba981c..ea9a1a6747a1a1e01da001c0d533d315c9c3fca9 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.