Merge pull request #177 from UniversalRobots/fix_robot_state_helper

Make robot_state helper wait for a first status from robot before advertising the set_mode action.

Author: Felix Exner

ur_robot_driver/include/ur_robot_driver/ros/robot_state_helper.h

@@ -59,6 +59,7 @@ class RobotStateHelper
   virtual ~RobotStateHelper() = default;
 
 private:
+  ros::NodeHandle nh_;
   void robotModeCallback(const ur_dashboard_msgs::RobotMode& msg);
   void safetyModeCallback(const ur_dashboard_msgs::SafetyMode& msg);
 
@@ -85,8 +86,9 @@ class RobotStateHelper
   // Action server functions
   void setModeGoalCallback();
   void setModePreemptCallback();
+  void startActionServer();
+  bool is_started_;
 
-  ros::NodeHandle nh_;
   RobotMode robot_mode_;
   SafetyMode safety_mode_;
 

ur_robot_driver/include/ur_robot_driver/ur/datatypes.h

@@ -33,6 +33,7 @@ namespace ur_driver
 {
 enum class RobotMode : int8_t
 {
+  UNKNOWN = -128,  // This is not defined by UR but only inside this driver
   NO_CONTROLLER = -1,
   DISCONNECTED = 0,
   CONFIRM_SAFETY = 1,

ur_robot_driver/src/ros/robot_state_helper.cpp

@@ -30,7 +30,12 @@
 #include <std_srvs/Trigger.h>
 namespace ur_driver
 {
-RobotStateHelper::RobotStateHelper(const ros::NodeHandle& nh) : nh_(nh), set_mode_as_(nh_, "set_mode", false)
+RobotStateHelper::RobotStateHelper(const ros::NodeHandle& nh)
+  : nh_(nh)
+  , is_started_(false)
+  , robot_mode_(RobotMode::UNKNOWN)
+  , safety_mode_(SafetyMode::UNDEFINED_SAFETY_MODE)
+  , set_mode_as_(nh_, "set_mode", false)
 {
   // Topic on which the robot_mode is published by the driver
   robot_mode_sub_ = nh_.subscribe("robot_mode", 1, &RobotStateHelper::robotModeCallback, this);
@@ -52,9 +57,9 @@ RobotStateHelper::RobotStateHelper(const ros::NodeHandle& nh) : nh_(nh), set_mod
   // Service to start UR program execution on the robot
   play_program_srv_ = nh_.serviceClient<std_srvs::Trigger>("dashboard/play");
 
+  play_program_srv_.waitForExistence();
   set_mode_as_.registerGoalCallback(std::bind(&RobotStateHelper::setModeGoalCallback, this));
   set_mode_as_.registerPreemptCallback(std::bind(&RobotStateHelper::setModePreemptCallback, this));
-  set_mode_as_.start();
 }
 
 void RobotStateHelper::robotModeCallback(const ur_dashboard_msgs::RobotMode& msg)
@@ -64,6 +69,10 @@ void RobotStateHelper::robotModeCallback(const ur_dashboard_msgs::RobotMode& msg
     robot_mode_ = RobotMode(msg.mode);
     ROS_INFO_STREAM("Robot mode is now " << robotModeString(robot_mode_));
     updateRobotState();
+    if (!is_started_)
+    {
+      startActionServer();
+    }
   }
 }
 
@@ -74,6 +83,10 @@ void RobotStateHelper::safetyModeCallback(const ur_dashboard_msgs::SafetyMode& m
     safety_mode_ = SafetyMode(msg.mode);
     ROS_INFO_STREAM("Robot's safety mode is now " << safetyModeString(safety_mode_));
     updateRobotState();
+    if (!is_started_)
+    {
+      startActionServer();
+    }
   }
 }
 
@@ -256,4 +269,13 @@ bool RobotStateHelper::safeDashboardTrigger(ros::ServiceClient* srv_client)
   }
   return srv.response.success;
 }
+
+void RobotStateHelper::startActionServer()
+{
+  if (robot_mode_ != RobotMode::UNKNOWN && safety_mode_ != SafetyMode::UNDEFINED_SAFETY_MODE)
+  {
+    set_mode_as_.start();
+    is_started_ = true;
+  }
+}
 }  // namespace ur_driver