Skip to content

Commit f0b450e

Browse files
mergify[bot]mergify[bot]
authored
Add a custom plugin for simulating actuator dynamics to the demos (backport #693) (#780)
1 parent 4bf1888 commit f0b450e

10 files changed

Lines changed: 931 additions & 12 deletions

File tree

‎doc/index.rst‎

Lines changed: 14 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -239,13 +239,14 @@ 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

247-
The respective GazeboSimSystemInterface is specified in a URDF model and is loaded when the
248-
robot model is loaded. For example, the following XML will load a custom plugin:
248+
The respective GazeboSimSystemInterface is specified in a URDF and is loaded when the
249+
robot model is loaded. For example, the following XML will load a custom plugin instead:
249250

250251
.. code-block:: xml
251252
@@ -261,6 +262,15 @@ robot model is loaded. For example, the following XML will load a custom plugin:
261262
</plugin>
262263
</gazebo>
263264
265+
The ``gz_ros2_control_demos/GazeboCustomSimSystem`` demonstrates how to implement actuator dynamics for a joint with
266+
velocity command interface by using a configurable low pass filter. Run
267+
268+
.. code-block:: shell
269+
270+
ros2 launch gz_ros2_control_demos cart_example_velocity_custom_plugin.launch.py
271+
272+
and compare it with the behavior of ``cart_example_velocity.launch.py`` using any plotting tool like plotjuggler.
273+
264274
Set up controllers
265275
-----------------------------------------------------------
266276

‎gz_ros2_control_demos/CMakeLists.txt‎

Lines changed: 56 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -16,11 +16,32 @@ 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(pluginlib REQUIRED)
2021
find_package(rclcpp REQUIRED)
2122
find_package(rclcpp_action REQUIRED)
23+
find_package(rclcpp_lifecycle REQUIRED)
2224
find_package(std_msgs REQUIRED)
23-
find_package(geometry_msgs REQUIRED)
25+
26+
find_package(control_msgs REQUIRED)
27+
find_package(control_toolbox REQUIRED)
28+
find_package(gz_ros2_control REQUIRED)
29+
find_package(hardware_interface REQUIRED)
30+
31+
set(GZ_SIM)
32+
33+
if("$ENV{GZ_VERSION}" STREQUAL "harmonic")
34+
find_package(gz-sim8 REQUIRED)
35+
set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
36+
message(STATUS "Compiling against Gazebo harmonic")
37+
set(GZ_SIM gz-sim${GZ_SIM_VER}::core)
38+
add_definitions(-DGZ_HEADERS)
39+
else()
40+
find_package(ignition-gazebo6 REQUIRED)
41+
set(GZ_SIM_VER ${ignition-gazebo6_VERSION_MAJOR})
42+
message(STATUS "Compiling against Gazebo fortress")
43+
set(GZ_SIM ignition-gazebo${GZ_SIM_VER}::core)
44+
endif()
2445

2546
install(DIRECTORY
2647
launch
@@ -77,6 +98,28 @@ ament_target_dependencies(example_gripper
7798
std_msgs
7899
)
79100

101+
#########
102+
103+
add_library(gz_custom_hardware_plugins SHARED
104+
src/gz_custom_system.cpp
105+
)
106+
target_include_directories(gz_custom_hardware_plugins PUBLIC
107+
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
108+
$<INSTALL_INTERFACE:include/gz_custom_hardware_plugins>
109+
)
110+
target_link_libraries(gz_custom_hardware_plugins
111+
${GZ_SIM}
112+
)
113+
ament_target_dependencies(gz_custom_hardware_plugins
114+
control_toolbox
115+
hardware_interface
116+
gz_ros2_control
117+
rclcpp
118+
rclcpp_lifecycle
119+
)
120+
121+
#########
122+
80123
if(BUILD_TESTING)
81124
find_package(ament_lint_auto REQUIRED)
82125

@@ -97,4 +140,15 @@ install(
97140
lib/${PROJECT_NAME}
98141
)
99142

143+
install(TARGETS
144+
gz_custom_hardware_plugins
145+
EXPORT export_gz_custom_hardware_plugins
146+
ARCHIVE DESTINATION lib
147+
LIBRARY DESTINATION lib
148+
RUNTIME DESTINATION bin
149+
)
150+
151+
ament_export_targets(export_gz_custom_hardware_plugins HAS_LIBRARY_TARGET)
152+
pluginlib_export_plugin_description_file(gz_ros2_control gz_custom_hardware_plugins.xml)
153+
100154
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::HardwareInfo & system_info)
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+
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: 13 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
<?xml version="1.0"?>
2-
<package format="2">
2+
<package format="3">
33
<name>gz_ros2_control_demos</name>
44
<version>0.7.18</version>
55
<description>gz_ros2_control_demos</description>
@@ -17,12 +17,19 @@
1717

1818
<buildtool_depend>ament_cmake</buildtool_depend>
1919

20-
<build_depend>control_msgs</build_depend>
20+
<!-- Harmonic -->
21+
<depend condition="$GZ_VERSION == harmonic">gz-sim8</depend>
22+
<!-- Fortress (default) -->
23+
<depend condition="$GZ_VERSION == '' or $GZ_VERSION == fortress">ignition-gazebo6</depend>
24+
2125
<build_depend>geometry_msgs</build_depend>
26+
<build_depend>pluginlib</build_depend>
2227
<build_depend>rclcpp</build_depend>
2328
<build_depend>rclcpp_action</build_depend>
2429
<build_depend>std_msgs</build_depend>
2530

31+
<depend>rclcpp_lifecycle</depend>
32+
2633
<exec_depend>ament_index_python</exec_depend>
2734
<exec_depend>geometry_msgs</exec_depend>
2835
<exec_depend>launch_ros</exec_depend>
@@ -36,13 +43,14 @@
3643
<exec_depend>xacro</exec_depend>
3744

3845
<!-- ros2_control -->
46+
<depend>control_msgs</depend>
47+
<depend>control_toolbox</depend>
48+
<depend>gz_ros2_control</depend>
49+
<depend>hardware_interface</depend>
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)