moveit_py -> use_sim_time: True fetching Error rclcpp::exceptions::InvalidParameterValueException

[ron007d]

Description

Using panda_moveit_config from moveit resources,
added another launch file to launch the robot in gazebo.

• Gazebo model loaded = ✅
• rviz loaded = ✅
• Rvix motion planning able to control = ✅
• moveit py = ❎

Overview of your issue here.

Your environment

• ROS Distro: Humble
• OS Version: Ubuntu 22.04
• Source
• main

Steps to reproduce

1. clone this package > moveit resources - https://github.com/moveit/moveit_resources

git clone https://github.com/moveit/moveit_resources

2. in the panda_moveit_config package create a file name demo_gazebo.launch.py and copy and paste this python code

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument,IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.conditions import IfCondition
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory,get_package_prefix
from moveit_configs_utils import MoveItConfigsBuilder


def generate_launch_description():
    os.environ['GAZEBO_MODEL_PATH'] =f"{get_package_prefix('moveit_resources_panda_description')}/share/:{os.environ['GAZEBO_MODEL_PATH']}" 
    # print(os.environ['GAZEBO_MODEL_PATH'])

    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
        )
    
    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                    arguments=['-topic', 'robot_description',
                                '-entity', 'panda', '-x', '0.0','-y', '0.0','-z', '0.0'],
                    output='screen')
    
    # Command-line arguments
    rviz_config_arg = DeclareLaunchArgument(
        "rviz_config",
        default_value="moveit.rviz",
        description="RViz configuration file",
    )

    db_arg = DeclareLaunchArgument(
        "db", default_value="False", description="Database flag"
    )

    ros2_control_hardware_type = DeclareLaunchArgument(
        "ros2_control_hardware_type",
        default_value="mock_components",
        description="ROS 2 control hardware interface type to use for the launch file -- possible values: [mock_components, isaac]",
    )

    moveit_config = (
        MoveItConfigsBuilder("moveit_resources_panda")
        .robot_description(
            file_path="config/panda.gazebo.urdf",
        )
        .robot_description_semantic(file_path="config/panda.srdf")
        .planning_scene_monitor(
            publish_robot_description=True, publish_robot_description_semantic=True
        )
        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
        .planning_pipelines(
            pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"]
        )
        .to_moveit_configs()
    )

    # Start the actual move_group node/action server
    move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[moveit_config.to_dict(),{'use_sim_time':True}],
        arguments=["--ros-args", "--log-level", "info"],
    )

    # RViz
    rviz_base = LaunchConfiguration("rviz_config")
    rviz_config = PathJoinSubstitution(
        [FindPackageShare("moveit_resources_panda_moveit_config"), "launch", rviz_base]
    )
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.planning_pipelines,
            moveit_config.robot_description_kinematics,
            moveit_config.joint_limits,
        ],
    )

    # Static TF
    static_tf_node = 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", "panda_link0"],
    )

    # Publish TF
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[moveit_config.robot_description,{'use_sim_time':True}],
    )

    # ros2_control using FakeSystem as hardware
    # ros2_controllers_path = os.path.join(
    #     get_package_share_directory("moveit_resources_panda_moveit_config"),
    #     "config",
    #     "ros2_controllers.yaml",
    # )
    # ros2_control_node = Node(
    #     package="controller_manager",
    #     executable="ros2_control_node",
    #     parameters=[ros2_controllers_path],
    #     remappings=[
    #         ("/controller_manager/robot_description", "/robot_description"),
    #     ],
    #     output="screen",
    # )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "joint_state_broadcaster",
            "--controller-manager",
            "/controller_manager",
        ],
    )

    panda_arm_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["panda_arm_controller", "-c", "/controller_manager"],
    )

    panda_hand_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["panda_hand_controller", "-c", "/controller_manager"],
    )

    # Warehouse mongodb server
    db_config = LaunchConfiguration("db")
    mongodb_server_node = Node(
        package="warehouse_ros_mongo",
        executable="mongo_wrapper_ros.py",
        parameters=[
            {"warehouse_port": 33829},
            {"warehouse_host": "localhost"},
            {"warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection"},
        ],
        output="screen",
        condition=IfCondition(db_config),
    )

    return LaunchDescription(
        [
            gazebo,
            rviz_config_arg,
            db_arg,
            ros2_control_hardware_type,
            rviz_node,
            # static_tf_node,
            robot_state_publisher,
            spawn_entity,
            move_group_node,
            # ros2_control_node,
            joint_state_broadcaster_spawner,
            panda_arm_controller_spawner,
            panda_hand_controller_spawner,
            mongodb_server_node,
        ]
    )

ALSO add this gazebo ros controller added urdf in the config directory as panda.gazebo.urdf
https://justpaste.it/fc7xe

3. colcon build the packages of this repo
4. launch the demo_gazebo.launch.py

ros2 launch moveit_resources_panda_moveit_config demo_gazebo.launch.py 

