Skip to content

Commit f5d821c

Browse files
mahaarbobmagyardestogl
authored
[Example 2]: Multiple command interfaces using prepare - perform switch (#83)
* prepare - perform switch * Update ros2_control_demo_robot/config/rrbot_multi_interface_forward_controllers.yaml Co-authored-by: Bence Magyar <[email protected]> * Add demo doc for multi interface * Trying to appease linter * Accidental double ifndef, minor style fix * Apply format * uint -> size_t * std::equal -> std::all_of Co-authored-by: Bence Magyar <[email protected]> Co-authored-by: Denis Štogl <[email protected]>
1 parent 2b2d9be commit f5d821c

File tree

9 files changed

+719
-0
lines changed

9 files changed

+719
-0
lines changed

‎doc/index.rst‎

Lines changed: 94 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,3 +4,97 @@ Demos
44
-----
55

66
This repository provides templates for the development of ros2_control-enabled robots and a simple simulations to demonstrate and prove ros2_control concepts.
7+
8+
9+
Mode switching demo
10+
^^^^^^^^^^^^^^^^^^^
11+
12+
Start up the multi interface rrbot system:
13+
14+
.. code-block:: bash
15+
16+
ros2 launch ros2_control_demo_robot rrbot_system_multi_interface.launch.py
17+
18+
List the available interfaces
19+
20+
.. code-block:: bash
21+
22+
$ ros2 control list_hardware_interfaces
23+
command interfaces
24+
joint1/acceleration [unclaimed]
25+
joint1/position [unclaimed]
26+
joint1/velocity [unclaimed]
27+
joint2/acceleration [unclaimed]
28+
joint2/position [unclaimed]
29+
joint2/velocity [unclaimed]
30+
state interfaces
31+
joint1/acceleration
32+
joint1/position
33+
joint1/velocity
34+
joint2/acceleration
35+
joint2/position
36+
joint2/velocity
37+
38+
Load and configure all controllers
39+
40+
.. code-block:: bash
41+
42+
ros2 control load_controller forward_command_controller_position --state configure
43+
ros2 control load_controller forward_command_controller_velocity --state configure
44+
ros2 control load_controller forward_command_controller_acceleration --state configure
45+
ros2 control load_controller forward_command_controller_illegal1 --state configure
46+
ros2 control load_controller forward_command_controller_illegal2 --state configure
47+
ros2 control load_controller joint_state_controller --state configure
48+
49+
50+
Start the position controller
51+
52+
.. code-block:: bash
53+
54+
ros2 control set_controller_state forward_command_controller_position start
55+
56+
Check the hardware interfaces, position interfaces should be claimed now
57+
58+
.. code-block:: bash
59+
60+
$ ros2 control list_hardware_interfaces
61+
command interfaces
62+
joint1/acceleration [unclaimed]
63+
joint1/position [claimed]
64+
joint1/velocity [unclaimed]
65+
joint2/acceleration [unclaimed]
66+
joint2/position [claimed]
67+
joint2/velocity [unclaimed]
68+
state interfaces
69+
joint1/acceleration
70+
joint1/position
71+
joint1/velocity
72+
joint2/acceleration
73+
joint2/position
74+
joint2/velocity
75+
76+
Let's switch controllers now to velocity
77+
78+
.. code-block:: bash
79+
80+
ros2 control switch_controllers --stop-controllers forward_command_controller_position --start-controllers forward_command_controller_velocity
81+
82+
List hardware interfaces again to see that indeed position interfaces have been unclaimed while velocity is claimed now
83+
84+
.. code-block:: bash
85+
86+
$ ros2 control list_hardware_interfaces
87+
command interfaces
88+
joint1/acceleration [unclaimed]
89+
joint1/position [unclaimed]
90+
joint1/velocity [claimed]
91+
joint2/acceleration [unclaimed]
92+
joint2/position [unclaimed]
93+
joint2/velocity [claimed]
94+
state interfaces
95+
joint1/acceleration
96+
joint1/position
97+
joint1/velocity
98+
joint2/acceleration
99+
joint2/position
100+
joint2/velocity

‎ros2_control_demo_hardware/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@ add_library(
2121
${PROJECT_NAME}
2222
SHARED
2323
src/rrbot_system_position_only.cpp
24+
src/rrbot_system_multi_interface.cpp
2425
)
2526
target_include_directories(
2627
${PROJECT_NAME}

‎ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp‎

Lines changed: 97 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,97 @@
1+
// Copyright 2021 Department of Engineering Cybernetics, NTNU
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 ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_MULTI_INTERFACE_HPP_
16+
#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_MULTI_INTERFACE_HPP_
17+
18+
#include <cstdint>
19+
#include <memory>
20+
#include <string>
21+
#include <vector>
22+
23+
#include "rclcpp/macros.hpp"
24+
25+
#include "hardware_interface/base_interface.hpp"
26+
#include "hardware_interface/handle.hpp"
27+
#include "hardware_interface/hardware_info.hpp"
28+
#include "hardware_interface/system_interface.hpp"
29+
#include "hardware_interface/types/hardware_interface_return_values.hpp"
30+
#include "hardware_interface/types/hardware_interface_status_values.hpp"
31+
#include "ros2_control_demo_hardware/visibility_control.h"
32+
33+
using hardware_interface::return_type;
34+
35+
namespace ros2_control_demo_hardware
36+
{
37+
class RRBotSystemMultiInterfaceHardware
38+
: public hardware_interface::BaseInterface<hardware_interface::SystemInterface>
39+
{
40+
public:
41+
RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemMultiInterfaceHardware);
42+
43+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
44+
return_type configure(const hardware_interface::HardwareInfo & info) override;
45+
46+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
47+
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
48+
49+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
50+
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
51+
52+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
53+
virtual return_type prepare_command_mode_switch(
54+
const std::vector<std::string> & start_interfaces,
55+
const std::vector<std::string> & stop_interfaces);
56+
57+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
58+
return_type start() override;
59+
60+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
61+
return_type stop() override;
62+
63+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
64+
return_type read() override;
65+
66+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
67+
return_type write() override;
68+
69+
private:
70+
// Parameters for the RRBot simulation
71+
double hw_start_sec_;
72+
double hw_stop_sec_;
73+
double hw_slowdown_;
74+
75+
// Store the commands for the simulated robot
76+
std::vector<double> hw_commands_positions_;
77+
std::vector<double> hw_commands_velocities_;
78+
std::vector<double> hw_commands_accelerations_;
79+
std::vector<double> hw_positions_;
80+
std::vector<double> hw_velocities_;
81+
std::vector<double> hw_accelerations_;
82+
83+
// Enum defining at which control level we are
84+
// Dumb way of maintaining the command_interface type per joint.
85+
enum class integration_lvl_t : std::uint8_t
86+
{
87+
UNDEFINED = 0,
88+
POSITION = 1,
89+
VELOCITY = 2,
90+
ACCELERATION = 3
91+
};
92+
93+
std::vector<integration_lvl_t> control_lvl_;
94+
};
95+
96+
} // namespace ros2_control_demo_hardware
97+
#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_MULTI_INTERFACE_HPP_

‎ros2_control_demo_hardware/ros2_control_demo_hardware.xml‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,4 +6,11 @@
66
The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1.
77
</description>
88
</class>
9+
<class name="ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware"
10+
type="ros2_control_demo_hardware::RRBotSystemMultiInterfaceHardware"
11+
base_class_type="hardware_interface::SystemInterface">
12+
<description>
13+
The ROS2 Control minimal robot protocol for a system with multiple command and state interfaces.
14+
</description>
15+
</class>
916
</library>

0 commit comments

Comments
 (0)