Skip to content

Commit

Permalink
linter fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
sgmurray committed Dec 15, 2022
1 parent dde8a23 commit d63e019
Show file tree
Hide file tree
Showing 5 changed files with 34 additions and 24 deletions.
22 changes: 14 additions & 8 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -485,15 +485,17 @@ controller_interface::return_type ControllerManager::configure_controller(
resource_manager_->import_controller_reference_interfaces(controller_name, interfaces);
}

std::vector<ControllerSpec> & unused_controller_list = rt_controllers_wrapper_.get_unused_list(guard);
std::vector<ControllerSpec> & unused_controller_list =
rt_controllers_wrapper_.get_unused_list(guard);
unused_controller_list = controllers;

std::unordered_map<std::string, int> preceeding_controller_count = {};
std::unordered_map<std::string, std::vector<std::string>> following_controllers = {};

for (const auto & controller1 : unused_controller_list)
{
if (!controller1.c->is_chainable() || (!is_controller_active(controller1.c) && !is_controller_inactive(controller1.c)) )
if (!controller1.c->is_chainable() || (
!is_controller_active(controller1.c) && !is_controller_inactive(controller1.c)) )
{
continue;
}
Expand All @@ -505,17 +507,18 @@ controller_interface::return_type ControllerManager::configure_controller(
}
if (controller2.info.name == controller1.info.name)
{
continue;
continue;
}
for (const auto & reference1 : resource_manager_->get_controller_reference_interface_names(controller1.info.name))
for (const auto & reference1 :
resource_manager_->get_controller_reference_interface_names(controller1.info.name))
{
const auto controller2_commands = controller2.c->command_interface_configuration().names;

std::string commands_string = "";
for (auto command : controller2_commands){
commands_string += " " + command;
}
auto same = std::find(controller2_commands.begin(), controller2_commands.end(), reference1);
auto same = std::find(controller2_commands.begin(), controller2_commands.end(), reference1);
if (same != controller2_commands.end())
{
// controller 2 preceeds controller1
Expand All @@ -525,9 +528,12 @@ controller_interface::return_type ControllerManager::configure_controller(
}
else
{
following_controllers.emplace(controller2.info.name, std::initializer_list<std::string>{controller1.info.name});
following_controllers.emplace(
controller2.info.name,
std::initializer_list<std::string>{controller1.info.name});
}
if (preceeding_controller_count.find(controller1.info.name) != preceeding_controller_count.end()){
if (preceeding_controller_count.find(controller1.info.name) !=
preceeding_controller_count.end()){
preceeding_controller_count[controller1.info.name] += 1;
}
else
Expand Down
8 changes: 4 additions & 4 deletions controller_manager/test/test_controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -105,7 +105,7 @@ TEST_P(TestControllerManager, controller_lifecycle)

// configure controller
auto configure_future = std::async(
std::launch::async, &controller_manager::ControllerManager::configure_controller,
std::launch::async, &controller_manager::ControllerManager::configure_controller,
cm_, test_controller::TEST_CONTROLLER_NAME);
configure_future.wait_for(std::chrono::milliseconds(100));
EXPECT_EQ(
Expand All @@ -114,7 +114,7 @@ TEST_P(TestControllerManager, controller_lifecycle)
EXPECT_EQ(configure_future.get(), controller_interface::return_type::OK);

configure_future = std::async(
std::launch::async, &controller_manager::ControllerManager::configure_controller,
std::launch::async, &controller_manager::ControllerManager::configure_controller,
cm_, TEST_CONTROLLER2_NAME);
configure_future.wait_for(std::chrono::milliseconds(100));
EXPECT_EQ(
Expand Down Expand Up @@ -229,10 +229,10 @@ TEST_P(TestControllerManager, per_controller_update_rate)
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());

test_controller->get_node()->set_parameter({"update_rate", 4});

// configure controller
auto configure_future = std::async(
std::launch::async, &controller_manager::ControllerManager::configure_controller,
std::launch::async, &controller_manager::ControllerManager::configure_controller,
cm_, test_controller::TEST_CONTROLLER_NAME);
configure_future.wait_for(std::chrono::milliseconds(100));
EXPECT_EQ(
Expand Down
2 changes: 1 addition & 1 deletion controller_manager/test/test_controller_manager_srvs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -263,7 +263,7 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv)
ASSERT_EQ(2u, result->controller.size());
// check chainable controller

// cm_->configure_controller() sorts the controllers
// cm_->configure_controller() sorts the controllers
// test controller now preceeds chainable controller
ASSERT_EQ(result->controller[1].name, "test_chainable_controller_name");
ASSERT_EQ(result->controller[1].type, "controller_manager/test_chainable_controller");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,6 @@ class TestableTestChainableController : public test_chainable_controller::TestCh
FRIEND_TEST(
TestControllerChainingWithControllerManager,
test_chained_controllers_random_order);

};

class TestableControllerManager : public controller_manager::ControllerManager
Expand Down Expand Up @@ -1005,8 +1004,6 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_ran
cm_->add_controller(
position_tracking_controller, POSITION_TRACKING_CONTROLLER,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);



CheckIfControllersAreAddedCorrectly();

Expand Down
23 changes: 15 additions & 8 deletions controller_manager/test/test_load_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -162,16 +162,18 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller)
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id());

// Activate configured controller
auto configure_future = std::async(
std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_, controller_name1);
auto configure_future = std::async(std::launch::async,
&controller_manager::ControllerManager::configure_controller,
cm_,
controller_name1);
configure_future.wait_for(std::chrono::milliseconds(100));

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
configure_future.get();
start_test_controller(test_param.strictness);

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

Expand Down Expand Up @@ -255,10 +257,12 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure
// Configure from inactive state
test_controller->simulate_cleanup_failure = false;

auto configure_future = std::async(
std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_, controller_name1);
auto configure_future = std::async(std::launch::async,
&controller_manager::ControllerManager::configure_controller,
cm_,
controller_name1);
configure_future.wait_for(std::chrono::milliseconds(100));

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
Expand Down Expand Up @@ -439,7 +443,10 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers)
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id());

auto configure_future = std::async(
std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_, controller_name2);
std::launch::async,
&controller_manager::ControllerManager::configure_controller,
cm_,
controller_name2);
configure_future.wait_for(std::chrono::milliseconds(1000));
EXPECT_EQ(
controller_interface::return_type::OK,
Expand Down

0 comments on commit d63e019

Please sign in to comment.