search

moveit / moveit2

:robot: MoveIt for ROS 2
https://moveit.ai/
BSD 3-Clause "New" or "Revised" License
1.09k stars 528 forks source link

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

Closed MikelBueno closed 2 years ago

MikelBueno commented 2 years ago

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

Steps to reproduce

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

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

Screenshot from 2022-08-05 15-21-45

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,
                ]
            )
        ),
    ])
vatanaksoytezer commented 2 years ago

Hello @MikelBueno thank you for reporting your issue. It seems like times are mismatching between Simulator and MoveIt, can you try setting up use_sim_time parameter to True for MoveIt? If you can push a reproducible example and instructions to run it somewhere public I would love to try to reproduce and help you debg in more detail.

MikelBueno commented 2 years ago

Hi @vatanaksoytezer, it worked, thank you very much! With regard to the source code, in https://github.com/IFRA-Cranfield/ros2_RobotSimulation you can find Gazebo + MoveIt!2 simulation packages for ROS2 Foxy, and probably in a few weeks' time I will create a new branch and upload the packages for ROS2 Rolling. Thanks again :)

vatanaksoytezer commented 2 years ago

🥳 Glad it worked, and thank you for making it open source! I'm going to close the issue since it seems resolved but please feel free to continue the conversation or open a new issue if you face hardships. Thank you!

patilyashr commented 2 years ago

Hi @MikelBueno, can you post how you used "use_sim_time" parameter for Moveit, I can't seem to find it. Thanks!

TheConstructAi commented 1 year ago

Yes please, can somewhen tel how to set this use_sim_time in Humble Moveit please? Having the same exact issue

MikelBueno commented 1 year ago

Hi @TheConstructAi and @patilyashr (I apologise for the late reply):

This is how I have managed to set the use_sim_time in my .launch.py file in ROS2 Humble:

1. In robot_state_publisher node:
node_robot_state_publisher = Node(
    package='robot_state_publisher',
    executable='robot_state_publisher',
    output='both',
    parameters=[
        robot_description,
        {"use_sim_time": True}
    ]
)

2. In move_group node:
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,
        {"use_sim_time": True},
    ],
)

3. In RVIZ node:
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,
        {"use_sim_time": True},
    ],
    condition=UnlessCondition(load_RVIZfile),
)

Thanks to those extra lines, I am now able to jog the robot in Gazebo using MoveIt!2+RVIZ.

Hope this works.

Best regards, Mikel Bueno

omermaayani commented 1 year ago

Hi @MikelBueno I am working with similar configuration, when I trying to get my arm joint state with the command: moveit::core::RobotStatePtr current_state = move_group_interface.getCurrentState(); I get this error: Screenshot from 2023-01-12 15-47-37

It's happening only when gazebo is up, if I am trying it with only RVIZ on it's work. have you ever encounter this error message? all of my nodes are with use_sim_time param set to True.

thank's, Omer

MikelBueno commented 1 year ago

Hi @omermaayani,

Thanks for reporting this issue.

It seems that I am not the only one having this issue. Something similar happens to me when I try to execute ROS2 Actions/Services that trigger movegroup() executions externally (from Python/C++ scripts). Robot manipulation through RVIZ works fine but, when I try to move the robot using MoveGroup Interface from a C++ script, I receive the same exact message as the one you have shared above. I believe it is an issue related to the system clock, and unfortunately, I have no idea about how it can be solved.

@vatanaksoytezer any idea about why this is happening? Thanks in advance.

Best regards, Mikel Bueno

ozangungor12 commented 1 year ago

Hi, @MikelBueno

I am facing the problem of being able to plan but not execute the movements with ROS2 humble ur_moveit_config setup.

I didn't understand if I am I supposed to set use_sim_true in ur_moveit.launch.py file or in another launch file?

Thanks!

MikelBueno commented 1 year ago

Hi @ozangungortuhh,

You should set the use_sim_time parameter to True in the .launch.py file where you are launching the move_group node, as I do here: https://github.com/ros-planning/moveit2/issues/1480#issuecomment-1352820099. What are you trying to achieve? In your case, if trying to manipulate a UR Robot in Gazebo, I believe it should be in the ur_moveit.launch.py file.

I hope this answer helps, otherwise feel free to answer back :)

Regards, Mikel Bueno

qboticslabs commented 1 year ago

You can also check the solution mentioned in the https://github.com/ros-planning/moveit2_tutorials/issues/587. This worked for me.

yeyanlei commented 1 year ago