5. Run this python file

import rclpy
from rclpy.logging import get_logger
from moveit.core.robot_state import RobotState
from moveit.planning import MoveItPy, MultiPipelinePlanRequestParameters
from moveit_configs_utils import MoveItConfigsBuilder
from ament_index_python import get_package_share_directory
import time
import os

moveit_config = (
        MoveItConfigsBuilder("moveit_resources_panda")
        .robot_description(
            file_path="config/panda.gazebo.urdf",
        )
        .robot_description_semantic(file_path="config/panda.srdf")
        .planning_scene_monitor(
            publish_robot_description=True, publish_robot_description_semantic=True
        )
        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
        .planning_pipelines(
            pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"]
        )
        .moveit_cpp(file_path=os.path.join(
                get_package_share_directory("moveit2_tutorials"),
                "config",
                "jupyter_notebook_prototyping.yaml"))

        .to_moveit_configs()
    )

def plan_and_execute(
    robot,
    planning_component,
    logger,
    single_plan_parameters=None,
    multi_plan_parameters=None,
    sleep_time=0.0,
):
    """Helper function to plan and execute a motion."""
    # plan to goal
    logger.info("Planning trajectory")
    if multi_plan_parameters is not None:
        plan_result = planning_component.plan(
            multi_plan_parameters=multi_plan_parameters
        )
    elif single_plan_parameters is not None:
        plan_result = planning_component.plan(
            single_plan_parameters=single_plan_parameters
        )
    else:
        plan_result = planning_component.plan()

    # execute the plan
    if plan_result:
        logger.info("Executing plan")
        robot_trajectory = plan_result.trajectory
        robot.execute(robot_trajectory, controllers=[])
    else:
        logger.error("Planning failed")

    time.sleep(sleep_time)


def main():
    config_dict = moveit_config.to_dict()
    config_dict.update({'use_sim_time' : True})
    rclpy.init()
    logger = get_logger("moveit_py.pose_goal")

    # instantiate MoveItPy instance and get planning component
    panda = MoveItPy(node_name="moveit_py", config_dict= config_dict)
    panda_arm = panda.get_planning_component("panda_arm")
    logger.info("MoveItPy instance created")


     # set plan start state to current state
    panda_arm.set_start_state_to_current_state()

    # set pose goal with PoseStamped message
    panda_arm.set_goal_state(configuration_name="extended")

    plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)


if __name__ == '__main__':
    main()

Expected behaviour

Robot moving to extented position

Actual behaviour

python code error, kernel crash in jupyter notebook

Console output

[INFO] [1722406022.160341719] [moveit_3747324773.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for panda_arm_controller
[INFO] [1722406022.160445906] [moveit_3747324773.moveit.plugins.simple_controller_manager]: Max effort set to 0.0
[INFO] [1722406022.163541288] [moveit_3747324773.moveit.plugins.simple_controller_manager]: Added GripperCommand controller for panda_hand_controller
[INFO] [1722406022.163789564] [moveit_3747324773.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[INFO] [1722406022.163845204] [moveit_3747324773.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[INFO] [1722406022.164344371] [moveit_3747324773.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers
terminate called after throwing an instance of 'rclcpp::exceptions::InvalidParameterValueException'
  what():  parameter 'qos_overrides./clock.subscription.durability' could not be set: 
Aborted (core dumped)

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[ra-georgi]

Facing the same issue

[ron007d]

Facing the same issue

this helped me

config_dict = moveit_config.to_dict()
config_dict.update({'use_sim_time':True})

[ogios]

this helped me

config_dict = moveit_config.to_dict()
config_dict.update({'use_sim_time':True})

@ron007d
what does this mean? isn't this the cause of the issue?
i have the same problem here with this value set

[ogios]

alright. i used to directly run the script inside console.
but now, i created a new launch file to include this script, and move moveit_config into the launch file, pass to the script as the parameter.
and it worked...

I don't understand why

[ogios]

Ah! Found one better solution.

When i'm using the launch file, i notice the argument passed to the script: --params-file /tmp/launch_params_xp_ss6gq.
And as i go though MoveItPy cpp code here, i found it containing lots of launch_params related stuff which also includes an argument: --params-file.

The logic writes either config_dict or launch_params_filepaths, and they all end up with --params-file none the less. just that config_dict is converted into yaml by moveit::utils::create_params_file_from_dict.

So i compare two files, and found the one major difference is that the node_name at the root, the one from launch file is /**, and the one from config_dict is the node_name i passed into MoveItPy.
(Solution) And i try to convert the dict outside of MoveItPy and pass in as launch_params_filepaths:

moveit_config.update({"use_sim_time": True})
file = create_params_file_from_dict(moveit_config, "/**")
arm = MoveItPy(
    node_name=node_name,
    launch_params_filepaths=[file],
)

And it worked, just simple as that.

but idk why node name causes this to happen...