From f70255926b327b238582c5589266345246aed38c Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 16 Aug 2017 16:56:21 +0200 Subject: [PATCH 1/2] Implement activation modes of robot_enable service Adds parameter 'require_activation' to configure when the service should be called (Always, Never, OnStartup). --- .../ur_modern_driver/ros/service_stopper.h | 10 ++++++- launch/ur_common.launch | 6 +++- src/ros/service_stopper.cpp | 30 +++++++++++++++++-- 3 files changed, 42 insertions(+), 4 deletions(-) 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 +} From 231840fabff8ed5671f0eaabf49623c4ea516206 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 17 Aug 2017 13:39:05 +0200 Subject: [PATCH 2/2] Change default activation mode to 'Never' Maintains default behavior of indigo that no controller activation is required. Enabling required activation can be done by passing 'require_activation' as Always/OnStartup to the ur_common.launch or by modifying corresponding launch files. --- .../ur_modern_driver/ros/service_stopper.h | 2 +- launch/ur_common.launch | 2 +- src/ros/service_stopper.cpp | 21 +++++++++++-------- 3 files changed, 14 insertions(+), 11 deletions(-) diff --git a/include/ur_modern_driver/ros/service_stopper.h b/include/ur_modern_driver/ros/service_stopper.h index f0b2c16..aec39a9 100644 --- a/include/ur_modern_driver/ros/service_stopper.h +++ b/include/ur_modern_driver/ros/service_stopper.h @@ -14,8 +14,8 @@ enum class RobotState enum class ActivationMode { - Always, Never, + Always, OnStartup }; diff --git a/launch/ur_common.launch b/launch/ur_common.launch index 25aa71f..95c3198 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -17,7 +17,7 @@ - + diff --git a/src/ros/service_stopper.cpp b/src/ros/service_stopper.cpp index 1d1a3e6..fcbf1a1 100644 --- a/src/ros/service_stopper.cpp +++ b/src/ros/service_stopper.cpp @@ -4,26 +4,29 @@ 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) + , activation_mode_(ActivationMode::Never) { std::string mode; - ros::param::param("~require_activation", mode, std::string("Always")); - if (mode == "Never") + ros::param::param("~require_activation", mode, std::string("Never")); + if (mode == "Always") { - activation_mode_ = ActivationMode::Never; - notify_all(RobotState::Running); + activation_mode_ = ActivationMode::Always; } else if (mode == "OnStartup") { activation_mode_ = ActivationMode::OnStartup; } - else if (mode != "Always") + else { - LOG_WARN("Found invalid value for param service_stopper_mode: '%s'\nShould be one of Always, Never, OnStartup", mode.c_str()); - mode = "Always"; + 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("ActivationMode mode: %s", mode.c_str()); + LOG_INFO("Service 'ur_driver/robot_enable' activation mode: %s", mode.c_str()); } bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp)