diff --git a/ros2_control_demo_robot/controllers/rrbot_gazebo_forward_controller_position.yaml b/ros2_control_demo_robot/controllers/rrbot_gazebo_forward_controller_position.yaml
new file mode 100644
index 000000000..5df0e5d8e
--- /dev/null
+++ b/ros2_control_demo_robot/controllers/rrbot_gazebo_forward_controller_position.yaml
@@ -0,0 +1,16 @@
+controller_manager:
+ ros__parameters:
+ update_rate: 100 # Hz
+
+ joint_trajectory_controller:
+ type: joint_trajectory_controller/JointTrajectoryController
+
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+
+joint_trajectory_controller:
+ ros__parameters:
+ joints:
+ - joint1
+ - joint2
+ interface_name: position
diff --git a/ros2_control_demo_robot/description/rrbot/rrbot.gazebo.xacro b/ros2_control_demo_robot/description/rrbot/rrbot.gazebo.xacro
index bdbc2a7f8..347d6f61f 100644
--- a/ros2_control_demo_robot/description/rrbot/rrbot.gazebo.xacro
+++ b/ros2_control_demo_robot/description/rrbot/rrbot.gazebo.xacro
@@ -9,9 +9,8 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
-
- /rrbot
- gazebo_ros_control/DefaultRobotHWSim
+
+ $(find ros2_control_demo_robot)/controllers/rrbot_gazebo_forward_controller_position.yaml
diff --git a/ros2_control_demo_robot/description/rrbot/rrbot_system_position_only.ros2_control.xacro b/ros2_control_demo_robot/description/rrbot/rrbot_system_position_only.ros2_control.xacro
index a97ecdeeb..633b05e6e 100644
--- a/ros2_control_demo_robot/description/rrbot/rrbot_system_position_only.ros2_control.xacro
+++ b/ros2_control_demo_robot/description/rrbot/rrbot_system_position_only.ros2_control.xacro
@@ -1,15 +1,24 @@
-
+
-
- ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware
- 2.0
- 3.0
- 2.0
-
+
+
+
+ gazebo_ros2_control/GazeboSystem
+
+
+
+
+ ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware
+ 2.0
+ 3.0
+ 2.0
+
+
+
-1
@@ -29,4 +38,3 @@
-
diff --git a/ros2_control_demo_robot/description/rrbot_system_position_only.urdf.xacro b/ros2_control_demo_robot/description/rrbot_system_position_only.urdf.xacro
index c25494ab7..9f6c0f5a5 100644
--- a/ros2_control_demo_robot/description/rrbot_system_position_only.urdf.xacro
+++ b/ros2_control_demo_robot/description/rrbot_system_position_only.urdf.xacro
@@ -5,7 +5,8 @@ Copied and modified from ROS1 example:
https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro
-->
-
+
+
@@ -34,6 +35,6 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
-
+
diff --git a/ros2_control_demo_robot/launch/rrbot_system_position_only_gazebo.launch.py b/ros2_control_demo_robot/launch/rrbot_system_position_only_gazebo.launch.py
new file mode 100644
index 000000000..0114209d5
--- /dev/null
+++ b/ros2_control_demo_robot/launch/rrbot_system_position_only_gazebo.launch.py
@@ -0,0 +1,60 @@
+# Copyright 2021 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node
+
+import xacro
+
+
+def generate_launch_description():
+ gazebo = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource([os.path.join(
+ get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
+ )
+
+ robot_description_path = os.path.join(
+ get_package_share_directory('ros2_control_demo_robot'),
+ 'description',
+ 'rrbot_system_position_only.urdf.xacro')
+ robot_description_config = xacro.process_file(
+ robot_description_path,
+ mappings={'use_sim': 'true'})
+ robot_description = {'robot_description': robot_description_config.toxml()}
+
+ node_robot_state_publisher = Node(
+ package='robot_state_publisher',
+ executable='robot_state_publisher',
+ output='screen',
+ parameters=[robot_description]
+ )
+
+ spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
+ arguments=['-topic', 'robot_description',
+ '-entity', 'rrbot_system_position'],
+ output='screen')
+
+ return LaunchDescription([
+ gazebo,
+ node_robot_state_publisher,
+ spawn_entity,
+ ])
diff --git a/ros2_control_demo_robot/package.xml b/ros2_control_demo_robot/package.xml
index b0c761395..4322595a3 100644
--- a/ros2_control_demo_robot/package.xml
+++ b/ros2_control_demo_robot/package.xml
@@ -14,6 +14,9 @@
hardware_interface
pluginlib
rclcpp
+ gazebo_ros2_control
+
+ xacro
ros2_control_demo_hardware