moveit2: moveit2 + Gazebo simulation not working: Robot state (joint_states) not received

Description

I have developed ready-to-use Gazebo + MoveIt!2 simulation packages for ROS2 Foxy (which can be found here, and are public for everyone: https://github.com/IFRA-Cranfield/ros2_RobotSimulation), and I am now in the process of porting them to ROS2 Humble/Rolling. The reason behind this is that ros2_control and ros2_controllers change from Foxy to Humble/Rolling version, and I need a ROS2 machine able to control ABB Robots using MoveIt!2 in both Gazebo and Real World (using abb_ros2 driver, which only works in Humble/Rolling - https://github.com/PickNikRobotics/abb_ros2/issues/24 -). Of course, I believe it is simpler to port a ROS2 Gazebo+MoveIt!2 simulation package from Foxy to Humble/Rolling rather than the abb_ros2 driver from Humble/Rolling to Foxy…

The Gazebo simulation works well in ROS2 Rolling, but the problem comes when launching the Gazebo simulation and the MoveIt!2 framework (in order to control a robot in Gazebo from MoveIt!2). Everything seems fine, the environment is perfectly loaded, but when I try to plan and execute a movement MoveIt!2 returns an error message: The ROBOT STATE (joint states) could not be received. I have checked the /joint_states topic, and joint_state_broadcaster publishes the joint values properly. Therefore, I believe it is an internal error in MoveIt!2, which disables the communication between the joint states in Gazebo and the MoveIt!2 robot state monitor.

I have been researching a bit, and I believe this problem could be related to this issue in the past: https://github.com/ros-planning/moveit2/issues/1190

Your environment

• ROS Distro: ROS2.0 Rolling
• OS Version: Ubuntu 22.04
• Binary build.

Steps to reproduce

I have a Ubuntu 22.04 machine with ROS2.0 Rolling and the following packages installed:

• ros2_control and ros2_controllers from source.
• ros_rolling_gazebo_ros_pkgs (binary) and gazebo_ros2_control (from source).
• MoveIt!2 for rolling: Binary install (the problem arises when installing moveit from source as well).

I have encountered this problem when testing the ABB IRB120 robot Gazebo+MoveIt! simulation packages in ROS2 Rolling:

• The irb120_ros2_gazebo package works well, it spawns the robot in Gazebo and both joint_state_broadcaster and joint_trajectory_controller are loaded and work well.
• The problem comes when simulating and executing the Gazebo+MoveIt!2 simulation in the irb120_ros2_moveit2 package. Both Gazebo and MoveIt!2 are loaded properly:

But when trying to make a simple movement from the MoveIt!2 interface (jogging the robot, and clicking Plan & Execute)…

Expected behaviour

MoveIt!2 should plan and execute the requested Robot trigger, and the robot should move accordingly in Gazebo.

Actual behaviour

MoveIt!2 interface freezes for about a second, then returns that the execution failed. The robot does not move.

Backtrace or Console output

[move_group-9] [INFO] [1659709492.803656219] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-9] [INFO] [1659709492.803882945] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[rviz2-8] [INFO] [1659709493.104111226] [move_group_interface]: Plan and Execute request accepted
[move_group-9] [INFO] [1659709493.804062189] [moveit_ros.current_state_monitor]: Didn't received robot state (joint angles) with recent timestamp within 1.000000 seconds.
[move_group-9] Check clock synchronization if your are running ROS across multiple machines!
[move_group-9] [WARN] [1659709493.804105954] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Failed to fetch current robot state.
[move_group-9] [INFO] [1659709493.804156537] [moveit_move_group_default_capabilities.move_action_capability]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[move_group-9] [INFO] [1659709493.804248543] [moveit_ros.plan_execution]: Planning attempt 1 of at most 1
[move_group-9] [INFO] [1659709493.804264240] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'move_group'
[move_group-9] [INFO] [1659709493.805225161] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'irb120_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-9] [WARN] [1659709493.822210258] [moveit_trajectory_processing.time_optimal_trajectory_generation]: Joint acceleration limits are not defined. Using the default 1 rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.
[move_group-9] [INFO] [1659709493.824519089] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-9] [INFO] [1659709493.824550994] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-9] [INFO] [1659709493.824641046] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-9] [INFO] [1659709494.824803334] [moveit_ros.current_state_monitor]: Didn't received robot state (joint angles) with recent timestamp within 1.000000 seconds.
[move_group-9] Check clock synchronization if your are running ROS across multiple machines!
[move_group-9] [WARN] [1659709494.824881732] [moveit_ros.trajectory_execution_manager]: Failed to validate trajectory: couldn't receive full current joint state within 1s
[move_group-9] [INFO] [1659709494.825018741] [moveit_move_group_default_capabilities.move_action_capability]: Solution found but controller failed during execution
[rviz2-8] [INFO] [1659709495.107404577] [move_group_interface]: Plan and Execute request aborted
[rviz2-8] [ERROR] [1659709495.207530855] [move_group_interface]: MoveGroupInterface::move() failed or timeout reached
[rviz2-8] [WARN] [1659709495.828536052] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Maybe failed to update robot state, time diff: 1659709239.445s

Extra: LAUNCH FILE

The launch file I am using to launch the simulation is the following:

#!/usr/bin/python3

# Import libraries:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler, DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
import xacro
import yaml

# LOAD FILE:
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
# LOAD YAML:
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

