0
$\begingroup$

Rosanswers logo

Hi everyone,

I have a question. So, is there any single plugin that works with individual wheel on robot with 4 wheels?

I'm using Gazebo11 and Foxy (ros2).

As I'm aware that libgazebo_ros_diff_drive controls 2 wheels and libgazebo_ros_ackermann_drive controls 4 wheels. I wanted to have a plugin where I'm able to control a single wheel or run with all wheels individual. Do you know the name of the plugin? Any tutorial too?

Thank you so much for your time!

Update 2

Here is my current launch file:

#!/usr/bin/env python3


from launch.substitutions import Command, LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch import LaunchDescription
from launch_ros.actions import Node
from launch import LaunchDescription, LaunchContext
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
import launch_ros
import os
import sys
import launch_ros.actions
import xacro
import launch


def generate_launch_description():
    pkg_path = FindPackageShare("my_robot_description")

    use_sim = LaunchConfiguration("use_sim")
    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")

    xacro_path = PathJoinSubstitution([pkg_path, "urdf", "my_robot.urdf"])
    assert os.path.exists(xacro_path.perform(LaunchContext())), (
    "The xacro file doesn't exist (" + str(xacro_path.perform(LaunchContext())) + ")"
    )



    robot_description_content = Command(
    [
        PathJoinSubstitution([FindExecutable(name="xacro")]),
        " ",
        PathJoinSubstitution(
            [
                PathJoinSubstitution([FindExecutable(name="xacro")]),
                " ",
                xacro_path,
                " ",
                "use_sim:=",
                use_sim,
                " ",
                "use_fake_hardware:=",
                use_fake_hardware,
                " ",
                "fake_sensor_commands:=",
                fake_sensor_commands,

                FindPackageShare("my_robot_description"),
                "src/description",
                "my_robot.urdf",
            ]
        ),
    ]
    )

    robot_description = {"robot_description": robot_description_content}

    controller_config_path = PathJoinSubstitution(
    [
        pkg_path,
        "config",
        "gazebo_ros_control_params.yaml",
    ]
    )
    assert os.path.exists(controller_config_path.perform(LaunchContext())), (
    "The controller config doesn't exist ("
    + str(controller_config_path.perform(LaunchContext()))
    + ")"
    )

    robot_description = {"robot_description": robot_description_content}
    rviz_config_file = PathJoinSubstitution(
    [FindPackageShare("my_robot_description"), "rviz", "urdf_config.rviz"]
    )

    rviz_node = Node(
    package="rviz2",
    executable="rviz2",
    name="rviz2",
    output="log",
    arguments=["-d", rviz_config_file],
    )
    robot_state_pub_node = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    output="both",
    parameters=[robot_description],
    )
    spawn_entity = launch_ros.actions.Node(
    package='gazebo_ros',
    executable='spawn_entity.py',
    arguments=['-entity', 'my_robot', '-topic', 'robot_description'],
    output='screen'
    )

    return launch.LaunchDescription([
    #
    # Launch Arguments
    #
    DeclareLaunchArgument(
        "use_sim",
        default_value="false",
        description="Use simulation (Gazebo) clock if true",
    ),
    DeclareLaunchArgument(
        "use_fake_hardware",
        default_value="true",
        description="Start robot with fake hardware mirroring command to its states.",
    ),
    DeclareLaunchArgument(
        "fake_sensor_commands",
        default_value="false",
        description="Enable fake command interfaces for sensors used for simple simulations. \
                Used only if 'use_fake_hardware' parameter is true.",
    ),
    Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[robot_description, controller_config_path],
        output={
            "stdout": "screen",
            "stderr": "screen",
        },
        condition=UnlessCondition(use_sim),
    ),
    Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        parameters=[robot_description, {"use_sim_time": use_sim}],
    ),
    Node(
        package="controller_manager",
        executable="spawner.py",
        arguments=[
            "joint_state_broadcaster",
            "--controller-manager",
            "/controller_manager",
        ],
    ),
    Node(
        package="controller_manager",
        executable="spawner.py",
        arguments=["wheel_velocity_controller", "-c", "/controller_manager"],
    ),
    launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'], output='screen'),
    launch.actions.ExecuteProcess(
        name='fake_joint_calibration',
        cmd=['ros2', 'topic', 'pub', '/joint_states', 'sensor_msgs/msg/JointState', '"{data True}"'],
        output='screen',
        shell='True'
    ),

    robot_state_pub_node,
    rviz_node,
    spawn_entity,
    ])

Originally posted by kak13 on ROS Answers with karma: 113 on 2021-08-10

Post score: 1

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I'm not really an expert on this and I myself struggled the last two weeks with ROS2, Gazebo and ROS2_Control, but I think you should be able to use ROS2_Control for that.

