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.

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

Full functionality of chainable controllers #667

Merged
merged 31 commits into from
Jun 30, 2022
Merged
Show file tree
Hide file tree
Changes from 30 commits
Commits
Show all changes
31 commits
Select commit Hold shift + click to select a range
d19c724
Extending ControllerInterface with methods for chainable controllers.
destogl Feb 26, 2022
03ae2b2
Add ControllerInterfaceBase class and use it in Controller Manager.
destogl May 15, 2022
c5d7383
Make nice user interface to export controllers.
destogl May 15, 2022
862987e
Revert some changes to work on this stage.
destogl May 16, 2022
0b7bd7f
Extend resource manager to manage reference interfaces from controllers.
destogl Mar 4, 2022
2a4af6b
Adjust interface between CM and RM for managing controllers' referenc…
destogl Mar 6, 2022
44b17e0
Small fixes in controller manager tests.
destogl Feb 27, 2022
b0cc0a7
Added first tests up to configuration of chainable controllers.
destogl Mar 3, 2022
f883120
Chained controllers can be manually activated.
destogl Mar 6, 2022
0a46e6c
Successful value propagation when controllers are updated individually 🎉
destogl Mar 7, 2022
1b16a1b
Successful updating all controllers at once when added into execution…
destogl Mar 8, 2022
50fee31
Add support for 'chained_controllers' parameter.
destogl Mar 11, 2022
a3f928d
Split test into smaller reusable chunks and start test for auto-switc…
destogl Mar 12, 2022
a0b64da
Make code of controller manager more readable and better documented.
destogl Mar 12, 2022
1b05bf2
Added functionality for auto-switching of chained mode in controllers.
destogl Mar 15, 2022
5d7b040
Fixed and optimization for running example.
destogl Mar 24, 2022
fac6a89
Added interface-matching approach for managing chaining controllers.
destogl Mar 29, 2022
a2be1d1
Clean controller manager from obsolte code using parameter for chaine…
destogl Mar 29, 2022
0348e8d
Return error is chained controller does not export any interfaces
destogl Apr 22, 2022
ed0d8d6
Update chainable controller for tests.
destogl Apr 26, 2022
648d039
Add ControllerInterfaceBase class and use it in Controller Manager.
destogl May 15, 2022
65bcd7c
Make nice user interface to export controllers.
destogl May 15, 2022
cad977b
Fixup tests to the new API.
destogl May 16, 2022
1cb9154
Merge branch 'master' into chainable-controllers
destogl May 17, 2022
b0ff012
Merge branch 'master' into chainable-controllers
destogl May 18, 2022
d652a34
Format code using clang-format-12 to make resolution of merge conflic…
destogl Jun 18, 2022
8ead608
Merge branch 'master' into chainable-controllers
destogl Jun 18, 2022
4b81f80
Make trivial changes according to review comments.
destogl Jun 18, 2022
fe20955
Update switch_chained_mode methods per review.
destogl Jun 18, 2022
702f69f
Move checks for chained controllers into separate methods.
destogl Jun 18, 2022
e6d2f23
Second round of reviews.
destogl Jun 29, 2022
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase
/**
* Controller is not chainable, therefore no chained mode can be set.
*
* \returns false;
* \returns false.
*/
CONTROLLER_INTERFACE_PUBLIC
bool set_chained_mode(bool chained_mode) final;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,11 @@ bool get_ordered_interfaces(
{
if (!interface_type.empty())
{
if ((name == interface.get_name()) && (interface_type == interface.get_interface_name()))
// check case where:
// (<joint> == <joint> AND <interface> == <interface>) OR <joint>/<interface> == 'full name'
if (
destogl marked this conversation as resolved.
Show resolved Hide resolved
((name == interface.get_name()) && (interface_type == interface.get_interface_name())) ||
((name + "/" + interface_type) == interface.get_full_name()))
{
ordered_interfaces.push_back(std::ref(interface));
}
Expand Down
18 changes: 18 additions & 0 deletions controller_manager/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,16 @@ if(BUILD_TESTING)
DESTINATION lib
)

add_library(test_chainable_controller SHARED test/test_chainable_controller/test_chainable_controller.cpp)
target_include_directories(test_chainable_controller PRIVATE include)
target_link_libraries(test_chainable_controller controller_manager)
target_compile_definitions(test_chainable_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
pluginlib_export_plugin_description_file(
controller_interface test/test_chainable_controller/test_chainable_controller.xml)
install(TARGETS test_chainable_controller
DESTINATION lib
)

ament_add_gmock(
test_controller_manager
test/test_controller_manager.cpp
Expand All @@ -114,6 +124,14 @@ if(BUILD_TESTING)
target_link_libraries(test_load_controller ${PROJECT_NAME} test_controller test_controller_failed_init)
ament_target_dependencies(test_load_controller ros2_control_test_assets)

ament_add_gmock(
test_controllers_chaining_with_controller_manager
test/test_controllers_chaining_with_controller_manager.cpp
)
target_include_directories(test_controllers_chaining_with_controller_manager PRIVATE include)
target_link_libraries(test_controllers_chaining_with_controller_manager controller_manager test_chainable_controller test_controller)
ament_target_dependencies(test_controllers_chaining_with_controller_manager ros2_control_test_assets)

ament_add_gmock(
test_controller_manager_srvs
test/test_controller_manager_srvs.cpp
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,9 +15,11 @@
#ifndef CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_
#define CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_

#include <map>
#include <memory>
#include <string>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "controller_interface/chainable_controller_interface.hpp"
Expand All @@ -40,15 +42,21 @@
#include "controller_manager_msgs/srv/switch_controller.hpp"
#include "controller_manager_msgs/srv/unload_controller.hpp"

#include "hardware_interface/handle.hpp"
#include "hardware_interface/resource_manager.hpp"

#include "pluginlib/class_loader.hpp"

#include "rclcpp/executor.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/node_interfaces/node_logging_interface.hpp"
#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
#include "rclcpp/parameter.hpp"

namespace controller_manager
{
using ControllersListIterator = std::vector<controller_manager::ControllerSpec>::const_iterator;
bmagyar marked this conversation as resolved.
Show resolved Hide resolved

class ControllerManager : public rclcpp::Node
{
public:
Expand Down Expand Up @@ -169,6 +177,18 @@ class ControllerManager : public rclcpp::Node
CONTROLLER_MANAGER_PUBLIC
void stop_controllers();

/**
* Switch chained mode for all the controllers with respect to the following cases:
* - a preceding controller is getting activated --> switch controller to chained mode;
* - all preceding controllers are deactivated --> switch controller from chained mode.
*
* \param[in] chained_mode_switch_list list of controller to switch chained mode.
* \param[in] to_chained_mode flag if controller should be switched *to* or *from* chained mode.
*/
CONTROLLER_MANAGER_PUBLIC
void switch_chained_mode(
const std::vector<std::string> & chained_mode_switch_list, bool to_chained_mode);

CONTROLLER_MANAGER_PUBLIC
void start_controllers();

Expand Down Expand Up @@ -243,11 +263,78 @@ class ControllerManager : public rclcpp::Node
// Per controller update rate support
unsigned int update_loop_counter_ = 0;
unsigned int update_rate_ = 100;
std::vector<std::vector<std::string>> chained_controllers_configuration_;

std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

umm... were moved here for testability? can we have them in a protected section instead perhaps?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is a protected section. Yes, I moved it for simpler testing, otherwise doesn't work, or didn't come with a simple solution to do so.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is why we moved things to protected and use a "TestableX` class wrapper or friend to write the tests where you can define a getter for these functions and do you assertions

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Again: those members are protected or am I seeing something wrong?


private:
std::vector<std::string> get_controller_names();

std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
/**
* Clear request lists used when switching controllers. The lists are shared between "callback" and
* "control loop" threads.
*/
void clear_requests();

/**
* If a controller is deactivated all following controllers (if any exist) should be switched
* 'from' the chained mode.
*
* \param[in] controllers list with controllers.
*/
void propagate_deactivation_of_chained_mode(const std::vector<ControllerSpec> & controllers);

/// Check if all the following controllers will be in active state and in the chained mode
/// after controllers' switch.
/**
* Check recursively that all following controllers of the @controller_it
* - are already active,
* - will not be deactivated,
* - or will be activated.
* The following controllers are added to the request to switch in the chained mode or removed
* from the request to switch from the chained mode.
*
* For each controller the whole chain of following controllers is checked.
*
* NOTE: The automatically adding of following controller into starting list is not implemented
* yet.
*
* \param[in] controllers list with controllers.
* \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following
* controllers will be automatically added to the activate request list if they are not in the
* deactivate request.
* \param[in] controller_it iterator to the controller for which the following controllers are
* checked.
*
* \returns return_type::OK if all following controllers pass the checks, otherwise
* return_type::ERROR.
*/
controller_interface::return_type check_following_controllers_for_activate(
const std::vector<ControllerSpec> & controllers, int strictness,
const ControllersListIterator controller_it);

/// Check if all the preceding controllers will be in inactive state after controllers' switch.
/**
* Check that all preceding controllers of the @controller_it
* - are inactive,
* - will be deactivated,
* - and will not be activated.
*
* NOTE: The automatically adding of preceding controllers into stopping list is not implemented
* yet.
*
* \param[in] controllers list with controllers.
* \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all preceding
* controllers will be automatically added to the deactivate request list.
* \param[in] controller_it iterator to the controller for which the preceding controllers are
* checked.
*
* \returns return_type::OK if all preceding controllers pass the checks, otherwise
* return_type::ERROR.
*/
controller_interface::return_type check_preceeding_controllers_for_deactivate(
const std::vector<ControllerSpec> & controllers, int strictness,
const ControllersListIterator controller_it);

std::shared_ptr<rclcpp::Executor> executor_;

Expand Down Expand Up @@ -366,6 +453,7 @@ class ControllerManager : public rclcpp::Node
set_hardware_component_state_service_;

std::vector<std::string> start_request_, stop_request_;
std::vector<std::string> to_chained_mode_request_, from_chained_mode_request_;
std::vector<std::string> start_command_interface_request_, stop_command_interface_request_;

struct SwitchParams
Expand Down
Loading