Skip to content

Commit

Permalink
controller_manager: Fix doSwitch execution point
Browse files Browse the repository at this point in the history 
The doSwitch method needs to be executed in the update() method,  that is, in
the real-time path, which is where controller switching actually takes place.

It was previously done in the switchController callback, which is non real-time.
In this method controller switching is scheduled, but not actually executed.

This changeset fixes a bug in which hardware interface  modes could switch
before controllers, leading to undefined behavior.
  • Loading branch information
@adolfo-rt
Adolfo Rodriguez Tsouroukdissian authored and adolfo-rt committed Nov 13, 2015
1 parent a5067ae commit 693f249
Show file tree
Hide file tree
Showing 3 changed files with 31 additions and 15 deletions.
1 change: 1 addition & 0 deletions controller_manager/include/controller_manager/controller_manager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -179,6 +179,7 @@ class ControllerManager{
/** \name Controller Switching
*\{*/
std::vector<controller_interface::ControllerBase*> start_request_, stop_request_;
std::list<hardware_interface::ControllerInfo> switch_start_list_, switch_stop_list_;

This comment has been minimized.

Sign in to view

Copy link
@po1

po1 Apr 30, 2016

adding members to a class generally breaks ABI. It just happened for me :/

This comment has been minimized.

Sign in to view

Copy link
@jacquelinekay

jacquelinekay Apr 30, 2016

ABI consistency between patch releases is actually not in any REP or official policy we have, but we do encourage maintainers to be aware of ABI breakages and warn users when it's about to happen.

Unfortunately the rules for ABI consistency are extensive, with a lot of exceptions, and are generally hard to remember unless you are a C++ compiler writer... therefore we usually don't catch an ABI break until it happens... Additionally, "fixing" the ABI break usually means breaking ABI again, so there isn't much we can do.

This is a fairly recent problem since the last Indigo sync was April 22nd. Perhaps you could email the robot control SIG mailing list to spread awareness of the ABI breakage? It's not a very active list but it will spread awareness to people who frequently use the package.

https://groups.google.com/forum/#!forum/ros-sig-robot-control

This comment has been minimized.

Sign in to view

Copy link
@po1

po1 May 2, 2016

Sure, I can email the control SIG mailng list (to which I should probably subscribe...)
The warning process can be automated, as ABI compliance checking tools exist: https://github.com/lvc/abi-compliance-checker
I feel like this could be integrated into the build farm at reasonable cost.

This comment has been minimized.

Sign in to view

Copy link
@bmagyar

bmagyar May 2, 2016 •
edited
Loading

Member

Maybe we could add it to the Travis config: ros-controls/realtime_tools#8
bool please_switch_;
int switch_strictness_;
/*\}*/
Expand Down
43 changes: 29 additions & 14 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,9 @@ void ControllerManager::update(const ros::Time& time, const ros::Duration& perio
// there are controllers to start/stop
if (please_switch_)
{
// switch hardware interfaces (if any)
robot_hw_->doSwitch(switch_start_list_, switch_stop_list_);

// stop controllers
for (unsigned int i=0; i<stop_request_.size(); i++)
if (!stop_request_[i]->stopRequest(time))
Expand Down Expand Up @@ -408,24 +411,37 @@ bool ControllerManager::switchController(const std::vector<std::string>& start_c
ROS_DEBUG("Start request vector has size %i", (int)start_request_.size());

// Do the resource management checking
std::list<hardware_interface::ControllerInfo> info_list, start_list, stop_list;
std::list<hardware_interface::ControllerInfo> info_list;
switch_start_list_.clear();
switch_stop_list_.clear();

std::vector<ControllerSpec> &controllers = controllers_lists_[current_controllers_list_];
for (size_t i = 0; i < controllers.size(); ++i)
{
bool in_stop_list = false;

for(size_t j = 0; j < stop_request_.size(); j++)
in_stop_list = in_stop_list || (stop_request_[j] == controllers[i].c.get());
{
if (stop_request_[j] == controllers[i].c.get())
{
in_stop_list = true;
break;
}
}

bool in_start_list = false;
for(size_t j = 0; j < start_request_.size(); j++)
in_start_list = in_start_list || (start_request_[j] == controllers[i].c.get());
{
if (start_request_[j] == controllers[i].c.get())
{
in_start_list = true;
break;
}
}

bool add_to_list = controllers[i].c->isRunning();
const bool is_running = controllers[i].c->isRunning();
hardware_interface::ControllerInfo &info = controllers[i].info;

if(!add_to_list && in_stop_list){ // check for double stop
if(!is_running && in_stop_list){ // check for double stop
if(strictness == controller_manager_msgs::SwitchController::Request::STRICT){
ROS_ERROR_STREAM("Could not stop controller '" << info.name << "' since it is not running");
stop_request_.clear();
Expand All @@ -436,7 +452,7 @@ bool ControllerManager::switchController(const std::vector<std::string>& start_c
}
}

if(add_to_list && !in_stop_list && in_start_list){ // check for doubled start
if(is_running && !in_stop_list && in_start_list){ // check for doubled start
if(strictness == controller_manager_msgs::SwitchController::Request::STRICT){
ROS_ERROR_STREAM("Controller '" << info.name << "' is already running");
stop_request_.clear();
Expand All @@ -447,13 +463,14 @@ bool ControllerManager::switchController(const std::vector<std::string>& start_c
}
}

if(add_to_list && in_stop_list && !in_start_list){ // running and real stop
stop_list.push_back(info);
if(is_running && in_stop_list && !in_start_list){ // running and real stop
switch_stop_list_.push_back(info);
}
else if(!in_stop_list && in_start_list){ // start, but no restart
start_list.push_back(info);
else if(!is_running && !in_stop_list && in_start_list){ // start, but no restart
switch_start_list_.push_back(info);
}

bool add_to_list = is_running;
if (in_stop_list)
add_to_list = false;
if (in_start_list)
Expand All @@ -472,16 +489,14 @@ bool ControllerManager::switchController(const std::vector<std::string>& start_c
return false;
}

if (!robot_hw_->prepareSwitch(start_list, stop_list))
if (!robot_hw_->prepareSwitch(switch_start_list_, switch_stop_list_))

This comment has been minimized.

Sign in to view

Copy link
@adolfo-rt

adolfo-rt Nov 13, 2015

Member

Notice use of prepareSwitch
{
ROS_ERROR("Could not switch controllers. The hardware interface combination for the requested controllers is unfeasible.");
stop_request_.clear();
start_request_.clear();
return false;
}

robot_hw_->doSwitch(start_list, stop_list);

// start the atomic controller switching
switch_strictness_ = strictness;
please_switch_ = true;
Expand Down
2 changes: 1 addition & 1 deletion hardware_interface/include/hardware_interface/robot_hw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,7 @@ class RobotHW : public InterfaceManager
const std::list<ControllerInfo>& stop_list) { return canSwitch(start_list, stop_list); }

/**
* Perform (in non-realtime) all necessary hardware interface switches in order to start and stop the given controllers.
* Perform (in realtime) all necessary hardware interface switches in order to start and stop the given controllers.
* Start and stop list are disjoint. The feasability was checked in canSwitch() beforehand.
*/
virtual void doSwitch(const std::list<ControllerInfo>& /*start_list*/,
Expand Down

0 comments on commit 693f249

Please sign in to comment.