Here some useful links:

You have to add a ros2_control tag to your URDF file. (ref). Mine looks like this:

  <!-- ROS Control for the rims -->
  <ros2_control name="cassiopeia_rim_control" type="system">
    <xacro:if value="$(arg use_sim)">
      <hardware>
        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
      </hardware>
    </xacro:if>
    <xacro:unless value="$(arg use_sim)">
      <hardware>
        <xacro:if value="$(arg use_fake_hardware)">
          <plugin>fake_components/GenericSystem</plugin>
          <param name="fake_sensor_commands">$(arg fake_sensor_commands)</param>
          <param name="state_following_offset">0.0</param>
        </xacro:if>
        <xacro:unless value="$(arg use_fake_hardware)">
          <!-- TODO: If we want this feature, we have to write an
                     hardware controller plugin in C++ for ROS2 Control.
            -->
        </xacro:unless>
      </hardware>
    </xacro:unless>

    <joint name="a_rim_joint">
      <command_interface name="velocity">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="b_rim_joint">
      <command_interface name="velocity">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="c_rim_joint">
      <command_interface name="velocity">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
  </ros2_control>

  <gazebo>
    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <parameters>$(find pkg_name)/config/controllers.yaml</parameters>
    </plugin>
  </gazebo>

It is worth noting that, if you want to use ROS2 Control with gazebo, you have to use one ros2_control tag with type system for all joints. Additionally, the xacro:if in the beginning is if you want to use the same model in Gazebo and on the real robot. If you are only interested in Gazebo, just evaluate use_sim to true.

Then you'll also need the specified controllers.yaml. Here is mine:

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    wheel_velocity_controller:
      type: velocity_controllers/JointGroupVelocityController


wheel_velocity_controller:
  ros__parameters:
    joints:
      - a_rim_joint
      - b_rim_joint
      - c_rim_joint

    command_interfaces:
      - velocity

    state_interfaces:
      - position
      - velocity

Here we have one controller for all three joints, that can be controlled with topic: /wheel_velocity_controller/commands of type: Type: std_msgs/msg/Float64MultiArray. If you want different topics for each joint, just define more controllers in the yaml file.

Update 1: Here is my launch file:

from launch import LaunchDescription, LaunchContext
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

import os


def generate_launch_description():
    pkg_path = FindPackageShare("cassiopeia_description")

    use_sim = LaunchConfiguration("use_sim")
    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")

    xacro_path = PathJoinSubstitution([pkg_path, "urdf", "cassiopeia.urdf.xacro"])
    assert os.path.exists(xacro_path.perform(LaunchContext())), (
        "The xacro file doesn't exist (" + str(xacro_path.perform(LaunchContext())) + ")"
    )

    # Get URDF from Xacro
    robot_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            xacro_path,
            " ",
            "use_sim:=",
            use_sim,
            " ",
            "use_fake_hardware:=",
            use_fake_hardware,
            " ",
            "fake_sensor_commands:=",
            fake_sensor_commands,
        ]
    )
    robot_description = {"robot_description": robot_description_content}

    controller_config_path = PathJoinSubstitution(
        [
            pkg_path,
            "config",
            "controllers.yaml",
        ]
    )
    assert os.path.exists(controller_config_path.perform(LaunchContext())), (
        "The controller config doesn't exist ("
        + str(controller_config_path.perform(LaunchContext()))
        + ")"
    )

    return LaunchDescription(
        [
            #
            # Launch Arguments
            #
            DeclareLaunchArgument(
                "use_sim",
                default_value="false",
                description="Use simulation (Gazebo) clock if true",
            ),
            DeclareLaunchArgument(
                "use_fake_hardware",
                default_value="true",
                description="Start robot with fake hardware mirroring command to its states.",
            ),
            DeclareLaunchArgument(
                "fake_sensor_commands",
                default_value="false",
                description="Enable fake command interfaces for sensors used for simple simulations. \
                Used only if 'use_fake_hardware' parameter is true.",
            ),
            #
            # Nodes
            #
            Node(
                package="controller_manager",
                executable="ros2_control_node",
                parameters=[robot_description, controller_config_path],
                output={
                    "stdout": "screen",
                    "stderr": "screen",
                },
                condition=UnlessCondition(use_sim),
            ),
            Node(
                package="robot_state_publisher",
                executable="robot_state_publisher",
                output="both",
                parameters=[robot_description, {"use_sim_time": use_sim}],
            ),
            Node(
                package="controller_manager",
                executable="spawner.py",
                arguments=[
                    "joint_state_broadcaster",
                    "--controller-manager",
                    "/controller_manager",
                ],
            ),
            Node(
                package="controller_manager",
                executable="spawner.py",
                arguments=["wheel_velocity_controller", "-c", "/controller_manager"],
            ),
            #
            # Launch Files
            #
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    [
                        PathJoinSubstitution(
                            [FindPackageShare("kamaro_gazebo"), "launch", "simulation.launch.py"]
                        )
                    ]
                ),
                launch_arguments={"verbose": "false"}.items(),
                condition=IfCondition(use_sim),
            ),
        ]
    )

