[ros2] Joint states publisher and Create from param

vatanaksoytezer commented 3 years ago

Environment

In ROS + gazebo invoking <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" args=" -urdf -model robot -param robot_description" respawn="false" output="screen" /> statement opens up gazebo, spawns the model and starts to publish /joint_states.

I am currently trying to replicate a similar behaviours with ROS2 Foxy. My setup is a docker image with ROS2 Foxy, Ubuntu 20.04, source build of ros2 branch of ros_ign and Binary install of Ignition Edifice.

This how my launch file looks like:

import os
import yaml
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_ros.actions import Node

def load_file(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return file.read()
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None

def load_yaml(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return yaml.safe_load(file)
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None

def generate_launch_description():

    # Import model
    # gazebo_pkg_path =  os.path.join(get_package_share_directory("stretch_gazebo"), "urdf", "stretch_gazebo.urdf.xacro")
    # gazebo_pkg_path =  os.path.join(get_package_share_directory("stretch_description"), "urdf", "description_test.urdf.xacro")
    gazebo_pkg_path =  os.path.join(get_package_share_directory("stretch_description"), "urdf", "stretch_main.xacro")
    robot_description_config = xacro.process_file(
        gazebo_pkg_path
    )
    robot_description = {"robot_description": robot_description_config.toxml()}

    # Robot state publisher
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[robot_description],
    )

    # Ignition gazebo
    pkg_ros_ign_gazebo = get_package_share_directory('ros_ign_gazebo')
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_ros_ign_gazebo, 'launch', 'ign_gazebo.launch.py')),
        launch_arguments={'ign_args': '-r empty.sdf'}.items(),
    )

    # RViz
    pkg_stretch_gazebo = get_package_share_directory('stretch_gazebo')
    rviz = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', os.path.join(pkg_stretch_gazebo, 'config', 'stretch_gazebo.rviz')],
        parameters=[robot_description]
    )

    # Spawn
    spawn = Node(package='ros_ign_gazebo', executable='create',
                arguments=[
                    '-name', 'robot',
                    # '-file', gazebo_pkg_path
                    # '-topic', '/robot_description'
                    '-param', robot_description
                    ],
                output='screen')

    # Ign Bridge
    bridge = Node(
        package='ros_ign_bridge',
        executable='parameter_bridge',
        arguments=['/cmd_vel@geometry_msgs/msg/Twist@ignition.msgs.Twist',
                '/model/robot/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry',
                # '/model/robot/tf@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V',
                '/joint_states@sensor_msgs/msg/JointState@ignition.msgs.Model',
                '/clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock'
                ],
        remappings=[
            # ("/model/robot/tf", "tf"),
            ("/model/robot/odometry", "odom")
        ],
        output='screen'
    )

    return LaunchDescription([
        robot_state_publisher,
        rviz,
        gazebo,
        spawn,
        bridge,
    ])

Description

Expected behavior: Launching ign gazebo and publishing joint states
Actual behavior: It fails to read the params from robot_description. I tried a similar behaviour with sdf files too. I manage to give cmd_vel commands and read odom from ign. Though I didn't get any joint_states. I can confirm that this is not a bridge issue by issuing ign topic -l command and there is no joint_states. My initial goal is to spawn the robot on ign, and show the robot movements on rviz. With ros_ign_bridge of tf I can show the frames in rviz but since I don't have the joint_states I can't show the model of the robot. Also in tf messages I can only see the base_link but no other links. I think this is mostly because diff_drive_plugin publishes it, not the ign gazebo. I would be happy to share any information you would like and would be happy to contribute with some PRs if there are any bugs / missing features related to these issues if you can brief me.

Steps to reproduce

You should be able to reproduce by sourcing the workspace and launching the file provided. I think it can be reproduced with any xacro file. If you want to take a deeper look I can send the full description file as well.

Output

[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [4724]
[INFO] [ign gazebo-2]: process started with pid [4726]
[INFO] [create-3]: process started with pid [4728]
[create-3] [INFO] [1620676458.214857039] [ros_ign_gazebo]: Requesting list of world names.
[robot_state_publisher-1] Parsing robot urdf xml string.
[robot_state_publisher-1] The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[robot_state_publisher-1] Link caster_link had 0 children
[robot_state_publisher-1] Link link_left_wheel had 0 children
[robot_state_publisher-1] Link link_mast had 0 children
[robot_state_publisher-1] Link link_right_wheel had 0 children
[robot_state_publisher-1] [INFO] [1620676458.215944360] [robot_state_publisher]: got segment base_link
[robot_state_publisher-1] [INFO] [1620676458.215994193] [robot_state_publisher]: got segment caster_link
[robot_state_publisher-1] [INFO] [1620676458.216001299] [robot_state_publisher]: got segment link_left_wheel
[robot_state_publisher-1] [INFO] [1620676458.216005618] [robot_state_publisher]: got segment link_mast
[robot_state_publisher-1] [INFO] [1620676458.216009563] [robot_state_publisher]: got segment link_right_wheel
[ign gazebo-2] QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-root'
[create-3] [ERROR] [1620676458.590403867] [ros_ign_gazebo]: Failed to get XML from param [robot_description].
[ERROR] [create-3]: process has died [pid 4728, exit code 255, cmd '/root/ws_stretch/install/ros_ign_gazebo/lib/ros_ign_gazebo/create -name robot -param robot_description --ros-args'].

vatanaksoytezer commented 3 years ago

I realized we need the joint states plugin to achieve that. I can open a new PR to document it if it's appropriate.

chapulina commented 3 years ago

I can open a new PR to document it if it's appropriate.

Thanks for offering! My suggestion would be to add a simple demo to ros_ign_gazebo_demos using the joint states plugin.

vatanaksoytezer commented 3 years ago

Thanks for checking out! Sure I can do that. Will open one soon.

gazebosim / ros_gz