diff --git a/include/ur_modern_driver/ros/service_stopper.h b/include/ur_modern_driver/ros/service_stopper.h index ffbe106..f0b2c16 100644 --- a/include/ur_modern_driver/ros/service_stopper.h +++ b/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 services_; RobotState last_state_; + ActivationMode activation_mode_; void notify_all(RobotState state); bool handle(SharedRobotModeData& data, bool error); @@ -59,4 +67,4 @@ public: { return true; } -}; \ No newline at end of file +}; diff --git a/launch/ur_common.launch b/launch/ur_common.launch index 7650156..25aa71f 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -15,6 +15,9 @@ + + + @@ -30,7 +33,8 @@ - + + diff --git a/src/ros/service_stopper.cpp b/src/ros/service_stopper.cpp index 2f46fd5..1d1a3e6 100644 --- a/src/ros/service_stopper.cpp +++ b/src/ros/service_stopper.cpp @@ -4,12 +4,33 @@ ServiceStopper::ServiceStopper(std::vector 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; -} \ No newline at end of file +}