Skip to content

Commit 0dab1d5

Browse files
christophfroehlichahcorde
christophfroehlich
and
ahcorde
authored
Add a custom plugin for simulating actuator dynamics to the demos (#693)
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
1 parent 2e53b95 commit 0dab1d5

File tree

10 files changed

+892
-11
lines changed

10 files changed

+892
-11
lines changed

‎doc/index.rst‎

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

243-
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).
243+
The *gz_ros2_control* Gazebo plugin also provides a pluginlib-based interface to implement custom interfaces between
244+
Gazebo and *ros2_control* for simulating more complex mechanisms (nonlinear springs, linkages, etc) or actuator dynamics.
244245

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

248249
The respective GazeboSimSystemInterface is specified in a URDF or SDF model and is loaded when the
249-
robot model is loaded. For example, the following XML will load a custom plugin:
250+
robot model is loaded. For example, the following XML will load a custom plugin instead:
250251

251252
.. tabs::
252253

@@ -280,6 +281,15 @@ robot model is loaded. For example, the following XML will load a custom plugin:
280281
...
281282
</plugin>
282283
284+
The ``gz_ros2_control_demos/GazeboCustomSimSystem`` demonstrates how to implement actuator dynamics for a joint with
285+
velocity command interface by using a configurable low pass filter. Run
286+
287+
.. code-block:: shell
288+
289+
ros2 launch gz_ros2_control_demos cart_example_velocity_custom_plugin.launch.py
290+
291+
and compare it with the behavior of ``cart_example_velocity.launch.py`` using any plotting tool like plotjuggler.
292+
283293
Set up controllers
284294
-----------------------------------------------------------
285295

‎gz_ros2_control_demos/CMakeLists.txt‎

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

1212
find_package(ament_cmake REQUIRED)
13-
find_package(control_msgs REQUIRED)
13+
find_package(geometry_msgs REQUIRED)
14+
find_package(gz_sim_vendor REQUIRED)
15+
find_package(gz-sim REQUIRED)
16+
find_package(pluginlib REQUIRED)
1417
find_package(rclcpp REQUIRED)
1518
find_package(rclcpp_action REQUIRED)
19+
find_package(rclcpp_lifecycle REQUIRED)
1620
find_package(std_msgs REQUIRED)
17-
find_package(geometry_msgs REQUIRED)
21+
22+
find_package(control_msgs REQUIRED)
23+
find_package(control_toolbox REQUIRED)
24+
find_package(gz_ros2_control REQUIRED)
25+
find_package(hardware_interface REQUIRED)
1826

1927
install(DIRECTORY
2028
launch
@@ -60,6 +68,27 @@ target_link_libraries(example_gripper PUBLIC
6068
rclcpp::rclcpp
6169
)
6270

71+
#########
72+
73+
add_library(gz_custom_hardware_plugins SHARED
74+
src/gz_custom_system.cpp
75+
)
76+
target_include_directories(gz_custom_hardware_plugins PUBLIC
77+
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
78+
$<INSTALL_INTERFACE:include/gz_custom_hardware_plugins>
79+
)
80+
target_link_libraries(gz_custom_hardware_plugins
81+
PUBLIC
82+
gz-sim::gz-sim
83+
control_toolbox::control_toolbox
84+
gz_ros2_control::gz_ros2_control-system
85+
hardware_interface::hardware_interface
86+
rclcpp::rclcpp
87+
rclcpp_lifecycle::rclcpp_lifecycle
88+
)
89+
90+
#########
91+
6392
if(BUILD_TESTING)
6493
find_package(ament_lint_auto REQUIRED)
6594

@@ -78,4 +107,15 @@ install(
78107
lib/${PROJECT_NAME}
79108
)
80109

110+
install(TARGETS
111+
gz_custom_hardware_plugins
112+
EXPORT export_gz_custom_hardware_plugins
113+
ARCHIVE DESTINATION lib
114+
LIBRARY DESTINATION lib
115+
RUNTIME DESTINATION bin
116+
)
117+
118+
ament_export_targets(export_gz_custom_hardware_plugins HAS_LIBRARY_TARGET)
119+
pluginlib_export_plugin_description_file(gz_ros2_control gz_custom_hardware_plugins.xml)
120+
81121
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: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -16,12 +16,15 @@
1616

1717
<buildtool_depend>ament_cmake</buildtool_depend>
1818

19-
<build_depend>control_msgs</build_depend>
2019
<build_depend>geometry_msgs</build_depend>
20+
<build_depend>pluginlib</build_depend>
2121
<build_depend>rclcpp</build_depend>
2222
<build_depend>rclcpp_action</build_depend>
2323
<build_depend>std_msgs</build_depend>
2424

25+
<depend>gz_sim_vendor</depend>
26+
<depend>rclcpp_lifecycle</depend>
27+
2528
<exec_depend>ament_index_python</exec_depend>
2629
<exec_depend>geometry_msgs</exec_depend>
2730
<exec_depend>launch_ros</exec_depend>
@@ -35,14 +38,15 @@
3538
<exec_depend>xacro</exec_depend>
3639

3740
<!-- ros2_control -->
41+
<depend>control_msgs</depend>
42+
<depend>control_toolbox</depend>
43+
<depend>gz_ros2_control</depend>
44+
<depend>hardware_interface</depend>
3845
<depend>ros2_control_cmake</depend>
3946
<exec_depend>ackermann_steering_controller</exec_depend>
40-
<exec_depend>control_msgs</exec_depend>
4147
<exec_depend>diff_drive_controller</exec_depend>
4248
<exec_depend>effort_controllers</exec_depend>
4349
<exec_depend>force_torque_sensor_broadcaster</exec_depend>
44-
<exec_depend>gz_ros2_control</exec_depend>
45-
<exec_depend>hardware_interface</exec_depend>
4650
<exec_depend>imu_sensor_broadcaster</exec_depend>
4751
<exec_depend>joint_state_broadcaster</exec_depend>
4852
<exec_depend>joint_trajectory_controller</exec_depend>

0 commit comments

Comments
 (0)