Skip to content

Commit 8f5597c

Browse files
christophfroehlichmergify[bot]
christophfroehlich
authored and
mergify[bot]
committed
Add a custom plugin for simulating actuator dynamics to the demos (#693)
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com> (cherry picked from commit 0dab1d5) # Conflicts: # doc/index.rst # gz_ros2_control_demos/package.xml # gz_ros2_control_tests/tests/CMakeLists.txt
1 parent 4bf1888 commit 8f5597c

File tree

10 files changed

+902
-9
lines changed

10 files changed

+902
-9
lines changed

‎doc/index.rst‎

Lines changed: 17 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -239,13 +239,19 @@ Additionally, one can specify a namespace and remapping rules, which will be for
239239
Advanced: custom gz_ros2_control Simulation Plugins
240240
-----------------------------------------------------------
241241

242-
The *gz_ros2_control* Gazebo plugin also provides a pluginlib-based interface to implement custom interfaces between Gazebo and *ros2_control* for simulating more complex mechanisms (nonlinear springs, linkages, etc).
242+
The *gz_ros2_control* Gazebo plugin also provides a pluginlib-based interface to implement custom interfaces between
243+
Gazebo and *ros2_control* for simulating more complex mechanisms (nonlinear springs, linkages, etc) or actuator dynamics.
243244

244245
These plugins must inherit the ``gz_ros2_control::GazeboSimSystemInterface``, which implements a simulated *ros2_control*
245-
``hardware_interface::SystemInterface``. SystemInterface provides API-level access to read and command joint properties.
246+
``hardware_interface::SystemInterface``.
246247

248+
<<<<<<< HEAD
247249
The respective GazeboSimSystemInterface is specified in a URDF model and is loaded when the
248250
robot model is loaded. For example, the following XML will load a custom plugin:
251+
=======
252+
The respective GazeboSimSystemInterface is specified in a URDF or SDF model and is loaded when the
253+
robot model is loaded. For example, the following XML will load a custom plugin instead:
254+
>>>>>>> 0dab1d5 (Add a custom plugin for simulating actuator dynamics to the demos (#693))
249255

250256
.. code-block:: xml
251257
@@ -261,6 +267,15 @@ robot model is loaded. For example, the following XML will load a custom plugin:
261267
</plugin>
262268
</gazebo>
263269
270+
The ``gz_ros2_control_demos/GazeboCustomSimSystem`` demonstrates how to implement actuator dynamics for a joint with
271+
velocity command interface by using a configurable low pass filter. Run
272+
273+
.. code-block:: shell
274+
275+
ros2 launch gz_ros2_control_demos cart_example_velocity_custom_plugin.launch.py
276+
277+
and compare it with the behavior of ``cart_example_velocity.launch.py`` using any plotting tool like plotjuggler.
278+
264279
Set up controllers
265280
-----------------------------------------------------------
266281

‎gz_ros2_control_demos/CMakeLists.txt‎

Lines changed: 42 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -16,11 +16,19 @@ if(NOT WIN32)
1616
endif()
1717

1818
find_package(ament_cmake REQUIRED)
19-
find_package(control_msgs REQUIRED)
19+
find_package(geometry_msgs REQUIRED)
20+
find_package(gz_sim_vendor REQUIRED)
21+
find_package(gz-sim REQUIRED)
22+
find_package(pluginlib REQUIRED)
2023
find_package(rclcpp REQUIRED)
2124
find_package(rclcpp_action REQUIRED)
25+
find_package(rclcpp_lifecycle REQUIRED)
2226
find_package(std_msgs REQUIRED)
23-
find_package(geometry_msgs REQUIRED)
27+
28+
find_package(control_msgs REQUIRED)
29+
find_package(control_toolbox REQUIRED)
30+
find_package(gz_ros2_control REQUIRED)
31+
find_package(hardware_interface REQUIRED)
2432

2533
install(DIRECTORY
2634
launch
@@ -77,6 +85,27 @@ ament_target_dependencies(example_gripper
7785
std_msgs
7886
)
7987

88+
#########
89+
90+
add_library(gz_custom_hardware_plugins SHARED
91+
src/gz_custom_system.cpp
92+
)
93+
target_include_directories(gz_custom_hardware_plugins PUBLIC
94+
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
95+
$<INSTALL_INTERFACE:include/gz_custom_hardware_plugins>
96+
)
97+
target_link_libraries(gz_custom_hardware_plugins
98+
PUBLIC
99+
gz-sim::gz-sim
100+
control_toolbox::control_toolbox
101+
gz_ros2_control::gz_ros2_control-system
102+
hardware_interface::hardware_interface
103+
rclcpp::rclcpp
104+
rclcpp_lifecycle::rclcpp_lifecycle
105+
)
106+
107+
#########
108+
80109
if(BUILD_TESTING)
81110
find_package(ament_lint_auto REQUIRED)
82111

@@ -97,4 +126,15 @@ install(
97126
lib/${PROJECT_NAME}
98127
)
99128

129+
install(TARGETS
130+
gz_custom_hardware_plugins
131+
EXPORT export_gz_custom_hardware_plugins
132+
ARCHIVE DESTINATION lib
133+
LIBRARY DESTINATION lib
134+
RUNTIME DESTINATION bin
135+
)
136+
137+
ament_export_targets(export_gz_custom_hardware_plugins HAS_LIBRARY_TARGET)
138+
pluginlib_export_plugin_description_file(gz_ros2_control gz_custom_hardware_plugins.xml)
139+
100140
ament_package()

‎gz_ros2_control_demos/gz_custom_hardware_plugins.xml‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
<library path="gz_custom_hardware_plugins">
2+
<class
3+
name="gz_ros2_control_demos/GazeboCustomSimSystem"
4+
type="gz_ros2_control_demos::GazeboCustomSimSystem"
5+
base_class_type="gz_ros2_control::GazeboSimSystemInterface">
6+
<description>
7+
Custom ros2_control hardware component to be loaded by the gazebo plugin.
8+
</description>
9+
</class>
10+
</library>

‎gz_ros2_control_demos/include/gz_ros2_control_demos/gz_custom_system.hpp‎

Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
// Copyright 2025 AIT Austrian Institute of Technology GmbH
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+
16+
#ifndef GZ_ROS2_CONTROL_DEMOS__GZ_CUSTOM_SYSTEM_HPP_
17+
#define GZ_ROS2_CONTROL_DEMOS__GZ_CUSTOM_SYSTEM_HPP_
18+
19+
#include <map>
20+
#include <memory>
21+
#include <string>
22+
#include <vector>
23+
24+
#include "gz_ros2_control/gz_system_interface.hpp"
25+
#include "rclcpp_lifecycle/state.hpp"
26+
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
27+
28+
namespace gz_ros2_control_demos
29+
{
30+
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
31+
32+
// Forward declaration
33+
class GazeboSimSystemPrivate;
34+
35+
// These class must inherit `gz_ros2_control::GazeboSimSystemInterface` which implements a
36+
// simulated `ros2_control` `hardware_interface::SystemInterface`.
37+
38+
class GazeboCustomSimSystem : public gz_ros2_control::GazeboSimSystemInterface
39+
{
40+
public:
41+
// Documentation Inherited
42+
CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams & params)
43+
override;
44+
45+
CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
46+
47+
// Documentation Inherited
48+
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
49+
50+
// Documentation Inherited
51+
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
52+
53+
// Documentation Inherited
54+
CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
55+
56+
// Documentation Inherited
57+
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
58+
59+
// Documentation Inherited
60+
hardware_interface::return_type read(
61+
const rclcpp::Time & time,
62+
const rclcpp::Duration & period) override;
63+
64+
// Documentation Inherited
65+
hardware_interface::return_type write(
66+
const rclcpp::Time & time,
67+
const rclcpp::Duration & period) override;
68+
69+
// Documentation Inherited
70+
bool initSim(
71+
rclcpp::Node::SharedPtr & model_nh,
72+
std::map<std::string, sim::Entity> & joints,
73+
const hardware_interface::HardwareInfo & hardware_info,
74+
sim::EntityComponentManager & _ecm,
75+
unsigned int update_rate) override;
76+
77+
private:
78+
/// \brief Private data class
79+
std::unique_ptr<GazeboSimSystemPrivate> dataPtr;
80+
};
81+
82+
} // namespace gz_ros2_control_demos
83+
84+
#endif // GZ_ROS2_CONTROL_DEMOS__GZ_CUSTOM_SYSTEM_HPP_

‎gz_ros2_control_demos/launch/cart_example_velocity_custom_plugin.launch.py‎

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
# Copyright 2025 AIT Austrian Institute of Technology GmbH
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+
from launch import LaunchDescription
16+
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
17+
from launch.actions import RegisterEventHandler
18+
from launch.event_handlers import OnProcessExit
19+
from launch.launch_description_sources import PythonLaunchDescriptionSource
20+
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
21+
22+
from launch_ros.actions import Node
23+
from launch_ros.substitutions import FindPackageShare
24+
25+
26+
def generate_launch_description():
27+
# Launch Arguments
28+
use_sim_time = LaunchConfiguration('use_sim_time', default=True)
29+
gz_args = LaunchConfiguration('gz_args', default='')
30+
31+
# Get URDF via xacro
32+
robot_description_content = Command(
33+
[
34+
PathJoinSubstitution([FindExecutable(name='xacro')]),
35+
' ',
36+
PathJoinSubstitution(
37+
[FindPackageShare('gz_ros2_control_demos'),
38+
'urdf', 'test_cart_velocity_custom_plugin.xacro.urdf']
39+
),
40+
]
41+
)
42+
robot_description = {'robot_description': robot_description_content}
43+
robot_controllers = PathJoinSubstitution(
44+
[
45+
FindPackageShare('gz_ros2_control_demos'),
46+
'config',
47+
'cart_controller_velocity.yaml',
48+
]
49+
)
50+
51+
node_robot_state_publisher = Node(
52+
package='robot_state_publisher',
53+
executable='robot_state_publisher',
54+
output='screen',
55+
parameters=[robot_description]
56+
)
57+
58+
gz_spawn_entity = Node(
59+
package='ros_gz_sim',
60+
executable='create',
61+
output='screen',
62+
arguments=['-topic', 'robot_description',
63+
'-name', 'cart', '-allow_renaming', 'true'],
64+
)
65+
66+
joint_state_broadcaster_spawner = Node(
67+
package='controller_manager',
68+
executable='spawner',
69+
arguments=['joint_state_broadcaster'],
70+
)
71+
joint_trajectory_controller_spawner = Node(
72+
package='controller_manager',
73+
executable='spawner',
74+
arguments=[
75+
'joint_trajectory_controller',
76+
'--param-file',
77+
robot_controllers,
78+
],
79+
)
80+
81+
# Bridge
82+
bridge = Node(
83+
package='ros_gz_bridge',
84+
executable='parameter_bridge',
85+
arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
86+
output='screen'
87+
)
88+
89+
return LaunchDescription([
90+
# Launch gazebo environment
91+
IncludeLaunchDescription(
92+
PythonLaunchDescriptionSource(
93+
[PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
94+
'launch',
95+
'gz_sim.launch.py'])]),
96+
launch_arguments=[('gz_args', [gz_args, ' -r -v 1 empty.sdf'])]),
97+
RegisterEventHandler(
98+
event_handler=OnProcessExit(
99+
target_action=gz_spawn_entity,
100+
on_exit=[joint_state_broadcaster_spawner],
101+
)
102+
),
103+
RegisterEventHandler(
104+
event_handler=OnProcessExit(
105+
target_action=joint_state_broadcaster_spawner,
106+
on_exit=[joint_trajectory_controller_spawner],
107+
)
108+
),
109+
bridge,
110+
node_robot_state_publisher,
111+
gz_spawn_entity,
112+
# Launch Arguments
113+
DeclareLaunchArgument(
114+
'use_sim_time',
115+
default_value=use_sim_time,
116+
description='If true, use simulated clock'),
117+
])

‎gz_ros2_control_demos/package.xml‎

Lines changed: 12 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -17,12 +17,15 @@
1717

1818
<buildtool_depend>ament_cmake</buildtool_depend>
1919

20-
<build_depend>control_msgs</build_depend>
2120
<build_depend>geometry_msgs</build_depend>
21+
<build_depend>pluginlib</build_depend>
2222
<build_depend>rclcpp</build_depend>
2323
<build_depend>rclcpp_action</build_depend>
2424
<build_depend>std_msgs</build_depend>
2525

26+
<depend>gz_sim_vendor</depend>
27+
<depend>rclcpp_lifecycle</depend>
28+
2629
<exec_depend>ament_index_python</exec_depend>
2730
<exec_depend>geometry_msgs</exec_depend>
2831
<exec_depend>launch_ros</exec_depend>
@@ -36,13 +39,18 @@
3639
<exec_depend>xacro</exec_depend>
3740

3841
<!-- ros2_control -->
42+
<<<<<<< HEAD
43+
=======
44+
<depend>control_msgs</depend>
45+
<depend>control_toolbox</depend>
46+
<depend>gz_ros2_control</depend>
47+
<depend>hardware_interface</depend>
48+
<depend>ros2_control_cmake</depend>
49+
>>>>>>> 0dab1d5 (Add a custom plugin for simulating actuator dynamics to the demos (#693))
3950
<exec_depend>ackermann_steering_controller</exec_depend>
40-
<exec_depend>control_msgs</exec_depend>
4151
<exec_depend>diff_drive_controller</exec_depend>
4252
<exec_depend>effort_controllers</exec_depend>
4353
<exec_depend>force_torque_sensor_broadcaster</exec_depend>
44-
<exec_depend>gz_ros2_control</exec_depend>
45-
<exec_depend>hardware_interface</exec_depend>
4654
<exec_depend>imu_sensor_broadcaster</exec_depend>
4755
<exec_depend>joint_state_broadcaster</exec_depend>
4856
<exec_depend>joint_trajectory_controller</exec_depend>

0 commit comments

Comments
 (0)