0
$\begingroup$

I am following the Articulated Robotics tutorial on simulated ros2_control. However, when it comes to running the launch file (launch_sim.launch.py), this is the output:

[INFO] [launch]: All log files can be found below /home/flamethrower/.ros/log/2024-08-04-18-35-28-656714-robot-3619
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [3621]
[INFO] [gzserver-2]: process started with pid [3623]
[INFO] [gzclient   -3]: process started with pid [3625]
[INFO] [spawn_entity.py-4]: process started with pid [3628]
[INFO] [spawner.py-5]: process started with pid [3631]
[INFO] [spawner.py-6]: process started with pid [3633]
[robot_state_publisher-1] Parsing robot urdf xml string.
[robot_state_publisher-1] Link FL_wheel had 0 children
[robot_state_publisher-1] Link FR_wheel had 0 children
[robot_state_publisher-1] Link RL_wheel had 0 children
[robot_state_publisher-1] Link RR_wheel had 0 children
[robot_state_publisher-1] Link chassis had 1 children
[robot_state_publisher-1] Link laser_frame had 0 children
[robot_state_publisher-1] [INFO] [1722789331.093667060] [robot_state_publisher]: got segment FL_wheel
[robot_state_publisher-1] [INFO] [1722789331.098568099] [robot_state_publisher]: got segment FR_wheel
[robot_state_publisher-1] [INFO] [1722789331.098785080] [robot_state_publisher]: got segment RL_wheel
[robot_state_publisher-1] [INFO] [1722789331.098873561] [robot_state_publisher]: got segment RR_wheel
[robot_state_publisher-1] [INFO] [1722789331.098906653] [robot_state_publisher]: got segment base_link
[robot_state_publisher-1] [INFO] [1722789331.098934339] [robot_state_publisher]: got segment chassis
[robot_state_publisher-1] [INFO] [1722789331.098960764] [robot_state_publisher]: got segment laser_frame
[spawner.py-5] [INFO] [1722789332.642764464] [spawner_diff_cont]: Waiting for /controller_manager services
[spawner.py-6] [INFO] [1722789332.753680821] [spawner_joint_broad]: Waiting for /controller_manager services
[spawn_entity.py-4] [INFO] [1722789333.814943066] [spawn_entity]: Spawn Entity started
[spawn_entity.py-4] [INFO] [1722789333.817251300] [spawn_entity]: Loading entity published on topic robot_description
[spawn_entity.py-4] [INFO] [1722789333.824613498] [spawn_entity]: Waiting for entity xml on robot_description
[spawn_entity.py-4] [INFO] [1722789333.840584852] [spawn_entity]: Waiting for service /spawn_entity, timeout = 30
[spawn_entity.py-4] [INFO] [1722789333.842771012] [spawn_entity]: Waiting for service /spawn_entity
[spawner.py-5] [INFO] [1722789334.688257733] [spawner_diff_cont]: Waiting for /controller_manager services
[spawner.py-6] [INFO] [1722789334.793537609] [spawner_joint_broad]: Waiting for /controller_manager services
[spawner.py-5] [INFO] [1722789336.754853755] [spawner_diff_cont]: Waiting for /controller_manager services
[spawner.py-6] [INFO] [1722789336.890463941] [spawner_joint_broad]: Waiting for /controller_manager services
[spawner.py-5] [INFO] [1722789338.799318653] [spawner_diff_cont]: Waiting for /controller_manager services
[spawner.py-6] [INFO] [1722789338.942930759] [spawner_joint_broad]: Waiting for /controller_manager services
[spawn_entity.py-4] [INFO] [1722789339.373300524] [spawn_entity]: Calling service /spawn_entity
[spawn_entity.py-4] [INFO] [1722789340.397202490] [spawn_entity]: Spawn status: SpawnEntity: Successfully spawned entity [robot]
[gzserver-2] [WARN] [1722789340.508036852] [rcl]: Found remap rule '~/out:=scan'. This syntax is deprecated. Use '--ros-args --remap ~/out:=scan' instead.
[gzserver-2] [WARN] [1722789340.586504745] [rcl]: Found remap rule '~/out:=scan'. This syntax is deprecated. Use '--ros-args --remap ~/out:=scan' instead.
[INFO] [spawn_entity.py-4]: process has finished cleanly [pid 3628]
[gzserver-2] [INFO] [1722789340.836605840] [gazebo_ros2_control]: Loading gazebo_ros2_control plugin
[gzserver-2] [INFO] [1722789340.856579331] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in namespace: /
[gzserver-2] [INFO] [1722789340.860006190] [gazebo_ros2_control]: Starting gazebo_ros2_control plugin in ros 2 node: gazebo_ros2_control
[gzserver-2] [INFO] [1722789340.860619318] [gazebo_ros2_control]: Loading parameter files /home/flamethrower/robot_ws/install/robot/share/robot/config/my_controllers.yaml
[spawner.py-5] [INFO] [1722789340.877581837] [spawner_diff_cont]: Waiting for /controller_manager services
[gzserver-2] [INFO] [1722789340.884953905] [gazebo_ros2_control]: connected to service!! robot_state_publisher
[gzserver-2] [INFO] [1722789340.893442322] [gazebo_ros2_control]: Recieved urdf from param server, parsing...
[gzserver-2] [INFO] [1722789340.948132774] [gazebo_ros2_control]: Loading joint: FL_wheel_joint
[gzserver-2] [INFO] [1722789340.948472162] [gazebo_ros2_control]:   State:
[gzserver-2] [INFO] [1722789340.948567995] [gazebo_ros2_control]:        velocity
[gzserver-2] [INFO] [1722789340.948771920] [gazebo_ros2_control]:        position
[gzserver-2] [INFO] [1722789340.948853605] [gazebo_ros2_control]:   Command:
[gzserver-2] [INFO] [1722789340.948888198] [gazebo_ros2_control]:        velocity
[gzserver-2] [INFO] [1722789340.949095438] [gazebo_ros2_control]: Loading joint: FR_wheel_joint
[gzserver-2] [INFO] [1722789340.949171771] [gazebo_ros2_control]:   State:
[gzserver-2] [INFO] [1722789340.949207993] [gazebo_ros2_control]:        velocity
[gzserver-2] [INFO] [1722789340.949243345] [gazebo_ros2_control]:        position
[gzserver-2] [INFO] [1722789340.949275752] [gazebo_ros2_control]:   Command:
[gzserver-2] [INFO] [1722789340.949304863] [gazebo_ros2_control]:        velocity
[gzserver-2] [INFO] [1722789340.949344659] [gazebo_ros2_control]: Loading joint: RL_wheel_joint
[gzserver-2] [INFO] [1722789340.949382381] [gazebo_ros2_control]:   State:
[gzserver-2] [INFO] [1722789340.949414789] [gazebo_ros2_control]:        velocity
[gzserver-2] [INFO] [1722789340.949445659] [gazebo_ros2_control]:        position
[gzserver-2] [INFO] [1722789340.949479307] [gazebo_ros2_control]:   Command:
[gzserver-2] [INFO] [1722789340.949507511] [gazebo_ros2_control]:        velocity
[gzserver-2] [INFO] [1722789340.949543029] [gazebo_ros2_control]: Loading joint: RR_wheel_joint
[gzserver-2] [INFO] [1722789340.949577140] [gazebo_ros2_control]:   State:
[gzserver-2] [INFO] [1722789340.949608121] [gazebo_ros2_control]:        velocity
[gzserver-2] [INFO] [1722789340.949638640] [gazebo_ros2_control]:        position
[gzserver-2] [INFO] [1722789340.949670269] [gazebo_ros2_control]:   Command:
[gzserver-2] [INFO] [1722789340.949699621] [gazebo_ros2_control]:        velocity
[gzserver-2] [INFO] [1722789340.951049691] [gazebo_ros2_control]: Loading controller_manager
[gzserver-2] terminate called after throwing an instance of 'std::system_error'
[gzserver-2]   what():  Invalid argument
[gzserver-2] Aborted (core dumped)
[ERROR] [gzserver-2]: process has died [pid 3623, exit code 134, cmd 'gzserver                                                                       -s libgazebo_ros_init.so   -s libgazebo_ros_factory.so   -s libgazebo_ros_force_system.so       '].

