search

moveit / moveit2

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

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

Open ron007d opened 3 months ago

ron007d commented 3 months ago

Description

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

Overview of your issue here.

Your environment

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](https://justpaste.it/fc7xe)

3. colcon build the packages of this repo
4. launch the demo_gazebo.launch.py
```console
ros2 launch moveit_resources_panda_moveit_config demo_gazebo.launch.py
  1. 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
```console
[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[bot] commented 2 months ago

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 commented 1 month ago

Facing the same issue

ron007d commented 1 month ago

Facing the same issue

this helped me

config_dict = moveit_config.to_dict()
config_dict.update({'use_sim_time':True})
ogios commented 2 weeks ago

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 commented 2 weeks ago

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 commented 2 weeks ago

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...