Please note, that in foxy and below, you have to avoid : in your xml comments in the xacro file. See here: https://answers.ros.org/question/382000/ros2-makes-launch-files-crazy-too-soon-to-be-migrating/?answer=382141#post-id-382141

Update 2: It's not surprising, that you have a lot of issues. Here are some things that are don't make any sense.

  1. How to read an URDF/Xacro file into an param to give it to the robot_state_publisher
robot_description_content = Command(
    [
        PathJoinSubstitution([FindExecutable(name="xacro")]),
        " ",
        PathJoinSubstitution(
            [
                PathJoinSubstitution([FindExecutable(name="xacro")]),
                " ",
                xacro_path,
                " ",
                "use_sim:=",
                use_sim,
                " ",
                "use_fake_hardware:=",
                use_fake_hardware,
                " ",
                "fake_sensor_commands:=",
                fake_sensor_commands,

                FindPackageShare("my_robot_description"),
                "src/description",
                "my_robot.urdf",
            ]
        ),
    ]
)

Here is what you are doing. run /path/to/xacro path/to/xacro path/to/model.urdf.xacro arg1:=a arg2:=b arg3:=c /path/to/robot_desc/src/desc/my_robot.urdf Xacro is an command which takes a path to an xacro file und returns an urdf file. You can run xacro --help to see this.

Here is what you should do: run xacro /file/to/model.urdf.xacro arg1:=a. The arg part is only needed if you pass arguments to your xacro file. I'll make an example later. You can also think of PathJoinSubstitution as os.path.join. The only thing it does, is evaluating the params a bit later. For now, here is how this launch looks like:

robot_description_content = Command(
    [
        PathJoinSubstitution([FindExecutable(name="xacro")]),
        " ",
        xacro_path,
        " ",
        "use_sim:=",
        use_sim,
        " ",
        "use_fake_hardware:=",
        use_fake_hardware,
        " ",
        "fake_sensor_commands:=",
        fake_sensor_commands,
    ]
)

In the end, this will make a call like this: /opt/ros/foxy/bin/xacro ~/ws/install/robot_description/share/robot_description/urdf/robot.urdf.xacro use_sim:=false use_fake_hardware:=true fake_sensor_commands:=false

And as I sayed, you only need those arguments at the end, if you use them in your xacro file. Here is how you do this:

In your robot.urdf.xacro file:

<!-- Enable setting arguments from the launch file -->
<xacro:arg name="param1" default="true" />
<xacro:arg name="param2" default="1337" />

<!-- Use xacro params as if-else -->
<xacro:if value="$(arg use_sim)">
  Test
</xacro:if>
<param name="name">$(arg param2)</param>

You can always debug your xacro file with the command xacro path/to/robot.urdf.xacro.

  1. robot_description = {"robot_description": robot_description_content} is there twice.

  2. I gave up and created a launch file, that does anything I think you'll need. Read through the ros2 control demos and other questions here. The Xacro params I use, like use_sim are for making the robot_description usable on the real robot as well as in gazebo. For what I understand, you only need the Gazebo capability. So don't forget your <ros2_control name="..." type="system"> tag in your xacro file, as well as, this gazebo tag:

    $(find cassiopeia_description)/config/controllers.yaml

And here is your launch file:

from launch import LaunchDescription, LaunchContext
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node

import os


