Implement activation modes of robot_enable service

Adds parameter 'require_activation' to configure when the service should be called (Always, Never, OnStartup).

include/ur_modern_driver/ros/service_stopper.h

@@ -12,6 +12,13 @@ enum class RobotState
   ProtectiveStopped
 };
 
+enum class ActivationMode
+{
+  Always,
+  Never,
+  OnStartup
+};
+
 class Service
 {
 public:
@@ -25,6 +32,7 @@ private:
   ros::ServiceServer enable_service_;
   std::vector<Service*> services_;
   RobotState last_state_;
+  ActivationMode activation_mode_;
 
   void notify_all(RobotState state);
   bool handle(SharedRobotModeData& data, bool error);

launch/ur_common.launch

@@ -16,6 +16,9 @@
   <arg name="base_frame" default="$(arg prefix)base" />
   <arg name="tool_frame" default="$(arg prefix)tool0_controller" />
 
+  <!-- require_activation defines when the service /ur_driver/robot_enable needs to be called. -->
+  <arg name="require_activation" default="Always" /> <!-- Always, Never, OnStartup -->
+    
   <!-- The max_velocity parameter is only used for debugging in the ur_driver. It's not related to actual velocity limits -->
   <arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
 
@@ -32,5 +35,6 @@
     <param name="servoj_time" type="double" value="$(arg servoj_time)" />
     <param name="base_frame" type="str" value="$(arg base_frame)"/>
     <param name="tool_frame" type="str" value="$(arg tool_frame)"/>
+    <param name="require_activation" type="str" value="$(arg require_activation)" />
   </node>
 </launch>

src/ros/service_stopper.cpp

@@ -4,12 +4,33 @@ ServiceStopper::ServiceStopper(std::vector<Service*> services)
   : enable_service_(nh_.advertiseService("ur_driver/robot_enable", &ServiceStopper::enableCallback, this))
   , services_(services)
   , last_state_(RobotState::Error)
+  , activation_mode_(ActivationMode::Always)
 {
-  // enable_all();
+  std::string mode;
+  ros::param::param("~require_activation", mode, std::string("Always"));
+  if (mode == "Never")
+  {
+    activation_mode_ = ActivationMode::Never;
+    notify_all(RobotState::Running);
+  }
+  else if (mode == "OnStartup")
+  {
+    activation_mode_ = ActivationMode::OnStartup;
+  }
+  else if (mode != "Always")
+  {
+    LOG_WARN("Found invalid value for param service_stopper_mode: '%s'\nShould be one of Always, Never, OnStartup", mode.c_str());
+    mode = "Always";
+  }
+
+  LOG_INFO("ActivationMode mode: %s", mode.c_str());
 }
 
 bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp)
 {
+  // After the startup call OnStartup and Never behave the same
+  if (activation_mode_ == ActivationMode::OnStartup)
+    activation_mode_ = ActivationMode::Never;
   notify_all(RobotState::Running);
   return true;
 }
@@ -41,6 +62,11 @@ bool ServiceStopper::handle(SharedRobotModeData& data, bool error)
   {
     notify_all(RobotState::Error);
   }
+  else if (activation_mode_ == ActivationMode::Never)
+  {
+    // No error encountered, the user requested automatic reactivation
+    notify_all(RobotState::Running);
+  }
 
   return true;
 }