Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 10:00:48 +02:00
Code Releases Activity

Implement activation modes of robot_enable service

Browse Source 
Adds parameter 'require_activation' to configure when the service should be called (Always, Never, OnStartup).
This commit is contained in:
Henning Kayser
2017-08-16 16:56:21 +02:00
parent b57e327a09
commit f70255926b
3 changed files with 42 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
src/ros/service_stopper.cpp
View File

@@ -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;
}
}