And anything else that involves the controller manager doesn't work either.

These are the files in my package:

CMakeLists.txt:

cmake_minimum_required(VERSION 3.5)
project(robot)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

install(
  DIRECTORY config description launch worlds
  DESTINATION share/${PROJECT_NAME}
)

ament_package()

launch_sim.launch.py:

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



def generate_launch_description():


    # Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
    # !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!!

    package_name='robot' #<--- CHANGE ME

    rsp = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    get_package_share_directory(package_name),'launch','rsp.launch.py'
                )]), launch_arguments={'use_sim_time': 'true'}.items()
    )

    # Include the Gazebo launch file, provided by the gazebo_ros package
    gazebo = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
             )

    # Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                        arguments=['-topic', 'robot_description',
                                   '-entity', 'robot'],
                        output='screen')


    diff_drive_spawner = Node(
        package="controller_manager",
        executable="spawner.py",
        arguments=["diff_cont"],
    )

    joint_broad_spawner = Node(
        package="controller_manager",
        executable="spawner.py",
        arguments=["joint_broad"],
    )



    # Launch them all!
    return LaunchDescription([
        rsp,
        gazebo,
        spawn_entity,
        diff_drive_spawner,
        joint_broad_spawner
    ])

rsp.launch.py:

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node

import xacro


def generate_launch_description():

    # Check if we're told to use sim time
    use_sim_time = LaunchConfiguration('use_sim_time')

    # Process the URDF file
    pkg_path = os.path.join(get_package_share_directory('robot'))
    xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
    robot_description_config = xacro.process_file(xacro_file)
    
    # Create a robot_state_publisher node
    params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[params]
    )


    # Launch!
    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use sim time if true'),

        node_robot_state_publisher
    ])

