Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
ron007d opened this issue Jul 31, 2024 · 4 comments
Labels
bug Something isn't working

Comments

@ron007d
Copy link

ron007d commented Jul 31, 2024

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

  1. colcon build the packages of this repo
  2. launch the demo_gazebo.launch.py
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

[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)
@ron007d ron007d added the bug Something isn't working label Jul 31, 2024
Copy link

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.

@github-actions github-actions bot added the stale Inactive issues and PRs are marked as stale and may be closed automatically. label Sep 16, 2024
@ra-georgi
Copy link

Facing the same issue

@ron007d
Copy link
Author

ron007d commented Oct 9, 2024

Facing the same issue

this helped me

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

@github-actions github-actions bot removed the stale Inactive issues and PRs are marked as stale and may be closed automatically. label Oct 10, 2024
@ogios
Copy link

ogios commented Nov 7, 2024

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
bug Something isn't working
Projects
None yet
Development

No branches or pull requests

3 participants