Skip to content

Add ros2 control managers #147

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

Closed
destogl wants to merge 29 commits into from
Closed

Add ros2 control managers #147

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
29 commits
Select commit Hold shift + click to select a range
80d32e3
Add resource manager.
destogl Sep 14, 2020
08a9623
Add ros2_control_node and main manager for the components Concept
destogl Sep 15, 2020
afc2068
ResourceManager move to hardware_interface package because of depende…
destogl Sep 26, 2020
d36b5c5
ResourceManager: interface check and handles claiming initial impleme…
destogl Sep 29, 2020
74de76c
Joint and Sensor have now virtual memeber to enable overriding. Resou…
destogl Oct 1, 2020
d88a734
ControllerManagerNew add missing functions. Control Manager can initi…
destogl Oct 1, 2020
8ef26d8
Merge branch 'master' into add_ros2_control_managers
destogl Oct 1, 2020
834343f
Correct build.
destogl Oct 1, 2020
a124bb5
Revert executor namespace and add compile definition to disable boost…
destogl Oct 1, 2020
8b2de3f
Added LifecycleNode name output
destogl Oct 1, 2020
9e0573b
Add the test where LifecycleNode name is checked.
destogl Oct 2, 2020
fb003db
Apply linters and uncrustify
destogl Oct 2, 2020
a70dc60
Use foxy uncrustify
destogl Oct 2, 2020
bc3f662
Logger name is defined in a constant
bmagyar Oct 3, 2020
50b9107
Restructuring the controller_manager with components
destogl Oct 5, 2020
49a096d
Delete old files
destogl Oct 5, 2020
bfede8b
Merge branch 'add_ros2_control_managers' of https://github.com/destog…
destogl Oct 5, 2020
61d0405
Merge branch 'master' into add_ros2_control_managers
destogl Oct 5, 2020
8292c4f
Refractoring and cleaning
destogl Oct 5, 2020
775e0e6
Small build correction
destogl Oct 5, 2020
6b3ee94
Update hardware_interface/src/resource_manager.cpp
bmagyar Oct 6, 2020
94ae85d
Throw execption from the contstructor
destogl Oct 6, 2020
bbe0df0
Change namespace of ResourceManager
destogl Oct 6, 2020
f86a7d4
Merge branch 'add_ros2_control_managers' of https://github.com/destog…
destogl Oct 6, 2020
3887008
Revert controller_loader_plugin_interface. Add more checks into Contr…
destogl Oct 7, 2020
0eda72a
Include reviews
destogl Oct 7, 2020
135278a
Rever spin changes because multi-threading does not work otherwise
destogl Oct 7, 2020
8495d0a
Make the rest of the reviews in
destogl Oct 7, 2020
b0f842e
Merge branch 'master' into HEAD
bmagyar Oct 14, 2020
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
2 changes: 2 additions & 0 deletions controller_interface/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@ ament_target_dependencies(
# Causes the visibility macros to use dllexport rather than dllimport,
# which is appropriate when building the dll but not consuming it.
target_compile_definitions(controller_interface PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL")
# prevent pluginlib from using boost
target_compile_definitions(controller_interface PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")

install(DIRECTORY include/
DESTINATION include
Expand Down
6 changes: 3 additions & 3 deletions controller_interface/include/controller_interface/controller_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@

#include "controller_interface/visibility_control.h"

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

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
Expand Down Expand Up @@ -48,7 +48,7 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN
virtual
return_type
init(
std::weak_ptr<hardware_interface::RobotHardware> robot_hardware,
std::weak_ptr<hardware_interface::ResourceManager> resource_manager,
const std::string & controller_name);

CONTROLLER_INTERFACE_PUBLIC
Expand All @@ -61,7 +61,7 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN
get_lifecycle_node();

protected:
std::weak_ptr<hardware_interface::RobotHardware> robot_hardware_;
std::weak_ptr<hardware_interface::ResourceManager> resource_manager_;
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> lifecycle_node_;
};

Expand Down
9 changes: 6 additions & 3 deletions controller_interface/src/controller_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,11 +22,14 @@ namespace controller_interface

return_type
ControllerInterface::init(
std::weak_ptr<hardware_interface::RobotHardware> robot_hardware,
std::weak_ptr<hardware_interface::ResourceManager> resource_manager,
const std::string & controller_name)
{
robot_hardware_ = robot_hardware;
lifecycle_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(controller_name);
resource_manager_ = resource_manager;
lifecycle_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
controller_name,
rclcpp::NodeOptions().allow_undeclared_parameters(true).
automatically_declare_parameters_from_overrides(true));

lifecycle_node_->register_on_configure(
std::bind(&ControllerInterface::on_configure, this, std::placeholders::_1));
Expand Down
138 changes: 77 additions & 61 deletions controller_manager/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,76 +37,90 @@ target_compile_definitions(controller_manager PRIVATE "CONTROLLER_MANAGER_BUILDI
# prevent pluginlib from using boost
target_compile_definitions(controller_manager PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")

add_executable(ros2_control_node src/ros2_control_node.cpp)
target_include_directories(ros2_control_node PRIVATE include)
target_link_libraries(ros2_control_node controller_manager)
ament_target_dependencies(ros2_control_node
controller_interface
hardware_interface
rclcpp
)

install(TARGETS controller_manager
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
)

install(TARGETS ros2_control_node
RUNTIME DESTINATION lib/${PROJECT_NAME}
)

install(DIRECTORY include/
DESTINATION include
)

if(BUILD_TESTING)
find_package(ament_cmake_gmock REQUIRED)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_lint_auto REQUIRED)

ament_lint_auto_find_test_dependencies()

ament_index_get_prefix_path(ament_index_build_path SKIP_AMENT_PREFIX_PATH)
# Get the first item (it will be the build space version of the build path).
list(GET ament_index_build_path 0 ament_index_build_path)
if(WIN32)
# On Windows prevent CMake errors and prevent it being evaluated as a list.
string(REPLACE "\\" "/" ament_index_build_path "${ament_index_build_path}")
endif()

add_library(test_controller SHARED test/test_controller/test_controller.cpp)
target_include_directories(test_controller PRIVATE include)
target_link_libraries(test_controller controller_manager)
target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")

ament_add_gmock(
test_controller_manager
test/test_controller_manager.cpp
)
target_include_directories(test_controller_manager PRIVATE include)
target_link_libraries(test_controller_manager controller_manager test_controller)
ament_target_dependencies(
test_controller_manager
test_robot_hardware
)

ament_add_gmock(
test_load_controller
test/test_load_controller.cpp
APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$<CONFIG>
)
target_include_directories(test_load_controller PRIVATE include)
target_link_libraries(test_load_controller controller_manager)
ament_target_dependencies(
test_load_controller
test_robot_hardware
)

ament_add_gmock(
test_controller_manager_srvs
test/test_controller_manager_srvs.cpp
APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$<CONFIG>
)
target_include_directories(test_controller_manager_srvs PRIVATE include)
target_link_libraries(test_controller_manager_srvs controller_manager test_controller)
ament_target_dependencies(
test_controller_manager_srvs
test_robot_hardware
)

pluginlib_export_plugin_description_file(controller_interface test/test_controller.xml)

install(TARGETS test_controller
DESTINATION lib
)
endif()
# if(BUILD_TESTING)
# find_package(ament_cmake_gmock REQUIRED)
# find_package(ament_cmake_gtest REQUIRED)
# find_package(ament_lint_auto REQUIRED)
#
# ament_lint_auto_find_test_dependencies()
#
# ament_index_get_prefix_path(ament_index_build_path SKIP_AMENT_PREFIX_PATH)
# # Get the first item (it will be the build space version of the build path).
# list(GET ament_index_build_path 0 ament_index_build_path)
# if(WIN32)
# # On Windows prevent CMake errors and prevent it being evaluated as a list.
# string(REPLACE "\\" "/" ament_index_build_path "${ament_index_build_path}")
# endif()
#
# add_library(test_controller SHARED test/test_controller/test_controller.cpp)
# target_include_directories(test_controller PRIVATE include)
# target_link_libraries(test_controller controller_manager)
# target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
#
# ament_add_gmock(
# test_controller_manager
# test/test_controller_manager.cpp
# )
# target_include_directories(test_controller_manager PRIVATE include)
# target_link_libraries(test_controller_manager controller_manager test_controller)
# ament_target_dependencies(
# test_controller_manager
# test_robot_hardware
# )
#
# ament_add_gmock(
# test_load_controller
# test/test_load_controller.cpp
# APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$<CONFIG>
# )
# target_include_directories(test_load_controller PRIVATE include)
# target_link_libraries(test_load_controller controller_manager)
# ament_target_dependencies(
# test_load_controller
# test_robot_hardware
# )
#
# ament_add_gmock(
# test_controller_manager_srvs
# test/test_controller_manager_srvs.cpp
# APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$<CONFIG>
# )
# target_include_directories(test_controller_manager_srvs PRIVATE include)
# target_link_libraries(test_controller_manager_srvs controller_manager test_controller)
# ament_target_dependencies(
# test_controller_manager_srvs
# test_robot_hardware
# )
#
# pluginlib_export_plugin_description_file(controller_interface test/test_controller.xml)
#
# install(TARGETS test_controller
# DESTINATION lib
# )
# endif()

ament_export_libraries(
controller_manager
Expand All @@ -117,6 +131,8 @@ ament_export_include_directories(
ament_export_dependencies(
controller_interface
controller_manager_msgs
hardware_interface
pluginlib
rclcpp
)
ament_package()
14 changes: 12 additions & 2 deletions 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 @@ -31,6 +31,7 @@
#include "controller_manager_msgs/srv/switch_controller.hpp"
#include "controller_manager_msgs/srv/unload_controller.hpp"

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

#include "pluginlib/class_loader.hpp"
Expand All @@ -49,14 +50,17 @@ class ControllerManager : public rclcpp::Node

CONTROLLER_MANAGER_PUBLIC
ControllerManager(
std::shared_ptr<hardware_interface::RobotHardware> hw,
std::shared_ptr<rclcpp::Executor> executor,
const std::string & name = "controller_manager");

CONTROLLER_MANAGER_PUBLIC
virtual
~ControllerManager() = default;

CONTROLLER_MANAGER_PUBLIC
controller_interface::return_type
configure();

CONTROLLER_MANAGER_PUBLIC
controller_interface::ControllerInterfaceSharedPtr
load_controller(
Expand Down Expand Up @@ -161,10 +165,16 @@ class ControllerManager : public rclcpp::Node
private:
std::vector<std::string> get_controller_names();

std::shared_ptr<hardware_interface::RobotHardware> hw_;
std::shared_ptr<rclcpp::Executor> executor_;
std::shared_ptr<pluginlib::ClassLoader<controller_interface::ControllerInterface>> loader_;

std::shared_ptr<hardware_interface::ResourceManager> resource_manager_;

rclcpp::callback_group::CallbackGroup::SharedPtr realtime_callback_group_;
rclcpp::callback_group::CallbackGroup::SharedPtr services_callback_group_;

rclcpp::TimerBase::SharedPtr timer_;

/**
* @brief 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
Loading