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

[WIP] Distributed control poc #1

Draft
wants to merge 19 commits into
base: master
Choose a base branch
from
+1,608 −116
Draft

[WIP] Distributed control poc #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
19 commits
Select commit Hold shift + click to select a range
2dbe482
create topic for publishing StateInterface's value via loan
mamueluth Nov 10, 2022
9f0d1b0
working now
mamueluth Nov 11, 2022
3d1e985
add visibility control, rename to distributed since no component anymore
mamueluth Nov 11, 2022
c54ff02
rename include guard
mamueluth Nov 14, 2022
6feda98
crude registration of StatePublishers at central controller manager
mamueluth Nov 17, 2022
8f200fb
move StatePublisher to Hardware side
mamueluth Nov 18, 2022
1e156a5
remove not needed rclcpp warn and info outputs
mamueluth Nov 18, 2022
0297099
add DistributedStateInterface and adjusted LoanedStateInterface
mamueluth Nov 21, 2022
e52740b
restructure handles
mamueluth Nov 23, 2022
48069e3
move creation of publishers into resource manager
mamueluth Nov 23, 2022
513201a
subscription in central controller
mamueluth Nov 30, 2022
4f5d16e
rename miss leading name of function
mamueluth Nov 30, 2022
de73b6a
rename StatePublisherDescription to PublisherDescription
mamueluth Nov 30, 2022
e8931ce
!Not working! Add registration of DistributedCommandInterfaces
mamueluth Dec 1, 2022
cad7070
change commandinterface from obj to shared_ptr
mamueluth Dec 2, 2022
92874f1
Add own qualitiy of service and register command forwarders
mamueluth Dec 2, 2022
49347d1
public inheritance in handle
mamueluth Dec 5, 2022
167e8dc
add distributed interfaces to available list
mamueluth Dec 14, 2022
71f4757
add some output for knowing whats going on
mamueluth Dec 21, 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
64 changes: 32 additions & 32 deletions controller_interface/test/test_force_torque_sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,20 +47,20 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_default_names)
std::vector<std::string> interface_names = force_torque_sensor_->get_state_interface_names();

// assign values to force
hardware_interface::StateInterface force_x{
sensor_name_, fts_interface_names_[0], &force_values_[0]};
hardware_interface::StateInterface force_y{
sensor_name_, fts_interface_names_[1], &force_values_[1]};
hardware_interface::StateInterface force_z{
sensor_name_, fts_interface_names_[2], &force_values_[2]};
auto force_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[0], &force_values_[0]);
auto force_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[1], &force_values_[1]);
auto force_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[2], &force_values_[2]);

// assign values to torque
hardware_interface::StateInterface torque_x{
sensor_name_, fts_interface_names_[3], &torque_values_[0]};
hardware_interface::StateInterface torque_y{
sensor_name_, fts_interface_names_[4], &torque_values_[1]};
hardware_interface::StateInterface torque_z{
sensor_name_, fts_interface_names_[5], &torque_values_[2]};
auto torque_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[3], &torque_values_[0]);
auto torque_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[4], &torque_values_[1]);
auto torque_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[5], &torque_values_[2]);

// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
Expand Down Expand Up @@ -131,16 +131,16 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_custom_names)
std::vector<std::string> interface_names = force_torque_sensor_->get_state_interface_names();

// assign values to force.x and force.z
hardware_interface::StateInterface force_x{
sensor_name_, fts_interface_names_[0], &force_values_[0]};
hardware_interface::StateInterface force_z{
sensor_name_, fts_interface_names_[2], &force_values_[2]};
auto force_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[0], &force_values_[0]);
auto force_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[2], &force_values_[2]);

// assign values to torque.y and torque.z
hardware_interface::StateInterface torque_y{
sensor_name_, fts_interface_names_[4], &torque_values_[1]};
hardware_interface::StateInterface torque_z{
sensor_name_, fts_interface_names_[5], &torque_values_[2]};
auto torque_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[4], &torque_values_[1]);
auto torque_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[5], &torque_values_[2]);

// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
Expand Down Expand Up @@ -213,20 +213,20 @@ TEST_F(ForceTorqueSensorTest, validate_all_custom_names)
ASSERT_EQ(force_torque_sensor_->interface_names_[5], sensor_name_ + "/" + "torque.z");

// assign values to force
hardware_interface::StateInterface force_x{
sensor_name_, fts_interface_names_[0], &force_values_[0]};
hardware_interface::StateInterface force_y{
sensor_name_, fts_interface_names_[1], &force_values_[1]};
hardware_interface::StateInterface force_z{
sensor_name_, fts_interface_names_[2], &force_values_[2]};
auto force_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[0], &force_values_[0]);
auto force_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[1], &force_values_[1]);
auto force_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[2], &force_values_[2]);

// assign values to torque
hardware_interface::StateInterface torque_x{
sensor_name_, fts_interface_names_[3], &torque_values_[0]};
hardware_interface::StateInterface torque_y{
sensor_name_, fts_interface_names_[4], &torque_values_[1]};
hardware_interface::StateInterface torque_z{
sensor_name_, fts_interface_names_[5], &torque_values_[2]};
auto torque_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[3], &torque_values_[0]);
auto torque_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[4], &torque_values_[1]);
auto torque_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, fts_interface_names_[5], &torque_values_[2]);

// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
Expand Down
40 changes: 20 additions & 20 deletions controller_interface/test/test_imu_sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,30 +46,30 @@ TEST_F(IMUSensorTest, validate_all)
std::vector<std::string> interface_names = imu_sensor_->get_state_interface_names();

// assign values to orientation
hardware_interface::StateInterface orientation_x{
sensor_name_, imu_interface_names_[0], &orientation_values_[0]};
hardware_interface::StateInterface orientation_y{
sensor_name_, imu_interface_names_[1], &orientation_values_[1]};
hardware_interface::StateInterface orientation_z{
sensor_name_, imu_interface_names_[2], &orientation_values_[2]};
hardware_interface::StateInterface orientation_w{
sensor_name_, imu_interface_names_[3], &orientation_values_[4]};
auto orientation_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[0], &orientation_values_[0]);
auto orientation_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[1], &orientation_values_[1]);
auto orientation_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[2], &orientation_values_[2]);
auto orientation_w = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[3], &orientation_values_[4]);

// assign values to angular velocity
hardware_interface::StateInterface angular_velocity_x{
sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]};
hardware_interface::StateInterface angular_velocity_y{
sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]};
hardware_interface::StateInterface angular_velocity_z{
sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]};
auto angular_velocity_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]);
auto angular_velocity_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]);
auto angular_velocity_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]);

// assign values to linear acceleration
hardware_interface::StateInterface linear_acceleration_x{
sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]};
hardware_interface::StateInterface linear_acceleration_y{
sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]};
hardware_interface::StateInterface linear_acceleration_z{
sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]};
auto linear_acceleration_x = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]);
auto linear_acceleration_y = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]);
auto linear_acceleration_z = std::make_shared<hardware_interface::StateInterface>(
sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]);

// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
Expand Down
9 changes: 6 additions & 3 deletions controller_interface/test/test_semantic_component_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,9 +94,12 @@ TEST_F(SemanticComponentInterfaceTest, validate_state_interfaces)
std::vector<double> interface_values = {1.1, 3.3, 5.5};

// assign 1.1 to interface_1, 3.3 to interface_3 and 5.5 to interface_5
hardware_interface::StateInterface interface_1{component_name_, "1", &interface_values[0]};
hardware_interface::StateInterface interface_3{component_name_, "3", &interface_values[1]};
hardware_interface::StateInterface interface_5{component_name_, "5", &interface_values[2]};
auto interface_1 = std::make_shared<hardware_interface::StateInterface>(
component_name_, "1", &interface_values[0]);
auto interface_3 = std::make_shared<hardware_interface::StateInterface>(
component_name_, "3", &interface_values[1]);
auto interface_5 = std::make_shared<hardware_interface::StateInterface>(
component_name_, "5", &interface_values[2]);

// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
Expand Down
2 changes: 1 addition & 1 deletion controller_manager/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,12 +27,12 @@ endforeach()
add_library(${PROJECT_NAME} SHARED
src/controller_manager.cpp
)

target_include_directories(${PROJECT_NAME} PRIVATE include)
ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
# Causes the visibility macros to use dllexport rather than dllimport,
# which is appropriate when building the dll but not consuming it.
target_compile_definitions(${PROJECT_NAME} PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")

add_executable(ros2_control_node src/ros2_control_node.cpp)
target_include_directories(ros2_control_node PRIVATE include)
target_link_libraries(ros2_control_node
Expand Down
32 changes: 31 additions & 1 deletion controller_manager/include/controller_manager/controller_manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
#include "controller_manager_msgs/srv/list_hardware_components.hpp"
#include "controller_manager_msgs/srv/list_hardware_interfaces.hpp"
#include "controller_manager_msgs/srv/load_controller.hpp"
#include "controller_manager_msgs/srv/register_sub_controller_manager.hpp"
#include "controller_manager_msgs/srv/reload_controller_libraries.hpp"
#include "controller_manager_msgs/srv/set_hardware_component_state.hpp"
#include "controller_manager_msgs/srv/switch_controller.hpp"
Expand All @@ -51,7 +52,6 @@
#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;
Expand Down Expand Up @@ -190,6 +190,27 @@ class ControllerManager : public rclcpp::Node
CONTROLLER_MANAGER_PUBLIC
void init_services();

CONTROLLER_MANAGER_PUBLIC
void configure_controller_manager();

CONTROLLER_MANAGER_PUBLIC
void init_distributed_main_controller_services();

CONTROLLER_MANAGER_PUBLIC
void register_sub_controller_manager_srv_cb(
const std::shared_ptr<controller_manager_msgs::srv::RegisterSubControllerManager::Request>
request,
std::shared_ptr<controller_manager_msgs::srv::RegisterSubControllerManager::Response> response);

CONTROLLER_MANAGER_PUBLIC
void add_hardware_state_publishers();

CONTROLLER_MANAGER_PUBLIC
void add_hardware_command_forwarders();

CONTROLLER_MANAGER_PUBLIC
void register_sub_controller_manager();

CONTROLLER_MANAGER_PUBLIC
controller_interface::ControllerInterfaceBaseSharedPtr add_controller_impl(
const ControllerSpec & controller);
Expand Down Expand Up @@ -395,6 +416,10 @@ class ControllerManager : public rclcpp::Node
*/
rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_;

bool distributed_ = false;
bool sub_controller_manager_ = false;

rclcpp::CallbackGroup::SharedPtr distributed_system_srv_callback_group_;
/**
* The RTControllerListWrapper class wraps a double-buffered list of controllers
* to avoid needing to lock the real-time thread when switching controllers in
Expand Down Expand Up @@ -492,6 +517,11 @@ class ControllerManager : public rclcpp::Node
rclcpp::Service<controller_manager_msgs::srv::SetHardwareComponentState>::SharedPtr
set_hardware_component_state_service_;

// services for distributed control
std::mutex central_controller_manager_srv_lock_;
rclcpp::Service<controller_manager_msgs::srv::RegisterSubControllerManager>::SharedPtr
register_sub_controller_manager_srv_;

std::vector<std::string> activate_request_, deactivate_request_;
std::vector<std::string> to_chained_mode_request_, from_chained_mode_request_;
std::vector<std::string> activate_command_interface_request_,
Expand Down
Loading