Inside description:

robot.urdf.xacro:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"  name="robot">

    <xacro:include filename="robot_core.xacro" />
    <xacro:include filename="ros2_control.xacro"/>
    <xacro:include filename="lidar.xacro"/>

</robot>

ros2_control.xacro:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">


    <ros2_control name="GazeboSystem" type="system">
        <hardware>
            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </hardware>
        <joint name="FL_wheel_joint">
            <command_interface name="velocity">
                <param name="min">-10</param>
                <param name="max">10</param>
            </command_interface>
            <state_interface name="velocity"/>
            <state_interface name="position"/>
        </joint>
        <joint name="FR_wheel_joint">
            <command_interface name="velocity">
                <param name="min">-10</param>
                <param name="max">10</param>
            </command_interface>
            <state_interface name="velocity"/>
            <state_interface name="position"/>
        </joint>
        <joint name="RL_wheel_joint">
            <command_interface name="velocity">
                <param name="min">-10</param>
                <param name="max">10</param>
            </command_interface>
            <state_interface name="velocity"/>
            <state_interface name="position"/>
        </joint>
        <joint name="RR_wheel_joint">
            <command_interface name="velocity">
                <param name="min">-10</param>
                <param name="max">10</param>
            </command_interface>
            <state_interface name="velocity"/>
            <state_interface name="position"/>
        </joint>
    </ros2_control>
    
    <gazebo>
        <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
            <parameters>/home/ubuntu/robot_ws/src/robot/config/gaz_ros2_ctl_use_sim.yaml</parameters>
        </plugin>
    </gazebo>

</robot>

my_controllers.yaml (in config):

controller_manager:
  ros__parameters:
    update_rate: 30
    use_sim_time: true

    diff_cont:
      type: diff_drive_controller/DiffDriveController

    joint_broad:
      type: joint_state_broadcaster/JointStateBroadcaster

