diff --git a/include/robot_state_publisher/joint_state_listener.h b/include/robot_state_publisher/joint_state_listener.h index 863847c..e8e9341 100644 --- a/include/robot_state_publisher/joint_state_listener.h +++ b/include/robot_state_publisher/joint_state_listener.h @@ -62,6 +62,12 @@ class JointStateListener { /// Destructor ~JointStateListener(); + // Default value for number of attempts to read robot_description from parameter server + static const int default_description_read_repetitions_; + + // Default value for delay between the attempts to read robot_description + static const double default_description_read_delay_; + protected: virtual void callbackJointState(const JointStateConstPtr& state); virtual void callbackFixedJoint(const ros::TimerEvent& e); diff --git a/src/joint_state_listener.cpp b/src/joint_state_listener.cpp index 8dee23e..e081d44 100644 --- a/src/joint_state_listener.cpp +++ b/src/joint_state_listener.cpp @@ -46,6 +46,8 @@ using namespace std; using namespace ros; using namespace KDL; using namespace robot_state_publisher; +const int JointStateListener::default_description_read_repetitions_ = 0; +const double JointStateListener::default_description_read_delay_ = 0.5; JointStateListener::JointStateListener(const KDL::Tree& tree, const MimicMap& m, const urdf::Model& model) : state_publisher_(tree, model), mimic_(m) @@ -161,6 +163,36 @@ int main(int argc, char** argv) ROS_WARN("The 'state_publisher' executable is deprecated. Please use 'robot_state_publisher' instead"); ///////////////////////////////////////// end deprecation warning + // Checks if the robot_description is present on the parameter server + // If not, it will repeat the check for description_read_repetitions times with + // description_read_delay seconds delay between the checks (both are read from parameter server). + int description_read_repetitions; + node.param("description_read_repetitions", description_read_repetitions, + JointStateListener::default_description_read_repetitions_); + if (description_read_repetitions < 0) { + ROS_WARN_STREAM("Parameter description_read_repetitions is smaller than 0. Default value of " << + JointStateListener::default_description_read_repetitions_ << " will be used."); + description_read_repetitions = JointStateListener::default_description_read_repetitions_; + } + double description_read_delay; + node.param("description_read_delay", description_read_delay, JointStateListener::default_description_read_delay_); + if (description_read_delay <= 0) { + ROS_WARN_STREAM("Parameter description_read_delay is smaller than or equal 0. Default value of " << + JointStateListener::default_description_read_delay_ << " seconds will be used."); + description_read_delay = JointStateListener::default_description_read_delay_; + } + for (size_t i = 0; i < description_read_repetitions; ++i) { + if (!node.hasParam("robot_description")) { + ROS_WARN_STREAM("No robot_description parameter in namespace " << node.getNamespace() << + ". Will try " << description_read_repetitions - i << " more times with " << + description_read_delay << " second(s) delay between repetitions."); + ros::Duration(description_read_delay).sleep(); + } + else { + break; + } + } + // gets the location of the robot description on the parameter server urdf::Model model; if (!model.initParam("robot_description"))