Robotics StackExchange | Archived questions

Moveit2: Execution of a planned trajectory fails (Rviz MotionPlanning Plugin)

Hi there, I'm trying to get my first own robot project working with ros2 (humble).

I've created a simple URDF file with 6 revolute joints, so its only for purpose for me to understand how moveit etc. works - there is no real robot atm that I want to control. I also created a moveit-config package with the Setup Assistant. Before I test this tutorial I wanted to test whether the moveit_config pkg etc works. Turns out, I can plan (MotionPlanning in RViz), but when I click "execute" in RViz, it only says 'failed'.

I assume the problem being me not understanding the correct architecture to get it working; so feel free to correct my assumptions:

Based on the Log output, I assume it has something to do with the controller manager but I cannot figure it out unfortunately :( Here are important pieces of my launch file:

    move_group_node = Node(
    package="moveit_ros_move_group",
    executable="move_group",
    output="screen",
    arguments = ['--ros-args', '--log-level', 'WARN'],
    parameters=[
        robot_description,
        robot_description_semantic,
    ],
)

# Load controllers
controllers_path = os.path.join(
    get_package_share_directory(MOVEIT_CONFIG_PKG),
    "config",
    MOVEIT_CONTROLLERS_FILE_NAME,
)
control_node = Node(
    package="controller_manager",
    executable="ros2_control_node",
    parameters=[robot_description, controllers_path],
    output="log",
    arguments = ['--ros-args', '--log-level', 'WARN']
)


# launch controllers
load_controllers = []
for controller in [
    "robot_arm_controller",
    "joint_state_broadcaster",
]:
    load_controllers += [
        ExecuteProcess(
            cmd=["ros2 run controller_manager spawner  {}".format(
                controller)],
            shell=True,
            output="screen",
        )
    ]

And:

 # RViz
rviz_config_file = (
    os.path.join(get_package_share_directory(DESCRIPTION_PKG), "rviz", RVIZ_FILE_NAME)
)


rviz_node = Node(
    package="rviz2",
    executable="rviz2",
    output="log",
    arguments=["-d", rviz_config_file,'--ros-args', '--log-level', 'WARN'],
    parameters=[
        robot_description,
        robot_description_semantic,
    ],
)

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

# Publish TF
robot_state_publisher = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    name="robot_state_publisher",
    output="log",
    parameters=[robot_description],
)

joint_state_pub = Node(
    package="joint_state_publisher",
    executable="joint_state_publisher",
    name="joint_state_publisher",
    condition=UnlessCondition(LaunchConfiguration(JOINT_GUI)),
)

joint_state_pub_gui = Node(
    package="joint_state_publisher_gui",
    executable="joint_state_publisher_gui",
    name="joint_state_publisher_gui",
    condition=IfCondition(LaunchConfiguration(JOINT_GUI)),
)

return LaunchDescription(
    declared_arguments +
    [   
    control_node,
    move_group_node,
    rviz_node,
    static_tf,
    robot_state_publisher,
    joint_state_pub,
    joint_state_pub_gui,
    ]
    + load_controllers
)

And here a part of the log; It even says that I can plan (And in rviz it looks like it got a plan; it states the time it needed for planning, like 0.01sec).