diff_cont:
  ros__parameters:

    publish_rate: 50.0

    base_frame_id: base_link

    left_wheel_names: ['FL_wheel_joint', 'RL_wheel_joint']
    right_wheel_names: ['FR_wheel_joint', 'RR_wheel_joint']
    wheel_separation: 0.35
    wheel_radius: 0.05

    use_stamped_vel: false

    # open_loop: false    

    wheels_per_side: 2
    # wheel_separation_multiplier: x
    # left_wheel_radius_multiplier: x
    # right_wheel_radius_multiplier: x

    # odom_frame_id: x
    # pose_covariance_diagonal: x
    # twist_covariance_diagonal: x
    # open_loop: x
    # enable_odom_tf: x

    # cmd_vel_timeout: x
    # publish_limited_velocity: x
    # velocity_rolling_window_size: x
    

    # linear.x.has_velocity_limits: false
    # linear.x.has_acceleration_limits: false
    # linear.x.has_jerk_limits: false
    # linear.x.max_velocity: NAN
    # linear.x.min_velocity: NAN
    # linear.x.max_acceleration: NAN
    # linear.x.min_acceleration: NAN
    # linear.x.max_jerk: NAN
    # linear.x.min_jerk: NAN

    # angular.z.has_velocity_limits: false
    # angular.z.has_acceleration_limits: false
    # angular.z.has_jerk_limits: false
    # angular.z.max_velocity: NAN
    # angular.z.min_velocity: NAN
    # angular.z.max_acceleration: NAN
    # angular.z.min_acceleration: NAN
    # angular.z.max_jerk: NAN
    # angular.z.min_jerk: NAN




# joint_broad:
#   ros__parameters:

EDIT: I've come to realize the root of the problem is gazebo_ros2_control crashing when trying to load controller_manager, as this package replaces the node that would be in the launch file and leaves the controllers waiting for its services. I still don't know how to solve it, though. Any help is appreciated.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

The relevant error message is

[gzserver-2] [ERROR] [1719162820.077538149] [gazebo_ros2_control]: parser error Couldn't parse params file: '--params-file /home/ubuntu/robot_ws/src/robot/config/gaz_ros2_ctl_use_sim.yaml'. Error: Error opening YAML file, at /tmp/binarydeb/ros-foxy-rcl-yaml-param-parser-1.1.14/src/parser.c:198, at /tmp/binarydeb/ros-foxy-rcl-1.1.14/src/rcl/arguments.c:388

be aware that foxy is EOL and not maintained any more. Is the yaml file accessible, cat /home/ubuntu/robot_ws/src/robot/config/gaz_ros2_ctl_use_sim.yaml?

Improve this answer
$\endgroup$
14
  • $\begingroup$ I don't have that file. Will creating it solve the error? And what do I need to put in it? $\endgroup$
    – Flamethrower
    Commented Jun 24 at 6:50
  • $\begingroup$ You have specified it in ros2_control.xacro, but I guess this should be your my_controllers.yaml $\endgroup$
    – Christoph Froehlich
    Commented Jun 24 at 7:21
  • $\begingroup$ I added the file and wrote use_sim_time: true in it, but it's still giving me the same output (the parsing error is still there and /controller_manager is not available) $\endgroup$
    – Flamethrower
    Commented Jun 24 at 9:18
  • $\begingroup$ No, you use the file in your xacro: <parameters>/home/ubuntu/robot_ws/src/robot/config/gaz_ros2_ctl_use_sim.yaml</parameters> but it should point to your my_controllers.yaml instead. $\endgroup$
    – Christoph Froehlich
    Commented Jun 24 at 12:48
  • $\begingroup$ I changed it and it's done nothing. It seems controller_manager is not running at all, even though I have all the packages installed. $\endgroup$
    – Flamethrower
    Commented Jun 24 at 13:32

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.