def generate_launch_description():
    pkg_path = FindPackageShare("my_robot_description")

    # Check if xacro file does exist
    xacro_path = PathJoinSubstitution([pkg_path, "urdf", "my_robot.urdf"])
    assert os.path.exists(xacro_path.perform(LaunchContext())), (
        "The xacro file doesn't exist (" + str(xacro_path.perform(LaunchContext())) + ")"
    )

    # Create robot_description
    robot_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            xacro_path,
        ]
    )
    robot_description = {"robot_description": robot_description_content}

    # Check if controller config does exist
    controller_config_path = PathJoinSubstitution(
        [
            pkg_path,
            "config",
            "gazebo_ros_control_params.yaml",
        ]
    )
    assert os.path.exists(controller_config_path.perform(LaunchContext())), (
        "The controller config doesn't exist ("
        + str(controller_config_path.perform(LaunchContext()))
        + ")"
    )

    # Check if rviz file does exist
    rviz_config_file = PathJoinSubstitution([pkg_path, "rviz", "urdf_config.rviz"])
    assert os.path.exists(rviz_config_file.perform(LaunchContext())), (
        "The controller config doesn't exist ("
        + str(rviz_config_file.perform(LaunchContext()))
        + ")"
    )

    # Launch configuration variables specific to simulation
    gui = LaunchConfiguration("gui")
    world = LaunchConfiguration("world")

    return LaunchDescription(
        [
            # Launch arguments used for gazebo
            DeclareLaunchArgument(
                "gui", default_value="True", description="Whether to execute gzclient"
            ),
            DeclareLaunchArgument(
                "world",
                default_value=PathJoinSubstitution([pkg_path, "worlds", "willowgarage.world"]),
                description="Full path to world model file to load",
            ),
            # Launch gazebo server and client
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    [
                        PathJoinSubstitution(
                            [FindPackageShare("gazebo_ros"), "launch", "gzserver.launch.py"]
                        )
                    ]
                ),
                launch_arguments={"verbose": "true", "world": world}.items(),
            ),
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    [
                        PathJoinSubstitution(
                            [FindPackageShare("gazebo_ros"), "launch", "gzclient.launch.py"]
                        )
                    ]
                ),
                condition=IfCondition(gui),
            ),
            # Launch rviz
            Node(
                package="rviz2",
                executable="rviz2",
                name="rviz2",
                output="log",
                arguments=["-d", rviz_config_file],
            ),
            # Launch robot state publisher
            Node(
                package="robot_state_publisher",
                executable="robot_state_publisher",
                output="both",
                parameters=[robot_description],
            ),
            # Spawn the robot in gazebo from the robot_description provided by robot_state_publisher
            Node(
                package="gazebo_ros",
                executable="spawn_entity.py",
                arguments=["-topic", "robot_description", "-entity", "robot"],
                output="screen",
            ),
            # Spawn joint_state_broadcaster
            Node(
                package="controller_manager",
                executable="spawner.py",
                arguments=[
                    "joint_state_broadcaster",
                    "--controller-manager",
                    "/controller_manager",
                ],
            ),
            # Spawn custom controller
            # YOU PROBABLY NEED TO MODIFY THIS AND ADD ONE OF THOSE FOR EACH CONTROLLER
            # IN YOUR controller_config_path
            Node(
                package="controller_manager",
                executable="spawner.py",
                arguments=["wheel_velocity_controller", "-c", "/controller_manager"],
            ),
        ]
    )

Originally posted by Darkproduct with karma: 100 on 2021-08-24

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by kak13 on 2021-08-24:
what does your launch look like?

Because I get the error output like this

[wheel_velocity_controller-2] The passed value needs to be a dictionary in YAML format
[ERROR] [wheel_velocity_controller-2]: process has died [pid 1009427, exit code 1, cmd 'ros2 topic pub /joint_states sensor_msgs/msg/JointState "{data: True}"']

in my launch, it is:

return launch.LaunchDescription([
    launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'], output='screen'),
    launch.actions.ExecuteProcess(
        name='wheel_velocity_controller',
        cmd = ['ros2', 'topic', 'pub', '/joint_states', 'sensor_msgs/msg/JointState', '"{data: True}"'],
        output = 'screen',
        shell='True'
  ),

Comment by Darkproduct on 2021-08-24:
I updated my answer with the launch file, because it is no big for a comment. (Update 1)

Comment by Darkproduct on 2021-08-24:
You can use urdf only, but I would advice to use it, if you want to do more then just a small demo.

Comment by Darkproduct on 2021-08-24:
I would advice to look at the ROS2 Control Demo pkg, that I linked above. Mine is basically a copy pasta of that and other examples/tutorials. Unfortunately, the ROS2 Documentation of most packages (even of the core packages, like ros_launch) are absolute garbage right now.

Comment by kak13 on 2021-08-24:
I followed your launch and I'm having a lot of issues now. I will take a break then resume on this one.

I merged my launch with the launch you provide. I updated my launch in my post as well

Comment by Darkproduct on 2021-08-24:
I added Update 2 and I think thats all I can give you. If you have smaller questions regarding this, open a new question. My answer is already too long and not fun to edit.

Comment by kak13 on 2021-08-24:
Thank you so much!!!

I end up have a last question after trying to fix this issue for almost 2 hours now lol I'm going to make a new question! I'm going to mark your question as an answer since it answers the question!

I wish I could cashapp you to donate u 50 dollars! haha

Comment by Darkproduct on 2021-08-25:
I'm glad I could help. I know the ROS2 Documentation is lacking and many packages don't even provide one.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.