@MikelBueno @qboticslabs @vatanaksoytezer I encountered the same issue. Moveit2, Rviz, and Gazebo work fine together, but when I try to use a C++ file, it throws an error. Is there a solution for this problem? I would really appreciate any help. environment: ros2 humble, panda robot.
below is the error log:

ros2 launch moveit2_tutorials move_group_interface_tutorial.launch.py [INFO] [launch]: All log files can be found below /home/robot/.ros/log/2023-04-06-14-15-28-145544-c-77661 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [move_group_interface_tutorial-1]: process started with pid [77662] [move_group_interface_tutorial-1] [INFO] [1680761728.460822917] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.027535 seconds [move_group_interface_tutorial-1] [INFO] [1680761728.460890742] [moveit_robot_model.robot_model]: Loading robot model 'panda'... [move_group_interface_tutorial-1] [INFO] [1680761728.490081465] [move_group_interface]: Ready to take commands for planning group panda_arm. [move_group_interface_tutorial-1] [INFO] [1680761728.499258444] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states' [move_group_interface_tutorial-1] [WARN] [1680761728.499917416] [moveit_ros.current_state_monitor]: Unable to update multi-DOF joint 'virtual_joint':Failure to lookup transform between 'world'and 'panda_link0' with TF exception: "world" passed to lookupTransform argument target_frame does not exist. [move_group_interface_tutorial-1] [INFO] [1680761729.499355668] [moveit_ros.current_state_monitor]: Didn't received robot state (joint angles) with recent timestamp within 1.000000 seconds. [move_group_interface_tutorial-1] Check clock synchronization if your are running ROS across multiple machines! [move_group_interface_tutorial-1] [ERROR] [1680761729.499391777] [move_group_interface]: Failed to fetch current robot state [move_group_interface_tutorial-1] Stack trace (most recent call last): [move_group_interface_tutorial-1] #4 Object "", at 0xffffffffffffffff, in [move_group_interface_tutorial-1] #3 Object "/home/robot/ws_moveit2/install/moveit2_tutorials/lib/moveit2_tutorials/move_group_interface_tutorial", at 0x56108a185be4, in _start [move_group_interface_tutorial-1] #2 Source "../csu/libc-start.c", line 392, in __libc_start_main_impl [0x7f892d77ce3f] [move_group_interface_tutorial-1] #1 Source "../sysdeps/nptl/libc_start_call_main.h", line 58, in libc_start_call_main [0x7f892d77cd8f] [move_group_interface_tutorial-1] #0 Object "/home/robot/ws_moveit2/install/moveit2_tutorials/lib/moveit2_tutorials/move_group_interface_tutorial", at 0x56108a182b67, in main [move_group_interface_tutorial-1] Segmentation fault (Address not mapped to object [(nil)]) [ERROR] [move_group_interface_tutorial-1]: process has died [pid 77662, exit code -11, cmd '/home/robot/ws_moveit2/install/moveit2_tutorials/lib/moveit2_tutorials/move_group_interface_tutorial --ros-args -r node:=move_group_interface_tutorial --params-file /tmp/launch_params_9b3w9erx --params-file /tmp/launch_params_cqokziq4 --params-file /tmp/launch_params_xjm9ss5z'].

yeyanlei commented 1 year ago

@omermaayani Has your issue been resolved?

yeyanlei commented 1 year ago

const moveit::core::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

log:

[move_group_interface_tutorial-1] [INFO] [1680775155.255027553] [moveit_ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 1680775154.254896, but latest received state has time 454.390000.

yeyanlei commented 1 year ago

I have already resolved this problem, thank you everyone. by adding {"use_sim_time": True} move_group_interface_tutorial.launch.py

from launch import LaunchDescription from launch_ros.actions import Node from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description(): moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_moveit_configs()

# MoveGroupInterface demo executable
move_group_demo = Node(
    name="move_group_interface_tutorial",
    package="moveit2_tutorials",
    executable="move_group_interface_tutorial",
    output="screen",
    parameters=[
        moveit_config.robot_description,
        moveit_config.robot_description_semantic,
        moveit_config.robot_description_kinematics,
        {"use_sim_time": True},
    ],
)

return LaunchDescription([move_group_demo])
omermaayani commented 1 year ago

@omermaayani Has your issue been resolved?

I didn't found solution. I'm working around it with xyz commands that does not need the previous arm's state

yeyanlei commented 1 year ago

I have already resolved this issue by adding {"use_sim_time": True} to move_group_interface_tutorial.launch.py. Thanks to @omermaayani for the suggestion.