0
$\begingroup$

Rosanswers logo

Hi. Sorry for all mistakes, English is not my native language. So I'm launching my gazebo simulation with JointStateBroadcaster and DiffDrive controllers. Gazebo loads without notable errors but then controllers start to load I receive following logs in terminal:

[gzserver-6] [INFO] [1670244707.718918444] [gazebo_ros2_control]: Loading controller_manager
[gzserver-6] [WARN] [1670244707.830804032] [gazebo_ros2_control]:  Desired controller update period (0.02 s) is slower than the gazebo simulation period (0.001 s).
[gzserver-6] [INFO] [1670244707.831187703] [gazebo_ros2_control]: Loaded gazebo_ros2_control.
[gzserver-6] [INFO] [1670244707.880241583] [controller_manager]: Loading controller 'minicar_cll'
[gzserver-6] [INFO] [1670244707.969517659] [controller_manager]: Setting use_sim_time=True for minicar_cll to match controller manager (see ros2_control#325 for details)
[spawner.py-3] [INFO] [1670244707.973501681] [spawner_minicar_cll]: Loaded minicar_cll
[gzserver-6] [INFO] [1670244707.977437769] [controller_manager]: Configuring controller 'minicar_cll'
[gzserver-6] [INFO] [1670244708.048451910] [controller_manager]: Loading controller 'joint_s_b'
[gzserver-6] [INFO] [1670244708.178457173] [controller_manager]: Setting use_sim_time=True for joint_s_b to match controller manager (see ros2_control#325 for details)
[spawner.py-5] [INFO] [1670244708.200924658] [spawner_joint_s_b]: Loaded joint_s_b
[gzserver-6] [INFO] [1670244708.204831040] [controller_manager]: Loading controller 'joint_s_b'
[gzserver-6] [ERROR] [1670244708.205196239] [controller_manager]: A controller named 'joint_s_b' was already loaded inside the controller manager
[gzserver-6] [ERROR] [1670244708.226174298] [controller_manager]: Can't activate controller 'minicar_cll': Command interface with 'front_left_steering_joint/velocity' does not exist
[spawner.py-3] [INFO] [1670244708.255965445] [spawner_minicar_cll]: Configured and started minicar_cll
[gzserver-6] [INFO] [1670244708.262011951] [controller_manager]: Configuring controller 'joint_s_b'
[ERROR] [spawner.py-2]: process has died [pid 5069, exit code 1, cmd '/opt/ros/foxy/lib/controller_manager/spawner.py joint_s_b --ros-args'].
[INFO] [spawner.py-3]: process has finished cleanly [pid 5071]
[spawner.py-5] [INFO] [1670244708.351449614] [spawner_joint_s_b]: Configured and started joint_s_b
[INFO] [spawner.py-5]: process has finished cleanly [pid 5075]
[INFO] [spawner.py-9]: process started with pid [5305]
[gzclient-7] context mismatch in svga_surface_destroy
[gzclient-7] context mismatch in svga_surface_destroy
[spawner.py-9] [INFO] [1670244708.899426522] [spawner_minicar_cll]: Controller already loaded, skipping load_controller
[gzserver-6] [INFO] [1670244708.902372504] [controller_manager]: Configuring controller 'minicar_cll'
[gzserver-6] [ERROR] [1670244708.998457025] [controller_manager]: Can't activate controller 'minicar_cll': Command interface with 'front_left_steering_joint/velocity' does not exist
[spawner.py-9] [INFO] [1670244709.020476614] [spawner_minicar_cll]: Configured and started minicar_cll

minicar_cll here is diff_drive controller.

Here is yaml file with controllers settings:

controller_manager:
  ros__parameters:
    update_rate: 50  # Hz
    use_sim_time: True

    joint_s_b:
      type: joint_state_broadcaster/JointStateBroadcaster

    minicar_cll:
      type: diff_drive_controller/DiffDriveController

minicar_cll:
    ros__parameters:
        left_wheel_names: ['front_left_wheel_joint', 'rear_left_wheel_joint', 'front_left_steering_joint', 'rear_left_steering_joint']
        right_wheel_names: ['front_right_wheel_joint', 'rear_right_wheel_joint', 'front_right_steering_joint', 'rear_right_steering_joint']
        publish_rate: 50.0
        pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
        twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
        cmd_vel_timeout: 0.5

        wheels_per_side: 1
        preserve_turning_radius: true 
        base_frame_id: rs_t265_pose_frame

        enable_odom_tf: false

        angular:
            steer:
                front_max_angle: 1.047
                rear_max_angle: 0.0

And that is transmission description:

   <transmission name="${wheel_prefix}_steering_trans" type="SimpleTransmission">
      <type>transmission_interface/SimpleTransmission</type>
      <actuator name="${wheel_prefix}_steering_motor">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
      <joint name="${wheel_prefix}_steering_joint">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
      </joint>
    </transmission>

    <transmission name="${wheel_prefix}_wheel_trans" type="SimpleTransmission">
      <type>transmission_interface/SimpleTransmission</type>
      <actuator name="${wheel_prefix}_wheel_motor">
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
      <joint name="${wheel_prefix}_wheel_joint">
        <hardwareInterface>VelocityJointInterface</hardwareInterface>
      </joint>
    </transmission>
  </xacro:macro>

and ros2_control tag in robot description with gazebo plugin

<ros2_control name="GazeboSystem" type="system">
  <hardware>
    <plugin>gazebo_ros2_control/GazeboSystem</plugin> 
  </hardware>
    <joint name="front_left_steering_joint">
      <command_interface name="position"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="front_right_steering_joint">
      <command_interface name="position"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="rear_left_steering_joint">
      <command_interface name="position"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="rear_right_steering_joint">
      <command_interface name="position"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="rear_right_wheel_joint">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="rear_left_wheel_joint">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="front_left_wheel_joint">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="front_right_wheel_joint">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>    
</ros2_control>

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

And my question is why is it trying to load JointStateBroadcaster several times and why is he complaining about "Command interface with 'front_left_steering_joint/velocity' does not exist"? Appreciate any help.

UPD. Add full terminal output and launch files used to start gazebo simulation.

Terminal:

This code block was moved to the following github gist: https://gist.github.com/answers-se-migration-openrobotics/cccb826d947e62e554eb61c37050efc3

launch file gazebo_world.launch.py:

import os
import xacro

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.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node


def generate_launch_description():
    package_dir = os.path.join(get_package_share_directory('minicar_simulator'))
    controller_file = os.path.join(get_package_share_directory('minicar_control'), "config", "control.yaml")
    doc = os.path.join(package_dir, 'urdf', 'minicar.gazebo.xacro')
    xacro_file = xacro.parse(open(doc))
    xacro.process_doc(xacro_file)

    robot_description = {"robot_description": xacro_file.toxml()}
    
    world_file = PathJoinSubstitution(
        [FindPackageShare("minicar_simulator"),
        "worlds",
        "test_walls.world"],
    )

    gazebo_launch = PathJoinSubstitution(
        [FindPackageShare("minicar_simulator"),
        "launch",
        "gazebo.launch.py"],
    )

    node_controller_manager = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[robot_description, controller_file],
        output={
            "stdout": "screen",
            "stderr": "screen",
        },
    )

    spawn_controller = Node(
        package="controller_manager",
        executable="spawner.py",
        arguments=["joint_s_b"],
        output="screen",
    )

    spawn_diff_velocity_controller = Node(
        package="controller_manager",
        executable="spawner.py",
        arguments=["minicar_cll"],
        output="screen",
    )

    gazebo_sim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([gazebo_launch]),
        launch_arguments={'world_path': world_file}.items(),
    )

    ld = LaunchDescription()
    ld.add_action(node_controller_manager)
    ld.add_action(spawn_controller)
    ld.add_action(spawn_diff_velocity_controller)
    ld.add_action(gazebo_sim)

    return ld

