Skip to content

Commit

Permalink
Merge branch 'master' into mock-hw-remove-deprecated-functions
Browse files Browse the repository at this point in the history
  • Loading branch information
bmagyar authored Nov 6, 2023
2 parents 1568857 + 8bf1a8a commit d5f4003
Show file tree
Hide file tree
Showing 12 changed files with 170 additions and 165 deletions.
6 changes: 0 additions & 6 deletions .github/workflows/ci-coverage-build.yml
Original file line number Diff line number Diff line change
@@ -1,8 +1,6 @@
name: Coverage Build
on:
workflow_dispatch:
branches:
- master
push:
branches:
- master
Expand Down Expand Up @@ -31,11 +29,7 @@ jobs:
package-name:
controller_interface
controller_manager
controller_manager_msgs
hardware_interface
ros2controlcli
ros2_control
ros2_control_test_assets
transmission_interface

vcs-repo-file-url: |
Expand Down
1 change: 1 addition & 0 deletions README.md
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
# ros2_control

[![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)
[![codecov](https://codecov.io/gh/ros-controls/ros2_control/graph/badge.svg?token=idvm1zJXOf)](https://codecov.io/gh/ros-controls/ros2_control)

This package is a part of the ros2_control framework.
For more, please check the [documentation](https://control.ros.org/).
Expand Down
4 changes: 2 additions & 2 deletions codecov.yml
Original file line number Diff line number Diff line change
Expand Up @@ -14,12 +14,12 @@ fixes:
comment:
layout: "diff, flags, files"
behavior: default
ignore:
- "**/test"
flags:
unittests:
paths:
- controller_interface
- controller_manager
- hardware_interface
- joint_limits_interface
- test_robot_hardware
- transmission_interface
62 changes: 33 additions & 29 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -148,9 +148,6 @@ std::vector<std::string> get_following_controller_names(

return following_controllers;
}
// If the controller is not configured, return empty
if (!(is_controller_active(controller_it->c) || is_controller_inactive(controller_it->c)))
return following_controllers;
const auto cmd_itfs = controller_it->c->command_interface_configuration().names;
for (const auto & itf : cmd_itfs)
{
Expand Down Expand Up @@ -209,13 +206,11 @@ std::vector<std::string> get_preceding_controller_names(
}
for (const auto & ctrl : controllers)
{
// If the controller is not configured, return empty
if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c)))
return preceding_controllers;
auto cmd_itfs = ctrl.c->command_interface_configuration().names;
for (const auto & itf : cmd_itfs)
{
if (itf.find(controller_name) != std::string::npos)
auto split_pos = itf.find_first_of('/');
if ((split_pos != std::string::npos) && (itf.substr(0, split_pos) == controller_name))
{
preceding_controllers.push_back(ctrl.info.name);
auto ctrl_names = get_preceding_controller_names(ctrl.info.name, controllers);
Expand Down Expand Up @@ -768,10 +763,35 @@ controller_interface::return_type ControllerManager::configure_controller(
return controller_interface::return_type::ERROR;
}
resource_manager_->import_controller_reference_interfaces(controller_name, interfaces);
}

// Now let's reorder the controllers
// lock controllers
std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);
const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);

// Copy all controllers from the 'from' list to the 'to' list
to = from;

// Reordering the controllers
std::sort(
to.begin(), to.end(),
std::bind(
&ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2,
to));

// TODO(destogl): check and resort controllers in the vector
RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:");
for (const auto & ctrl : to)
{
RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str());
}

// switch lists
rt_controllers_wrapper_.switch_updated_list(guard);
// clear unused list
rt_controllers_wrapper_.get_unused_list(guard).clear();

return controller_interface::return_type::OK;
}

Expand Down Expand Up @@ -1233,19 +1253,6 @@ controller_interface::return_type ControllerManager::switch_controller(
}
}

// Reordering the controllers
std::sort(
to.begin(), to.end(),
std::bind(
&ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2,
to));

RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:");
for (const auto & ctrl : to)
{
RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str());
}

// switch lists
rt_controllers_wrapper_.switch_updated_list(guard);
// clear unused list
Expand Down Expand Up @@ -2435,13 +2442,6 @@ bool ControllerManager::controller_sorting(
const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b,
const std::vector<controller_manager::ControllerSpec> & controllers)
{
// If the controllers are not active, then should be at the end of the list
if (!is_controller_active(ctrl_a.c) || !is_controller_active(ctrl_b.c))
{
if (is_controller_active(ctrl_a.c)) return true;
return false;
}

const std::vector<std::string> cmd_itfs = ctrl_a.c->command_interface_configuration().names;
const std::vector<std::string> state_itfs = ctrl_a.c->state_interface_configuration().names;
if (cmd_itfs.empty() || !ctrl_a.c->is_chainable())
Expand Down Expand Up @@ -2513,7 +2513,11 @@ bool ControllerManager::controller_sorting(
// TODO(saikishor): deal with the state interface chaining in the sorting algorithm
auto state_it = std::find_if(
state_itfs.begin(), state_itfs.end(),
[ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); });
[ctrl_b](auto itf)
{
auto index = itf.find_first_of('/');
return ((index != std::string::npos) && (itf.substr(0, index) == ctrl_b.info.name));
});
if (state_it != state_itfs.end())
{
return false;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,33 +32,13 @@ TestChainableController::TestChainableController()
controller_interface::InterfaceConfiguration
TestChainableController::command_interface_configuration() const
{
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return cmd_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get command interface configuration until the controller is configured.");
}
return cmd_iface_cfg_;
}

controller_interface::InterfaceConfiguration
TestChainableController::state_interface_configuration() const
{
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return state_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get state interface configuration until the controller is configured.");
}
return state_iface_cfg_;
}

