Add retries to read robot_description. Number of retries and delay be…

…tween them are controlled by parameters. It defaults to the current behaviour.

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

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