Added waitForServer to the joint trajectory controller action client

pal-robotics · Apr 20, 2021 · 24bff74 · 24bff74
1 parent d8190a0
commit 24bff74
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/play_motion/src/move_joint_group.cpp b/play_motion/src/move_joint_group.cpp
@@ -50,7 +50,13 @@ namespace play_motion
       controller_name_(controller_name),
       joint_names_(joint_names),
       client_(controller_name_ + "/follow_joint_trajectory", false)
-  { }
+  { 
+    //Wait for server
+    if(!client_.waitForServer(ros::Duration(0.5)))
+    {
+      ROS_ERROR_STREAM("Error connecting to the client : " << controller_name_ << "/follow_joint_trajectory !");
+    }
+  }
 
   void MoveJointGroup::alCallback()
   {