diff --git a/include/ur_modern_driver/ros/service_stopper.h b/include/ur_modern_driver/ros/service_stopper.h index ffbe106..aec39a9 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 +{ + Never, + Always, + 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..95c3198 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..fcbf1a1 100644 --- a/src/ros/service_stopper.cpp +++ b/src/ros/service_stopper.cpp @@ -4,12 +4,36 @@ 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::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; -} \ No newline at end of file +}