Merge pull request #2 from TAMS-Group/fork_master

Activation modes for service /ur_driver/robot_enable

include/ur_modern_driver/ros/service_stopper.h

@@ -12,6 +12,13 @@ enum class RobotState
   ProtectiveStopped
 };
 
+enum class ActivationMode
+{
+  Never,
+  Always,
+  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="Never" /> <!-- Never, Always, 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,36 @@ 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::Never)
 {
-  // enable_all();
+  std::string mode;
+  ros::param::param("~require_activation", mode, std::string("Never"));
+  if (mode == "Always")
+  {
+    activation_mode_ = ActivationMode::Always;
+  }
+  else if (mode == "OnStartup")
+  {
+    activation_mode_ = ActivationMode::OnStartup;
+  }
+  else
+  {
+    if (mode != "Never")
+    {
+      LOG_WARN("Found invalid value for param require_activation: '%s'\nShould be one of Never, OnStartup, Always", mode.c_str());
+      mode = "Never";
+    }
+    notify_all(RobotState::Running);
+  }
+
+  LOG_INFO("Service 'ur_driver/robot_enable' activation 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 +65,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;
 }