Skip to content

ROS2 Control: Position control

ROS2 control docs

Gazebo simulation

controller manager

The controller manager loaded by gazebo

urdf/xacro
1
2
3
4
5
6
7
8
9
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <gazebo>
        <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
            <parameters>
                $(find tutorial_bringup)/config/robot_controller.yaml
            </parameters>
        </plugin>
    </gazebo>
</robot>

Once gazebo launch it's load the control plugin and parse the <ros2_control> block.

  • Init the hardware interface
  • Launch the controller manager
xacro load gazebo control plugin and set ros2_control tag
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
    <gazebo>
        <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
            <parameters>
                $(find tutorial_bringup)/config/position_controllers.yaml
            </parameters>
        </plugin>

    </gazebo>

    <ros2_control name="robot" type="system">
        <hardware>
            <plugin>gz_ros2_control/GazeboSimSystem</plugin>
        </hardware>

        <joint name="first_joint">
            <command_interface name="position">
                <param name="min">-1.571</param>
                <param name="max">1.571</param>
            </command_interface>

            <state_interface name="position" />
        </joint>

    </ros2_control>
</robot>
controllers yaml file 
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
controller_manager:
  ros__parameters:
    update_rate: 10


    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    position_controller:
      type: position_controllers/JointGroupPositionController


position_controller:
  ros__parameters:
    joints:
      - first_joint
launch file 
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration, Command
from launch_ros.parameter_descriptions import ParameterValue
import xacro
from pathlib import Path

PKG_BRINGUP = "tutorial_bringup"
PKG_DESCRIPTION = "tutorial_description"
ROBOT = "robot.xacro"
CONFIG = "config"
URDF = "urdf"
BRIDGE_CONFIG = "gz_bridge.yaml"

def generate_launch_description():
    ld = LaunchDescription()

    pkg_path = os.path.join(get_package_share_directory(PKG_DESCRIPTION))
    xacro_file = os.path.join(pkg_path, URDF, ROBOT)
    robot_description = xacro.process_file(xacro_file).toxml()

    node_robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="screen",
        parameters=[{"robot_description": robot_description, "use_sim_time": True}],
    )

    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [
                PathJoinSubstitution(
                    [
                        get_package_share_directory("ros_gz_sim"),
                        "launch",
                        "gz_sim.launch.py",
                    ]
                )
            ]
        ),
        launch_arguments=[("gz_args", " -r -v 3 my_world.sdf")],
    )

    gz_spawn_entity = Node(
        package="ros_gz_sim",
        executable="create",
        output="screen",
        arguments=["-name", "robot", "-topic", "/robot_description"],
    )


    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster"],
        output="screen",
    )

    control_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["position_controller"],
        output="screen",
    )

    gazebo_bridge = Node(
        package="ros_gz_bridge",
        executable="parameter_bridge",
        name="parameter_bridge",
        output="screen",
        arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"],
    )


    ld.add_action(node_robot_state_publisher)
    ld.add_action(gazebo)
    ld.add_action(gz_spawn_entity)
    ld.add_action(gazebo_bridge)
    ld.add_action(control_spawner)
    ld.add_action(joint_state_broadcaster_spawner)

    return ld

usage

send command
1
ros2 topic pub -1 /position_controller/commands std_msgs/msg/Float64MultiArray "{ data: [1.0] }"

State

State define in ros2_control

1
<state_interface name="position" />
echo state
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
ros2 topic echo /dynamic_joint_states
#
---
header:
  stamp:
    sec: 167
    nanosec: 299000000
  frame_id: ''
joint_names:
- first_joint
interface_values:
- interface_names:
  - position
  values:
  - 1.000000000000095
---

Extent the state interface

Add state to ros2_control section 

1
2
<state_interface name="position" />
<state_interface name="velocity" />


 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
header:
stamp:
    sec: 85
    nanosec: 799000000
frame_id: ''
joint_names:
- first_joint
interface_values:
- interface_names:
- position
- velocity
values:
- 1.00
- 0