[move_group-2] You can start planning now!
[move_group-2] 
[joint_state_publisher-6] [INFO] [1682078430.667513934] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic...
[rviz2-3] 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-3]          at line 253 in /opt/ros/humble/include/class_loader/class_loader/class_loader_core.hpp
[ERROR] [ros2_control_node-1]: process has died [pid 61056, exit code -6, cmd '/opt/ros/humble/lib/controller_manager/ros2_control_node --ros-args --log-level WARN --ros-args --params-file /tmp/launch_params__jbj3pr9 --params-file /home/robin/dev/ros2_ws/install/my_example_robot_moveit_config/share/my_example_robot_moveit_config/config/moveit_controllers.yaml'].
[ros2 run controller_manager spawner  joint_state_broadcaster-8] [INFO] [1682078432.731240393] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' node to exist
[ros2 run controller_manager spawner  robot_arm_controller-7] [INFO] [1682078432.743007441] [spawner_robot_arm_controller]: Waiting for '/controller_manager' node to exist
[rviz2-3] [ERROR] [1682078434.130097097] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-3] [WARN] [1682078434.149729542] [moveit_robot_model.robot_model]: Could not identify parent group for end-effector 'tcp_ee'
[rviz2-3] [WARN] [1682078434.208486867] [moveit_robot_model.robot_model]: Could not identify parent group for end-effector 'tcp_ee'
[rviz2-3] [WARN] [1682078434.211248282] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!
[ros2 run controller_manager spawner  joint_state_broadcaster-8] [INFO] [1682078434.740636405] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' node to exist
[ros2 run controller_manager spawner  robot_arm_controller-7] [INFO] [1682078434.752073604] [spawner_robot_arm_controller]: Waiting for '/controller_manager' node to exist
[ros2 run controller_manager spawner  joint_state_broadcaster-8] [INFO] [1682078436.748796976] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' node to exist
[ros2 run controller_manager spawner  robot_arm_controller-7] [INFO] [1682078436.760095966] [spawner_robot_arm_controller]: Waiting for '/controller_manager' node to exist
[ros2 run controller_manager spawner  joint_state_broadcaster-8] [INFO] [1682078438.757041182] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' node to exist
[ros2 run controller_manager spawner  robot_arm_controller-7] [INFO] [1682078438.768145853] [spawner_robot_arm_controller]: Waiting for '/controller_manager' node to exist
[ros2 run controller_manager spawner  joint_state_broadcaster-8] [ERROR] [1682078440.764886247] [spawner_joint_state_broadcaster]: Controller manager not available
[ros2 run controller_manager spawner  robot_arm_controller-7] [ERROR] [1682078440.775944617] [spawner_robot_arm_controller]: Controller manager not available
[ros2 run controller_manager spawner  joint_state_broadcaster-8] [ros2run]: Process exited with failure 1
[ERROR] [ros2 run controller_manager spawner  joint_state_broadcaster-8]: process has died [pid 61070, exit code 1, cmd 'ros2 run controller_manager spawner  joint_state_broadcaster'].
[ros2 run controller_manager spawner  robot_arm_controller-7] [ros2run]: Process exited with failure 1
[ERROR] [ros2 run controller_manager spawner  robot_arm_controller-7]: process has died [pid 61068, exit code 1, cmd 'ros2 run controller_manager spawner  robot_arm_controller'].
[move_group-2] [ERROR] [1682078453.030081442] [moveit_ros.trajectory_execution_manager]: Unable to identify any set of controllers that can actuate the specified joints: [ joint1 joint2 joint3 joint4 joint5 joint6 ]
[move_group-2] [ERROR] [1682078453.030120072] [moveit_ros.trajectory_execution_manager]: Known controllers and their joints:
[move_group-2] 
[rviz2-3] [ERROR] [1682078453.129861733] [move_group_interface]: MoveGroupInterface::execute() failed or timeout reached
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[robot_state_publisher-5] [INFO] [1682078456.897545387] [rclcpp]: signal_handler(signum=2)
[static_transform_publisher-4] [INFO] [1682078456.897554497] [rclcpp]: signal_handler(signum=2)
[rviz2-3] [WARN] [1682078456.917116572] [interactive_marker_display_94407756079328]: Server not available while running, resetting
[move_group-2] Stack trace (most recent call last):
[move_group-2] #11   Object "", at 0xffffffffffffffff, in 
[move_group-2] #10   Object "/opt/ros/humble/lib/moveit_ros_move_group/move_group", at 0x5559641b2784, in 
[joint_state_publisher-6] Traceback (most recent call last):
[joint_state_publisher-6]   File "/opt/ros/humble/lib/joint_state_publisher/joint_state_publisher", line 33, in <module>
[joint_state_publisher-6]     sys.exit(load_entry_point('joint-state-publisher==2.3.0', 'console_scripts', 'joint_state_publisher')())
[joint_state_publisher-6]   File "/opt/ros/humble/lib/python3.10/site-packages/joint_state_publisher/joint_state_publisher.py", line 423, in main
[joint_state_publisher-6]     rclpy.shutdown()
[joint_state_publisher-6]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/__init__.py", line 126, in shutdown
[joint_state_publisher-6]     _shutdown(context=context)
[joint_state_publisher-6]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/utilities.py", line 58, in shutdown
[joint_state_publisher-6]     return context.shutdown()
[joint_state_publisher-6]   File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/context.py", line 102, in shutdown
[joint_state_publisher-6]     self.__context.shutdown()
[joint_state_publisher-6] rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:241
[move_group-2] #9    Source "../csu/libc-start.c", line 392, in __libc_start_main_impl [0x7f523f429e3f]
[move_group-2] #8    Source "../sysdeps/nptl/libc_start_call_main.h", line 58, in __libc_start_call_main [0x7f523f429d8f]
[move_group-2] #7    Object "/opt/ros/humble/lib/moveit_ros_move_group/move_group", at 0x5559641b1701, in 
[move_group-2] #6    Object "/opt/ros/humble/lib/moveit_ros_move_group/move_group", at 0x5559641b3769, in 
[move_group-2] #5    Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f523fcf8cbe, in rclcpp::Node::~Node()
[move_group-2] #4    Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f523fcd47c9, in 
[move_group-2] #3    Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f523fd022d9, in 
[move_group-2] #2    Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f523fd02220, in rclcpp::node_interfaces::NodeBase::~NodeBase()
[move_group-2] #1    Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f523fcd47c9, in 
[move_group-2] #0    Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f523fcd9511, in rclcpp::CallbackGroup::~CallbackGroup()
[move_group-2] Segmentation fault (Address not mapped to object [0x7f52342539a0])

Thank you a lot! I'm searching for hours now :( Hope I provided necessary information.

Asked by robin_rob96 on 2023-04-21 07:21:14 UTC

Comments

Answers