search

moveit / moveit2_tutorials

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

Collision object is not published in `moveit_cpp` tutorial #869

Closed sea-bass closed 7 months ago

sea-bass commented 7 months ago

Description

When running the MoveItCpp tutorial with ros2 launch moveit2_tutorials moveit_cpp_tutorial.launch.py, the collision object meant to be added to the scene is not added.

Indeed, if I change RViz to look at /monitored_planning_scene instead of /moveit_cpp/monitored_planning_scene, it displays! (edited)

image

Your environment

Steps to reproduce

ros2 launch moveit2_tutorials moveit_cpp_tutorial.launch.py

Then, step through using the RViz widget.

Expected behaviour

Collision object should appear, but it does not.

sea-bass commented 7 months ago

Log:

~/workspace/moveit_ws$ ros2 launch moveit2_tutorials moveit_cpp_tutorial.launch.py 
[INFO] [launch]: All log files can be found below /home/sebastian/.ros/log/2024-02-09-09-32-43-331002-sebastian-thinkpad-t15-515879
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [static_transform_publisher-1]: process started with pid [515885]
[INFO] [robot_state_publisher-2]: process started with pid [515887]
[INFO] [rviz2-3]: process started with pid [515889]
[INFO] [moveit_cpp_tutorial-4]: process started with pid [515891]
[INFO] [ros2_control_node-5]: process started with pid [515893]
[INFO] [ros2 run controller_manager spawner panda_arm_controller-6]: process started with pid [515895]
[INFO] [ros2 run controller_manager spawner panda_hand_controller-7]: process started with pid [515897]
[INFO] [ros2 run controller_manager spawner joint_state_broadcaster-8]: process started with pid [515900]
[static_transform_publisher-1] [INFO] [1707489163.638642103] [static_transform_publisher]: Spinning until stopped - publishing transform
[static_transform_publisher-1] translation: ('0.000000', '0.000000', '0.000000')
[static_transform_publisher-1] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-1] from 'world' to 'panda_link0'
[robot_state_publisher-2] [WARN] [1707489163.640749780] [kdl_parser]: The root link panda_link0 has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[robot_state_publisher-2] [INFO] [1707489163.640913114] [robot_state_publisher]: Robot initialized
[ros2_control_node-5] [INFO] [1707489163.652084233] [controller_manager]: Subscribing to '/robot_description' topic for robot description.
[ros2_control_node-5] [INFO] [1707489163.652684725] [controller_manager]: update rate is 100 Hz
[ros2_control_node-5] [WARN] [1707489163.653061408] [controller_manager]: Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO RT scheduling. See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details.
[ros2_control_node-5] [INFO] [1707489163.662712350] [controller_manager]: Received robot description from topic.
[ros2_control_node-5] [INFO] [1707489163.663042446] [resource_manager]: Loading hardware 'PandaFakeSystem' 
[moveit_cpp_tutorial-4] [INFO] [1707489163.663505948] [moveit_cpp_tutorial]: Initialize node
[ros2_control_node-5] [INFO] [1707489163.663993009] [resource_manager]: Initialize hardware 'PandaFakeSystem' 
[ros2_control_node-5] [INFO] [1707489163.664057907] [resource_manager]: Successful initialization of hardware 'PandaFakeSystem'
[ros2_control_node-5] [INFO] [1707489163.664079350] [resource_manager]: Loading hardware 'PandaHandFakeSystem' 
[ros2_control_node-5] [INFO] [1707489163.664088935] [resource_manager]: Initialize hardware 'PandaHandFakeSystem' 
[ros2_control_node-5] [INFO] [1707489163.664100996] [resource_manager]: Successful initialization of hardware 'PandaHandFakeSystem'
[ros2_control_node-5] [INFO] [1707489163.664139194] [resource_manager]: 'configure' hardware 'PandaHandFakeSystem' 
[ros2_control_node-5] [INFO] [1707489163.664146732] [resource_manager]: Successful 'configure' of hardware 'PandaHandFakeSystem'
[ros2_control_node-5] [INFO] [1707489163.664151077] [resource_manager]: 'activate' hardware 'PandaHandFakeSystem' 
[ros2_control_node-5] [INFO] [1707489163.664154401] [resource_manager]: Successful 'activate' of hardware 'PandaHandFakeSystem'
[ros2_control_node-5] [INFO] [1707489163.664157970] [resource_manager]: 'configure' hardware 'PandaFakeSystem' 
[ros2_control_node-5] [INFO] [1707489163.664166720] [resource_manager]: Successful 'configure' of hardware 'PandaFakeSystem'
[ros2_control_node-5] [INFO] [1707489163.664174186] [resource_manager]: 'activate' hardware 'PandaFakeSystem' 
[ros2_control_node-5] [INFO] [1707489163.664177517] [resource_manager]: Successful 'activate' of hardware 'PandaFakeSystem'
[ros2_control_node-5] [INFO] [1707489164.281994045] [controller_manager]: Loading controller 'joint_state_broadcaster'
[ros2_control_node-5] [INFO] [1707489164.295062834] [controller_manager]: Loading controller 'panda_arm_controller'
[ros2 run controller_manager spawner joint_state_broadcaster-8] [INFO] [1707489164.315343538] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ros2_control_node-5] [INFO] [1707489164.315699523] [controller_manager]: Loading controller 'panda_hand_controller'
[ros2_control_node-5] [INFO] [1707489164.333587148] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[ros2_control_node-5] [INFO] [1707489164.333688159] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[ros2 run controller_manager spawner panda_arm_controller-6] [INFO] [1707489164.337443218] [spawner_panda_arm_controller]: Loaded panda_arm_controller
[ros2_control_node-5] [INFO] [1707489164.343665286] [controller_manager]: Configuring controller 'panda_arm_controller'
[ros2_control_node-5] [INFO] [1707489164.343841363] [panda_arm_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ros2_control_node-5] [INFO] [1707489164.343864497] [panda_arm_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[ros2_control_node-5] [INFO] [1707489164.343874521] [panda_arm_controller]: Using 'splines' interpolation method.
[ros2_control_node-5] [INFO] [1707489164.345704124] [panda_arm_controller]: Action status changes will be monitored at 20.00 Hz.
[ros2 run controller_manager spawner panda_hand_controller-7] [INFO] [1707489164.350264462] [spawner_panda_hand_controller]: Loaded panda_hand_controller
[ros2_control_node-5] [INFO] [1707489164.353654979] [controller_manager]: Configuring controller 'panda_hand_controller'
[ros2_control_node-5] [INFO] [1707489164.353718265] [panda_hand_controller]: Action status changes will be monitored at 20Hz.
[ros2 run controller_manager spawner joint_state_broadcaster-8] [INFO] [1707489164.384419762] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[ros2 run controller_manager spawner panda_arm_controller-6] [INFO] [1707489164.404337274] [spawner_panda_arm_controller]: Configured and activated panda_arm_controller
[ros2 run controller_manager spawner panda_hand_controller-7] [INFO] [1707489164.424429542] [spawner_panda_hand_controller]: Configured and activated panda_hand_controller
[INFO] [ros2 run controller_manager spawner joint_state_broadcaster-8]: process has finished cleanly [pid 515900]
[INFO] [ros2 run controller_manager spawner panda_arm_controller-6]: process has finished cleanly [pid 515895]
[INFO] [ros2 run controller_manager spawner panda_hand_controller-7]: process has finished cleanly [pid 515897]
[moveit_cpp_tutorial-4] [INFO] [1707489164.693592360] [moveit_cpp_tutorial]: Starting MoveIt Tutorials...
[moveit_cpp_tutorial-4] [WARN] [1707489164.695045711] [rcl.logging_rosout]: Publisher already registered for node name: 'moveit_cpp_tutorial'. If this is due to multiple nodes with the same name then all logs for the logger named 'moveit_cpp_tutorial' 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_cpp_tutorial-4] [INFO] [1707489164.702680894] [moveit_cpp_tutorial.RDFLoader]: Loaded robot model in 0.0028474 seconds
[moveit_cpp_tutorial-4] [INFO] [1707489164.702730558] [moveit_cpp_tutorial.robot_model]: Loading robot model 'panda'...
[moveit_cpp_tutorial-4] [WARN] [1707489164.715787282] [kdl_parser]: The root link panda_link0 has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[moveit_cpp_tutorial-4] [INFO] [1707489164.715916228] [moveit_cpp_tutorial.kdl_kinematics_plugin]: Joint weights for group 'panda_arm': 1 1 1 1 1 1 1
[moveit_cpp_tutorial-4] [INFO] [1707489164.755228899] [moveit_cpp_tutorial.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[moveit_cpp_tutorial-4] [INFO] [1707489164.755389087] [moveit_cpp_tutorial.moveit_cpp]: Listening to '/joint_states' for joint states
[moveit_cpp_tutorial-4] [INFO] [1707489164.757450357] [moveit_cpp_tutorial.current_state_monitor]: Listening to joint states on topic '/joint_states'
[moveit_cpp_tutorial-4] [INFO] [1707489164.758105988] [moveit_cpp_tutorial.planning_scene_monitor]: Listening to '/moveit_cpp/planning_scene_monitor' for attached collision objects
[moveit_cpp_tutorial-4] [INFO] [1707489164.758127287] [moveit_cpp_tutorial.planning_scene_monitor]: Starting planning scene monitor
[moveit_cpp_tutorial-4] [INFO] [1707489164.758686085] [moveit_cpp_tutorial.planning_scene_monitor]: Listening to '/moveit_cpp/publish_planning_scene'
[moveit_cpp_tutorial-4] [INFO] [1707489164.758703619] [moveit_cpp_tutorial.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[moveit_cpp_tutorial-4] [INFO] [1707489164.759333890] [moveit_cpp_tutorial.planning_scene_monitor]: Listening to 'collision_object'
[moveit_cpp_tutorial-4] [INFO] [1707489164.760000271] [moveit_cpp_tutorial.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[moveit_cpp_tutorial-4] [WARN] [1707489164.761209924] [moveit_cpp_tutorial.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[moveit_cpp_tutorial-4] [ERROR] [1707489164.761252512] [moveit_cpp_tutorial.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates
[moveit_cpp_tutorial-4] [INFO] [1707489164.789739873] [moveit_cpp_tutorial.planning_pipeline]: Successfully loaded planner 'OMPL'
[moveit_cpp_tutorial-4] [INFO] [1707489164.794262995] [moveit_cpp_tutorial]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[moveit_cpp_tutorial-4] [INFO] [1707489164.796413542] [moveit_cpp_tutorial]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[moveit_cpp_tutorial-4] [INFO] [1707489164.796454615] [moveit_cpp_tutorial]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[moveit_cpp_tutorial-4] [INFO] [1707489164.796753684] [moveit_cpp_tutorial]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[moveit_cpp_tutorial-4] [INFO] [1707489164.796769324] [moveit_cpp_tutorial]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds'
[moveit_cpp_tutorial-4] [INFO] [1707489164.796808635] [moveit_cpp_tutorial]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds'
[moveit_cpp_tutorial-4] [INFO] [1707489164.796814785] [moveit_cpp_tutorial]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision'
[moveit_cpp_tutorial-4] [INFO] [1707489164.796827147] [moveit_cpp_tutorial]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision'
[moveit_cpp_tutorial-4] [INFO] [1707489164.799964030] [moveit_cpp_tutorial]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization'
[moveit_cpp_tutorial-4] [INFO] [1707489164.802159755] [moveit_cpp_tutorial]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization'
[moveit_cpp_tutorial-4] [INFO] [1707489164.802181129] [moveit_cpp_tutorial]: Try loading adapter 'default_planning_response_adapters/ValidateSolution'
[moveit_cpp_tutorial-4] [INFO] [1707489164.804467181] [moveit_cpp_tutorial]: Loaded adapter 'default_planning_response_adapters/ValidateSolution'
[moveit_cpp_tutorial-4] [INFO] [1707489164.804491345] [moveit_cpp_tutorial]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath'
[moveit_cpp_tutorial-4] [INFO] [1707489164.805020371] [moveit_cpp_tutorial]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath'
[moveit_cpp_tutorial-4] [WARN] [1707489164.807756113] [rcl.logging_rosout]: Publisher already registered for node name: 'moveit_cpp_tutorial'. If this is due to multiple nodes with the same name then all logs for the logger named 'moveit_cpp_tutorial' 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.
[rviz2-3] [INFO] [1707489164.831864730] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-3] [INFO] [1707489164.833102006] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[moveit_cpp_tutorial-4] [INFO] [1707489164.842321290] [moveit_cpp_tutorial.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for panda_arm_controller
[moveit_cpp_tutorial-4] [INFO] [1707489164.842413976] [moveit_cpp_tutorial.moveit_simple_controller_manager]: Max effort set to 0.0
[moveit_cpp_tutorial-4] [INFO] [1707489164.845200038] [moveit_cpp_tutorial.moveit_simple_controller_manager]: Added GripperCommand controller for panda_hand_controller
[moveit_cpp_tutorial-4] [INFO] [1707489164.845373600] [moveit_cpp_tutorial.moveit_simple_controller_manager]: Returned 2 controllers in list
[moveit_cpp_tutorial-4] [INFO] [1707489164.845418046] [moveit_cpp_tutorial.moveit_simple_controller_manager]: Returned 2 controllers in list
[moveit_cpp_tutorial-4] [INFO] [1707489164.846964681] [moveit_cpp_tutorial.trajectory_execution_manager]: Trajectory execution is managing controllers
[moveit_cpp_tutorial-4] [INFO] [1707489164.856790057] [moveit_cpp_tutorial.remote_control]: RemoteControl Ready.
[moveit_cpp_tutorial-4] [INFO] [1707489164.857718007] [moveit_cpp_tutorial.rviz_visual_tools]: Topic /moveit_cpp_tutorial waiting 5 seconds for subscriber.
[rviz2-3] [INFO] [1707489164.880832079] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-3] [WARN] [1707489164.980532273] [rcl.logging_rosout]: Publisher already registered for node name: 'rviz2'. If this is due to multiple nodes with the same name then all logs for the logger named 'rviz2' 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_cpp_tutorial-4] [INFO] [1707489165.002966487] [moveit_cpp_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window to start the demo
[rviz2-3] [INFO] [1707489165.008785784] [rviz2.RDFLoader]: Loaded robot model in 0.00216271 seconds
[rviz2-3] [INFO] [1707489165.008861775] [rviz2.robot_model]: Loading robot model 'panda'...
[rviz2-3] [WARN] [1707489165.242853697] [rcl.logging_rosout]: Publisher already registered for node name: 'rviz2'. If this is due to multiple nodes with the same name then all logs for the logger named 'rviz2' 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.
[rviz2-3] [INFO] [1707489165.266269935] [rviz2.RDFLoader]: Loaded robot model in 0.00250945 seconds
[rviz2-3] [INFO] [1707489165.266348126] [rviz2.robot_model]: Loading robot model 'panda'...
[rviz2-3] [WARN] [1707489165.281753012] [rviz2.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!
[rviz2-3] [INFO] [1707489165.336936383] [rviz2.planning_scene_monitor]: Starting planning scene monitor
[rviz2-3] [INFO] [1707489165.339136378] [rviz2.planning_scene_monitor]: Listening to '/moveit_cpp/monitored_planning_scene'
sea-bass commented 7 months ago

Should be fixed by https://github.com/ros-planning/moveit2/pull/2680