controller_interface::return_type TestChainableController::update_reference_from_subscribers(
Expand Down
24 changes: 2 additions & 22 deletions controller_manager/test/test_controller/test_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,32 +31,12 @@ TestController::TestController()

controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const
{
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return cmd_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get command interface configuration until the controller is configured.");
}
return cmd_iface_cfg_;
}

controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const
{
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return state_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get state interface configuration until the controller is configured.");
}
return state_iface_cfg_;
}

controller_interface::return_type TestController::update(
Expand Down
22 changes: 17 additions & 5 deletions controller_manager/test/test_controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -104,8 +104,11 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());

// configure controller
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
cm_->configure_controller(TEST_CONTROLLER2_NAME);
{
ControllerManagerRunner cm_runner(this);
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
cm_->configure_controller(TEST_CONTROLLER2_NAME);
}
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
Expand Down Expand Up @@ -217,7 +220,10 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)

test_controller->get_node()->set_parameter({"update_rate", 4});
// configure controller
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
{
ControllerManagerRunner cm_runner(this);
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
}
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
Expand Down Expand Up @@ -296,7 +302,10 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
rclcpp::Parameter update_rate_parameter("update_rate", static_cast<int>(ctrl_update_rate));
test_controller->get_node()->set_parameter(update_rate_parameter);
// configure controller
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
{
ControllerManagerRunner cm_runner(this);
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
}
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
Expand Down Expand Up @@ -389,7 +398,10 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)

test_controller->get_node()->set_parameter({"update_rate", static_cast<int>(ctrl_update_rate)});
// configure controller
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
{
ControllerManagerRunner cm_runner(this);
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
}
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
Expand Down
79 changes: 11 additions & 68 deletions controller_manager/test/test_controller_manager_srvs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -261,27 +261,28 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv)
// get controller list after configure
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(2u, result->controller.size());
// At this stage, the controllers are already reordered
// check chainable controller
ASSERT_EQ(result->controller[0].state, "inactive");
ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 0u);
ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 1u);
ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 3u);
ASSERT_EQ(result->controller[0].required_state_interfaces.size(), 2u);
ASSERT_EQ(result->controller[0].is_chainable, true);
ASSERT_EQ(result->controller[0].is_chainable, false);
ASSERT_EQ(result->controller[0].is_chained, false);
ASSERT_EQ(result->controller[0].reference_interfaces.size(), 2u);
ASSERT_EQ("joint1/position", result->controller[0].reference_interfaces[0]);
ASSERT_EQ("joint1/velocity", result->controller[0].reference_interfaces[1]);
ASSERT_EQ(result->controller[0].reference_interfaces.size(), 0u);
ASSERT_EQ(result->controller[0].chain_connections.size(), 1u);

ASSERT_EQ(result->controller[0].chain_connections.size(), 0u);
// check test controller
ASSERT_EQ(result->controller[1].state, "inactive");
ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 0u);
ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 3u);
ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 1u);
ASSERT_EQ(result->controller[1].required_state_interfaces.size(), 2u);
ASSERT_EQ(result->controller[1].is_chainable, false);
ASSERT_EQ(result->controller[1].is_chainable, true);
ASSERT_EQ(result->controller[1].is_chained, false);
ASSERT_EQ(result->controller[1].reference_interfaces.size(), 0u);
ASSERT_EQ(result->controller[1].chain_connections.size(), 1u);
ASSERT_EQ(result->controller[1].reference_interfaces.size(), 2u);
ASSERT_EQ(result->controller[1].chain_connections.size(), 0u);
ASSERT_EQ("joint1/position", result->controller[1].reference_interfaces[0]);
ASSERT_EQ("joint1/velocity", result->controller[1].reference_interfaces[1]);
// activate controllers
cm_->switch_controller(
{test_chainable_controller::TEST_CONTROLLER_NAME}, {},
Expand Down Expand Up @@ -603,22 +604,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers)
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(6u, result->controller.size());

// activate controllers
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_1}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2,
TEST_CHAINED_CONTROLLER_4},
{}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
rclcpp::Duration(0, 0));
cm_->switch_controller(
{test_controller::TEST_CONTROLLER_NAME}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
// get controller list after activate
result = call_service_and_wait(*client, request, srv_executor);

ASSERT_EQ(6u, result->controller.size());
// reordered controllers
ASSERT_EQ(result->controller[0].name, "test_controller_name");
ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_5);
Expand Down Expand Up @@ -776,25 +761,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers)
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(8u, result->controller.size());

// activate controllers
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_1}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_3}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2,
TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_7},
{}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
rclcpp::Duration(0, 0));
cm_->switch_controller(
{test_controller::TEST_CONTROLLER_NAME}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
// get controller list after activate
result = call_service_and_wait(*client, request, srv_executor);

ASSERT_EQ(8u, result->controller.size());
// reordered controllers
ASSERT_EQ(result->controller[0].name, "test_controller_name");
ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_7);
Expand Down Expand Up @@ -1009,29 +975,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers)
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(10u, result->controller.size());

// activate controllers
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_1}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_4}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_7}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2,
TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_8},
{}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
rclcpp::Duration(0, 0));
cm_->switch_controller(
{TEST_CONTROLLER_1, TEST_CONTROLLER_2}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
// get controller list after activate
result = call_service_and_wait(*client, request, srv_executor);

ASSERT_EQ(10u, result->controller.size());

auto get_ctrl_pos = [result](const std::string & controller_name) -> int
{
auto it = std::find_if(
Expand Down
Loading

0 comments on commit d5f4003

Please sign in to comment.