launch file gazebo.launch.py:

import os

import xacro

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

from ament_index_python.packages import get_package_share_directory

ARGUMENTS = [
    DeclareLaunchArgument('world_path', default_value='',
                        description='The world path, by default is empty.world'),
    DeclareLaunchArgument('verbose', default_value='true',
                        description='Set "true" to increase messages written to terminal.'),
]


def generate_launch_description():

    # Launch args
    world_path = LaunchConfiguration('world_path')
    verbose = 'true'
    package_dir = os.path.join(get_package_share_directory('minicar_simulator'))
    doc = os.path.join(package_dir, 'urdf', 'minicar.gazebo.xacro')
    xacro_file = xacro.parse(open(doc))
    xacro.process_doc(xacro_file)

    robot_description = {"robot_description": xacro_file.toxml()}

    spawn_diff_velocity_controller = Node(
        package='controller_manager',
        executable='spawner.py',
        arguments=['minicar_cll', '-c', '/controller_manager'],
        output='screen',
    )

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

    spawn_joint_state_broadcaster = Node(
        package='controller_manager',
        executable='spawner.py',
        arguments=['joint_s_b', '-c', '/controller_manager'],
        output='screen',
    )

    diffdrive_controller_spawn_callback = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=spawn_joint_state_broadcaster,
            on_exit=[spawn_diff_velocity_controller],
        )
    )

    gzserver = ExecuteProcess(
        cmd=['gzserver',
            '-s', 'libgazebo_ros_init.so',
            '-s', 'libgazebo_ros_factory.so',
            world_path,
            verbose],
        output='screen',
    )

    # Gazebo client
    gzclient = ExecuteProcess(
        cmd=['gzclient'],
        output='screen',

    )

    # Spawn robot
    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                        arguments=['-topic', 'robot_description',
                                '-x', '-3.0',
                                '-y', '-1.5',
                                '-z', '2.719',
                                '-entity', 'minicar'],
                        output='screen')

    ld = LaunchDescription(ARGUMENTS)
    ld.add_action(node_robot_state_publisher)
    ld.add_action(spawn_joint_state_broadcaster)
    ld.add_action(diffdrive_controller_spawn_callback)
    ld.add_action(gzserver)
    ld.add_action(gzclient)
    ld.add_action(spawn_entity)

    return ld

