moveit / moveit2_tutorials

A sphinx-based centralized documentation repo for MoveIt 2
https://moveit.picknik.ai
BSD 3-Clause "New" or "Revised" License
135 stars 185 forks source link

Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds #912

Open SyZbidi opened 1 month ago

SyZbidi commented 1 month ago

Description

Followed the motion_planning_python_api tutorial example, the launch file is almost the same, just added planning_pipelines to moveit_config, and my MoveitPy node is only trying to plan and execute one example of named_target. I don't understand Python launch files much, and also new to ROS2 unfortunately. So I can't get to locate the reason behind having this error

Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds

Your environment

Steps to reproduce

I made my moveit_config package with moveit_setup_assistant. Then I only changed the acceleration_limits to True and set a value >0. After that I created a launch file similar to motion_planning_python_api_tutorial.launch.py with moveit_utils being my node

launch.py

"""
A launch file for running the motion planning python api tutorial
"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration
from moveit_configs_utils import MoveItConfigsBuilder
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    moveit_config = (
        MoveItConfigsBuilder(
            robot_name="thermoplast_abb_irb4600_camera_cell", package_name="abb_irb4600_webcam_moveit_config"
        )
        .robot_description(file_path="config/thermoplast_abb_irb4600_camera_cell.urdf.xacro")
        .trajectory_execution(file_path="config/moveit_controllers.yaml")
        .moveit_cpp(
            file_path=get_package_share_directory("abb_irb4600_webcam_moveit_config")
            + "/config/planning.yaml"
        )
        .to_moveit_configs()
    )

    example_file = DeclareLaunchArgument(
        "example_file",
        default_value="moveit_utils",
        description="Python API tutorial file name",
    )

    moveit_py_node = Node(
        name="moveit_utils",
        package="simple_motion_planner",
        executable=LaunchConfiguration("example_file"),
        output="both",
        parameters=[moveit_config.to_dict()],
    )

    rviz_config_file = os.path.join(
        get_package_share_directory("abb_irb4600_webcam_moveit_config"),
        "config",
        "moveit.rviz",
    )

    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config_file],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
        ],
    )

    static_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["--frame-id", "world", "--child-frame-id", "pedestal_base_link"],
    )

    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="log",
        parameters=[moveit_config.robot_description],
    )

    ros2_controllers_path = os.path.join(
        get_package_share_directory("abb_irb4600_webcam_moveit_config"),
        "config",
        "ros2_controllers.yaml",
    )
    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[moveit_config.robot_description, ros2_controllers_path],
        output="log",
    )

    load_controllers = []
    for controller in [
        "manipulator_controller",
    ]:
        load_controllers += [
            ExecuteProcess(
                cmd=["ros2 run controller_manager spawner {}".format(controller)],
                shell=True,
                output="log",
            )
        ]

    ld = LaunchDescription(
        [
            example_file,
            moveit_py_node,
            robot_state_publisher,
            ros2_control_node,
            rviz_node,
            static_tf,
        ]
        + load_controllers
    )
    # move_group_launch_path = os.path.join(
    #     get_package_share_directory("abb_irb4600_webcam_moveit_config"),
    #     "launch",
    #     "move_group.launch.py",
    # )
    # ld.add_action(
    #     IncludeLaunchDescription(
    #         PythonLaunchDescriptionSource(
    #             move_group_launch_path
    #         ),
    #     )
    # )
    return ld

moveit_py_node script

def main():
    rclpy.init()
    logger = get_logger("moveit_py")
    robot = MoveItPy(node_name="moveit_py")
    logger.info("node started :D")
    move_group_name = "manipulator"
    eef_move_group_name = ""
    move_group = robot.get_planning_component(move_group_name)

    robot_model = robot.get_robot_model()
    robot_state = RobotState(robot_model)
    move_group.set_start_state_to_current_state()
    robot_state.update()
    move_group.set_start_state(configuration_name="home")
    move_group.set_goal_state(configuration_name="perch_table")
    plan = move_group.plan()
    time.sleep(3.0)
    traj = plan.trajectory
    robot.execute(traj, controllers=[])

if __name__ == "__main__":
    main()

planning.yaml

# planning_scene_monitor_options:
#   name: "planning_scene_monitor"
#   robot_description: "robot_description"
#   joint_state_topic: "/joint_states"
#   attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
#   publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
#   monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
#   wait_for_initial_state_timeout: 10.0

planning_pipelines:
  pipeline_names: ["ompl", "pilz_industrial_motion_planner"]

plan_request_params:
  planning_attempts: 1
  planning_pipeline: ompl
  max_velocity_scaling_factor: 1.0
  max_acceleration_scaling_factor: 1.0

ompl_rrtc:  # Namespace for individual plan request
  plan_request_params:  # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp
    planning_attempts: 1  # Number of attempts the planning pipeline tries to solve a given motion planning problem
    planning_pipeline: ompl  # Name of the pipeline that is being used
    planner_id: "RRTConnectkConfigDefault"  # Name of the specific planner to be used by the pipeline
    max_velocity_scaling_factor: 1.0  # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
    max_acceleration_scaling_factor: 1.0  # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
    planning_time: 1.0  # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned

pilz_lin:
  plan_request_params:
    planning_attempts: 1
    planning_pipeline: pilz_industrial_motion_planner
    planner_id: "PTP"
    max_velocity_scaling_factor: 1.0
    max_acceleration_scaling_factor: 1.0
    planning_time: 0.8

# chomp_planner:
#   plan_request_params:
#     planning_attempts: 1
#     planning_pipeline: chomp
#     max_velocity_scaling_factor: 1.0
#     max_acceleration_scaling_factor: 1.0
#     planning_time: 1.5

I commented out the planning scene monitor due to an error that I wanted to move past it [moveit_utils-1] RuntimeError: Unable to configure planning scene monitor

Expected behaviour

Tell us what should happen

I expected the motion to be planned and executed as simple as that

Backtrace or Console output

Use gist.github.com to copy-paste the console output or segfault backtrace using gdb.

[INFO] [launch]: All log files can be found below /home/ros/.ros/log/2024-06-06-19-41-54-006135-syrine-OptiPlex-7060-234583
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [moveit_utils-1]: process started with pid [234585]
[INFO] [robot_state_publisher-2]: process started with pid [234587]
[INFO] [ros2_control_node-3]: process started with pid [234589]
[INFO] [rviz2-4]: process started with pid [234591]
[INFO] [static_transform_publisher-5]: process started with pid [234593]
[INFO] [ros2 run controller_manager spawner manipulator_controller-6]: process started with pid [234598]
[rviz2-4] QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-ros'
[static_transform_publisher-5] [INFO] [1717702914.561383358] [static_transform_publisher]: Spinning until stopped - publishing transform
[static_transform_publisher-5] translation: ('0.000000', '0.000000', '0.000000')
[static_transform_publisher-5] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-5] from 'world' to 'pedestal_base_link'
[ros2_control_node-3] [WARN] [1717702914.566146874] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[ros2_control_node-3] [INFO] [1717702914.566457946] [resource_manager]: Loading hardware 'FakeSystem' 
[ros2_control_node-3] [INFO] [1717702914.566771832] [resource_manager]: Initialize hardware 'FakeSystem' 
[ros2_control_node-3] [WARN] [1717702914.566808488] [mock_generic_system]: Parsing of optional initial interface values failed or uses a deprecated format. Add initial values for every state interface in the ros2_control.xacro. For example: 
[ros2_control_node-3] <state_interface name="velocity"> 
[ros2_control_node-3]   <param name="initial_value">0.0</param> 
[ros2_control_node-3] </state_interface>
[ros2_control_node-3] [INFO] [1717702914.566819169] [resource_manager]: Successful initialization of hardware 'FakeSystem'
[ros2_control_node-3] [INFO] [1717702914.566855383] [resource_manager]: 'configure' hardware 'FakeSystem' 
[ros2_control_node-3] [INFO] [1717702914.566862644] [resource_manager]: Successful 'configure' of hardware 'FakeSystem'
[ros2_control_node-3] [INFO] [1717702914.566868771] [resource_manager]: 'activate' hardware 'FakeSystem' 
[ros2_control_node-3] [INFO] [1717702914.566873632] [resource_manager]: Successful 'activate' of hardware 'FakeSystem'
[ros2_control_node-3] [INFO] [1717702914.573160241] [controller_manager]: update rate is 100 Hz
[ros2_control_node-3] [INFO] [1717702914.573306264] [controller_manager]: RT kernel is recommended for better performance
[robot_state_publisher-2] [INFO] [1717702914.595393682] [robot_state_publisher]: got segment abb_irb4600_base
[robot_state_publisher-2] [INFO] [1717702914.595507431] [robot_state_publisher]: got segment abb_irb4600_base_link
[robot_state_publisher-2] [INFO] [1717702914.595516642] [robot_state_publisher]: got segment abb_irb4600_flange
[robot_state_publisher-2] [INFO] [1717702914.595521965] [robot_state_publisher]: got segment abb_irb4600_link_1
[robot_state_publisher-2] [INFO] [1717702914.595526505] [robot_state_publisher]: got segment abb_irb4600_link_2
[robot_state_publisher-2] [INFO] [1717702914.595530988] [robot_state_publisher]: got segment abb_irb4600_link_3
[robot_state_publisher-2] [INFO] [1717702914.595535434] [robot_state_publisher]: got segment abb_irb4600_link_4
[robot_state_publisher-2] [INFO] [1717702914.595539909] [robot_state_publisher]: got segment abb_irb4600_link_5
[robot_state_publisher-2] [INFO] [1717702914.595544329] [robot_state_publisher]: got segment abb_irb4600_link_6
[robot_state_publisher-2] [INFO] [1717702914.595548666] [robot_state_publisher]: got segment abb_irb4600_tool0
[robot_state_publisher-2] [INFO] [1717702914.595553120] [robot_state_publisher]: got segment dresspack_base_link
[robot_state_publisher-2] [INFO] [1717702914.595557462] [robot_state_publisher]: got segment ft660_base_link
[robot_state_publisher-2] [INFO] [1717702914.595561885] [robot_state_publisher]: got segment ft660_flange
[robot_state_publisher-2] [INFO] [1717702914.595566245] [robot_state_publisher]: got segment ft660_robot_adapter
[robot_state_publisher-2] [INFO] [1717702914.595570534] [robot_state_publisher]: got segment ft660_sensor
[robot_state_publisher-2] [INFO] [1717702914.595574902] [robot_state_publisher]: got segment ft660_sensor_adapter
[robot_state_publisher-2] [INFO] [1717702914.595579672] [robot_state_publisher]: got segment ft660_sensor_extension
[robot_state_publisher-2] [INFO] [1717702914.595584103] [robot_state_publisher]: got segment left_table_base_link
[robot_state_publisher-2] [INFO] [1717702914.595588493] [robot_state_publisher]: got segment left_table_wobj_frame
[robot_state_publisher-2] [INFO] [1717702914.595592866] [robot_state_publisher]: got segment pedestal_base_link
[robot_state_publisher-2] [INFO] [1717702914.595597294] [robot_state_publisher]: got segment pedestal_robot_link
[robot_state_publisher-2] [INFO] [1717702914.595601744] [robot_state_publisher]: got segment thorlabs_mount_base_link
[robot_state_publisher-2] [INFO] [1717702914.595606044] [robot_state_publisher]: got segment thorlabs_mount_flange
[robot_state_publisher-2] [INFO] [1717702914.595610454] [robot_state_publisher]: got segment wall_base_link
[robot_state_publisher-2] [INFO] [1717702914.595614962] [robot_state_publisher]: got segment wall_cabletray_link
[robot_state_publisher-2] [INFO] [1717702914.595619372] [robot_state_publisher]: got segment wall_firehose_link
[robot_state_publisher-2] [INFO] [1717702914.595623691] [robot_state_publisher]: got segment webcam_base_link
[robot_state_publisher-2] [INFO] [1717702914.595628210] [robot_state_publisher]: got segment webcam_holder
[robot_state_publisher-2] [INFO] [1717702914.595632510] [robot_state_publisher]: got segment webcam_optical_frame
[robot_state_publisher-2] [INFO] [1717702914.595636918] [robot_state_publisher]: got segment world
[moveit_utils-1] [INFO] [1717702914.681530863] [moveit_3672297761.moveit.py.cpp_initializer]: Initialize rclcpp
[moveit_utils-1] [INFO] [1717702914.681555943] [moveit_3672297761.moveit.py.cpp_initializer]: Initialize node parameters
[moveit_utils-1] [INFO] [1717702914.681561703] [moveit_3672297761.moveit.py.cpp_initializer]: Initialize node and executor
[moveit_utils-1] [INFO] [1717702914.686204777] [moveit_3672297761.moveit.py.cpp_initializer]: Spin separate thread
[rviz2-4] [INFO] [1717702914.708221755] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-4] [INFO] [1717702914.708374251] [rviz2]: OpenGl version: 4.5 (GLSL 4.5)
[moveit_utils-1] [INFO] [1717702914.714864426] [moveit_3672297761.moveit.ros.rdf_loader]: Loaded robot model in 0.0285557 seconds
[moveit_utils-1] [INFO] [1717702914.714901899] [moveit_3672297761.moveit.core.robot_model]: Loading robot model 'thermoplast_abb_irb4600_camera_cell'...
[moveit_utils-1] [INFO] [1717702914.714908483] [moveit_3672297761.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[moveit_utils-1] [INFO] [1717702914.736218496] [moveit_3672297761.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'manipulator': 1 1 1 1 1 1
[rviz2-4] [INFO] [1717702914.759859393] [rviz2]: Stereo is NOT SUPPORTED
[moveit_utils-1] [INFO] [1717702914.794378177] [moveit_3672297761.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[moveit_utils-1] [INFO] [1717702914.794475370] [moveit_3672297761.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states
[moveit_utils-1] [INFO] [1717702914.794949812] [moveit_3672297761.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[moveit_utils-1] [INFO] [1717702914.795232132] [moveit_3672297761.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[moveit_utils-1] [INFO] [1717702914.795242414] [moveit_3672297761.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher.
[moveit_utils-1] [INFO] [1717702914.795746547] [moveit_3672297761.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene.
[moveit_utils-1] [INFO] [1717702914.796148738] [moveit_3672297761.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[moveit_utils-1] [INFO] [1717702914.796224909] [moveit_3672297761.moveit.ros.planning_scene_monitor]: Starting planning scene monitor
[moveit_utils-1] [INFO] [1717702914.796548832] [moveit_3672297761.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene'
[moveit_utils-1] [INFO] [1717702914.796559188] [moveit_3672297761.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[moveit_utils-1] [INFO] [1717702914.796804931] [moveit_3672297761.moveit.ros.planning_scene_monitor]: Listening to 'collision_object'
[moveit_utils-1] [INFO] [1717702914.797039396] [moveit_3672297761.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[moveit_utils-1] [WARN] [1717702914.797294681] [moveit_3672297761.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[moveit_utils-1] [ERROR] [1717702914.797318094] [moveit_3672297761.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates
[moveit_utils-1] [INFO] [1717702914.812041381] [moveit_3672297761.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL'
[moveit_utils-1] [INFO] [1717702914.813134059] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[moveit_utils-1] [INFO] [1717702914.813720098] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[moveit_utils-1] [INFO] [1717702914.813738825] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[moveit_utils-1] [INFO] [1717702914.813885774] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[moveit_utils-1] [INFO] [1717702914.813896469] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds'
[moveit_utils-1] [INFO] [1717702914.813914662] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds'
[moveit_utils-1] [INFO] [1717702914.813921328] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision'
[moveit_utils-1] [INFO] [1717702914.813931397] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision'
[moveit_utils-1] [INFO] [1717702914.814953346] [moveit_py]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization'
[moveit_utils-1] [INFO] [1717702914.815959576] [moveit_py]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization'
[moveit_utils-1] [INFO] [1717702914.815999384] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution'
[moveit_utils-1] [INFO] [1717702914.817141846] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution'
[moveit_utils-1] [INFO] [1717702914.817162528] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath'
[moveit_utils-1] [INFO] [1717702914.817675537] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath'
[moveit_utils-1] [INFO] [1717702914.819892050] [moveit_3672297761.moveit.planners.pilz.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[moveit_utils-1] [INFO] [1717702914.822401929] [moveit_3672297761.moveit.planners.pilz.command_planner]: Available plugins: pilz_industrial_motion_planner/PlanningContextLoaderCIRC pilz_industrial_motion_planner/PlanningContextLoaderLIN pilz_industrial_motion_planner/PlanningContextLoaderPTP 
[moveit_utils-1] [INFO] [1717702914.822417741] [moveit_3672297761.moveit.planners.pilz.command_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderCIRC
[moveit_utils-1] [INFO] [1717702914.823159939] [moveit_3672297761.moveit.planners.pilz.command_planner]: Registered Algorithm [CIRC]
[moveit_utils-1] [INFO] [1717702914.823172397] [moveit_3672297761.moveit.planners.pilz.command_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderLIN
[moveit_utils-1] [INFO] [1717702914.823698685] [moveit_3672297761.moveit.planners.pilz.command_planner]: Registered Algorithm [LIN]
[moveit_utils-1] [INFO] [1717702914.823709597] [moveit_3672297761.moveit.planners.pilz.command_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderPTP
[moveit_utils-1] [INFO] [1717702914.824232360] [moveit_3672297761.moveit.planners.pilz.command_planner]: Registered Algorithm [PTP]
[moveit_utils-1] [INFO] [1717702914.824244347] [moveit_3672297761.moveit.ros.planning_pipeline]: Successfully loaded planner 'Pilz Industrial Motion Planner'
[moveit_utils-1] [INFO] [1717702914.825386209] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[moveit_utils-1] [INFO] [1717702914.825475938] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[moveit_utils-1] [INFO] [1717702914.825487263] [moveit_py]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[moveit_utils-1] [INFO] [1717702914.825728322] [moveit_py]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[moveit_utils-1] [INFO] [1717702914.825740642] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds'
[moveit_utils-1] [INFO] [1717702914.825760972] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds'
[moveit_utils-1] [INFO] [1717702914.825767964] [moveit_py]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision'
[moveit_utils-1] [INFO] [1717702914.825779138] [moveit_py]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision'
[moveit_utils-1] [INFO] [1717702914.826818157] [moveit_py]: Try loading adapter 'default_planning_response_adapters/ValidateSolution'
[moveit_utils-1] [INFO] [1717702914.827792714] [moveit_py]: Loaded adapter 'default_planning_response_adapters/ValidateSolution'
[moveit_utils-1] [INFO] [1717702914.827817629] [moveit_py]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath'
[moveit_utils-1] [INFO] [1717702914.828054921] [moveit_py]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath'
[moveit_utils-1] [INFO] [1717702914.846701434] [moveit_3672297761.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for manipulator_controller
[moveit_utils-1] [INFO] [1717702914.846822950] [moveit_3672297761.moveit.plugins.simple_controller_manager]: Returned 1 controllers in list
[moveit_utils-1] [INFO] [1717702914.846838462] [moveit_3672297761.moveit.plugins.simple_controller_manager]: Returned 1 controllers in list
[moveit_utils-1] [INFO] [1717702914.847181574] [moveit_3672297761.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers
[rviz2-4] Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occurred with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open.
[rviz2-4]          at line 253 in /opt/ros/humble/include/class_loader/class_loader/class_loader_core.hpp
[rviz2-4] [WARN] [1717702914.850100840] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[moveit_utils-1] [INFO] [1717702914.854984353] [moveit_py]: node started :D
[moveit_utils-1] [WARN] [1717702914.855094138] [moveit_py]: Parameter 'plan_request_params.planner_id' not found in config use default value instead, check parameter type and namespace in YAML file
[moveit_utils-1] [WARN] [1717702914.855130718] [moveit_py]: Parameter 'plan_request_params.planning_time' not found in config use default value instead, check parameter type and namespace in YAML file
[moveit_utils-1] [INFO] [1717702914.855289272] [moveit_py]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
[moveit_utils-1] [INFO] [1717702914.855367754] [moveit_py]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[moveit_utils-1] [WARN] [1717702914.855378603] [moveit_3672297761.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[moveit_utils-1] [INFO] [1717702914.855394654] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[moveit_utils-1] [INFO] [1717702914.855421133] [moveit_py]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[moveit_utils-1] [INFO] [1717702914.855888061] [moveit_3672297761.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[moveit_utils-1] [INFO] [1717702914.855983585] [moveit_py]: Calling Planner 'OMPL'
[moveit_utils-1] [INFO] [1717702914.872383945] [moveit_py]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
[moveit_utils-1] [INFO] [1717702914.873422948] [moveit_py]: Calling PlanningResponseAdapter 'ValidateSolution'
[moveit_utils-1] [INFO] [1717702914.874222550] [moveit_py]: Calling PlanningResponseAdapter 'DisplayMotionPath'
[moveit_utils-1] [WARN] [1717702914.874300171] [moveit_3672297761.moveit.ros.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[ros2_control_node-3] [INFO] [1717702915.233214484] [controller_manager]: Loading controller 'manipulator_controller'
[ros2_control_node-3] [WARN] [1717702915.263300385] [manipulator_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[ros2 run controller_manager spawner manipulator_controller-6] [INFO] [1717702915.275758814] [spawner_manipulator_controller]: Loaded manipulator_controller
[ros2_control_node-3] [INFO] [1717702915.278241653] [controller_manager]: Configuring controller 'manipulator_controller'
[ros2_control_node-3] [INFO] [1717702915.278854543] [manipulator_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ros2_control_node-3] [INFO] [1717702915.278927893] [manipulator_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[ros2_control_node-3] [INFO] [1717702915.278964521] [manipulator_controller]: Using 'splines' interpolation method.
[ros2_control_node-3] [INFO] [1717702915.283252164] [manipulator_controller]: Controller state will be published at 50.00 Hz.
[ros2_control_node-3] [INFO] [1717702915.290360796] [manipulator_controller]: Action status changes will be monitored at 20.00 Hz.
[ros2 run controller_manager spawner manipulator_controller-6] [INFO] [1717702915.325848479] [spawner_manipulator_controller]: Configured and activated manipulator_controller
[INFO] [ros2 run controller_manager spawner manipulator_controller-6]: process has finished cleanly [pid 234598]
[rviz2-4] [ERROR] [1717702917.877206322] [rviz2.moveit.ros.motion_planning_frame]: Action server: /recognize_objects not available
[moveit_utils-1] [INFO] [1717702917.877237943] [moveit_3672297761.moveit.plugins.simple_controller_manager]: Returned 1 controllers in list
[moveit_utils-1] [INFO] [1717702917.877311077] [moveit_3672297761.moveit.plugins.simple_controller_manager]: Returned 1 controllers in list
[moveit_utils-1] [INFO] [1717702917.877426291] [moveit_3672297761.moveit.plugins.simple_controller_manager]: Returned 1 controllers in list
[moveit_utils-1] [INFO] [1717702917.877458818] [moveit_3672297761.moveit.plugins.simple_controller_manager]: Returned 1 controllers in list
[moveit_utils-1] [INFO] [1717702917.877536799] [moveit_3672297761.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[rviz2-4] [INFO] [1717702917.893085048] [rviz2.moveit.ros.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-4] [ERROR] [1717702917.972281146] [rviz2.moveit.ros.trajectory_visualization]: No robot state or robot model loaded
[rviz2-4] [INFO] [1717702917.995918730] [rviz2.moveit.ros.rdf_loader]: Loaded robot model in 0.0233417 seconds
[rviz2-4] [INFO] [1717702917.995975989] [rviz2.moveit.core.robot_model]: Loading robot model 'thermoplast_abb_irb4600_camera_cell'...
[rviz2-4] [INFO] [1717702917.995991730] [rviz2.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-4] [WARN] [1717702918.018743635] [rviz2.moveit.ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!
[rviz2-4] [INFO] [1717702918.091420688] [rviz2.moveit.ros.planning_scene_monitor]: Starting planning scene monitor
[rviz2-4] [INFO] [1717702918.091852952] [rviz2.moveit.ros.planning_scene_monitor]: Listening to '/monitored_planning_scene'
[rviz2-4] [INFO] [1717702918.243863969] [interactive_marker_display_94116680968320]: Connected on namespace: /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic
[rviz2-4] [INFO] [1717702918.250131869] [rviz2.moveit.ros.robot_interaction]: No active joints or end effectors found for group 'manipulator'. Make sure that kinematics.yaml is loaded in this node's namespace.
[rviz2-4] [INFO] [1717702918.250898375] [rviz2.moveit.ros.robot_interaction]: No active joints or end effectors found for group 'manipulator'. Make sure that kinematics.yaml is loaded in this node's namespace.
[rviz2-4] [INFO] [1717702918.255510141] [rviz2.moveit.ros.motion_planning_frame]: group manipulator
[rviz2-4] [INFO] [1717702918.255528582] [rviz2.moveit.ros.motion_planning_frame]: Constructing new MoveGroup connection for group 'manipulator' in namespace ''
[rviz2-4] [INFO] [1717702918.271885302] [interactive_marker_display_94116680968320]: Sending request for interactive markers
[rviz2-4] [INFO] [1717702918.304191585] [interactive_marker_display_94116680968320]: Service response received for initialization
[moveit_utils-1] [INFO] [1717702918.877642308] [moveit_3672297761.moveit.ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 1717702917.877563, but latest received state has time 0.000000.
[moveit_utils-1] Check clock synchronization if your are running ROS across multiple machines!
[moveit_utils-1] [WARN] [1717702918.877714628] [moveit_3672297761.moveit.ros.trajectory_execution_manager]: Failed to validate trajectory: couldn't receive full current joint state within 1s
[moveit_utils-1] [INFO] [1717702918.881850550] [moveit_3672297761.moveit.ros.moveit_cpp]: Deleting MoveItCpp
[moveit_utils-1] [INFO] [1717702918.899369883] [moveit_3672297761.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene.
[moveit_utils-1] [INFO] [1717702918.902589224] [moveit_3672297761.moveit.ros.planning_scene_monitor]: Stopping world geometry monitor
[moveit_utils-1] [INFO] [1717702918.904708542] [moveit_3672297761.moveit.ros.planning_scene_monitor]: Stopping planning scene monitor
[moveit_utils-1] Warning: class_loader.ClassLoader: SEVERE WARNING!!! Attempting to unload library while objects created by this loader exist in the heap! You should delete your objects before attempting to unload the library or destroying the ClassLoader. The library will NOT be unloaded.
[moveit_utils-1]          at line 127 in ./src/class_loader.cpp
[INFO] [moveit_utils-1]: process has finished cleanly [pid 234585]
[rviz2-4] [INFO] [1717702978.272028392] [rviz2.moveit.ros.move_group_interface]: Ready to take commands for planning group manipulator.

Am I missing parameters? I have no idea where to look to fix the [rviz2-4] [ERROR] [1717702917.972281146] [rviz2.moveit.ros.trajectory_visualization]: No robot state or robot model loaded and moveit_utils-1] [INFO] [1717702918.877642308] [moveit_3672297761.moveit.ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 1717702917.877563, but latest received state has time 0.000000.

By the way use_sim_time doesn't fix i, I already tried to add it to the node vut I'm l=not using simulation/gazebo

Thanks!

okvik commented 1 month ago

Could be that your robot controller isn't up and publishing to joint_states topic by the time you are trying to plan. Seems plausible since you're launching everything from the same launch file and real robot drivers usually take a bit of time to establish connections to the hardware and get going.

You could easily test if this theory is correct by introducing a sleep of some duration before doing anything else in your program, just to give time for all the rest of the stuff in the launch file to come up. Alternatively you can subscribe to the joint_states topic yourself and wait until you get a first message (hint: this is more elegantly solved by liveliness QoS, which you may wanna look into at some date), at which point it may be safe to assume the controller / hardware is up and running and ready to receive commands. Pretty sure ros2_control provides a mechanism to establish the readyness of the controller / hardware, but I don't know whether MoveIt does anything with it -- I only worked with robots that don't use ros2_control.

Sorry it's late and I overlooked that you are using FakeHardware so this is unlikely an issue, though you are free to try if sleep helps anyway. At least, confirm you are actually getting something published on the joint_states topic.

SyZbidi commented 1 month ago

Sorry it's late and I overlooked that you are using FakeHardware so this is unlikely an issue, though you are free to try if sleep helps anyway. At least, confirm you are actually getting something published on the joint_states topic.

Your suggestion was actually helpful, thanks a lot! I let it sleep for 10 seconds and now I got to see the plan but no execution.

Also echoed the /joint_states and noticed that they're not being published so I added them to the launch file and finally got SUCCEEDED

[ros2_control_node-4] [INFO] [1717748497.252701086] [manipulator_controller]: Received new action goal
[ros2_control_node-4] [INFO] [1717748497.252793906] [manipulator_controller]: Accepted new action goal
[moveit_utils-1] [INFO] [1717748497.253029323] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: manipulator_controller started execution
[moveit_utils-1] [INFO] [1717748497.253073435] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[ros2_control_node-4] [INFO] [1717748499.763253884] [manipulator_controller]: Goal reached, success!
[moveit_utils-1] [INFO] [1717748499.803428641] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'manipulator_controller' successfully finished
[moveit_utils-1] [INFO] [1717748500.052285755] [moveit_3805631290.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ...
[moveit_utils-1] [INFO] [1717748500.060572846] [moveit_3805631290.moveit.ros.moveit_cpp]: Deleting MoveItCpp
[moveit_utils-1] [INFO] [1717748500.079781938] [moveit_3805631290.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene.
[moveit_utils-1] [INFO] [1717748500.080396127] [moveit_3805631290.moveit.ros.planning_scene_monitor]: Stopping world geometry monitor
[moveit_utils-1] [INFO] [1717748500.080647130] [moveit_3805631290.moveit.ros.planning_scene_monitor]: Stopping planning scene monitor

MicrosoftTeams-image (1)

sjahr commented 1 month ago

Sorry for the late reply, is your issue fixed now?

SyZbidi commented 1 month ago

Seems plausible since you're launching everything from the same launch file

@okvik I actually wanted to split them I don't like the idea of launching the moveit setup and the moveit_py node from the same file even if it's for fakehardware, but I didn't know what should the node launch file have for parameters.

I'd appreciate if you have a suggestion but you already solved my main issue so thank you again for that :)

SyZbidi commented 1 month ago

Sorry for the late reply, is your issue fixed now?

Hi @sjahr, not really. there's something wrong in my launch file above. adding a sleep time and a joint_state_publisher node did help with executing the motion but the /joint_states topic is publishing all zeros.

So though the fix suggested by @okvik worked, it doesn't seem like the way to go

SyZbidi commented 1 month ago

The solution was to add joint_state_broadcaster to load_controllers instead of joint_state_publisher. For some reason I removed that when I copied the tutorial launch files.

load_controllers = []
    for controller in [
        "manipulator_controller",
        "joint_state_broadcaster",
    ]:
        load_controllers += [
            ExecuteProcess(
                cmd=["ros2 run controller_manager spawner {}".format(controller)],
                shell=True,
                output="log",
            )
        ]
SyZbidi commented 1 month ago

@sjahr this is off-topic but also related to my work, it would be helpful if you take a look at this comment when you get the chance. Thank you

okvik commented 1 month ago

@okvik I actually wanted to split them I don't like the idea of launching the moveit setup and the moveit_py node from the same file even if it's for fakehardware, but I didn't know what should the node launch file have for parameters.

The way I do it is indeed to split each major hardware component (like a robot) launch from the control program logic and any other supporting nodes such as rviz, potentially the HMI node, etc. Basically, split the system in a way that lets me rebuild / start / restart without having to reboot the entire system from scratch every time. For production I additionally wrap these launches in their own systemd units, which lets me deal with dependencies, starting the system at boot, have more meaningful logging experience, easily clean up broken process trees, etc.