├── controlko_description ├── config │ └── .gitkeep ├── meshes │ └── rrbot │ │ └── visual │ │ └── .gitkeep ├── package.xml ├── CMakeLists.txt ├── urdf │ ├── common.xacro │ ├── rrbot.urdf.xacro │ └── rrbot │ │ ├── rrbot_macro.ros2_control.xacro │ │ └── rrbot_macro.xacro ├── launch │ └── view_rrbot.launch.py └── rviz │ └── rrbot.rviz ├── controlko_bringup ├── config │ ├── soho-12_controllers.yaml │ ├── test_goal_publishers_config.yaml │ └── rrbot_controllers.yaml ├── launch │ ├── test_forward_position_controller.launch.py │ ├── test_joint_trajectory_controller.launch.py │ ├── rrbot_sim_gazebo_classic.launch.py │ ├── rrbot_sim_gazebo.launch.py │ ├── soho-12.launch.py │ └── rrbot.launch.py ├── CMakeLists.txt └── package.xml ├── roscon2022_control_workshop ├── CMakeLists.txt └── package.xml ├── controlko_controllers ├── test │ ├── displacement_controller_params.yaml │ ├── displacement_controller_preceeding_params.yaml │ ├── test_load_displacement_controller.cpp │ ├── test_displacement_controller_preceeding.cpp │ ├── test_displacement_controller.cpp │ └── test_displacement_controller.hpp ├── controlko_controllers.xml ├── src │ ├── displacement_controller.yaml │ └── displacement_controller.cpp ├── package.xml ├── include │ └── controlko_controllers │ │ ├── validate_displacement_controller_parameters.hpp │ │ ├── visibility_control.h │ │ └── displacement_controller.hpp ├── CMakeLists.txt └── LICENSE ├── TODOs.md ├── .clang-format ├── controlko_hardware_interface ├── controlko_hardware_interface.xml ├── package.xml ├── test │ └── test_rrbot_hardware_interface.cpp ├── include │ └── controlko_hardware_interface │ │ ├── visibility_control.h │ │ ├── rrbot_hardware_interface.hpp │ │ └── dr_denis_rrbot_comms.hpp ├── CMakeLists.txt ├── src │ └── rrbot_hardware_interface.cpp └── LICENSE ├── roscon2022_workshop.repos ├── roscon2022_workshop.humble.repos ├── LICENSE └── README.md /controlko_description/config/.gitkeep: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /controlko_bringup/config/soho-12_controllers.yaml: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /controlko_description/meshes/rrbot/visual/.gitkeep: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /roscon2022_control_workshop/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(roscon2022_control_workshop) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | ament_package() 6 | -------------------------------------------------------------------------------- /controlko_controllers/test/displacement_controller_params.yaml: -------------------------------------------------------------------------------- 1 | test_displacement_controller: 2 | ros__parameters: 3 | 4 | joints: 5 | - joint1 6 | 7 | interface_name: acceleration 8 | -------------------------------------------------------------------------------- /controlko_controllers/test/displacement_controller_preceeding_params.yaml: -------------------------------------------------------------------------------- 1 | test_displacement_controller: 2 | ros__parameters: 3 | joints: 4 | - joint1 5 | 6 | interface_name: acceleration 7 | 8 | state_joints: 9 | - joint1state 10 | -------------------------------------------------------------------------------- /TODOs.md: -------------------------------------------------------------------------------- 1 | TODOs: 2 | 3 | - Link presenation and add change into the first commit. 4 | - Add figure with the ros2_control architecture into "Task Nr. 3"'s solution. And add it to the proper commit. 5 | - Add Nr. 2 into description of the TAsk Nr. 2 6 | - Review `rqt_trajectory_controller` PR -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | 5 | ColumnLimit: 100 6 | AccessModifierOffset: -2 7 | AlignAfterOpenBracket: AlwaysBreak 8 | BreakBeforeBraces: Allman 9 | ConstructorInitializerIndentWidth: 0 10 | ContinuationIndentWidth: 2 11 | DerivePointerAlignment: false 12 | PointerAlignment: Middle 13 | ReflowComments: false 14 | IncludeBlocks: Preserve 15 | ... 16 | -------------------------------------------------------------------------------- /controlko_controllers/controlko_controllers.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | DisplacementController ros2_control controller. 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /controlko_hardware_interface/controlko_hardware_interface.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ros2_control hardware interface. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /roscon2022_workshop.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | gazebo_ros2_control: 3 | type: git 4 | url: https://github.com/ros-controls/gazebo_ros2_control.git 5 | version: master 6 | gz_ros2_control: 7 | type: git 8 | url: https://github.com/ros-controls/gz_ros2_control.git 9 | version: master 10 | ros2_control: 11 | type: git 12 | url: https://github.com/ros-controls/ros2_control.git 13 | version: master 14 | ros2_controllers: 15 | type: git 16 | url: https://github.com/ros-controls/ros2_controllers.git 17 | version: roscon2022-workshop-additions 18 | -------------------------------------------------------------------------------- /roscon2022_workshop.humble.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | gazebo_ros2_control: 3 | type: git 4 | url: https://github.com/ros-controls/gazebo_ros2_control.git 5 | version: master 6 | gz_ros2_control: 7 | type: git 8 | url: https://github.com/ros-controls/gz_ros2_control.git 9 | version: master 10 | ros2_control: 11 | type: git 12 | url: https://github.com/ros-controls/ros2_control.git 13 | version: roscon2022-workshop-additions 14 | ros2_controllers: 15 | type: git 16 | url: https://github.com/ros-controls/ros2_controllers.git 17 | version: roscon2022-workshop-additions 18 | -------------------------------------------------------------------------------- /roscon2022_control_workshop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | roscon2022_control_workshop 5 | 0.0.0 6 | Support files for the ros2_control workshop at ROSCon 2022 in Kyoto, Japan. 7 | Denis Štogl 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | 13 | ament_cmake 14 | 15 | 16 | -------------------------------------------------------------------------------- /controlko_bringup/launch/test_forward_position_controller.launch.py: -------------------------------------------------------------------------------- 1 | # $LICENSE$ 2 | 3 | from launch import LaunchDescription 4 | from launch.substitutions import PathJoinSubstitution 5 | from launch_ros.actions import Node 6 | from launch_ros.substitutions import FindPackageShare 7 | 8 | 9 | def generate_launch_description(): 10 | 11 | position_goals = PathJoinSubstitution( 12 | [FindPackageShare("controlko_bringup"), "config", "test_goal_publishers_config.yaml"] 13 | ) 14 | 15 | return LaunchDescription( 16 | [ 17 | Node( 18 | package="ros2_controllers_test_nodes", 19 | executable="publisher_forward_position_controller", 20 | name="publisher_forward_position_controller", 21 | parameters=[position_goals], 22 | output="both", 23 | ) 24 | ] 25 | ) 26 | -------------------------------------------------------------------------------- /controlko_bringup/launch/test_joint_trajectory_controller.launch.py: -------------------------------------------------------------------------------- 1 | # $LICENSE$ 2 | 3 | from launch import LaunchDescription 4 | from launch.substitutions import PathJoinSubstitution 5 | from launch_ros.actions import Node 6 | from launch_ros.substitutions import FindPackageShare 7 | 8 | 9 | def generate_launch_description(): 10 | 11 | position_goals = PathJoinSubstitution( 12 | [FindPackageShare("controlko_bringup"), "config", "test_goal_publishers_config.yaml"] 13 | ) 14 | 15 | return LaunchDescription( 16 | [ 17 | Node( 18 | package="ros2_controllers_test_nodes", 19 | executable="publisher_joint_trajectory_controller", 20 | name="publisher_joint_trajectory_controller", 21 | parameters=[position_goals], 22 | output="both", 23 | ) 24 | ] 25 | ) 26 | -------------------------------------------------------------------------------- /controlko_bringup/config/test_goal_publishers_config.yaml: -------------------------------------------------------------------------------- 1 | publisher_forward_position_controller: 2 | ros__parameters: 3 | 4 | controller_name: "forward_position_controller" 5 | wait_sec_between_publish: 5 6 | 7 | goal_names: ["pos1", "pos2", "pos3", "pos4"] 8 | pos1: [0.785, 0.785] 9 | pos2: [0.0, 0.0] 10 | pos3: [-0.785, -0.785] 11 | pos4: [0.0, 0.0] 12 | 13 | 14 | publisher_joint_trajectory_controller: 15 | ros__parameters: 16 | 17 | controller_name: "joint_trajectory_controller" 18 | wait_sec_between_publish: 6 19 | repeat_the_same_goal: 1 # useful to simulate continuous inputs 20 | 21 | goal_time_from_start: 3.0 22 | goal_names: ["pos1", "pos2", "pos3", "pos4"] 23 | pos1: 24 | positions: [0.785, 0.785] 25 | pos2: 26 | positions: [0.0, 0.0] 27 | pos3: 28 | positions: [-0.785, -0.785] 29 | pos4: 30 | positions: [0.0, 0.0] 31 | 32 | joints: 33 | - joint1 34 | - joint2 35 | -------------------------------------------------------------------------------- /controlko_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | controlko_description 5 | 0.0.0 6 | Description package for the Controlko - demo robot for ros2_control @ ROSCon2022 7 | Denis Štogl 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | joint_state_publisher_gui 13 | hardware_interface 14 | robot_state_publisher 15 | rviz2 16 | xacro 17 | 18 | ament_lint_auto 19 | ament_lint_common 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /controlko_hardware_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | controlko_hardware_interface 5 | 0.0.0 6 | Description package for the Controlko - demo robot for ros2_control @ ROSCon2022 7 | Denis Štogl 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | hardware_interface 13 | 14 | pluginlib 15 | 16 | rclcpp 17 | 18 | rclcpp_lifecycle 19 | 20 | ament_lint_auto 21 | ament_lint_common 22 | ament_cmake_gmock 23 | ros2_control_test_assets 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /controlko_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(controlko_bringup) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | # find_package( REQUIRED) 13 | 14 | install( 15 | DIRECTORY config launch/ 16 | DESTINATION share/${PROJECT_NAME} 17 | ) 18 | 19 | if(BUILD_TESTING) 20 | find_package(ament_lint_auto REQUIRED) 21 | # the following line skips the linter which checks for copyrights 22 | # comment the line when a copyright and license is added to all source files 23 | set(ament_cmake_copyright_FOUND TRUE) 24 | # the following line skips cpplint (only works in a git repo) 25 | # comment the line when this package is in a git repo and when 26 | # a copyright and license is added to all source files 27 | set(ament_cmake_cpplint_FOUND TRUE) 28 | ament_lint_auto_find_test_dependencies() 29 | endif() 30 | 31 | ament_package() 32 | -------------------------------------------------------------------------------- /controlko_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(controlko_description) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | # find_package( REQUIRED) 13 | 14 | install( 15 | DIRECTORY config launch/ meshes rviz urdf 16 | DESTINATION share/${PROJECT_NAME} 17 | ) 18 | 19 | if(BUILD_TESTING) 20 | find_package(ament_lint_auto REQUIRED) 21 | # the following line skips the linter which checks for copyrights 22 | # comment the line when a copyright and license is added to all source files 23 | set(ament_cmake_copyright_FOUND TRUE) 24 | # the following line skips cpplint (only works in a git repo) 25 | # comment the line when this package is in a git repo and when 26 | # a copyright and license is added to all source files 27 | set(ament_cmake_cpplint_FOUND TRUE) 28 | ament_lint_auto_find_test_dependencies() 29 | endif() 30 | 31 | ament_package() 32 | -------------------------------------------------------------------------------- /controlko_controllers/src/displacement_controller.yaml: -------------------------------------------------------------------------------- 1 | displacement_controller: 2 | joints: { 3 | type: string_array, 4 | default_value: [], 5 | description: "Specifies joints used by the controller. If state joints parameter is defined, then only command joints are defined with this parameter.", 6 | read_only: true, 7 | validation: { 8 | unique<>: null, 9 | not_empty<>: null, 10 | } 11 | } 12 | state_joints: { 13 | type: string_array, 14 | default_value: [], 15 | description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", 16 | read_only: true, 17 | validation: { 18 | unique<>: null, 19 | } 20 | } 21 | interface_name: { 22 | type: string, 23 | default_value: "", 24 | description: "Name of the interface used by the controller on joints and command_joints.", 25 | read_only: true, 26 | validation: { 27 | not_empty<>: null, 28 | one_of<>: [["position", "velocity", "acceleration", "effort",]], 29 | forbidden_interface_name_prefix: null 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /controlko_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | controlko_bringup 5 | 0.0.0 6 | Bringup package for the Controlko - demo robot for ros2_control @ ROSCon2022 7 | Denis Štogl 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | controlko_description 13 | controller_manager 14 | forward_command_controller 15 | gazebo_ros 16 | gazebo_ros2_control 17 | joint_state_broadcaster 18 | joint_trajectory_controller 19 | robot_state_publisher 20 | ros2_controllers_test_nodes 21 | rviz2 22 | xacro 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /controlko_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | controlko_controllers 5 | 0.0.0 6 | Package with controllers for the Controlko - demo robot for ros2_control @ ROSCon2022 7 | Denis Štogl 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | generate_parameter_library 13 | 14 | control_msgs 15 | 16 | controller_interface 17 | 18 | hardware_interface 19 | 20 | pluginlib 21 | 22 | rclcpp 23 | 24 | rclcpp_lifecycle 25 | 26 | realtime_tools 27 | 28 | std_srvs 29 | 30 | ament_lint_auto 31 | ament_cmake_gmock 32 | controller_manager 33 | hardware_interface 34 | ros2_control_test_assets 35 | 36 | 37 | ament_cmake 38 | 39 | 40 | -------------------------------------------------------------------------------- /controlko_description/urdf/common.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 9 | 11 | 18 | 19 | 20 | 21 | 22 | 23 | 1 0 0 1 24 | 1 0 0 1 25 | 0.1 0.1 0.1 1 26 | 0 0 0 0 27 | 28 | 29 | 30 | 31 | 1 0 0 1 32 | 1 0 0 1 33 | 0.1 0.1 0.1 1 34 | 0 0 0 0 35 | 36 | 37 | 38 | 39 | 1 0 0 1 40 | 1 0 0 1 41 | 0.1 0.1 0.1 1 42 | 0 0 0 0 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /controlko_bringup/config/rrbot_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | forward_position_controller: 9 | type: forward_command_controller/ForwardCommandController 10 | 11 | joint_trajectory_controller: 12 | type: joint_trajectory_controller/JointTrajectoryController 13 | 14 | displacement_controller: 15 | type: displacement_controller/DisplacementController 16 | 17 | 18 | forward_position_controller: 19 | ros__parameters: 20 | joints: 21 | - joint1 22 | - joint2 23 | interface_name: position 24 | 25 | 26 | joint_trajectory_controller: 27 | ros__parameters: 28 | joints: 29 | - joint1 30 | - joint2 31 | 32 | command_interfaces: 33 | - position 34 | 35 | state_interfaces: 36 | - position 37 | - velocity 38 | 39 | state_publish_rate: 50.0 # Defaults to 50 40 | action_monitor_rate: 20.0 # Defaults to 20 41 | 42 | allow_partial_joints_goal: false # Defaults to false 43 | constraints: 44 | stopped_velocity_tolerance: 0.01 # Defaults to 0.01 45 | goal_time: 0.0 # Defaults to 0.0 (start immediately) 46 | 47 | 48 | displacement_controller: 49 | ros__parameters: 50 | joints: 51 | - joint1 52 | - joint2 53 | 54 | interface_name: position 55 | 56 | command_interfaces: 57 | - position 58 | 59 | state_interfaces: 60 | - position 61 | - velocity 62 | -------------------------------------------------------------------------------- /controlko_controllers/include/controlko_controllers/validate_displacement_controller_parameters.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef CONTROLKO_CONTROLLERS__VALIDATE_DISPLACEMENT_CONTROLLER_PARAMETERS_HPP_ 17 | #define CONTROLKO_CONTROLLERS__VALIDATE_DISPLACEMENT_CONTROLLER_PARAMETERS_HPP_ 18 | 19 | #include 20 | 21 | #include "parameter_traits/parameter_traits.hpp" 22 | 23 | namespace parameter_traits 24 | { 25 | Result forbidden_interface_name_prefix(rclcpp::Parameter const & parameter) 26 | { 27 | auto const & interface_name = parameter.as_string(); 28 | 29 | if (interface_name.rfind("blup_", 0) == 0) { 30 | return ERROR("'interface_name' parameter can not start with 'blup_'"); 31 | } 32 | 33 | return OK; 34 | } 35 | 36 | } // namespace parameter_traits 37 | 38 | #endif // CONTROLKO_CONTROLLERS__VALIDATE_DISPLACEMENT_CONTROLLER_PARAMETERS_HPP_ 39 | -------------------------------------------------------------------------------- /controlko_controllers/test/test_load_displacement_controller.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | #include 18 | 19 | #include "controller_manager/controller_manager.hpp" 20 | #include "hardware_interface/resource_manager.hpp" 21 | #include "rclcpp/executor.hpp" 22 | #include "rclcpp/executors/single_threaded_executor.hpp" 23 | #include "rclcpp/utilities.hpp" 24 | #include "ros2_control_test_assets/descriptions.hpp" 25 | 26 | TEST(TestLoadDisplacementController, load_controller) 27 | { 28 | rclcpp::init(0, nullptr); 29 | 30 | std::shared_ptr executor = 31 | std::make_shared(); 32 | 33 | controller_manager::ControllerManager cm( 34 | std::make_unique( 35 | ros2_control_test_assets::minimal_robot_urdf), 36 | executor, "test_controller_manager"); 37 | 38 | ASSERT_NO_THROW( 39 | cm.load_controller("test_controlko_controllers", "controlko_controllers/DisplacementController")); 40 | 41 | rclcpp::shutdown(); 42 | } 43 | -------------------------------------------------------------------------------- /controlko_description/urdf/rrbot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | $(arg simulation_controllers) 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | $(arg simulation_controllers) 51 | $(arg prefix)controller_manager 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /controlko_hardware_interface/test/test_rrbot_hardware_interface.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | 18 | #include 19 | 20 | #include "hardware_interface/resource_manager.hpp" 21 | #include "ros2_control_test_assets/components_urdfs.hpp" 22 | #include "ros2_control_test_assets/descriptions.hpp" 23 | 24 | class TestRRBotHardwareInterface : public ::testing::Test 25 | { 26 | protected: 27 | void SetUp() override 28 | { 29 | // TODO(anyone): Extend this description to your robot 30 | rrbot_hardware_interface_2dof_ = 31 | R"( 32 | 33 | 34 | controlko_hardware_interface/RRBotHardwareInterface 35 | 36 | 37 | 38 | 39 | 1.57 40 | 41 | 42 | 43 | 44 | 0.7854 45 | 46 | 47 | )"; 48 | } 49 | 50 | std::string rrbot_hardware_interface_2dof_; 51 | }; 52 | 53 | TEST_F(TestRRBotHardwareInterface, load_rrbot_hardware_interface_2dof) 54 | { 55 | auto urdf = ros2_control_test_assets::urdf_head + rrbot_hardware_interface_2dof_ + 56 | ros2_control_test_assets::urdf_tail; 57 | ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); 58 | } 59 | -------------------------------------------------------------------------------- /controlko_controllers/include/controlko_controllers/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef CONTROLKO_CONTROLLERS__VISIBILITY_CONTROL_H_ 17 | #define CONTROLKO_CONTROLLERS__VISIBILITY_CONTROL_H_ 18 | 19 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 20 | // https://gcc.gnu.org/wiki/Visibility 21 | 22 | #if defined _WIN32 || defined __CYGWIN__ 23 | #ifdef __GNUC__ 24 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) 25 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) 26 | #else 27 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) 28 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) 29 | #endif 30 | #ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL 31 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT 32 | #else 33 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT 34 | #endif 35 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 36 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL 37 | #else 38 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) 39 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT 40 | #if __GNUC__ >= 4 41 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) 42 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) 43 | #else 44 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 45 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL 46 | #endif 47 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE 48 | #endif 49 | 50 | #endif // CONTROLKO_CONTROLLERS__VISIBILITY_CONTROL_H_ 51 | -------------------------------------------------------------------------------- /controlko_hardware_interface/include/controlko_hardware_interface/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef CONTROLKO_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_ 17 | #define CONTROLKO_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_ 18 | 19 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 20 | // https://gcc.gnu.org/wiki/Visibility 21 | 22 | #if defined _WIN32 || defined __CYGWIN__ 23 | #ifdef __GNUC__ 24 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) 25 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) 26 | #else 27 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) 28 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) 29 | #endif 30 | #ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL 31 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT 32 | #else 33 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT 34 | #endif 35 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 36 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL 37 | #else 38 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) 39 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT 40 | #if __GNUC__ >= 4 41 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) 42 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) 43 | #else 44 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 45 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL 46 | #endif 47 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE 48 | #endif 49 | 50 | #endif // CONTROLKO_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_ 51 | -------------------------------------------------------------------------------- /controlko_hardware_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(controlko_hardware_interface) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(hardware_interface REQUIRED) 11 | find_package(pluginlib REQUIRED) 12 | find_package(rclcpp REQUIRED) 13 | find_package(rclcpp_lifecycle REQUIRED) 14 | 15 | add_library( 16 | controlko_hardware_interface 17 | SHARED 18 | src/rrbot_hardware_interface.cpp 19 | ) 20 | target_include_directories( 21 | controlko_hardware_interface 22 | PUBLIC 23 | include 24 | ) 25 | ament_target_dependencies( 26 | controlko_hardware_interface 27 | hardware_interface 28 | rclcpp 29 | rclcpp_lifecycle 30 | ) 31 | # prevent pluginlib from using boost 32 | target_compile_definitions(controlko_hardware_interface PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 33 | 34 | pluginlib_export_plugin_description_file( 35 | hardware_interface controlko_hardware_interface.xml) 36 | 37 | install( 38 | TARGETS 39 | controlko_hardware_interface 40 | RUNTIME DESTINATION bin 41 | ARCHIVE DESTINATION lib 42 | LIBRARY DESTINATION lib 43 | ) 44 | 45 | install( 46 | DIRECTORY include/ 47 | DESTINATION include 48 | ) 49 | 50 | if(BUILD_TESTING) 51 | find_package(ament_lint_auto REQUIRED) 52 | # the following line skips the linter which checks for copyrights 53 | # comment the line when a copyright and license is added to all source files 54 | set(ament_cmake_copyright_FOUND TRUE) 55 | # the following line skips cpplint (only works in a git repo) 56 | # comment the line when this package is in a git repo and when 57 | # a copyright and license is added to all source files 58 | set(ament_cmake_cpplint_FOUND TRUE) 59 | ament_lint_auto_find_test_dependencies() 60 | find_package(ament_cmake_gmock REQUIRED) 61 | find_package(ros2_control_test_assets REQUIRED) 62 | 63 | ament_add_gmock(test_rrbot_hardware_interface test/test_rrbot_hardware_interface.cpp) 64 | target_include_directories(test_rrbot_hardware_interface PRIVATE include) 65 | ament_target_dependencies( 66 | test_rrbot_hardware_interface 67 | hardware_interface 68 | pluginlib 69 | ros2_control_test_assets 70 | ) 71 | endif() 72 | 73 | ament_export_include_directories( 74 | include 75 | ) 76 | ament_export_libraries( 77 | controlko_hardware_interface 78 | ) 79 | ament_export_dependencies( 80 | hardware_interface 81 | pluginlib 82 | rclcpp 83 | rclcpp_lifecycle 84 | ) 85 | 86 | ament_package() 87 | -------------------------------------------------------------------------------- /controlko_controllers/test/test_displacement_controller_preceeding.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "test_displacement_controller.hpp" 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | using controlko_controllers::CMD_MY_ITFS; 25 | using controlko_controllers::control_mode_type; 26 | using controlko_controllers::STATE_MY_ITFS; 27 | 28 | class DisplacementControllerTest : public DisplacementControllerFixture 29 | { 30 | }; 31 | 32 | TEST_F(DisplacementControllerTest, all_parameters_set_configure_success) 33 | { 34 | SetUpController(); 35 | 36 | ASSERT_TRUE(controller_->params_.joints.empty()); 37 | ASSERT_TRUE(controller_->params_.state_joints.empty()); 38 | ASSERT_TRUE(controller_->params_.interface_name.empty()); 39 | 40 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); 41 | 42 | ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_)); 43 | ASSERT_THAT(controller_->params_.state_joints, testing::ElementsAreArray(state_joint_names_)); 44 | ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(state_joint_names_)); 45 | ASSERT_EQ(controller_->params_.interface_name, interface_name_); 46 | } 47 | 48 | TEST_F(DisplacementControllerTest, check_exported_intefaces) 49 | { 50 | SetUpController(); 51 | 52 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); 53 | 54 | auto command_intefaces = controller_->command_interface_configuration(); 55 | ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); 56 | for (size_t i = 0; i < command_intefaces.names.size(); ++i) { 57 | EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); 58 | } 59 | 60 | auto state_intefaces = controller_->state_interface_configuration(); 61 | ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); 62 | for (size_t i = 0; i < state_intefaces.names.size(); ++i) { 63 | EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); 64 | } 65 | } 66 | 67 | int main(int argc, char ** argv) 68 | { 69 | ::testing::InitGoogleTest(&argc, argv); 70 | rclcpp::init(argc, argv); 71 | int result = RUN_ALL_TESTS(); 72 | rclcpp::shutdown(); 73 | return result; 74 | } 75 | -------------------------------------------------------------------------------- /controlko_description/urdf/rrbot/rrbot_macro.ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 12 | 13 | 14 | 15 | mock_components/GenericSystem 16 | ${mock_sensor_commands} 17 | 18 | 19 | gazebo_ros2_control/GazeboSystem 20 | 21 | 22 | ign_ros2_control/IgnitionSystem 23 | 24 | 25 | controlko_hardware_interface/RRBotHardwareInterface 26 | 27 | 28 | 29 | 30 | 31 | 32 | 0.0 33 | 34 | 35 | 0.0 36 | 37 | 38 | 0.0 39 | 40 | 41 | 42 | 43 | 44 | 45 | 0.0 46 | 47 | 48 | 0.0 49 | 50 | 51 | 0.0 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | tool0 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /controlko_description/launch/view_rrbot.launch.py: -------------------------------------------------------------------------------- 1 | # $LICENSE$ 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction 5 | from launch.event_handlers import OnProcessStart 6 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 7 | from launch_ros.actions import Node 8 | from launch_ros.substitutions import FindPackageShare 9 | 10 | 11 | def generate_launch_description(): 12 | # Declare arguments 13 | declared_arguments = [] 14 | declared_arguments.append( 15 | DeclareLaunchArgument( 16 | "description_package", 17 | default_value="controlko_description", 18 | description="Description package of the rrbot. Usually the argument is not set, \ 19 | it enables use of a custom description.", 20 | ) 21 | ) 22 | declared_arguments.append( 23 | DeclareLaunchArgument( 24 | "prefix", 25 | default_value='""', 26 | description="Prefix of the joint names, useful for \ 27 | multi-robot setup. If changed than also joint names in the controllers' configuration \ 28 | have to be updated.", 29 | ) 30 | ) 31 | 32 | # Initialize Arguments 33 | description_package = LaunchConfiguration("description_package") 34 | prefix = LaunchConfiguration("prefix") 35 | 36 | # Get URDF via xacro 37 | robot_description_content = Command( 38 | [ 39 | PathJoinSubstitution([FindExecutable(name="xacro")]), 40 | " ", 41 | PathJoinSubstitution( 42 | [FindPackageShare(description_package), "urdf", "rrbot.urdf.xacro"] 43 | ), 44 | " ", 45 | "prefix:=", 46 | prefix, 47 | " ", 48 | ] 49 | ) 50 | 51 | robot_description = {"robot_description": robot_description_content} 52 | 53 | rviz_config_file = PathJoinSubstitution( 54 | [FindPackageShare(description_package), "rviz", "rrbot.rviz"] 55 | ) 56 | 57 | joint_state_publisher_node = Node( 58 | package="joint_state_publisher_gui", 59 | executable="joint_state_publisher_gui", 60 | ) 61 | robot_state_publisher_node = Node( 62 | package="robot_state_publisher", 63 | executable="robot_state_publisher", 64 | output="both", 65 | parameters=[robot_description], 66 | ) 67 | rviz_node = Node( 68 | package="rviz2", 69 | executable="rviz2", 70 | name="rviz2", 71 | output="log", 72 | arguments=["-d", rviz_config_file], 73 | ) 74 | 75 | delay_rviz_after_joint_state_publisher_node = RegisterEventHandler( 76 | event_handler=OnProcessStart( 77 | target_action=joint_state_publisher_node, 78 | on_start=[ 79 | TimerAction( 80 | period=2.0, 81 | actions=[rviz_node], 82 | ), 83 | ], 84 | ) 85 | ) 86 | 87 | return LaunchDescription( 88 | declared_arguments 89 | + [ 90 | joint_state_publisher_node, 91 | robot_state_publisher_node, 92 | delay_rviz_after_joint_state_publisher_node, 93 | ] 94 | ) 95 | -------------------------------------------------------------------------------- /controlko_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(controlko_controllers) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | set(THIS_PACKAGE_INCLUDE_DEPENDS 10 | control_msgs 11 | controller_interface 12 | hardware_interface 13 | pluginlib 14 | rclcpp 15 | rclcpp_lifecycle 16 | realtime_tools 17 | std_srvs 18 | ) 19 | 20 | find_package(ament_cmake REQUIRED) 21 | find_package(generate_parameter_library REQUIRED) 22 | foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) 23 | find_package(${Dependency} REQUIRED) 24 | endforeach() 25 | 26 | cmake_minimum_required(VERSION 3.8) 27 | project(controlko_controllers) 28 | 29 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 30 | add_compile_options(-Wall -Wextra -Wpedantic) 31 | endif() 32 | 33 | # find dependencies 34 | find_package(ament_cmake REQUIRED) 35 | 36 | # Add displacement_controller library related compile commands 37 | generate_parameter_library(displacement_controller_parameters 38 | src/displacement_controller.yaml 39 | include/controlko_controllers/validate_displacement_controller_parameters.hpp 40 | ) 41 | add_library( 42 | displacement_controller 43 | SHARED 44 | src/displacement_controller.cpp 45 | ) 46 | target_include_directories(displacement_controller PRIVATE include) 47 | target_link_libraries(displacement_controller displacement_controller_parameters) 48 | ament_target_dependencies(displacement_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) 49 | target_compile_definitions(displacement_controller PRIVATE "DISPLACEMENT_CONTROLLER_BUILDING_DLL") 50 | 51 | pluginlib_export_plugin_description_file( 52 | controller_interface controlko_controllers.xml) 53 | 54 | install( 55 | TARGETS 56 | displacement_controller 57 | RUNTIME DESTINATION bin 58 | ARCHIVE DESTINATION lib 59 | LIBRARY DESTINATION lib 60 | ) 61 | 62 | install( 63 | DIRECTORY include/ 64 | DESTINATION include 65 | ) 66 | 67 | if(BUILD_TESTING) 68 | find_package(ament_lint_auto REQUIRED) 69 | # the following line skips the linter which checks for copyrights 70 | # comment the line when a copyright and license is added to all source files 71 | set(ament_cmake_copyright_FOUND TRUE) 72 | # the following line skips cpplint (only works in a git repo) 73 | # comment the line when this package is in a git repo and when 74 | # a copyright and license is added to all source files 75 | set(ament_cmake_cpplint_FOUND TRUE) 76 | find_package(ament_cmake_gmock REQUIRED) 77 | find_package(controller_manager REQUIRED) 78 | find_package(hardware_interface REQUIRED) 79 | find_package(ros2_control_test_assets REQUIRED) 80 | 81 | ament_add_gmock(test_load_displacement_controller test/test_load_displacement_controller.cpp) 82 | target_include_directories(test_load_displacement_controller PRIVATE include) 83 | ament_target_dependencies( 84 | test_load_displacement_controller 85 | controller_manager 86 | hardware_interface 87 | ros2_control_test_assets 88 | ) 89 | 90 | add_rostest_with_parameters_gmock(test_displacement_controller test/test_displacement_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/displacement_controller_params.yaml) 91 | target_include_directories(test_displacement_controller PRIVATE include) 92 | target_link_libraries(test_displacement_controller displacement_controller) 93 | ament_target_dependencies( 94 | test_displacement_controller 95 | controller_interface 96 | hardware_interface 97 | ) 98 | 99 | add_rostest_with_parameters_gmock(test_displacement_controller_preceeding test/test_displacement_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/displacement_controller_preceeding_params.yaml) 100 | target_include_directories(test_displacement_controller_preceeding PRIVATE include) 101 | target_link_libraries(test_displacement_controller_preceeding displacement_controller) 102 | ament_target_dependencies( 103 | test_displacement_controller_preceeding 104 | controller_interface 105 | hardware_interface 106 | ) 107 | endif() 108 | 109 | ament_export_include_directories( 110 | include 111 | ) 112 | ament_export_dependencies( 113 | 114 | ) 115 | ament_export_libraries( 116 | displacement_controller 117 | ) 118 | 119 | ament_package() 120 | -------------------------------------------------------------------------------- /controlko_hardware_interface/include/controlko_hardware_interface/rrbot_hardware_interface.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CONTROLKO_HARDWARE_INTERFACE__RRBOT_HARDWARE_INTERFACE_HPP_ 16 | #define CONTROLKO_HARDWARE_INTERFACE__RRBOT_HARDWARE_INTERFACE_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "dr_denis_rrbot_comms.hpp" 23 | 24 | #include "controlko_hardware_interface/visibility_control.h" 25 | #include "hardware_interface/handle.hpp" 26 | #include "hardware_interface/hardware_info.hpp" 27 | #include "hardware_interface/system_interface.hpp" 28 | #include "hardware_interface/types/hardware_interface_return_values.hpp" 29 | #include "rclcpp/macros.hpp" 30 | #include "rclcpp_lifecycle/state.hpp" 31 | 32 | namespace controlko_hardware_interface 33 | { 34 | class RRBotHardwareInterface : public hardware_interface::SystemInterface 35 | { 36 | public: 37 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 38 | hardware_interface::CallbackReturn on_init( 39 | const hardware_interface::HardwareInfo & info) override; 40 | 41 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 42 | hardware_interface::CallbackReturn on_configure( 43 | const rclcpp_lifecycle::State & previous_state) override; 44 | 45 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 46 | std::vector export_state_interfaces() override; 47 | 48 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 49 | std::vector export_command_interfaces() override; 50 | 51 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 52 | hardware_interface::return_type prepare_command_mode_switch( 53 | const std::vector & start_interfaces, 54 | const std::vector & stop_interfaces) override; 55 | 56 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 57 | hardware_interface::return_type perform_command_mode_switch( 58 | const std::vector & start_interfaces, 59 | const std::vector & stop_interfaces) override; 60 | 61 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 62 | hardware_interface::CallbackReturn on_activate( 63 | const rclcpp_lifecycle::State & previous_state) override; 64 | 65 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 66 | hardware_interface::CallbackReturn on_deactivate( 67 | const rclcpp_lifecycle::State & previous_state) override; 68 | 69 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 70 | hardware_interface::CallbackReturn on_shutdown( 71 | const rclcpp_lifecycle::State & previous_state) override; 72 | 73 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 74 | hardware_interface::return_type read( 75 | const rclcpp::Time & time, const rclcpp::Duration & period) override; 76 | 77 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 78 | hardware_interface::return_type write( 79 | const rclcpp::Time & time, const rclcpp::Duration & period) override; 80 | 81 | private: 82 | std::vector joint_pos_commands_; 83 | std::vector joint_vel_commands_; 84 | std::vector joint_pos_states_; 85 | std::vector joint_vel_states_; 86 | std::vector joint_acc_states_; 87 | 88 | std::vector gpio_commands_; 89 | std::vector gpio_states_; 90 | std::vector gpio_ins_storage_; 91 | std::vector gpio_outs_storage_; 92 | std::vector gpio_cmds_storage_; 93 | std::vector sensor_states_; 94 | 95 | std::vector position_command_interface_names_; 96 | std::vector velocity_command_interface_names_; 97 | 98 | std::unique_ptr rrbot_comms_; 99 | }; 100 | 101 | } // namespace controlko_hardware_interface 102 | 103 | #endif // CONTROLKO_HARDWARE_INTERFACE__RRBOT_HARDWARE_INTERFACE_HPP_ 104 | -------------------------------------------------------------------------------- /controlko_controllers/include/controlko_controllers/displacement_controller.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CONTROLKO_CONTROLLERS__DISPLACEMENT_CONTROLLER_HPP_ 16 | #define CONTROLKO_CONTROLLERS__DISPLACEMENT_CONTROLLER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "controller_interface/controller_interface.hpp" 23 | #include "displacement_controller_parameters.hpp" 24 | #include "controlko_controllers/visibility_control.h" 25 | #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" 26 | #include "rclcpp_lifecycle/state.hpp" 27 | #include "realtime_tools/realtime_buffer.h" 28 | #include "realtime_tools/realtime_publisher.h" 29 | #include "std_srvs/srv/set_bool.hpp" 30 | 31 | // TODO(anyone): Replace with controller specific messages 32 | #include "control_msgs/msg/joint_controller_state.hpp" 33 | #include "control_msgs/msg/joint_jog.hpp" 34 | 35 | namespace controlko_controllers 36 | { 37 | // name constants for state interfaces 38 | static constexpr size_t STATE_MY_ITFS = 0; 39 | 40 | // name constants for command interfaces 41 | static constexpr size_t CMD_MY_ITFS = 0; 42 | 43 | // TODO(anyone: example setup for control mode (usually you will use some enums defined in messages) 44 | enum class control_mode_type : std::uint8_t { 45 | FAST = 0, 46 | SLOW = 1, 47 | }; 48 | 49 | class DisplacementController : public controller_interface::ControllerInterface 50 | { 51 | public: 52 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 53 | DisplacementController(); 54 | 55 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 56 | controller_interface::CallbackReturn on_init() override; 57 | 58 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 59 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 60 | 61 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 62 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 63 | 64 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 65 | controller_interface::CallbackReturn on_configure( 66 | const rclcpp_lifecycle::State & previous_state) override; 67 | 68 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 69 | controller_interface::CallbackReturn on_activate( 70 | const rclcpp_lifecycle::State & previous_state) override; 71 | 72 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 73 | controller_interface::CallbackReturn on_deactivate( 74 | const rclcpp_lifecycle::State & previous_state) override; 75 | 76 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 77 | controller_interface::return_type update( 78 | const rclcpp::Time & time, const rclcpp::Duration & period) override; 79 | 80 | // TODO(anyone): replace the state and command message types 81 | using ControllerReferenceMsg = control_msgs::msg::JointJog; 82 | using ControllerModeSrvType = std_srvs::srv::SetBool; 83 | using ControllerStateMsg = control_msgs::msg::JointControllerState; 84 | 85 | protected: 86 | std::shared_ptr param_listener_; 87 | displacement_controller::Params params_; 88 | 89 | std::vector state_joints_; 90 | 91 | // Command subscribers and Controller State publisher 92 | rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; 93 | realtime_tools::RealtimeBuffer> input_ref_; 94 | 95 | rclcpp::Service::SharedPtr set_slow_control_mode_service_; 96 | realtime_tools::RealtimeBuffer control_mode_; 97 | 98 | using ControllerStatePublisher = realtime_tools::RealtimePublisher; 99 | 100 | rclcpp::Publisher::SharedPtr s_publisher_; 101 | std::unique_ptr state_publisher_; 102 | 103 | private: 104 | // callback for topic interface 105 | TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL 106 | void reference_callback(const std::shared_ptr msg); 107 | }; 108 | 109 | } // namespace controlko_controllers 110 | 111 | #endif // CONTROLKO_CONTROLLERS__DISPLACEMENT_CONTROLLER_HPP_ 112 | -------------------------------------------------------------------------------- /controlko_description/urdf/rrbot/rrbot_macro.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | Gazebo/Orange 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | Gazebo/Black 52 | 53 | 54 | 55 | 56 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | Gazebo/Gray 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | -------------------------------------------------------------------------------- /controlko_hardware_interface/include/controlko_hardware_interface/dr_denis_rrbot_comms.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef CONTROLKO_HARDWARE_INTERFACE__DR_DENIS_RRBOT_COMMS_HPP_ 16 | #define CONTROLKO_HARDWARE_INTERFACE__DR_DENIS_RRBOT_COMMS_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | namespace dr_denis_rrbot_comms 23 | { 24 | 25 | enum class control_mode_type : std::uint8_t 26 | { 27 | POSITION = 0, 28 | VELOCITY = 1, 29 | }; 30 | 31 | static constexpr double MAX_VELOCITY = 0.5; 32 | static constexpr double SENSOR_MAX = 23.28; 33 | 34 | class DrDenisRRBotComms 35 | { 36 | public: 37 | DrDenisRRBotComms(const size_t number_of_dofs) 38 | : nr_dofs_(number_of_dofs), current_control_mode_(control_mode_type::POSITION), movement_enabled_(false) 39 | { 40 | joint_positions_.resize(nr_dofs_, 0.1); 41 | joint_velocities_.resize(nr_dofs_, 0.0); 42 | joint_accelerations_.resize(nr_dofs_, 0.0); 43 | digital_outs_.resize(nr_dofs_, false); 44 | } 45 | 46 | void init() 47 | { 48 | last_time_stamp_ = std::chrono::steady_clock::now(); 49 | movement_enabled_ = true; 50 | } 51 | 52 | void stop() 53 | { 54 | movement_enabled_ = false; 55 | } 56 | 57 | bool set_control_mode(const control_mode_type & control_mode) 58 | { 59 | current_control_mode_ = control_mode; 60 | 61 | return true; 62 | } 63 | 64 | control_mode_type get_control_mode() { return current_control_mode_; } 65 | 66 | bool write_joint_commands( 67 | const std::vector & position_command, const std::vector & velocity_command) 68 | { 69 | if (movement_enabled_) 70 | { 71 | const auto current_time = std::chrono::steady_clock::now(); 72 | const double delta_t = 73 | (std::chrono::duration_cast>(current_time - last_time_stamp_)) 74 | .count(); 75 | 76 | switch (current_control_mode_) 77 | { 78 | case control_mode_type::POSITION: 79 | for (size_t i = 0; i < nr_dofs_; ++i) 80 | { 81 | auto new_velocity = (position_command[i] - joint_positions_[i]) / delta_t; 82 | new_velocity = new_velocity > MAX_VELOCITY ? MAX_VELOCITY : new_velocity; 83 | new_velocity = new_velocity < -MAX_VELOCITY ? -MAX_VELOCITY : new_velocity; 84 | 85 | joint_positions_[i] += new_velocity * delta_t; 86 | 87 | joint_accelerations_[i] = (new_velocity - joint_velocities_[i]) / delta_t; 88 | joint_velocities_[i] = new_velocity; 89 | } 90 | break; 91 | 92 | case control_mode_type::VELOCITY: 93 | for (size_t i = 0; i < nr_dofs_; ++i) 94 | { 95 | auto new_velocity = velocity_command[i]; 96 | new_velocity = new_velocity > MAX_VELOCITY ? MAX_VELOCITY : new_velocity; 97 | new_velocity = new_velocity < MAX_VELOCITY ? -MAX_VELOCITY : new_velocity; 98 | 99 | joint_positions_[i] += new_velocity * delta_t; 100 | 101 | joint_accelerations_[i] = (new_velocity - joint_velocities_[i]) / delta_t; 102 | joint_velocities_[i] = new_velocity; 103 | } 104 | break; 105 | } 106 | } 107 | 108 | last_time_stamp_ = std::chrono::steady_clock::now(); 109 | 110 | return true; 111 | } 112 | 113 | bool read_joint_states( 114 | std::vector & position_state, std::vector & velocity_state, 115 | std::vector & acceleration_state) 116 | { 117 | position_state = joint_positions_; 118 | velocity_state = joint_velocities_; 119 | acceleration_state = joint_accelerations_; 120 | 121 | return true; 122 | } 123 | 124 | bool write_gpios(const std::vector & digital_outs) 125 | { 126 | digital_outs_ = digital_outs; 127 | return true; 128 | } 129 | 130 | bool read_gpio(std::vector & digital_ins, std::vector & digital_outs) 131 | { 132 | unsigned int seed = time(NULL); 133 | for (size_t i = 0; i < nr_dofs_; ++i) 134 | { 135 | digital_ins[i] = static_cast(rand_r(&seed) % 2); 136 | } 137 | 138 | digital_outs = digital_outs_; 139 | 140 | return true; 141 | } 142 | 143 | bool read_sensor_values(std::vector & sensor_values) 144 | { 145 | for (size_t i = 0; i < sensor_values.size(); ++i) 146 | { 147 | unsigned int seed = time(NULL) + i; 148 | sensor_values[i] = 149 | static_cast(rand_r(&seed)) / static_cast(RAND_MAX) * SENSOR_MAX; 150 | } 151 | 152 | return true; 153 | } 154 | 155 | private: 156 | size_t nr_dofs_; 157 | control_mode_type current_control_mode_; 158 | 159 | std::vector digital_outs_; 160 | 161 | std::vector joint_positions_; 162 | std::vector joint_velocities_; 163 | std::vector joint_accelerations_; 164 | 165 | std::chrono::steady_clock::time_point last_time_stamp_; 166 | 167 | bool movement_enabled_; 168 | }; 169 | } // namespace dr_denis_rrbot_comms 170 | 171 | #endif // CONTROLKO_HARDWARE_INTERFACE__DR_DENIS_RRBOT_COMMS_HPP_ 172 | -------------------------------------------------------------------------------- /controlko_bringup/launch/rrbot_sim_gazebo_classic.launch.py: -------------------------------------------------------------------------------- 1 | # $LICENSE$ 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import ( 5 | DeclareLaunchArgument, 6 | IncludeLaunchDescription, 7 | RegisterEventHandler, 8 | TimerAction, 9 | ) 10 | from launch.event_handlers import OnProcessExit, OnProcessStart 11 | from launch.launch_description_sources import PythonLaunchDescriptionSource 12 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 13 | from launch_ros.actions import Node 14 | from launch_ros.substitutions import FindPackageShare 15 | 16 | 17 | def generate_launch_description(): 18 | # Declare arguments 19 | declared_arguments = [] 20 | declared_arguments.append( 21 | DeclareLaunchArgument( 22 | "runtime_config_package", 23 | default_value="controlko_bringup", 24 | description='Package with the controller\'s configuration in "config" folder. \ 25 | Usually the argument is not set, it enables use of a custom setup.', 26 | ) 27 | ) 28 | declared_arguments.append( 29 | DeclareLaunchArgument( 30 | "controllers_file", 31 | default_value="rrbot_controllers.yaml", 32 | description="YAML file with the controllers configuration.", 33 | ) 34 | ) 35 | declared_arguments.append( 36 | DeclareLaunchArgument( 37 | "description_package", 38 | default_value="controlko_description", 39 | description="Description package with robot URDF/xacro files. Usually the argument \ 40 | is not set, it enables use of a custom description.", 41 | ) 42 | ) 43 | declared_arguments.append( 44 | DeclareLaunchArgument( 45 | "description_file", 46 | default_value="rrbot.urdf.xacro", 47 | description="URDF/XACRO description file with the robot.", 48 | ) 49 | ) 50 | declared_arguments.append( 51 | DeclareLaunchArgument( 52 | "prefix", 53 | default_value='""', 54 | description="Prefix of the joint names, useful for \ 55 | multi-robot setup. If changed than also joint names in the controllers' configuration \ 56 | have to be updated.", 57 | ) 58 | ) 59 | declared_arguments.append( 60 | DeclareLaunchArgument( 61 | "robot_controller", 62 | default_value="joint_trajectory_controller", 63 | choices=["forward_position_controller", "joint_trajectory_controller"], 64 | description="Robot controller to start.", 65 | ) 66 | ) 67 | 68 | # Initialize Arguments 69 | runtime_config_package = LaunchConfiguration("runtime_config_package") 70 | controllers_file = LaunchConfiguration("controllers_file") 71 | description_package = LaunchConfiguration("description_package") 72 | description_file = LaunchConfiguration("description_file") 73 | prefix = LaunchConfiguration("prefix") 74 | use_mock_hardware = LaunchConfiguration("use_mock_hardware") 75 | mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") 76 | robot_controller = LaunchConfiguration("robot_controller") 77 | 78 | 79 | robot_controllers = PathJoinSubstitution( 80 | [FindPackageShare(runtime_config_package), "config", controllers_file] 81 | ) 82 | rviz_config_file = PathJoinSubstitution( 83 | [FindPackageShare(description_package), "rviz", "rrbot.rviz"] 84 | ) 85 | 86 | # Get URDF via xacro 87 | robot_description_content = Command( 88 | [ 89 | PathJoinSubstitution([FindExecutable(name="xacro")]), 90 | " ", 91 | PathJoinSubstitution( 92 | [FindPackageShare(description_package), "urdf", description_file] 93 | ), 94 | " ", 95 | "prefix:=", 96 | prefix, 97 | " ", 98 | "use_mock_hardware:=false", 99 | " ", 100 | "mock_sensor_commands:=false", 101 | " ", 102 | "sim_gazebo_classic:=true", 103 | " ", 104 | "sim_gazebo:=false", 105 | " ", 106 | "simulation_controllers:=", 107 | robot_controllers, 108 | " ", 109 | ] 110 | ) 111 | robot_description = {"robot_description": robot_description_content} 112 | 113 | robot_state_pub_node = Node( 114 | package="robot_state_publisher", 115 | executable="robot_state_publisher", 116 | output="both", 117 | parameters=[robot_description], 118 | ) 119 | rviz_node = Node( 120 | package="rviz2", 121 | executable="rviz2", 122 | name="rviz2", 123 | output="log", 124 | arguments=["-d", rviz_config_file], 125 | ) 126 | 127 | # Gazebo nodes 128 | gazebo = IncludeLaunchDescription( 129 | PythonLaunchDescriptionSource( 130 | [FindPackageShare("gazebo_ros"), "/launch", "/gazebo.launch.py"] 131 | ), 132 | ) 133 | 134 | # Spawn robot 135 | gazebo_spawn_robot = Node( 136 | package="gazebo_ros", 137 | executable="spawn_entity.py", 138 | name="spawn_rrbot", 139 | arguments=["-entity", "rrbot", "-topic", "robot_description"], 140 | output="screen", 141 | ) 142 | 143 | joint_state_broadcaster_spawner = Node( 144 | package="controller_manager", 145 | executable="spawner", 146 | arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], 147 | ) 148 | 149 | robot_controllers = [robot_controller] 150 | robot_controller_spawners = [] 151 | for controller in robot_controllers: 152 | robot_controller_spawners += [ 153 | Node( 154 | package="controller_manager", 155 | executable="spawner", 156 | arguments=[controller, "-c", "/controller_manager"], 157 | ) 158 | ] 159 | 160 | # Delay loading and activation of `joint_state_broadcaster` after start of ros2_control_node 161 | delay_joint_state_broadcaster_spawner_after_gazebo_spawn_robot = RegisterEventHandler( 162 | event_handler=OnProcessStart( 163 | target_action=gazebo_spawn_robot, 164 | on_start=[joint_state_broadcaster_spawner], 165 | ) 166 | ) 167 | 168 | # Delay rviz start after Joint State Broadcaster to avoid unnecessary warning output. 169 | delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( 170 | event_handler=OnProcessExit( 171 | target_action=joint_state_broadcaster_spawner, 172 | on_exit=[rviz_node], 173 | ) 174 | ) 175 | 176 | # Delay loading and activation of robot_controller after `joint_state_broadcaster` 177 | delay_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] 178 | for controller in robot_controller_spawners: 179 | delay_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ 180 | RegisterEventHandler( 181 | event_handler=OnProcessExit( 182 | target_action=joint_state_broadcaster_spawner, 183 | on_exit=[ 184 | TimerAction( 185 | period=3.0, 186 | actions=[controller], 187 | ), 188 | ], 189 | ) 190 | ) 191 | ] 192 | 193 | return LaunchDescription( 194 | declared_arguments 195 | + [ 196 | gazebo, 197 | gazebo_spawn_robot, 198 | robot_state_pub_node, 199 | delay_rviz_after_joint_state_broadcaster_spawner, 200 | delay_joint_state_broadcaster_spawner_after_gazebo_spawn_robot, 201 | ] 202 | + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner 203 | ) 204 | -------------------------------------------------------------------------------- /controlko_bringup/launch/rrbot_sim_gazebo.launch.py: -------------------------------------------------------------------------------- 1 | # $LICENSE$ 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import ( 5 | DeclareLaunchArgument, 6 | IncludeLaunchDescription, 7 | RegisterEventHandler, 8 | TimerAction, 9 | ) 10 | from launch.event_handlers import OnProcessExit, OnProcessStart 11 | from launch.launch_description_sources import PythonLaunchDescriptionSource 12 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 13 | from launch_ros.actions import Node 14 | from launch_ros.substitutions import FindPackageShare 15 | 16 | 17 | def generate_launch_description(): 18 | # Declare arguments 19 | declared_arguments = [] 20 | declared_arguments.append( 21 | DeclareLaunchArgument( 22 | "runtime_config_package", 23 | default_value="controlko_bringup", 24 | description='Package with the controller\'s configuration in "config" folder. \ 25 | Usually the argument is not set, it enables use of a custom setup.', 26 | ) 27 | ) 28 | declared_arguments.append( 29 | DeclareLaunchArgument( 30 | "controllers_file", 31 | default_value="rrbot_controllers.yaml", 32 | description="YAML file with the controllers configuration.", 33 | ) 34 | ) 35 | declared_arguments.append( 36 | DeclareLaunchArgument( 37 | "description_package", 38 | default_value="controlko_description", 39 | description="Description package with robot URDF/xacro files. Usually the argument \ 40 | is not set, it enables use of a custom description.", 41 | ) 42 | ) 43 | declared_arguments.append( 44 | DeclareLaunchArgument( 45 | "description_file", 46 | default_value="rrbot.urdf.xacro", 47 | description="URDF/XACRO description file with the robot.", 48 | ) 49 | ) 50 | declared_arguments.append( 51 | DeclareLaunchArgument( 52 | "prefix", 53 | default_value='""', 54 | description="Prefix of the joint names, useful for \ 55 | multi-robot setup. If changed than also joint names in the controllers' configuration \ 56 | have to be updated.", 57 | ) 58 | ) 59 | declared_arguments.append( 60 | DeclareLaunchArgument( 61 | "robot_controller", 62 | default_value="joint_trajectory_controller", 63 | choices=["forward_position_controller", "joint_trajectory_controller"], 64 | description="Robot controller to start.", 65 | ) 66 | ) 67 | 68 | # Initialize Arguments 69 | runtime_config_package = LaunchConfiguration("runtime_config_package") 70 | controllers_file = LaunchConfiguration("controllers_file") 71 | description_package = LaunchConfiguration("description_package") 72 | description_file = LaunchConfiguration("description_file") 73 | prefix = LaunchConfiguration("prefix") 74 | use_mock_hardware = LaunchConfiguration("use_mock_hardware") 75 | mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") 76 | robot_controller = LaunchConfiguration("robot_controller") 77 | 78 | 79 | robot_controllers = PathJoinSubstitution( 80 | [FindPackageShare(runtime_config_package), "config", controllers_file] 81 | ) 82 | rviz_config_file = PathJoinSubstitution( 83 | [FindPackageShare(description_package), "rviz", "rrbot.rviz"] 84 | ) 85 | 86 | # Get URDF via xacro 87 | robot_description_content = Command( 88 | [ 89 | PathJoinSubstitution([FindExecutable(name="xacro")]), 90 | " ", 91 | PathJoinSubstitution( 92 | [FindPackageShare(description_package), "urdf", description_file] 93 | ), 94 | " ", 95 | "prefix:=", 96 | prefix, 97 | " ", 98 | "use_mock_hardware:=false", 99 | " ", 100 | "mock_sensor_commands:=false", 101 | " ", 102 | "sim_gazebo_classic:=false", 103 | " ", 104 | "sim_gazebo:=true", 105 | " ", 106 | "simulation_controllers:=", 107 | robot_controllers, 108 | " ", 109 | ] 110 | ) 111 | robot_description = {"robot_description": robot_description_content} 112 | 113 | robot_state_pub_node = Node( 114 | package="robot_state_publisher", 115 | executable="robot_state_publisher", 116 | output="both", 117 | parameters=[robot_description], 118 | ) 119 | rviz_node = Node( 120 | package="rviz2", 121 | executable="rviz2", 122 | name="rviz2", 123 | output="log", 124 | arguments=["-d", rviz_config_file], 125 | ) 126 | 127 | # Gazebo nodes 128 | gazebo = IncludeLaunchDescription( 129 | PythonLaunchDescriptionSource( 130 | [FindPackageShare("ros_ign_gazebo"), "/launch", "/ign_gazebo.launch.py"] 131 | ), 132 | launch_arguments={"ign_args": " -r -v 3 empty.sdf"}.items(), 133 | ) 134 | 135 | # Spawn robot 136 | gazebo_spawn_robot = Node( 137 | package="ros_ign_gazebo", 138 | executable="create", 139 | name="spawn_rrbot", 140 | arguments=["-name", "rrbot", "-topic", "robot_description"], 141 | output="screen", 142 | ) 143 | 144 | joint_state_broadcaster_spawner = Node( 145 | package="controller_manager", 146 | executable="spawner", 147 | arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], 148 | ) 149 | 150 | robot_controllers = [robot_controller] 151 | robot_controller_spawners = [] 152 | for controller in robot_controllers: 153 | robot_controller_spawners += [ 154 | Node( 155 | package="controller_manager", 156 | executable="spawner", 157 | arguments=[controller, "-c", "/controller_manager"], 158 | ) 159 | ] 160 | 161 | # Delay loading and activation of `joint_state_broadcaster` after start of ros2_control_node 162 | delay_joint_state_broadcaster_spawner_after_gazebo_spawn_robot = RegisterEventHandler( 163 | event_handler=OnProcessExit( 164 | target_action=gazebo_spawn_robot, 165 | on_exit=[joint_state_broadcaster_spawner], 166 | ) 167 | ) 168 | 169 | # Delay rviz start after Joint State Broadcaster to avoid unnecessary warning output. 170 | delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( 171 | event_handler=OnProcessExit( 172 | target_action=joint_state_broadcaster_spawner, 173 | on_exit=[rviz_node], 174 | ) 175 | ) 176 | 177 | # Delay loading and activation of robot_controller after `joint_state_broadcaster` 178 | delay_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] 179 | for controller in robot_controller_spawners: 180 | delay_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ 181 | RegisterEventHandler( 182 | event_handler=OnProcessExit( 183 | target_action=joint_state_broadcaster_spawner, 184 | on_exit=[ 185 | TimerAction( 186 | period=3.0, 187 | actions=[controller], 188 | ), 189 | ], 190 | ) 191 | ) 192 | ] 193 | 194 | return LaunchDescription( 195 | declared_arguments 196 | + [ 197 | gazebo, 198 | gazebo_spawn_robot, 199 | robot_state_pub_node, 200 | delay_rviz_after_joint_state_broadcaster_spawner, 201 | delay_joint_state_broadcaster_spawner_after_gazebo_spawn_robot, 202 | ] 203 | + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner 204 | ) 205 | -------------------------------------------------------------------------------- /controlko_bringup/launch/soho-12.launch.py: -------------------------------------------------------------------------------- 1 | # $LICENSE$ 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import ( 5 | DeclareLaunchArgument, 6 | RegisterEventHandler, 7 | TimerAction, 8 | ) 9 | from launch.event_handlers import OnProcessExit, OnProcessStart 10 | from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution 11 | from launch_ros.actions import Node 12 | from launch_ros.substitutions import FindPackageShare 13 | 14 | 15 | def generate_launch_description(): 16 | # Declare arguments 17 | declared_arguments = [] 18 | declared_arguments.append( 19 | DeclareLaunchArgument( 20 | "runtime_config_package", 21 | default_value="controlko_bringup", 22 | description='Package with the controller\'s configuration in "config" folder. \ 23 | Usually the argument is not set, it enables use of a custom setup.', 24 | ) 25 | ) 26 | declared_arguments.append( 27 | DeclareLaunchArgument( 28 | "controllers_file", 29 | default_value="soho-12_controllers.yaml", 30 | description="YAML file with the controllers configuration.", 31 | ) 32 | ) 33 | declared_arguments.append( 34 | DeclareLaunchArgument( 35 | "description_package", 36 | default_value="controlko_description", 37 | description="Description package with robot URDF/xacro files. Usually the argument \ 38 | is not set, it enables use of a custom description.", 39 | ) 40 | ) 41 | declared_arguments.append( 42 | DeclareLaunchArgument( 43 | "description_file", 44 | default_value="rrbot.urdf.xacro", 45 | description="URDF/XACRO description file with the robot.", 46 | ) 47 | ) 48 | declared_arguments.append( 49 | DeclareLaunchArgument( 50 | "prefix", 51 | default_value='""', 52 | description="Prefix of the joint names, useful for \ 53 | multi-robot setup. If changed than also joint names in the controllers' configuration \ 54 | have to be updated.", 55 | ) 56 | ) 57 | declared_arguments.append( 58 | DeclareLaunchArgument( 59 | "use_mock_hardware", 60 | default_value="true", 61 | description="Start robot with fake hardware mirroring command to its states.", 62 | ) 63 | ) 64 | declared_arguments.append( 65 | DeclareLaunchArgument( 66 | "mock_sensor_commands", 67 | default_value="false", 68 | description="Enable fake command interfaces for sensors used for simple simulations. \ 69 | Used only if 'use_mock_hardware' parameter is true.", 70 | ) 71 | ) 72 | declared_arguments.append( 73 | DeclareLaunchArgument( 74 | "robot_controller", 75 | default_value="displacement_controller", 76 | choices=["forward_position_controller", "joint_trajectory_controller", "displacement_controller"], 77 | description="Robot controller to start.", 78 | ) 79 | ) 80 | 81 | # Initialize Arguments 82 | runtime_config_package = LaunchConfiguration("runtime_config_package") 83 | controllers_file = LaunchConfiguration("controllers_file") 84 | description_package = LaunchConfiguration("description_package") 85 | description_file = LaunchConfiguration("description_file") 86 | prefix = LaunchConfiguration("prefix") 87 | use_mock_hardware = LaunchConfiguration("use_mock_hardware") 88 | mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") 89 | robot_controller = LaunchConfiguration("robot_controller") 90 | 91 | # Get URDF via xacro 92 | robot_description_content = Command( 93 | [ 94 | PathJoinSubstitution([FindExecutable(name="xacro")]), 95 | " ", 96 | PathJoinSubstitution( 97 | [FindPackageShare(description_package), "urdf", description_file] 98 | ), 99 | " ", 100 | "prefix:=", 101 | prefix, 102 | " ", 103 | "use_mock_hardware:=", 104 | use_mock_hardware, 105 | " ", 106 | "mock_sensor_commands:=", 107 | mock_sensor_commands, 108 | " ", 109 | ] 110 | ) 111 | 112 | robot_description = {"robot_description": robot_description_content} 113 | 114 | robot_controllers = PathJoinSubstitution( 115 | [FindPackageShare(runtime_config_package), "config", controllers_file] 116 | ) 117 | rviz_config_file = PathJoinSubstitution( 118 | [FindPackageShare(description_package), "rviz", "rrbot.rviz"] 119 | ) 120 | 121 | control_node = Node( 122 | package="controller_manager", 123 | executable="ros2_control_node", 124 | output="both", 125 | parameters=[robot_description, robot_controllers], 126 | ) 127 | robot_state_pub_node = Node( 128 | package="robot_state_publisher", 129 | executable="robot_state_publisher", 130 | output="both", 131 | parameters=[robot_description], 132 | ) 133 | rviz_node = Node( 134 | package="rviz2", 135 | executable="rviz2", 136 | name="rviz2", 137 | output="log", 138 | arguments=["-d", rviz_config_file], 139 | ) 140 | 141 | joint_state_broadcaster_spawner = Node( 142 | package="controller_manager", 143 | executable="spawner", 144 | arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], 145 | ) 146 | 147 | robot_controllers = [robot_controller] 148 | robot_controller_spawners = [] 149 | for controller in robot_controllers: 150 | robot_controller_spawners += [ 151 | Node( 152 | package="controller_manager", 153 | executable="spawner", 154 | arguments=[controller, "-c", "/controller_manager"], 155 | ) 156 | ] 157 | 158 | # Delay loading and activation of `joint_state_broadcaster` after start of ros2_control_node 159 | delay_joint_state_broadcaster_spawner_after_ros2_control_node = RegisterEventHandler( 160 | event_handler=OnProcessStart( 161 | target_action=control_node, 162 | on_start=[ 163 | TimerAction( 164 | period=1.0, 165 | actions=[joint_state_broadcaster_spawner], 166 | ), 167 | ], 168 | ) 169 | ) 170 | 171 | # Delay rviz start after Joint State Broadcaster to avoid unnecessary warning output. 172 | delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( 173 | event_handler=OnProcessExit( 174 | target_action=joint_state_broadcaster_spawner, 175 | on_exit=[rviz_node], 176 | ) 177 | ) 178 | 179 | # Delay loading and activation of robot_controller after `joint_state_broadcaster` 180 | delay_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] 181 | for controller in robot_controller_spawners: 182 | delay_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ 183 | RegisterEventHandler( 184 | event_handler=OnProcessExit( 185 | target_action=joint_state_broadcaster_spawner, 186 | on_exit=[ 187 | TimerAction( 188 | period=3.0, 189 | actions=[controller], 190 | ), 191 | ], 192 | ) 193 | ) 194 | ] 195 | 196 | return LaunchDescription( 197 | declared_arguments 198 | + [ 199 | control_node, 200 | robot_state_pub_node, 201 | delay_rviz_after_joint_state_broadcaster_spawner, 202 | delay_joint_state_broadcaster_spawner_after_ros2_control_node, 203 | ] 204 | + 205 | delay_robot_controller_spawners_after_joint_state_broadcaster_spawner 206 | ) 207 | -------------------------------------------------------------------------------- /controlko_bringup/launch/rrbot.launch.py: -------------------------------------------------------------------------------- 1 | # $LICENSE$ 2 | 3 | from launch import LaunchDescription 4 | from launch.actions import ( 5 | DeclareLaunchArgument, 6 | RegisterEventHandler, 7 | TimerAction, 8 | ) 9 | from launch.event_handlers import OnProcessExit, OnProcessStart 10 | from launch.substitutions import ( 11 | Command, 12 | FindExecutable, 13 | LaunchConfiguration, 14 | PathJoinSubstitution, 15 | ) 16 | from launch_ros.actions import Node 17 | from launch_ros.substitutions import FindPackageShare 18 | 19 | 20 | def generate_launch_description(): 21 | # Declare arguments 22 | declared_arguments = [] 23 | declared_arguments.append( 24 | DeclareLaunchArgument( 25 | "runtime_config_package", 26 | default_value="controlko_bringup", 27 | description='Package with the controller\'s configuration in "config" folder. \ 28 | Usually the argument is not set, it enables use of a custom setup.', 29 | ) 30 | ) 31 | declared_arguments.append( 32 | DeclareLaunchArgument( 33 | "controllers_file", 34 | default_value="rrbot_controllers.yaml", 35 | description="YAML file with the controllers configuration.", 36 | ) 37 | ) 38 | declared_arguments.append( 39 | DeclareLaunchArgument( 40 | "description_package", 41 | default_value="controlko_description", 42 | description="Description package with robot URDF/xacro files. Usually the argument \ 43 | is not set, it enables use of a custom description.", 44 | ) 45 | ) 46 | declared_arguments.append( 47 | DeclareLaunchArgument( 48 | "description_file", 49 | default_value="rrbot.urdf.xacro", 50 | description="URDF/XACRO description file with the robot.", 51 | ) 52 | ) 53 | declared_arguments.append( 54 | DeclareLaunchArgument( 55 | "prefix", 56 | default_value='""', 57 | description="Prefix of the joint names, useful for \ 58 | multi-robot setup. If changed than also joint names in the controllers' configuration \ 59 | have to be updated.", 60 | ) 61 | ) 62 | declared_arguments.append( 63 | DeclareLaunchArgument( 64 | "use_mock_hardware", 65 | default_value="true", 66 | description="Start robot with fake hardware mirroring command to its states.", 67 | ) 68 | ) 69 | declared_arguments.append( 70 | DeclareLaunchArgument( 71 | "mock_sensor_commands", 72 | default_value="false", 73 | description="Enable fake command interfaces for sensors used for simple simulations. \ 74 | Used only if 'use_mock_hardware' parameter is true.", 75 | ) 76 | ) 77 | declared_arguments.append( 78 | DeclareLaunchArgument( 79 | "robot_controller", 80 | default_value="displacement_controller", 81 | choices=["forward_position_controller", "joint_trajectory_controller", "displacement_controller"], 82 | description="Robot controller to start.", 83 | ) 84 | ) 85 | 86 | # Initialize Arguments 87 | runtime_config_package = LaunchConfiguration("runtime_config_package") 88 | controllers_file = LaunchConfiguration("controllers_file") 89 | description_package = LaunchConfiguration("description_package") 90 | description_file = LaunchConfiguration("description_file") 91 | prefix = LaunchConfiguration("prefix") 92 | use_mock_hardware = LaunchConfiguration("use_mock_hardware") 93 | mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") 94 | robot_controller = LaunchConfiguration("robot_controller") 95 | 96 | # Get URDF via xacro 97 | robot_description_content = Command( 98 | [ 99 | PathJoinSubstitution([FindExecutable(name="xacro")]), 100 | " ", 101 | PathJoinSubstitution( 102 | [FindPackageShare(description_package), "urdf", description_file] 103 | ), 104 | " ", 105 | "prefix:=", 106 | prefix, 107 | " ", 108 | "use_mock_hardware:=", 109 | use_mock_hardware, 110 | " ", 111 | "mock_sensor_commands:=", 112 | mock_sensor_commands, 113 | " ", 114 | ] 115 | ) 116 | 117 | robot_description = {"robot_description": robot_description_content} 118 | 119 | robot_controllers = PathJoinSubstitution( 120 | [FindPackageShare(runtime_config_package), "config", controllers_file] 121 | ) 122 | rviz_config_file = PathJoinSubstitution( 123 | [FindPackageShare(description_package), "rviz", "rrbot.rviz"] 124 | ) 125 | 126 | control_node = Node( 127 | package="controller_manager", 128 | executable="ros2_control_node", 129 | output="both", 130 | parameters=[robot_description, robot_controllers], 131 | ) 132 | robot_state_pub_node = Node( 133 | package="robot_state_publisher", 134 | executable="robot_state_publisher", 135 | output="both", 136 | parameters=[robot_description], 137 | ) 138 | rviz_node = Node( 139 | package="rviz2", 140 | executable="rviz2", 141 | name="rviz2", 142 | output="log", 143 | arguments=["-d", rviz_config_file], 144 | ) 145 | 146 | joint_state_broadcaster_spawner = Node( 147 | package="controller_manager", 148 | executable="spawner", 149 | arguments=[ 150 | "joint_state_broadcaster", 151 | "--controller-manager", 152 | "/controller_manager", 153 | ], 154 | ) 155 | 156 | robot_controllers = [robot_controller] 157 | robot_controller_spawners = [] 158 | for controller in robot_controllers: 159 | robot_controller_spawners += [ 160 | Node( 161 | package="controller_manager", 162 | executable="spawner", 163 | arguments=[controller, "-c", "/controller_manager"], 164 | ) 165 | ] 166 | 167 | # Delay loading and activation of `joint_state_broadcaster` after start of ros2_control_node 168 | delay_joint_state_broadcaster_spawner_after_ros2_control_node = ( 169 | RegisterEventHandler( 170 | event_handler=OnProcessStart( 171 | target_action=control_node, 172 | on_start=[ 173 | TimerAction( 174 | period=1.0, 175 | actions=[joint_state_broadcaster_spawner], 176 | ), 177 | ], 178 | ) 179 | ) 180 | ) 181 | 182 | # Delay rviz start after Joint State Broadcaster to avoid unnecessary warning output. 183 | delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( 184 | event_handler=OnProcessExit( 185 | target_action=joint_state_broadcaster_spawner, 186 | on_exit=[rviz_node], 187 | ) 188 | ) 189 | 190 | # Delay loading and activation of robot_controller after `joint_state_broadcaster` 191 | delay_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] 192 | for controller in robot_controller_spawners: 193 | delay_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ 194 | RegisterEventHandler( 195 | event_handler=OnProcessExit( 196 | target_action=joint_state_broadcaster_spawner, 197 | on_exit=[ 198 | TimerAction( 199 | period=3.0, 200 | actions=[controller], 201 | ), 202 | ], 203 | ) 204 | ) 205 | ] 206 | 207 | return LaunchDescription( 208 | declared_arguments 209 | + [ 210 | control_node, 211 | robot_state_pub_node, 212 | delay_rviz_after_joint_state_broadcaster_spawner, 213 | delay_joint_state_broadcaster_spawner_after_ros2_control_node, 214 | ] 215 | + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner 216 | ) 217 | -------------------------------------------------------------------------------- /controlko_description/rviz/rrbot.rviz: -------------------------------------------------------------------------------- 1 | 2 | Panels: 3 | - Class: rviz_common/Displays 4 | Help Height: 87 5 | Name: Displays 6 | Property Tree Widget: 7 | Expanded: 8 | - /Global Options1 9 | - /Status1 10 | - /RobotModel1 11 | - /TF1 12 | Splitter Ratio: 0.5 13 | Tree Height: 756 14 | - Class: rviz_common/Selection 15 | Name: Selection 16 | - Class: rviz_common/Tool Properties 17 | Expanded: 18 | - /2D Goal Pose1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz_common/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | Visualization Manager: 28 | Class: "" 29 | Displays: 30 | - Alpha: 0.5 31 | Cell Size: 1 32 | Class: rviz_default_plugins/Grid 33 | Color: 160; 160; 164 34 | Enabled: true 35 | Line Style: 36 | Line Width: 0.029999999329447746 37 | Value: Lines 38 | Name: Grid 39 | Normal Cell Count: 0 40 | Offset: 41 | X: 0 42 | Y: 0 43 | Z: 0 44 | Plane: XY 45 | Plane Cell Count: 10 46 | Reference Frame: 47 | Value: true 48 | - Alpha: 1 49 | Class: rviz_default_plugins/RobotModel 50 | Collision Enabled: false 51 | Description File: "" 52 | Description Source: Topic 53 | Description Topic: 54 | Depth: 5 55 | Durability Policy: Volatile 56 | History Policy: Keep Last 57 | Reliability Policy: Reliable 58 | Value: /robot_description 59 | Enabled: true 60 | Links: 61 | All Links Enabled: true 62 | Expand Joint Details: false 63 | Expand Link Details: false 64 | Expand Tree: false 65 | Link Tree Style: Links in Alphabetic Order 66 | base: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | base_link: 71 | Alpha: 1 72 | Show Axes: false 73 | Show Trail: false 74 | Value: true 75 | flange: 76 | Alpha: 1 77 | Show Axes: false 78 | Show Trail: false 79 | link_1: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | link_2: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | link_3: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | link_4: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | link_5: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | link_6: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | Value: true 109 | tool0: 110 | Alpha: 1 111 | Show Axes: false 112 | Show Trail: false 113 | world: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Name: RobotModel 118 | TF Prefix: "" 119 | Update Interval: 0 120 | Value: true 121 | Visual Enabled: true 122 | - Class: rviz_default_plugins/TF 123 | Enabled: true 124 | Frame Timeout: 15 125 | Frames: 126 | All Enabled: true 127 | base: 128 | Value: true 129 | base_link: 130 | Value: true 131 | flange: 132 | Value: true 133 | link_1: 134 | Value: true 135 | link_2: 136 | Value: true 137 | link_3: 138 | Value: true 139 | link_4: 140 | Value: true 141 | link_5: 142 | Value: true 143 | link_6: 144 | Value: true 145 | tool0: 146 | Value: true 147 | world: 148 | Value: true 149 | Marker Scale: 1 150 | Name: TF 151 | Show Arrows: true 152 | Show Axes: true 153 | Show Names: true 154 | Tree: 155 | world: 156 | base_link: 157 | base: 158 | {} 159 | link_1: 160 | link_2: 161 | link_3: 162 | link_4: 163 | link_5: 164 | link_6: 165 | tool0: 166 | flange: 167 | {} 168 | Update Interval: 0 169 | Value: true 170 | Enabled: true 171 | Global Options: 172 | Background Color: 48; 48; 48 173 | Fixed Frame: base_link 174 | Frame Rate: 30 175 | Name: root 176 | Tools: 177 | - Class: rviz_default_plugins/Interact 178 | Hide Inactive Objects: true 179 | - Class: rviz_default_plugins/MoveCamera 180 | - Class: rviz_default_plugins/Select 181 | - Class: rviz_default_plugins/FocusCamera 182 | - Class: rviz_default_plugins/Measure 183 | Line color: 128; 128; 0 184 | - Class: rviz_default_plugins/SetInitialPose 185 | Topic: 186 | Depth: 5 187 | Durability Policy: Volatile 188 | History Policy: Keep Last 189 | Reliability Policy: Reliable 190 | Value: /initialpose 191 | - Class: rviz_default_plugins/SetGoal 192 | Topic: 193 | Depth: 5 194 | Durability Policy: Volatile 195 | History Policy: Keep Last 196 | Reliability Policy: Reliable 197 | Value: /goal_pose 198 | - Class: rviz_default_plugins/PublishPoint 199 | Single click: true 200 | Topic: 201 | Depth: 5 202 | Durability Policy: Volatile 203 | History Policy: Keep Last 204 | Reliability Policy: Reliable 205 | Value: /clicked_point 206 | Transformation: 207 | Current: 208 | Class: rviz_default_plugins/TF 209 | Value: true 210 | Views: 211 | Current: 212 | Class: rviz_default_plugins/Orbit 213 | Distance: 2.962221622467041 214 | Enable Stereo Rendering: 215 | Stereo Eye Separation: 0.05999999865889549 216 | Stereo Focal Distance: 1 217 | Swap Stereo Eyes: false 218 | Value: false 219 | Focal Point: 220 | X: 0.34074074029922485 221 | Y: -0.6165840029716492 222 | Z: 0.3352876305580139 223 | Focal Shape Fixed Size: true 224 | Focal Shape Size: 0.05000000074505806 225 | Invert Z Axis: false 226 | Name: Current View 227 | Near Clip Distance: 0.009999999776482582 228 | Pitch: 0.3553987741470337 229 | Target Frame: 230 | Value: Orbit (rviz) 231 | Yaw: 4.713573932647705 232 | Saved: ~ 233 | Window Geometry: 234 | Displays: 235 | collapsed: false 236 | Height: 1027 237 | Hide Left Dock: false 238 | Hide Right Dock: false 239 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000398fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004400000398000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011000000398fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004400000398000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005040000039800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 240 | Selection: 241 | collapsed: false 242 | Tool Properties: 243 | collapsed: false 244 | Views: 245 | collapsed: false 246 | Width: 1920 247 | X: 1920 248 | Y: 24 249 | -------------------------------------------------------------------------------- /controlko_controllers/src/displacement_controller.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "controlko_controllers/displacement_controller.hpp" 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "controller_interface/helpers.hpp" 23 | 24 | namespace 25 | { // utility 26 | 27 | // TODO(destogl): remove this when merged upstream 28 | // Changed services history QoS to keep all so we don't lose any client service calls 29 | static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { 30 | RMW_QOS_POLICY_HISTORY_KEEP_ALL, 31 | 1, // message queue depth 32 | RMW_QOS_POLICY_RELIABILITY_RELIABLE, 33 | RMW_QOS_POLICY_DURABILITY_VOLATILE, 34 | RMW_QOS_DEADLINE_DEFAULT, 35 | RMW_QOS_LIFESPAN_DEFAULT, 36 | RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, 37 | RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, 38 | false}; 39 | 40 | using ControllerReferenceMsg = controlko_controllers::DisplacementController::ControllerReferenceMsg; 41 | 42 | // called from RT control loop 43 | void reset_controller_reference_msg( 44 | std::shared_ptr & msg, const std::vector & joint_names) 45 | { 46 | msg->joint_names = joint_names; 47 | msg->displacements.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); 48 | msg->velocities.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); 49 | msg->duration = std::numeric_limits::quiet_NaN(); 50 | } 51 | 52 | } // namespace 53 | 54 | namespace controlko_controllers 55 | { 56 | DisplacementController::DisplacementController() : controller_interface::ControllerInterface() {} 57 | 58 | controller_interface::CallbackReturn DisplacementController::on_init() 59 | { 60 | control_mode_.initRT(control_mode_type::FAST); 61 | 62 | try { 63 | param_listener_ = std::make_shared(get_node()); 64 | } catch (const std::exception & e) { 65 | fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); 66 | return controller_interface::CallbackReturn::ERROR; 67 | } 68 | 69 | return controller_interface::CallbackReturn::SUCCESS; 70 | } 71 | 72 | controller_interface::CallbackReturn DisplacementController::on_configure( 73 | const rclcpp_lifecycle::State & /*previous_state*/) 74 | { 75 | params_ = param_listener_->get_params(); 76 | 77 | if (!params_.state_joints.empty()) { 78 | state_joints_ = params_.state_joints; 79 | } else { 80 | state_joints_ = params_.joints; 81 | } 82 | 83 | if (params_.joints.size() != state_joints_.size()) { 84 | RCLCPP_FATAL( 85 | get_node()->get_logger(), 86 | "Size of 'joints' (%zu) and 'state_joints' (%zu) parameters has to be the same!", 87 | params_.joints.size(), state_joints_.size()); 88 | return CallbackReturn::FAILURE; 89 | } 90 | 91 | // topics QoS 92 | auto subscribers_qos = rclcpp::SystemDefaultsQoS(); 93 | subscribers_qos.keep_last(1); 94 | subscribers_qos.best_effort(); 95 | 96 | // Reference Subscriber 97 | ref_subscriber_ = get_node()->create_subscription( 98 | "~/reference", subscribers_qos, 99 | std::bind(&DisplacementController::reference_callback, this, std::placeholders::_1)); 100 | 101 | std::shared_ptr msg = std::make_shared(); 102 | reset_controller_reference_msg(msg, params_.joints); 103 | input_ref_.writeFromNonRT(msg); 104 | 105 | auto set_slow_mode_service_callback = 106 | [&]( 107 | const std::shared_ptr request, 108 | std::shared_ptr response) { 109 | if (request->data) { 110 | control_mode_.writeFromNonRT(control_mode_type::SLOW); 111 | } else { 112 | control_mode_.writeFromNonRT(control_mode_type::FAST); 113 | } 114 | response->success = true; 115 | }; 116 | 117 | set_slow_control_mode_service_ = get_node()->create_service( 118 | "~/set_slow_control_mode", set_slow_mode_service_callback, 119 | rmw_qos_profile_services_hist_keep_all); 120 | 121 | try { 122 | // State publisher 123 | s_publisher_ = 124 | get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); 125 | state_publisher_ = std::make_unique(s_publisher_); 126 | } catch (const std::exception & e) { 127 | fprintf( 128 | stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", 129 | e.what()); 130 | return controller_interface::CallbackReturn::ERROR; 131 | } 132 | 133 | // TODO(anyone): Reserve memory in state publisher depending on the message type 134 | state_publisher_->lock(); 135 | state_publisher_->msg_.header.frame_id = params_.joints[0]; 136 | state_publisher_->unlock(); 137 | 138 | RCLCPP_INFO(get_node()->get_logger(), "configure successful"); 139 | return controller_interface::CallbackReturn::SUCCESS; 140 | } 141 | 142 | void DisplacementController::reference_callback(const std::shared_ptr msg) 143 | { 144 | if (msg->joint_names.size() == params_.joints.size()) { 145 | input_ref_.writeFromNonRT(msg); 146 | } else { 147 | RCLCPP_ERROR( 148 | get_node()->get_logger(), 149 | "Received %zu , but expected %zu joints in command. Ignoring message.", 150 | msg->joint_names.size(), params_.joints.size()); 151 | } 152 | } 153 | 154 | controller_interface::InterfaceConfiguration DisplacementController::command_interface_configuration() const 155 | { 156 | controller_interface::InterfaceConfiguration command_interfaces_config; 157 | command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; 158 | 159 | command_interfaces_config.names.reserve(params_.joints.size()); 160 | for (const auto & joint : params_.joints) { 161 | command_interfaces_config.names.push_back(joint + "/" + params_.interface_name); 162 | } 163 | 164 | return command_interfaces_config; 165 | } 166 | 167 | controller_interface::InterfaceConfiguration DisplacementController::state_interface_configuration() const 168 | { 169 | controller_interface::InterfaceConfiguration state_interfaces_config; 170 | state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; 171 | 172 | state_interfaces_config.names.reserve(state_joints_.size()); 173 | for (const auto & joint : state_joints_) { 174 | state_interfaces_config.names.push_back(joint + "/" + params_.interface_name); 175 | } 176 | 177 | return state_interfaces_config; 178 | } 179 | 180 | controller_interface::CallbackReturn DisplacementController::on_activate( 181 | const rclcpp_lifecycle::State & /*previous_state*/) 182 | { 183 | // Set default value in command 184 | reset_controller_reference_msg(*(input_ref_.readFromRT)(), params_.joints); 185 | 186 | return controller_interface::CallbackReturn::SUCCESS; 187 | } 188 | 189 | controller_interface::CallbackReturn DisplacementController::on_deactivate( 190 | const rclcpp_lifecycle::State & /*previous_state*/) 191 | { 192 | for (size_t i = 0; i < command_interfaces_.size(); ++i) { 193 | command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); 194 | } 195 | return controller_interface::CallbackReturn::SUCCESS; 196 | } 197 | 198 | controller_interface::return_type DisplacementController::update( 199 | const rclcpp::Time & time, const rclcpp::Duration & /*period*/) 200 | { 201 | auto current_ref = input_ref_.readFromRT(); 202 | 203 | for (size_t i = 0; i < command_interfaces_.size(); ++i) { 204 | if (!std::isnan((*current_ref)->displacements[i])) { 205 | if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) { 206 | (*current_ref)->displacements[i] /= 2; 207 | } 208 | command_interfaces_[i].set_value( 209 | (*current_ref)->displacements[i] + state_interfaces_[i].get_value()); 210 | 211 | (*current_ref)->displacements[i] = std::numeric_limits::quiet_NaN(); 212 | } 213 | } 214 | 215 | if (state_publisher_ && state_publisher_->trylock()) { 216 | state_publisher_->msg_.header.stamp = time; 217 | state_publisher_->msg_.set_point = command_interfaces_[CMD_MY_ITFS].get_value(); 218 | state_publisher_->unlockAndPublish(); 219 | } 220 | 221 | return controller_interface::return_type::OK; 222 | } 223 | 224 | } // namespace controlko_controllers 225 | 226 | #include "pluginlib/class_list_macros.hpp" 227 | 228 | PLUGINLIB_EXPORT_CLASS( 229 | controlko_controllers::DisplacementController, controller_interface::ControllerInterface) 230 | -------------------------------------------------------------------------------- /controlko_hardware_interface/src/rrbot_hardware_interface.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "controlko_hardware_interface/rrbot_hardware_interface.hpp" 23 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 24 | #include "rclcpp/rclcpp.hpp" 25 | 26 | namespace controlko_hardware_interface 27 | { 28 | hardware_interface::CallbackReturn RRBotHardwareInterface::on_init( 29 | const hardware_interface::HardwareInfo & info) 30 | { 31 | if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { 32 | return CallbackReturn::ERROR; 33 | } 34 | 35 | joint_pos_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 36 | joint_vel_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 37 | joint_pos_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 38 | joint_vel_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 39 | joint_acc_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); 40 | 41 | gpio_commands_.resize( 42 | info_.gpios[0].command_interfaces.size(), std::numeric_limits::quiet_NaN()); 43 | gpio_states_.resize( 44 | info_.gpios[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); 45 | gpio_ins_storage_.resize(2, false); 46 | gpio_outs_storage_.resize(2, false); 47 | gpio_cmds_storage_.resize(2, false); 48 | sensor_states_.resize( 49 | info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); 50 | 51 | return CallbackReturn::SUCCESS; 52 | } 53 | 54 | hardware_interface::CallbackReturn RRBotHardwareInterface::on_configure( 55 | const rclcpp_lifecycle::State & /*previous_state*/) 56 | { 57 | rrbot_comms_ = std::make_unique(info_.joints.size()); 58 | 59 | return CallbackReturn::SUCCESS; 60 | } 61 | 62 | std::vector RRBotHardwareInterface::export_state_interfaces() 63 | { 64 | std::vector state_interfaces; 65 | state_interfaces.reserve(info_.joints.size() * 3); 66 | for (size_t i = 0; i < info_.joints.size(); ++i) 67 | { 68 | state_interfaces.emplace_back(hardware_interface::StateInterface( 69 | info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_pos_states_[i])); 70 | state_interfaces.emplace_back(hardware_interface::StateInterface( 71 | info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_vel_states_[i])); 72 | state_interfaces.emplace_back(hardware_interface::StateInterface( 73 | info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &joint_acc_states_[i])); 74 | } 75 | 76 | size_t i = 0; 77 | for (const auto & state_itf : info_.gpios[0].state_interfaces) 78 | { 79 | state_interfaces.emplace_back( 80 | hardware_interface::StateInterface(info_.gpios[0].name, state_itf.name, &sensor_states_[i])); 81 | ++i; 82 | } 83 | 84 | i = 0; 85 | for (const auto & state_itf : info_.sensors[0].state_interfaces) 86 | { 87 | state_interfaces.emplace_back( 88 | hardware_interface::StateInterface(info_.sensors[0].name, state_itf.name, &gpio_states_[i])); 89 | ++i; 90 | } 91 | 92 | return state_interfaces; 93 | } 94 | 95 | std::vector 96 | RRBotHardwareInterface::export_command_interfaces() 97 | { 98 | std::vector command_interfaces; 99 | command_interfaces.reserve(info_.joints.size() * 2); 100 | position_command_interface_names_.reserve(info_.joints.size()); 101 | velocity_command_interface_names_.reserve(info_.joints.size()); 102 | for (size_t i = 0; i < info_.joints.size(); ++i) 103 | { 104 | command_interfaces.emplace_back(hardware_interface::CommandInterface( 105 | info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_pos_commands_[i])); 106 | position_command_interface_names_.push_back(command_interfaces.back().get_name()); 107 | command_interfaces.emplace_back(hardware_interface::CommandInterface( 108 | info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_vel_commands_[i])); 109 | velocity_command_interface_names_.push_back(command_interfaces.back().get_name()); 110 | } 111 | 112 | size_t i = 0; 113 | for (const auto & command_itf : info_.sensors[0].command_interfaces) 114 | { 115 | command_interfaces.emplace_back(hardware_interface::CommandInterface( 116 | info_.sensors[0].name, command_itf.name, &gpio_states_[i])); 117 | ++i; 118 | } 119 | 120 | return command_interfaces; 121 | } 122 | 123 | hardware_interface::return_type RRBotHardwareInterface::prepare_command_mode_switch( 124 | const std::vector & start_interfaces, 125 | const std::vector & stop_interfaces) 126 | { 127 | bool starting_at_least_one_pos_itf = false; 128 | bool starting_at_least_one_vel_itf = false; 129 | 130 | for (const auto & itf : position_command_interface_names_) 131 | { 132 | auto find_it = std::find(start_interfaces.begin(), start_interfaces.end(), itf); 133 | if (find_it != start_interfaces.end()) 134 | { 135 | starting_at_least_one_pos_itf = true; 136 | } 137 | } 138 | for (const auto & itf : velocity_command_interface_names_) 139 | { 140 | auto find_it = std::find(start_interfaces.begin(), start_interfaces.end(), itf); 141 | if (find_it != start_interfaces.end()) 142 | { 143 | starting_at_least_one_vel_itf = true; 144 | } 145 | } 146 | 147 | bool stopping_all_pos_itfs = true; 148 | bool stopping_all_vel_itfs = true; 149 | 150 | for (const auto & itf : position_command_interface_names_) 151 | { 152 | auto find_it = std::find(stop_interfaces.begin(), stop_interfaces.end(), itf); 153 | if (find_it == stop_interfaces.end()) 154 | { 155 | stopping_all_pos_itfs = false; 156 | } 157 | } 158 | for (const auto & itf : velocity_command_interface_names_) 159 | { 160 | auto find_it = std::find(stop_interfaces.begin(), stop_interfaces.end(), itf); 161 | if (find_it == stop_interfaces.end()) 162 | { 163 | stopping_all_vel_itfs = false; 164 | } 165 | } 166 | 167 | auto current_control_mode = rrbot_comms_->get_control_mode(); 168 | 169 | return hardware_interface::return_type::OK; 170 | } 171 | 172 | hardware_interface::return_type RRBotHardwareInterface::perform_command_mode_switch( 173 | const std::vector & /*start_interfaces*/, 174 | const std::vector & /*stop_interfaces*/) 175 | { 176 | // switch between position and velocity modes 177 | return hardware_interface::return_type::OK; 178 | } 179 | 180 | hardware_interface::CallbackReturn RRBotHardwareInterface::on_activate( 181 | const rclcpp_lifecycle::State & /*previous_state*/) 182 | { 183 | rrbot_comms_->init(); 184 | 185 | // set some default values for joints 186 | rrbot_comms_->read_joint_states(joint_pos_states_, joint_vel_states_, joint_acc_states_); 187 | 188 | joint_pos_commands_ = joint_pos_states_; 189 | joint_vel_commands_ = joint_vel_states_; 190 | 191 | return CallbackReturn::SUCCESS; 192 | } 193 | 194 | hardware_interface::CallbackReturn RRBotHardwareInterface::on_deactivate( 195 | const rclcpp_lifecycle::State & /*previous_state*/) 196 | { 197 | rrbot_comms_->stop(); 198 | return CallbackReturn::SUCCESS; 199 | } 200 | 201 | hardware_interface::CallbackReturn RRBotHardwareInterface::on_shutdown( 202 | const rclcpp_lifecycle::State & /*previous_state*/) 203 | { 204 | rrbot_comms_.release(); 205 | return CallbackReturn::SUCCESS; 206 | } 207 | 208 | hardware_interface::return_type RRBotHardwareInterface::read( 209 | const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) 210 | { 211 | rrbot_comms_->read_joint_states(joint_pos_states_, joint_vel_states_, joint_acc_states_); 212 | rrbot_comms_->read_sensor_values(sensor_states_); 213 | 214 | rrbot_comms_->read_gpio(gpio_ins_storage_, gpio_outs_storage_); 215 | for (size_t i = 0; i < 2; ++i) 216 | { 217 | gpio_states_[i] = static_cast(gpio_ins_storage_[i]); 218 | gpio_states_[2 + i] = static_cast(gpio_outs_storage_[i]); 219 | } 220 | 221 | return hardware_interface::return_type::OK; 222 | } 223 | 224 | hardware_interface::return_type RRBotHardwareInterface::write( 225 | const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) 226 | { 227 | rrbot_comms_->write_joint_commands(joint_pos_commands_, joint_vel_commands_); 228 | 229 | for (size_t i = 0; i < info_.gpios[0].command_interfaces.size(); ++i) 230 | { 231 | gpio_cmds_storage_[i] = static_cast(gpio_commands_[i]); 232 | } 233 | rrbot_comms_->write_gpios(gpio_cmds_storage_); 234 | 235 | return hardware_interface::return_type::OK; 236 | } 237 | 238 | } // namespace controlko_hardware_interface 239 | 240 | #include "pluginlib/class_list_macros.hpp" 241 | 242 | PLUGINLIB_EXPORT_CLASS( 243 | controlko_hardware_interface::RRBotHardwareInterface, hardware_interface::SystemInterface) 244 | -------------------------------------------------------------------------------- /controlko_controllers/test/test_displacement_controller.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include "test_displacement_controller.hpp" 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | using controlko_controllers::CMD_MY_ITFS; 25 | using controlko_controllers::control_mode_type; 26 | using controlko_controllers::STATE_MY_ITFS; 27 | 28 | class DisplacementControllerTest : public DisplacementControllerFixture 29 | { 30 | }; 31 | 32 | TEST_F(DisplacementControllerTest, all_parameters_set_configure_success) 33 | { 34 | SetUpController(); 35 | 36 | ASSERT_TRUE(controller_->params_.joints.empty()); 37 | ASSERT_TRUE(controller_->params_.state_joints.empty()); 38 | ASSERT_TRUE(controller_->params_.interface_name.empty()); 39 | 40 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); 41 | 42 | ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_)); 43 | ASSERT_TRUE(controller_->params_.state_joints.empty()); 44 | ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(joint_names_)); 45 | ASSERT_EQ(controller_->params_.interface_name, interface_name_); 46 | } 47 | 48 | TEST_F(DisplacementControllerTest, check_exported_intefaces) 49 | { 50 | SetUpController(); 51 | 52 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); 53 | 54 | auto command_intefaces = controller_->command_interface_configuration(); 55 | ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); 56 | for (size_t i = 0; i < command_intefaces.names.size(); ++i) { 57 | EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); 58 | } 59 | 60 | auto state_intefaces = controller_->state_interface_configuration(); 61 | ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); 62 | for (size_t i = 0; i < state_intefaces.names.size(); ++i) { 63 | EXPECT_EQ(state_intefaces.names[i], joint_names_[i] + "/" + interface_name_); 64 | } 65 | } 66 | 67 | TEST_F(DisplacementControllerTest, activate_success) 68 | { 69 | SetUpController(); 70 | 71 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); 72 | ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); 73 | 74 | // check that the message is reset 75 | auto msg = controller_->input_ref_.readFromNonRT(); 76 | EXPECT_EQ((*msg)->displacements.size(), joint_names_.size()); 77 | for (const auto & cmd : (*msg)->displacements) { 78 | EXPECT_TRUE(std::isnan(cmd)); 79 | } 80 | EXPECT_EQ((*msg)->velocities.size(), joint_names_.size()); 81 | for (const auto & cmd : (*msg)->velocities) { 82 | EXPECT_TRUE(std::isnan(cmd)); 83 | } 84 | 85 | ASSERT_TRUE(std::isnan((*msg)->duration)); 86 | } 87 | 88 | TEST_F(DisplacementControllerTest, update_success) 89 | { 90 | SetUpController(); 91 | 92 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); 93 | ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); 94 | 95 | ASSERT_EQ( 96 | controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), 97 | controller_interface::return_type::OK); 98 | } 99 | 100 | TEST_F(DisplacementControllerTest, deactivate_success) 101 | { 102 | SetUpController(); 103 | 104 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); 105 | ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); 106 | ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); 107 | } 108 | 109 | TEST_F(DisplacementControllerTest, reactivate_success) 110 | { 111 | SetUpController(); 112 | 113 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); 114 | ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); 115 | ASSERT_EQ(controller_->command_interfaces_[CMD_MY_ITFS].get_value(), 101.101); 116 | ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); 117 | ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); 118 | ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); 119 | ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); 120 | 121 | ASSERT_EQ( 122 | controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), 123 | controller_interface::return_type::OK); 124 | } 125 | 126 | TEST_F(DisplacementControllerTest, test_setting_slow_mode_service) 127 | { 128 | SetUpController(); 129 | 130 | rclcpp::executors::MultiThreadedExecutor executor; 131 | executor.add_node(controller_->get_node()->get_node_base_interface()); 132 | executor.add_node(service_caller_node_->get_node_base_interface()); 133 | 134 | // initially set to false 135 | ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); 136 | 137 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); 138 | ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); 139 | 140 | // should stay false 141 | ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); 142 | 143 | // set to true 144 | ASSERT_NO_THROW(call_service(true, executor)); 145 | ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); 146 | 147 | // set back to false 148 | ASSERT_NO_THROW(call_service(false, executor)); 149 | ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); 150 | } 151 | 152 | TEST_F(DisplacementControllerTest, test_update_logic_fast) 153 | { 154 | SetUpController(); 155 | rclcpp::executors::MultiThreadedExecutor executor; 156 | executor.add_node(controller_->get_node()->get_node_base_interface()); 157 | executor.add_node(service_caller_node_->get_node_base_interface()); 158 | 159 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); 160 | ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); 161 | 162 | // set command statically 163 | static constexpr double TEST_DISPLACEMENT = 23.24; 164 | std::shared_ptr msg = std::make_shared(); 165 | msg->joint_names = joint_names_; 166 | msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); 167 | msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); 168 | msg->duration = std::numeric_limits::quiet_NaN(); 169 | controller_->input_ref_.writeFromNonRT(msg); 170 | 171 | ASSERT_EQ( 172 | controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), 173 | controller_interface::return_type::OK); 174 | 175 | EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); 176 | EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); 177 | EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); 178 | EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); 179 | } 180 | 181 | TEST_F(DisplacementControllerTest, test_update_logic_slow) 182 | { 183 | SetUpController(); 184 | rclcpp::executors::MultiThreadedExecutor executor; 185 | executor.add_node(controller_->get_node()->get_node_base_interface()); 186 | executor.add_node(service_caller_node_->get_node_base_interface()); 187 | 188 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); 189 | ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); 190 | 191 | // set command statically 192 | static constexpr double TEST_DISPLACEMENT = 23.24; 193 | std::shared_ptr msg = std::make_shared(); 194 | // When slow mode is enabled command ends up being half of the value 195 | msg->joint_names = joint_names_; 196 | msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); 197 | msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); 198 | msg->duration = std::numeric_limits::quiet_NaN(); 199 | controller_->input_ref_.writeFromNonRT(msg); 200 | controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); 201 | 202 | EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); 203 | ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); 204 | ASSERT_EQ( 205 | controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), 206 | controller_interface::return_type::OK); 207 | 208 | EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 2); 209 | EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); 210 | } 211 | 212 | TEST_F(DisplacementControllerTest, publish_status_success) 213 | { 214 | SetUpController(); 215 | 216 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); 217 | ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); 218 | 219 | ASSERT_EQ( 220 | controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), 221 | controller_interface::return_type::OK); 222 | 223 | ControllerStateMsg msg; 224 | subscribe_and_get_messages(msg); 225 | 226 | ASSERT_EQ(msg.set_point, 101.101); 227 | } 228 | 229 | TEST_F(DisplacementControllerTest, receive_message_and_publish_updated_status) 230 | { 231 | SetUpController(); 232 | rclcpp::executors::MultiThreadedExecutor executor; 233 | executor.add_node(controller_->get_node()->get_node_base_interface()); 234 | 235 | ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); 236 | ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); 237 | 238 | ASSERT_EQ( 239 | controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), 240 | controller_interface::return_type::OK); 241 | 242 | ControllerStateMsg msg; 243 | subscribe_and_get_messages(msg); 244 | 245 | ASSERT_EQ(msg.set_point, 101.101); 246 | 247 | publish_commands(); 248 | ASSERT_TRUE(controller_->wait_for_commands(executor)); 249 | 250 | ASSERT_EQ( 251 | controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), 252 | controller_interface::return_type::OK); 253 | 254 | EXPECT_EQ(joint_command_values_[CMD_MY_ITFS], 0.45); 255 | 256 | subscribe_and_get_messages(msg); 257 | 258 | ASSERT_EQ(msg.set_point, 0.45); 259 | } 260 | 261 | int main(int argc, char ** argv) 262 | { 263 | ::testing::InitGoogleTest(&argc, argv); 264 | rclcpp::init(argc, argv); 265 | int result = RUN_ALL_TESTS(); 266 | rclcpp::shutdown(); 267 | return result; 268 | } 269 | -------------------------------------------------------------------------------- /controlko_controllers/test/test_displacement_controller.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_DISPLACEMENT_CONTROLLER_HPP_ 17 | #define TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_DISPLACEMENT_CONTROLLER_HPP_ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "controlko_controllers/displacement_controller.hpp" 28 | #include "gmock/gmock.h" 29 | #include "hardware_interface/loaned_command_interface.hpp" 30 | #include "hardware_interface/loaned_state_interface.hpp" 31 | #include "hardware_interface/types/hardware_interface_return_values.hpp" 32 | #include "rclcpp/parameter_value.hpp" 33 | #include "rclcpp/time.hpp" 34 | #include "rclcpp/utilities.hpp" 35 | #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" 36 | 37 | // TODO(anyone): replace the state and command message types 38 | using ControllerStateMsg = controlko_controllers::DisplacementController::ControllerStateMsg; 39 | using ControllerReferenceMsg = controlko_controllers::DisplacementController::ControllerReferenceMsg; 40 | using ControllerModeSrvType = controlko_controllers::DisplacementController::ControllerModeSrvType; 41 | 42 | namespace 43 | { 44 | constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; 45 | constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; 46 | } // namespace 47 | 48 | // subclassing and friending so we can access member variables 49 | class TestableDisplacementController : public controlko_controllers::DisplacementController 50 | { 51 | FRIEND_TEST(DisplacementControllerTest, all_parameters_set_configure_success); 52 | FRIEND_TEST(DisplacementControllerTest, activate_success); 53 | FRIEND_TEST(DisplacementControllerTest, reactivate_success); 54 | FRIEND_TEST(DisplacementControllerTest, test_setting_slow_mode_service); 55 | FRIEND_TEST(DisplacementControllerTest, test_update_logic_fast); 56 | FRIEND_TEST(DisplacementControllerTest, test_update_logic_slow); 57 | 58 | public: 59 | controller_interface::CallbackReturn on_configure( 60 | const rclcpp_lifecycle::State & previous_state) override 61 | { 62 | auto ret = controlko_controllers::DisplacementController::on_configure(previous_state); 63 | // Only if on_configure is successful create subscription 64 | if (ret == CallbackReturn::SUCCESS) { 65 | ref_subscriber_wait_set_.add_subscription(ref_subscriber_); 66 | } 67 | return ret; 68 | } 69 | 70 | /** 71 | * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. 72 | * Requires that the executor is not spinned elsewhere between the 73 | * message publication and the call to this function. 74 | * 75 | * @return true if new ControllerReferenceMsg msg was received, false if timeout. 76 | */ 77 | bool wait_for_command( 78 | rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, 79 | const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) 80 | { 81 | bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; 82 | if (success) { 83 | executor.spin_some(); 84 | } 85 | return success; 86 | } 87 | 88 | bool wait_for_commands( 89 | rclcpp::Executor & executor, 90 | const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) 91 | { 92 | return wait_for_command(executor, ref_subscriber_wait_set_, timeout); 93 | } 94 | 95 | // TODO(anyone): add implementation of any methods of your controller is needed 96 | 97 | private: 98 | rclcpp::WaitSet ref_subscriber_wait_set_; 99 | }; 100 | 101 | // We are using template class here for easier reuse of Fixture in specializations of controllers 102 | template 103 | class DisplacementControllerFixture : public ::testing::Test 104 | { 105 | public: 106 | static void SetUpTestCase() {} 107 | 108 | void SetUp() 109 | { 110 | // initialize controller 111 | controller_ = std::make_unique(); 112 | 113 | command_publisher_node_ = std::make_shared("command_publisher"); 114 | command_publisher_ = command_publisher_node_->create_publisher( 115 | "/test_displacement_controller/commands", rclcpp::SystemDefaultsQoS()); 116 | 117 | service_caller_node_ = std::make_shared("service_caller"); 118 | slow_control_service_client_ = service_caller_node_->create_client( 119 | "/test_displacement_controller/set_slow_control_mode"); 120 | } 121 | 122 | static void TearDownTestCase() {} 123 | 124 | void TearDown() { controller_.reset(nullptr); } 125 | 126 | protected: 127 | void SetUpController(const std::string controller_name = "test_displacement_controller") 128 | { 129 | ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); 130 | 131 | std::vector command_ifs; 132 | command_itfs_.reserve(joint_command_values_.size()); 133 | command_ifs.reserve(joint_command_values_.size()); 134 | 135 | for (size_t i = 0; i < joint_command_values_.size(); ++i) { 136 | command_itfs_.emplace_back(hardware_interface::CommandInterface( 137 | joint_names_[i], interface_name_, &joint_command_values_[i])); 138 | command_ifs.emplace_back(command_itfs_.back()); 139 | } 140 | // TODO(anyone): Add other command interfaces, if any 141 | 142 | std::vector state_ifs; 143 | state_itfs_.reserve(joint_state_values_.size()); 144 | state_ifs.reserve(joint_state_values_.size()); 145 | 146 | for (size_t i = 0; i < joint_state_values_.size(); ++i) { 147 | state_itfs_.emplace_back(hardware_interface::StateInterface( 148 | joint_names_[i], interface_name_, &joint_state_values_[i])); 149 | state_ifs.emplace_back(state_itfs_.back()); 150 | } 151 | // TODO(anyone): Add other state interfaces, if any 152 | 153 | controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); 154 | } 155 | 156 | void subscribe_and_get_messages(ControllerStateMsg & msg) 157 | { 158 | // create a new subscriber 159 | rclcpp::Node test_subscription_node("test_subscription_node"); 160 | auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; 161 | auto subscription = test_subscription_node.create_subscription( 162 | "/test_displacement_controller/state", 10, subs_callback); 163 | 164 | // call update to publish the test value 165 | ASSERT_EQ( 166 | controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), 167 | controller_interface::return_type::OK); 168 | 169 | // call update to publish the test value 170 | // since update doesn't guarantee a published message, republish until received 171 | int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop 172 | rclcpp::WaitSet wait_set; // block used to wait on message 173 | wait_set.add_subscription(subscription); 174 | while (max_sub_check_loop_count--) { 175 | controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); 176 | // check if message has been received 177 | if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) { 178 | break; 179 | } 180 | } 181 | ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " 182 | "controller/broadcaster update loop"; 183 | 184 | // take message from subscription 185 | rclcpp::MessageInfo msg_info; 186 | ASSERT_TRUE(subscription->take(msg, msg_info)); 187 | } 188 | 189 | // TODO(anyone): add/remove arguments as it suites your command message type 190 | void publish_commands( 191 | const std::vector & displacements = {0.45}, 192 | const std::vector & velocities = {0.0}, const double duration = 1.25) 193 | { 194 | auto wait_for_topic = [&](const auto topic_name) { 195 | size_t wait_count = 0; 196 | while (command_publisher_node_->count_subscribers(topic_name) == 0) { 197 | if (wait_count >= 5) { 198 | auto error_msg = 199 | std::string("publishing to ") + topic_name + " but no node subscribes to it"; 200 | throw std::runtime_error(error_msg); 201 | } 202 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); 203 | ++wait_count; 204 | } 205 | }; 206 | 207 | wait_for_topic(command_publisher_->get_topic_name()); 208 | 209 | ControllerReferenceMsg msg; 210 | msg.joint_names = joint_names_; 211 | msg.displacements = displacements; 212 | msg.velocities = velocities; 213 | msg.duration = duration; 214 | 215 | command_publisher_->publish(msg); 216 | } 217 | 218 | std::shared_ptr call_service( 219 | const bool slow_control, rclcpp::Executor & executor) 220 | { 221 | auto request = std::make_shared(); 222 | request->data = slow_control; 223 | 224 | bool wait_for_service_ret = 225 | slow_control_service_client_->wait_for_service(std::chrono::milliseconds(500)); 226 | EXPECT_TRUE(wait_for_service_ret); 227 | if (!wait_for_service_ret) { 228 | throw std::runtime_error("Services is not available!"); 229 | } 230 | auto result = slow_control_service_client_->async_send_request(request); 231 | EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); 232 | 233 | return result.get(); 234 | } 235 | 236 | protected: 237 | // TODO(anyone): adjust the members as needed 238 | 239 | // Controller-related parameters 240 | std::vector joint_names_ = {"joint1"}; 241 | std::vector state_joint_names_ = {"joint1state"}; 242 | std::string interface_name_ = "acceleration"; 243 | std::array joint_state_values_ = {1.1}; 244 | std::array joint_command_values_ = {101.101}; 245 | 246 | std::vector state_itfs_; 247 | std::vector command_itfs_; 248 | 249 | // Test related parameters 250 | std::unique_ptr controller_; 251 | rclcpp::Node::SharedPtr command_publisher_node_; 252 | rclcpp::Publisher::SharedPtr command_publisher_; 253 | rclcpp::Node::SharedPtr service_caller_node_; 254 | rclcpp::Client::SharedPtr slow_control_service_client_; 255 | }; 256 | 257 | #endif // TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_DISPLACEMENT_CONTROLLER_HPP_ 258 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /controlko_controllers/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /controlko_hardware_interface/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROSCon 2022 workshop on ros2_control 2 | 3 | Thank you for your interest in ros2_control. 4 | This repository was developed with the purpose of supporting the workshop on *ros2_control* @ ROSCon2022 in Kyoto, Japan. 5 | Whether you participated or not, the repository will provide you with detailed instructions on how to use the *ros2_control* framework and explain functionality and purpose of its individual parts. 6 | 7 | ### Installing this repository 8 | 9 | After cloning the repository, go to your source workspace and execute the following commands to import the necessary repositories and to install all dependencies: 10 | ``` 11 | vcs import --input roscon2022_workshop/roscon2022_workshop.repos . 12 | rosdep update 13 | rosdep install -y -i --from-paths . 14 | ``` 15 | 16 | ## What is *ros2_control* 17 | 18 | In short, ros2_control is a control framework for ROS2. 19 | But actually, it is much more, it is the kernel of the ROS2 system that controls robots: 20 | - it abstracts hardware and low-level control for other frameworks such as MoveIt2 and Nav2; 21 | - it provides resource access management, 22 | - controls their lifecycle and so on. 23 | 24 | For more details check [this presentation](https://control.ros.org/master/doc/resources/resources.html#ros-world-2021) 25 | 26 | 27 | ## Structure of this repository (and workshop) 28 | 29 | The structure of the repository follows the flow of integrating robots with ROS2 and these are the following steps: 30 | 31 | 1. 📑 Setting up the hardware description for *ros2_control* 32 | 33 | 1. 🗒 Setting up the robot's URDF using XACRO 34 | 2. 📝 Extending the robot's URDF with the `` tag 35 | 36 | 2. 🖥 Using the *Mock Hardware* plugin for easy and generic testing of the setup (and how it can save you ton of time and nerves) 37 | 38 | 1. 🛠 How to setup *Mock Hardware* for a robot? 39 | 2. 🔩 How to test it with an off-the-shelf controller? 40 | 41 | 3. ⚙ Getting to know the roles of the main components of the *ros2_control* framework: *Controller Manager*, *Controllers*, *Resource Manager* and *Hardware Interface* 42 | 43 | 4. 🔬 Introspection of the *ros2_control* system 44 | 45 | 5. 💻 Simulating your hardware using Gazebo Classic and Gazebo 46 | 47 | 6. 🔃 Become familiar with the lifecycle of controllers and hardware. Learn how to use them. 48 | 49 | 7. 🤖 How to write a hardware interface for a robot 50 | 51 | 8. 🛂 How to write a controller 52 | 53 | 54 | ## Details about the workshop 55 | 56 | ### 1. 📑 Setting up the hardware description for *ros2_control* 57 | 58 | ##### GOAL 59 | 60 | - learn how to setup the URDF of a robot using XACRO macros 61 | - learn what URDF changes are needed to integrate a robot with `ros2_control` 62 | 63 | 64 | ##### 🗒 Setting up URDF using XACRO for a robot 65 | 66 | For any robot that is used with ROS/ROS2 an URDF description is needed. 67 | This description holds information about kinematics, visualization (e.g., for Rviz2) and collision data. 68 | This description is also used by popular ROS2 high-level libraries like, MoveIt2, Nav2 and Simulators. 69 | 70 | In this excercise we will focus on setting up the description using the XACRO format which is highly configurable and parameterizable and generally better to use than the static URDF format. 71 | 72 | ##### Task 73 | 74 | Branch: `1-robot-description/task` 75 | 76 | Task is to setup the XACRO for RRbot in a package called `controlko_description`. 77 | 78 | Kinematics: 79 | 80 | - 2 DoF 81 | - 1st joint is on a pedestal (box 30x30x30 cm) 30 cm above the ground and rotates around the axis orthogonal to the ground 82 | - 1st link is 50cm long (cylinder with 20cm diameter) 83 | - 2nd joint is rotation orthogonal to the first link's height 84 | - 2nd link is 60cm long (10x10cm cross-section 5x5cm) 85 | 86 | Hardware: 87 | 88 | - Force Torque Sensor at TCP (6D) 89 | - 2 digital inputs and output (outputs can be measured) 90 | 91 | References: 92 | 93 | - https://wiki.ros.org/urdf 94 | - https://wiki.ros.org/urdf/XML 95 | 96 | Files to create or adjust: 97 | 98 | - `rrbot_macro.xacro` - macro with kinematics and geometries for the `rrbot` 99 | - `rrbot.urdf.xacro` - main xacro file for the robot where macro is instantiated 100 | - `view_robot.launch.py` - loading and showing robot in `rviz2` 101 | 102 | 103 | **TIPP**: `RosTeamWS` tool has some scripts that can help you to solve this task faster (on the branch is this already implemented). Resources: 104 | 105 | - [Commonly used robot-package structure](https://stoglrobotics.github.io/ros_team_workspace/master/guidelines/robot_package_structure.html) 106 | - [Creating a new package](https://stoglrobotics.github.io/ros_team_workspace/master/use-cases/ros_packages/create_package.html) 107 | - [Setting up robot description](https://stoglrobotics.github.io/ros_team_workspace/master/use-cases/ros_packages/setup_robot_description_package.html) 108 | 109 | 110 | ##### Solution: 111 | 112 | Branch: `1-robot-description/solution` 113 | 114 | Check the files listed above and execute: 115 | ``` 116 | ros2 launch controlko_description view_rrbot.launch.py 117 | ``` 118 | to view the robot and move its joints using the `Joint State Publisher` GUI. 119 | 120 | 121 | ### 2. 🖥 Using *Mock Hardware* plugin for simple and generic testing of the setup 122 | 123 | ##### GOAL 124 | 125 | - learn what is *Mock Hardware* and how to use it 126 | - learn how you can fast and easily test you controller's setup and parameters before you deal with simulation or real hardware 127 | 128 | *Mock Hardware* is mocking ros2_control `Hardware Interface` based on the robot's description from the `ros2_control` tag. 129 | Its purpose is to simplify and boost the development process when creating a new controller or setting up their configuration. 130 | The advantage of using it, over simulation or real hardware, is a very fast start-up and lean functionality. 131 | It is a well tested module helping you focus on other components in your setup knowing that your "hardware" behaves ideally. 132 | *NOTE:* the functionality of *Mock Hardware* is intentionally limited and it only enables you to reflect commanded values on the state interfaces with the same name. Nevertheless, this is sufficient for most tasks. 133 | 134 | **TIPP**: 135 | 136 | - Dr. Denis recommends you to always start to develop things first with the Mock Hardware and then start switching to simulation or real hardware. 137 | This way you save time dealing with a broken setup with simulation or hardware in the loop. 138 | 139 | 140 | ##### 🛠 How to setup *Mock Hardware* for a robot? 141 | 142 | 1. Add `hardware` tag under the `ros2_control` tag with plugin `mock_components/GenericSystem` and set `mock_sensor_commands` parameter. 143 | The parameter create fake command interface for sensor values than enables you to simulate the values on the sensor. 144 | 145 | 2. Create a launch file named `rrbot.launch.py` that starts the ros2_control node with the correct robot description. 146 | 147 | **NOTE**: Currently, there is only `GenericSystem` mock component, which can mock also sensor or actuator components (because they just have a reduced feature set compared to a system). 148 | 149 | ##### 🔩 How to test it with an off-the-shelf controller? 150 | 151 | 1. Setup the following controllers for the `RRBot`: 152 | 153 | - `Joint State Broadcaster` - always needed to get `/joint_states` topic from a hardware. 154 | - `Forward Command Controller` - sending position commands for the joints. 155 | - `Joint Trajectory Controller` - interpolating trajectory between the position commands for the joint. 156 | 157 | 2. Add to launch file spawning (loading and activating) of controllers. 158 | 159 | 3. Test forward command controller by sending a reference to it using `ros2 topic pub` command. 160 | 161 | 4. Create a launch file that starts `ros2_controllers_test_nodes/publisher_joint_trajectory_controller` to publish goals for the JTC. 162 | 163 | **TIPP**: `RosTeamWS` tool has some scripts that can help you to solve this task faster. Resources: 164 | 165 | - [Commonly used robot-package structure](https://stoglrobotics.github.io/ros_team_workspace/master/guidelines/robot_package_structure.html) 166 | - [Creating a new package](https://stoglrobotics.github.io/ros_team_workspace/master/use-cases/ros_packages/create_package.html) 167 | - [Setting up robot bringup](https://stoglrobotics.github.io/ros_team_workspace/master/use-cases/ros_packages/setup_robot_bringup_package.html) 168 | 169 | ##### Solution: 170 | 171 | Branch: `2-robot-mock-hardware` 172 | 173 | Check the files listed above and execute: 174 | ``` 175 | ros2 launch controlko_bringup rrbot.launch.py 176 | ``` 177 | then publish a command to the forward command controller: 178 | ``` 179 | ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray " 180 | layout: 181 | dim: [] 182 | data_offset: 0 183 | data: 184 | - 0.7 185 | - 0.7" 186 | ``` 187 | 188 | To start `RRBot` with JTC execute: 189 | ``` 190 | ros2 launch controlko_bringup rrbot.launch.py robot_controller:=joint_trajectory_controller 191 | ``` 192 | and open a new terminal and execute: 193 | ``` 194 | ros2 launch controlko_bringup test_joint_trajectory_controller.launch.py 195 | ``` 196 | 197 | **NOTE**: delay between spawning controllers is usually not necessary, but useful when starting a complex setup. Adjust this specifically for the specific use-case. 198 | 199 | 200 | ### 3. ⚙ Getting know the roles of the main components of *ros2_control* framework 201 | 202 | Start the previous example one more time and try to answer the following questions: 203 | 204 | 1. What and where is *Controller Manager*? 205 | 2. What are *Controllers*? How they can be seen in ROS2? 206 | 3. What is *Resource Manager*? Where can you see it? How to access it? 207 | 4. What is *Hardware Interface*? Where is this stored? How to interact with it? 208 | 209 | ##### Solution: 210 | 211 | ![ros2_control Overview](https://github.com/ros-controls/control.ros.org/blob/master/doc/resources/images/ros2_control_overview.png) 212 | 213 | 214 | ### 4. 🔬 Introspection of *ros2_control* system 215 | 216 | There are two options to interact with the ros2_control, first, using the CLI interface with commands like `ros2 control ` (package `ros2controlcli`), and second, using services from the controller manager directly. 217 | 218 | Try to figure out how to answer the following questions using those tools: 219 | 220 | 1. What controllers are loaded in the system? 221 | 2. What is the state of the controllers? 222 | 3. What hardware interfaces are there and in which state? 223 | 4. Which interfaces are available? 224 | 5. How can we switch between `forward_position_controller` and `joint_trajectory_controller`? 225 | 6. What happens when you try to run all controllers in parallel? 226 | 7. What interfaces are controllers using? 227 | 228 | Also there are few graphical tools available for `ros2_control`: `rqt_controller_manager` and `rqt_joint_trajectory_controller`. Try to use those tools. 229 | 230 | ##### Solution: 231 | 232 | Answers to the questions: 233 | 234 | 1. `ros2 control list_controllers` 235 | 2. `ros2 control list_controllers` 236 | 3. `ros2 service call /controller_manager/list_hardware_components controller_manager_msgs/srv/ListHardwareComponents {}` 237 | 4. `ros2 control list_hardware_interfaces` 238 | 5. `ros2 run controller_manager spawner forward_position_controller --inactive` 239 | `ros2 control switch_controllers --deactivate joint_trajectory_controller --activate forward_position_controller` 240 | 6. See output in the terminal where `ros2_control_node` is running: `ros2 control switch_controllers --activate joint_trajectory_controller` 241 | 7. `ros2 control list_controllers -v` 242 | 243 | 244 | ### 5. 💻 Simulating your hardware using Gazebo Classic and Gazebo 245 | 246 | ros2_control is integrated with simulators using simulator-specific plugins. 247 | Those plugins extend controller manager with simulator-related functionalities and enables loading hardware interfaces that are created specifically for the simulator. 248 | Simulators are using description under `` to setup the interfaces. 249 | They are searched for interfaces with standard names, `position`, `velocity` and `effort`, to wire them with the internal simulator-states. 250 | 251 | The plugins and interfaces for the simulators are the following: 252 | 253 | **Gazebo Classic** 254 | - Package: `gazebo_ros2_control` 255 | - Simulator plugin: `libgazebo_ros2_control.so` 256 | - HW interface plugin: `gazebo_ros2_control/GazeboSystem` 257 | 258 | **Gazebo** 259 | - Package: `gz_ros2_control` 260 | - Simulator plugin: `libign_ros2_control-system.so` 261 | - HW interface plugin: `ign_ros2_control/IgnitionSystem` 262 | **NOTE** `ign` will be switched to `gz` very soon! 263 | 264 | 265 | Let's define those plugins for `RRBot`: 266 | 267 | 1. Extend `rrbot.urdf.xacro` with `` tags defining simulator plugins and parameters. 268 | 2. Add hardware interface plugins under `` tag in the file `rrbot_macro.ros2_control.xacro`. 269 | 3. Add new launch file `rrbot_sim_gazebo_class.launch.py` for starting Gazebo Classic simulation. 270 | 4. Add new launch file `rrobt_sim_gazebo.launch.py` for starting Gazebo simulation. 271 | 272 | ##### Solution: 273 | 274 | Branch: `5-simulation` 275 | 276 | Check updated files from the above list. 277 | To start the Gazebo Classic simulation use: 278 | ``` 279 | ros2 launch controlko_bringup rrbot_sim_gazebo_classic.launch.py 280 | ``` 281 | 282 | To start the Gazebo simulation use: 283 | ``` 284 | ros2 launch controlko_bringup rrbot_sim_gazebo.launch.py 285 | ``` 286 | 287 | Now execute the test script for joint trajectory controller to move the robot. 288 | ``` 289 | ros2 launch controlko_bringup test_joint_trajectory_controller.launch.py 290 | ``` 291 | 292 | **NOTE**: When running simulation be sure to set the joint limits defined in the macro file. 293 | 294 | 295 | ### 6. 🔃 Getting familiar with the lifecycle of controllers and hardware and how to use it 296 | 297 | *ros2_control* enables you to control the lifecycle of controllers and hardware components. 298 | The states and transitions are the same as for the `Lifecycle Nodes`. 299 | Check the diagram below for more details: 300 | 301 | ![Lifecycle of Hardware Interfaces](https://control.ros.org/master/_images/hardware_interface_lifecycle.png) 302 | 303 | ##### Task 304 | 305 | 1. Start the `RRbot` with `Mock System` 306 | 2. Check the lifecycle state of controllers and hardware interfaces 307 | 3. Activate the `joint_trajectory_controller` controller. What else do you have to do to achieve that? 308 | 4. Set hardware to `inactive` state. What is now the internal state of the *ros2_control* instance? 309 | 5. Have you heard of the `RQT Controller Manager` plugin? Try it! 310 | 311 | ##### Solution: 312 | 313 | Branch: `6-getting-know-lifecycle` 314 | 315 | Execute the following commands to get the answers from the task: 316 | 317 | 1. `ros2 launch controlko_bringup rrbot.launch.py` 318 | 2. Execute in another terminal: 319 | ``` 320 | ros2 control list_controllers 321 | ros2 service call /controller_manager/list_hardware_components controller_manager_msgs/srv/ListHardwareComponents {} 322 | ``` 323 | 3. Execute the following commands (this is one of multiple valid ways) 324 | ``` 325 | ros2 control load_controller joint_trajectory_controller 326 | ros2 control set_controller_state joint_trajectory_controller inactive 327 | ros2 control switch_controllers --deactivate forward_position_controller --activate joint_trajectory_controller 328 | ``` 329 | 330 | 4. Execute the following commands (this is one of multiple valid ways) 331 | ``` 332 | # stop controller 333 | ros2 control switch_controllers --deactivate joint_trajectory_controller 334 | # get component name 335 | ros2 service call /controller_manager/list_hardware_components controller_manager_msgs/srv/ListHardwareComponents {} 336 | # set component state 337 | ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " 338 | name: rrbot 339 | target_state: 340 | id: 0 341 | label: inactive" 342 | # check internals of ros2_control 343 | ros2 control list_controllers 344 | ros2 control list_hardware_interfaces 345 | ``` 346 | 347 | 348 | ### 7. 🤖 How to write a hardware interface for a robot 349 | 350 | Hardware interface is the lowest layer towards hardware in *ros2_control*. 351 | A hardware interface is a driver for a specific robot that exports interfaces to the framework for controllers to use them. 352 | Overview of *ros2_control* shows this graphically: 353 | 354 | ![Overview of *ros2_control*](https://control.ros.org/master/_images/ros2_control_overview.png) 355 | 356 | Lifecycle diagrams from the [Task 6]() explains in detail when which method is used. 357 | 358 | ##### Task 359 | 360 | Branch: `7-robot-hardware-interface/task` 361 | 362 | Write a hardware interface for the *RRBot*. 363 | 364 | 1. Create or adjust a package named `controlko_hardware_interface` with hardware interface files. 365 | 2. Write a hardware interface that uses a header-only library for the communication with *RRBot*: 366 | 367 | - check the file `controlko_hardware_interface/include/controlko_hardware_interface/dr_denis_rrbot_comms.hpp` 368 | - use [Writing a new hardware interface manual](https://control.ros.org/master/doc/ros2_control/hardware_interface/doc/writing_new_hardware_interface.html) to implement everything needed. 369 | - extend the URDF file to use hardware interface 370 | 371 | 3. During the implementation of the hardware interface take care about the following details: 372 | 373 | - Which control modes are supported? 374 | - What happens if an incompatible controller is activated? 375 | 376 | 4. What are the capabilities? 377 | 378 | **TIPP**: `RosTeamWS` tool has some scripts that can help you to solve this task faster. Resources: 379 | 380 | - [Commonly used robot-package structure](https://stoglrobotics.github.io/ros_team_workspace/master/guidelines/robot_package_structure.html) 381 | - [Creating a new package](https://stoglrobotics.github.io/ros_team_workspace/master/use-cases/ros_packages/create_package.html) 382 | - [Setup robot’s hardware package](https://stoglrobotics.github.io/ros_team_workspace/master/use-cases/ros2_control/setup_robot_hardware_interface.html) 383 | 384 | ##### Solution 385 | 386 | Branch: `7-robot-hardware-interface/solution` 387 | 388 | Execute the following commands to get the answers from the task: 389 | 390 | 1. `ros2 launch controlko_bringup rrbot.launch.py use_mock_hardware:=false` 391 | 392 | 2. Test execution with `forward_position_controller` and `joint_trajectory_controller` 393 | 394 | 3. Test activation of an incompatible controller using: 395 | ``` 396 | ros2 control load_controller incompatible_joint_trajectory_controller 397 | ros2 control set_controller_state incompatible_joint_trajectory_controller inactive 398 | ros2 control switch_controllers --deactivate forward_position_controller --activate incompatible_joint_trajectory_controller 399 | ``` 400 | 401 | 402 | ### 8. 🛂 How to write a controller 403 | 404 | Controllers in *ros2_control* are serving on the one hand as "interfaces" towards the ROS-world and on the other hand implement algorithms to control the hardware. 405 | A controller, when activated, gets loaned access to the exported hardware interface to read and write values directly from/to memory locations that the hardware interface is using. 406 | Although somewhat limited, this concept enables deterministic and reliable data flow between controllers and hardware interfaces (drivers). 407 | 408 | ![Overview of *ros2_control*](https://control.ros.org/master/_images/ros2_control_overview.png) 409 | 410 | ##### Task 411 | 412 | Branch: `8-write-controller/task` 413 | 414 | Write a controller for *RRBot* robot that takes joint displacements as input and updates new joint positions for it. 415 | 416 | 1. Add controller files into the `controlko_controllers` package. 417 | 2. During the implementation take care about the following details: 418 | 419 | - How is data exchanged between the controller's callbacks and the `update` method? 420 | - How are statuses from controller published to ROS topics? 421 | 422 | - Controller should have a *slow mode* where displacements are reduced to half. 423 | - Controller accepts a command only once. 424 | 425 | 3. Write a controller that uses `control_msgs/msg/JointJog` message for the input. 426 | 427 | - Check the definition of [`JointJog` message](https://github.com/ros-controls/control_msgs/blob/galactic-devel/control_msgs/msg/JointJog.msg) 428 | - Alternatively use the CLI command: `ros2 interface show control_msgs/msg/JointJog` 429 | - Use [Writing a new controller manual](https://control.ros.org/master/doc/ros2_controllers/doc/writing_new_controller.html) to implement the controller 430 | 431 | 432 | **TIPP**: `RosTeamWS` tool has some scripts that can help you solve this task faster. Resources: 433 | 434 | - [Creating a new package](https://stoglrobotics.github.io/ros_team_workspace/master/use-cases/ros_packages/create_package.html) 435 | - [Setup controller package](https://stoglrobotics.github.io/ros_team_workspace/master/use-cases/ros2_control/setup_controller.html) - choose setup of "normal" controller 436 | 437 | ##### Solution 438 | 439 | Branch: `8-write-controller/solution` 440 | 441 | First check the code: 442 | 443 | - *ros2_control* is now using PickNik's [generate_parameter_library](https://github.com/PickNikRobotics/generate_parameter_library) so simpler and cleaner parameters usage and definition. 444 | - parameters are defined in [./src/displacement_controller.yaml] file 445 | - example controller setup is in: 446 | - [./test/displacement_controller_params.yaml] - when controller is used directly with the hardware 447 | - [./test/displacement_controller_preceeding_params.yaml] - when controller is used at the beginning of the chain (see the next task for details!) 448 | 449 | Execute the following commands to see the new controller running: 450 | 451 | 1. `ros2 launch controlko_bringup rrbot.launch.py` 452 | --------------------------------------------------------------------------------