Use sim time with Moveit

Question

I am having a problem trying to use Moveit2 in simulation, I created the Moveit config package for my robot arm using the setup assistant, but the generated move_group.launcy.py file looks like this:

from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_move_group_launch


def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("ur", package_name="flower_catcher_moveit_config").to_moveit_configs()
    return generate_move_group_launch(moveit_config)

The problem with this format is that there is no way for me to set the use_sim_time parameter to true, and that causes the path execution to fail with this error:

[move_group-7] [INFO] [1696192877.744131744] [moveit_ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 1696192876.744054, but latest received state has time 148.003000.

I kind of solved the problem by setting the parameter afterward with the command

ros2 param set /move_group use_sim_time true

However, I want to know if there is a way to set this parameter in the launch file.

Thanks!

Answer 1

Unfortunately, there is no clean way to pass it through launch file, while using moveit_configs_utils.launches. This was raised previously in an issue and use_sim_time technically supposed to be added here in the source code. That said, here is an engineered solution around this:

from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import *
def generate_move_group_launch(moveit_config):
    ld = LaunchDescription()
    ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
    ld.add_action(
        DeclareBooleanLaunchArg("allow_trajectory_execution", default_value=True)
    )
    ld.add_action(
        DeclareBooleanLaunchArg("publish_monitored_planning_scene", default_value=True)
    )
    # load non-default MoveGroup capabilities (space separated)
    ld.add_action(DeclareLaunchArgument("capabilities", default_value=""))
    # inhibit these default MoveGroup capabilities (space separated)
    ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value=""))
    # do not copy dynamics information from /joint_states to internal robot monitoring
    # default to false, because almost nothing in move_group relies on this information
    ld.add_action(DeclareBooleanLaunchArg("monitor_dynamics", default_value=False))
    should_publish = LaunchConfiguration("publish_monitored_planning_scene")
    move_group_configuration = {
        "publish_robot_description_semantic": True,
        "allow_trajectory_execution": LaunchConfiguration("allow_trajectory_execution"),
        # Note: Wrapping the following values is necessary so that the parameter value can be the empty string
        "capabilities": ParameterValue(
            LaunchConfiguration("capabilities"), value_type=str
        ),
        "disable_capabilities": ParameterValue(
            LaunchConfiguration("disable_capabilities"), value_type=str
        ),
        # Publish the planning scene of the physical robot so that rviz plugin can know actual robot
        "publish_planning_scene": should_publish,
        "publish_geometry_updates": should_publish,
        "publish_state_updates": should_publish,
        "publish_transforms_updates": should_publish,
        "monitor_dynamics": False,
        "use_sim_time": True
    }
    move_group_params = [
        moveit_config.to_dict(),
        move_group_configuration,
    ]
    add_debuggable_node(
        ld,
        package="moveit_ros_move_group",
        executable="move_group",
        commands_file=str(moveit_config.package_path / "launch" / "gdb_settings.gdb"),
        output="screen",
        parameters=move_group_params,
        extra_debug_args=["--debug"],
        # Set the display variable, in case OpenGL code is used internally
        additional_env={"DISPLAY": ":0"},
    )
    return ld
def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("ur", package_name="ur_moveit").to_moveit_configs()
    return generate_move_group_launch(moveit_config)

You can observe how use_sim_time: True has been passed with the move_group_configuration dictionary.