# ========== **GENERATE LAUNCH DESCRIPTION** ========== #
def generate_launch_description():
    
    # *********************** Gazebo *********************** # 
    
    # DECLARE Gazebo WORLD file:
    irb120_ros2_gazebo = os.path.join(
        get_package_share_directory('irb120_ros2_gazebo'),
        'worlds',
        'irb120.world')
    # DECLARE Gazebo LAUNCH file:
    gazebo = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
                launch_arguments={'world': irb120_ros2_gazebo}.items(),
             )

    # ***** ROBOT DESCRIPTION ***** #
    # ABB-IRB120 Description file package:
    irb120_description_path = os.path.join(
        get_package_share_directory('irb120_ros2_gazebo'))
    # ABB-IRB120 ROBOT urdf file path:
    xacro_file = os.path.join(irb120_description_path,
                              'urdf',
                              'irb120.urdf.xacro')
    # Generate ROBOT_DESCRIPTION for ABB-IRB120:
    doc = xacro.parse(open(xacro_file))
    xacro.process_doc(doc)
    robot_description_config = doc.toxml()
    robot_description = {'robot_description': robot_description_config}

    # ROBOT STATE PUBLISHER NODE:
    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name="robot_state_publisher",
        output='both',
        parameters=[robot_description]
    )
    # Static TF:
    static_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"],
    )

    # SPAWN ROBOT TO GAZEBO:
    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                        arguments=['-topic', 'robot_description',
                                   '-entity', 'irb120'],
                        output='both')

    # ***** CONTROLLERS ***** #
    # Joint STATE BROADCASTER:
    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
    )
    # Joint TRAJECTORY Controller:
    joint_trajectory_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["irb120_controller", "-c", "/controller_manager"],
    )


    # *********************** MoveIt!2 *********************** #   
    
    # Command-line argument: RVIZ file?
    rviz_arg = DeclareLaunchArgument(
        "rviz_file", default_value="False", description="Load RVIZ file."
    )

    # *** PLANNING CONTEXT *** #
    # Robot description, URDF:
    robot_description_config = xacro.process_file(
        os.path.join(
            get_package_share_directory("irb120_ros2_gazebo"),
            "urdf",
            "irb120.urdf.xacro",
        )
    )
    robot_description = {"robot_description": robot_description_config.toxml()}
    # Robot description, SRDF:
    robot_description_semantic_config = load_file(
        "irb120_ros2_moveit2", "config/irb120.srdf"
    )
    robot_description_semantic = {
        "robot_description_semantic": robot_description_semantic_config
    }

    # Kinematics.yaml file:
    kinematics_yaml = load_yaml(
        "irb120_ros2_moveit2", "config/kinematics.yaml"
    )
    robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}

    # Move group: OMPL Planning.
    ompl_planning_pipeline_config = {
        "move_group": {
            "planning_plugin": "ompl_interface/OMPLPlanner",
            "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
            "start_state_max_bounds_error": 0.1,
        }
    }
    ompl_planning_yaml = load_yaml(
        "irb120_ros2_moveit2", "config/ompl_planning.yaml"
    )
    ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)

    # MoveIt!2 Controllers:
    moveit_simple_controllers_yaml = load_yaml(
        "irb120_ros2_moveit2", "config/irb120_controllers.yaml"
    )
    moveit_controllers = {
        "moveit_simple_controller_manager": moveit_simple_controllers_yaml,
        "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
    }
    trajectory_execution = {
        "moveit_manage_controllers": False,
        "trajectory_execution.allowed_execution_duration_scaling": 1.2,
        "trajectory_execution.allowed_goal_duration_margin": 0.5,
        "trajectory_execution.allowed_start_tolerance": 0.01,
    }
    planning_scene_monitor_parameters = {
        "publish_planning_scene": True,
        "publish_geometry_updates": True,
        "publish_state_updates": True,
        "publish_transforms_updates": True,
    }

    # START NODE -> MOVE GROUP:
    run_move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[
            robot_description,
            robot_description_semantic,
            kinematics_yaml,
            ompl_planning_pipeline_config,
            trajectory_execution,
            moveit_controllers,
            planning_scene_monitor_parameters,
        ],
    )

    # RVIZ:
    load_RVIZfile = LaunchConfiguration("rviz_file")
    rviz_base = os.path.join(get_package_share_directory("irb120_ros2_moveit2"), "launch")
    rviz_full_config = os.path.join(rviz_base, "irb120_moveit2.rviz")
    rviz_node_full = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_full_config],
        parameters=[
            robot_description,
            robot_description_semantic,
            ompl_planning_pipeline_config,
            kinematics_yaml,
        ],
        condition=UnlessCondition(load_RVIZfile),
    )

    # ***** RETURN LAUNCH DESCRIPTION ***** #
    return LaunchDescription([
        gazebo, 
        spawn_entity,
        node_robot_state_publisher,
        static_tf,
        RegisterEventHandler(
            OnProcessExit(
                target_action = spawn_entity,
                on_exit = [
                    joint_state_broadcaster_spawner,  
                ]
            )
        ),
        RegisterEventHandler(
            OnProcessExit(
                target_action = joint_state_broadcaster_spawner,
                on_exit = [
                    joint_trajectory_controller_spawner,
                ]
            )
        ),
        RegisterEventHandler(
            OnProcessExit(
                target_action = joint_trajectory_controller_spawner,
                on_exit = [
                    rviz_arg,
                    rviz_node_full,
                    run_move_group_node,
                ]
            )
        ),
    ])