I was told not to add full URDF description, so sorry for inconvenience, but I already double check my robot description and I didn't find any problem with it.

Originally posted by Edvard on ROS Answers with karma: 95 on 2022-12-05

Post score: 0

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

To summarize the cause of the two issues, in case others come across this question in the future:

  1. ros2_control/gazebo trying to load a controller multiple times

This shouldn't happen, and in this case was caused by multiple launch files loading the controllers.

  1. "Command interface with 'front_left_steering_joint/velocity' does not exist";

Your controller is looking for a velocity control interface for joint front_left_steering_joint, but your <ros2_control> tag in your URDF is telling the controller_manager that your "hardware" only supports position control for this joint. The diff_driver_controller currently only supports velocity commands to the wheels, and accepts position or velocity state interfaces (you can take a look at the source here for reference).

<ros2_control name="GazeboSystem" type="system">
  <hardware>
    <plugin>gazebo_ros2_control/GazeboSystem</plugin> 
  </hardware>
    <joint name="front_left_steering_joint">
      <command_interface name="position"/> <------------------- here
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

Since this is a gazebo question, you can resolve this by simply changing front_left, front_right, rear_left and rear_right joints to be velocity controlled in the <ros2_control> block of your URDF.

All that said, based on the naming convention you used, it sounds like you want those to be steerable joints. Perhaps you want to consider implementing an Ackermann controller or similar. The ros2_controllers repo has a tricycle_controller which you may find to be a good starting point. Alternately, you could take a crack at porting either the ros1 ackermann_controller or this swerve_steering_controller to ros2.

Originally posted by shonigmann with karma: 1567 on 2022-12-06

This answer was ACCEPTED on the original site

Post score: 2

Original comments

Comment by Edvard on 2022-12-07:
Thank you for your help, time and detailed explanation. I'll try to look tricycle_controller and ackermann_controller

Improve this answer
$\endgroup$

Your Answer

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