Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Controller Manager should not crash when trying to start finalized or unconfigured controller (backport #461) #524

Merged
merged 1 commit into from Sep 7, 2021
Merged

Controller Manager should not crash when trying to start finalized or unconfigured controller (backport #461) #524

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
33 changes: 27 additions & 6 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
Expand Up @@ -29,6 +29,11 @@ namespace controller_manager
static constexpr const char * kControllerInterfaceName = "controller_interface";
static constexpr const char * kControllerInterface = "controller_interface::ControllerInterface";

inline bool is_controller_inactive(const controller_interface::ControllerInterface & controller)
{
return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
}

inline bool is_controller_running(controller_interface::ControllerInterface & controller)
{
return controller.get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
Expand Down Expand Up @@ -381,7 +386,7 @@ controller_interface::return_type ControllerManager::switch_controller(
action.c_str(), controller.c_str());
return controller_interface::return_type::ERROR;
}
RCLCPP_DEBUG(
RCLCPP_WARN(
get_logger(),
"Could not '%s' controller with name '%s' because no controller with this name exists",
action.c_str(), controller.c_str());
Expand Down Expand Up @@ -451,6 +456,7 @@ controller_interface::return_type ControllerManager::switch_controller(
bool in_start_list = start_list_it != start_request_.end();

const bool is_running = is_controller_running(*controller.c);
const bool is_inactive = is_controller_inactive(*controller.c);

auto handle_conflict = [&](const std::string & msg) {
if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT)
Expand All @@ -462,13 +468,13 @@ controller_interface::return_type ControllerManager::switch_controller(
start_command_interface_request_.clear();
return controller_interface::return_type::ERROR;
}
RCLCPP_DEBUG(
get_logger(), "Could not stop controller '%s' since it is not running",
controller.info.name.c_str());
RCLCPP_WARN(get_logger(), "%s", msg.c_str());
return controller_interface::return_type::OK;
};

// check for double stop
if (!is_running && in_stop_list)
{ // check for double stop
{
auto ret = handle_conflict(
"Could not stop controller '" + controller.info.name + "' since it is not running");
if (ret != controller_interface::return_type::OK)
Expand All @@ -479,8 +485,9 @@ controller_interface::return_type ControllerManager::switch_controller(
stop_request_.erase(stop_list_it);
}

// check for doubled start
if (is_running && !in_stop_list && in_start_list)
{ // check for doubled start
{
auto ret = handle_conflict(
"Could not start controller '" + controller.info.name + "' since it is already running");
if (ret != controller_interface::return_type::OK)
Expand All @@ -491,6 +498,20 @@ controller_interface::return_type ControllerManager::switch_controller(
start_request_.erase(start_list_it);
}

// check for illegal start of an unconfigured/finalized controller
if (!is_inactive && !in_stop_list && in_start_list)
{
auto ret = handle_conflict(
"Could not start controller '" + controller.info.name +
"' since it is not in inactive state");
if (ret != controller_interface::return_type::OK)
{
return ret;
}
in_start_list = false;
start_request_.erase(start_list_it);
}

if (in_start_list)
{
list_interfaces(controller, start_command_interface_request_);
Expand Down
65 changes: 57 additions & 8 deletions controller_manager/test/test_load_controller.cpp
View file
Open in desktop
Expand Up @@ -198,6 +198,35 @@ TEST_F(TestLoadController, can_stop_active_controller)
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_current_state().id());
}

TEST_F(TestLoadController, can_not_start_finalized_controller)
{
// load and start controller with name1
auto controller_if = cm_->load_controller(controller_name1, TEST_CONTROLLER_CLASS_NAME);
ASSERT_NE(controller_if, nullptr);

ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id());

// Shutdown controller on purpose for testing
ASSERT_EQ(controller_if->shutdown().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);

// Start controller
strvec start_controllers = {controller_name1};
strvec stop_controllers = {};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));

ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());

// Can not configure unconfigured controller
EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::ERROR);
ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, controller_if->get_state().id());
}

TEST_F(TestLoadController, inactive_controller_cannot_be_cleaned_up)
{
auto controller_if = cm_->load_controller(controller_name1, TEST_CONTROLLER_CLASS_NAME);
Expand Down Expand Up @@ -439,8 +468,12 @@ TEST_F(TestLoadController, starting_and_stopping_a_controller)
auto controller_if = cm_->load_controller(controller_name1, TEST_CONTROLLER_CLASS_NAME);
ASSERT_NE(controller_if, nullptr);

ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id());

// Only testing with STRICT now for simplicity
{ // Test starting an stopped controller, and stopping afterwards
{
// Test starting unconfigured controller, and starting configured afterwards
RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller");
strvec start_controllers = {controller_name1};
strvec stop_controllers = {};
Expand All @@ -450,11 +483,11 @@ TEST_F(TestLoadController, starting_and_stopping_a_controller)
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));

ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";
{
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
}

ASSERT_EQ(
Expand Down Expand Up @@ -536,21 +569,20 @@ TEST_F(TestLoadController, switch_multiple_controllers)
}

{ // Stop controller 1, start controller 2
// Fails starting of controller 2 because it is not configured
// Both fails because controller 2 because it is not configured and STRICT is used
strvec start_controllers = {controller_name2};
strvec stop_controllers = {controller_name1};
RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1, starting controller #2 fails");
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));

ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());

ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_current_state().id());
ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id());
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
controller_if2->get_current_state().id());
Expand All @@ -559,6 +591,23 @@ TEST_F(TestLoadController, switch_multiple_controllers)
cm_->configure_controller(controller_name2);
}

{ // Stop controller 1
strvec start_controllers = {};
strvec stop_controllers = {controller_name1};
RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1, starting controller #2 fails");
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));

ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());

ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id());
ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id());
}

{ // Start controller 1 again
RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1");
strvec start_controllers = {controller_name1};
Expand Down