search

moveit / moveit2_tutorials

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

Cannot run "Pick and Place with MoveIt Task Constructor" tutorial with provided docker image #806

Open VincentDelort opened 1 year ago

VincentDelort commented 1 year ago

Description

Hi,

I cannot run the "Pick and Place with MoveIt Task Constructor" tutorial demo with the provided moveit2_container docker image : demo fails at the task execution step (last step) of 5. Running the demo

Your environment

Image inspect:

[
    {
        "Id": "sha256:a39ea3a666ff742bbaf5a35dec2eaca35900c48edb939cea3ae43bd1ca7e4bc9",
        "RepoTags": [
            "ghcr.io/ros-planning/moveit2_tutorials:rolling-tutorial"
        ],
        "RepoDigests": [
            "ghcr.io/ros-planning/moveit2_tutorials@sha256:c3baa1a87e49cce8d4fcfa1b39d3fb5bdbb6a019837f9efb6e90e723bd4b8fea"
        ],
        "Parent": "",
        "Comment": "buildkit.dockerfile.v0",
        "Created": "2023-10-03T16:07:45.863603919Z",
        "Container": "",
        "ContainerConfig": {
            "Hostname": "",
            "Domainname": "",
            "User": "",
            "AttachStdin": false,
            "AttachStdout": false,
            "AttachStderr": false,
            "Tty": false,
            "OpenStdin": false,
            "StdinOnce": false,
            "Env": null,
            "Cmd": null,
            "Image": "",
            "Volumes": null,
            "WorkingDir": "",
            "Entrypoint": null,
            "OnBuild": null,
            "Labels": null
        },
        "DockerVersion": "",
        "Author": "",
        "Config": {
            "Hostname": "",
            "Domainname": "",
            "User": "",
            "AttachStdin": false,
            "AttachStdout": false,
            "AttachStderr": false,
            "Tty": false,
            "OpenStdin": false,
            "StdinOnce": false,
            "Env": [
                "PATH=/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin",
                "LANG=C.UTF-8",
                "LC_ALL=C.UTF-8",
                "ROS_DISTRO=rolling",
                "TERM=xterm",
                "ROS_UNDERLAY=/root/ws_moveit/install"
            ],
            "Cmd": [
                "bash"
            ],
            "ArgsEscaped": true,
            "Image": "",
            "Volumes": null,
            "WorkingDir": "/root/ws_moveit/install/..",
            "Entrypoint": [
                "/ros_entrypoint.sh"
            ],
            "OnBuild": null,
            "Labels": {
                "maintainer": "Robert Haschke rhaschke@techfak.uni-bielefeld.de",
                "org.opencontainers.image.description": "This container has working versions of the tutorials discussed here: https://moveit.picknik.ai/main/doc/tutorials/tutorials.html"
            }
        },
        "Architecture": "amd64",
        "Os": "linux",
        "Size": 4820895661,
        "VirtualSize": 4820895661,
        "GraphDriver": {
            "Data": {
                "LowerDir": "/var/lib/docker/overlay2/1cfd714caccdc31b32313f216398aee9c6bfcb775dc8e138020a48d7f995c943/diff:/var/lib/docker/overlay2/a1029f14480a50aef5efebadc1f4c45e981ef55c1a1dc11d57825746793bfffb/diff:/var/lib/docker/overlay2/d6f848ab3ff87ceb8be4cb8be0d6a5f6dc478c8c43ec512f1be569800023ed44/diff:/var/lib/docker/overlay2/02925fa7f12c82a63632bdd7f9936f25f270b99bc76a40901439982006640659/diff:/var/lib/docker/overlay2/0dad50702fcf9ad7387f869146081dbc24867fd23f7312f5e52b29ffa9043d48/diff:/var/lib/docker/overlay2/7bc6db8e113d56422396f70430872d91c3d3a5f69450b02e34da94827dab2ab6/diff:/var/lib/docker/overlay2/d4444e594bd50ebb90da919408ed72ff68009686dd848005d528e88f173fc101/diff:/var/lib/docker/overlay2/316f595cd417f90a793c77265bdeb7b0aaea8be600041d2d45ceb88ebecbcf81/diff:/var/lib/docker/overlay2/1fdd768d1bec9772f0ef02958795262cb05ec4299bbc1c3b274fcc5d74f64b77/diff:/var/lib/docker/overlay2/fdfe98b118223a153142fe8dc1219d9065d552cdaa63f3f016c892b3d1ef8c29/diff:/var/lib/docker/overlay2/b488fa7c4c942589316473770617362069cee774d6c7590ea1e8dca16dc83d86/diff:/var/lib/docker/overlay2/d2cabc48f74e272ccd6c77291e07fef19c8849b2613c69ca3be2c0894294591a/diff:/var/lib/docker/overlay2/a32c7f162fa59c1cd27aa7d6e7e2917022ec8dd6cfd537c0c0cafbd6ee230564/diff:/var/lib/docker/overlay2/9438fbfd86d074443ec6f32f5c7b6f637a83c83155149d59c05f7c61d96360b1/diff:/var/lib/docker/overlay2/28de6fd0257da40176b0007a0cedf9fd2c8f21eb3c6683ea08140b734205612e/diff:/var/lib/docker/overlay2/418bec4418c208098f759480f8060392d265fbc79127356eb328d3420da8a1cf/diff:/var/lib/docker/overlay2/e1fe846f984d86cf0b6533892fa359ee6908ee675bb9b87061443452c79a5359/diff:/var/lib/docker/overlay2/61a9d5df7ef82a89715ea0a06c120cd95d6ff225ca926543b372b16bcc6a6cf0/diff:/var/lib/docker/overlay2/9b70a2170a56b8b7b1f727006a390cd3114ece3ad74e54c84996e5b976e17a44/diff:/var/lib/docker/overlay2/89462648a62897f51d04df31cf2a6c3eebccb63fded206907a0fabb375655bce/diff:/var/lib/docker/overlay2/64f82f103ddde0ada6401eb6f2ae8011708b839e629b01b2782ae89443a277bc/diff:/var/lib/docker/overlay2/610b69067ded826b216573428e023f707f2b200eec1047cf5e9a67568e7d1a77/diff:/var/lib/docker/overlay2/0b124813cae58f36d4dbfe7b33012bbbccf8ea11c293c3709e99cc837108e059/diff:/var/lib/docker/overlay2/974b4669d52bb26388db16b5ee652340cb073b4220cde9d21f3462c45dcdfccb/diff:/var/lib/docker/overlay2/f225b83978eaab0d5639e3bdd96529c9bd6b2658c1ed5d5b2658b909bebef887/diff:/var/lib/docker/overlay2/548f639eb58b70d1329aedb7dff6f5cea668fa96414fd8f5b3a0e8aab870f067/diff:/var/lib/docker/overlay2/11dda2a5936cbc1a5e12753ba240c330aa85e380a93ae2e2bcf447a226ebedaf/diff",
                "MergedDir": "/var/lib/docker/overlay2/fc5d990577920c8f280b5dd2ab960d7c44efc712450159dbe5eee68e2b2bfbde/merged",
                "UpperDir": "/var/lib/docker/overlay2/fc5d990577920c8f280b5dd2ab960d7c44efc712450159dbe5eee68e2b2bfbde/diff",
                "WorkDir": "/var/lib/docker/overlay2/fc5d990577920c8f280b5dd2ab960d7c44efc712450159dbe5eee68e2b2bfbde/work"
            },
            "Name": "overlay2"
        },
        "RootFS": {
            "Type": "layers",
            "Layers": [
                "sha256:3826c26db80f684b4e6c21afbfd309f585e6dabb102531bb48a902ee4dfc58f4",
                "sha256:551e91f516ec71c61cf24b40e13d38ca42c82b8eb4e042718ab49a135768ebdf",
                "sha256:fe0fb6e0485f7ae358a530079b6bb5fc0e4939a5a7ca30da7cd19e35c653565d",
                "sha256:4d2a31562aa4d389c230400dac9176445a52d9eba1fc9bc686df2ae1deaed5dc",
                "sha256:e3ae76f53cd6ed24775e3be52037ed7f1dbcb4c1a1d2e526ca0e3be052ac8ff7",
                "sha256:d96af052e60dff170dcc95457e865c26e44de2b4b289912c84e9a13463007649",
                "sha256:089aa9323b56fa6185f95d218442cfc70edf59996a436164394e8a3fea66f9c2",
                "sha256:effd0d29feca89708bef4893fe89d0e47e3eb4a0e81ebd6cda22479debc34dd1",
                "sha256:90f942de5ccf232dab92397ccfaadc15dd627f0d6f65478041d3cf8edfe1c33f",
                "sha256:5d2dab6aff430e57dba98829d71025053f880605fedaa5139d047cca871a6735",
                "sha256:607134be181009e4e17f7547f58bee9115d8803a9e56793bc0b5a03a72f1a97e",
                "sha256:3df92c960185bb07ede28e011d429e54da32f01a50178af810d025a06c591e0b",
                "sha256:36e655e3513f24479d9a7e993adbd731c7fdafc0fd603ca7c984dc97d95e539d",
                "sha256:c4659a771274209a0e7efb945e7ad3855278983ff5a1da1310ddf81db188e95a",
                "sha256:5f70bf18a086007016e948b04aed3b82103a36bea41755b6cddfaf10ace3c6ef",
                "sha256:06f25e8fe4aefdf28eb891658123907cdacefbcfde487afd710bc51e25f9b8d7",
                "sha256:437fc931de28a5002c7d19b2ae066f8aad2cd8567b79e98b320f2e36e6a296bd",
                "sha256:640016df220c74a1c7d1013ed3deab48d08c4c4a6fdd11a3e23cba2f8170a426",
                "sha256:e1ff2432a374c277314a0d0bc83038e437191684e49ff0f7c3384f7183b1eed6",
                "sha256:3f9f1910d48cc697cdb25c1c54ce001c73ea5ae88167b5c5353f0e0a629e7fa7",
                "sha256:d99db7faba191e2054750c0a15afcfe23bb05e5e2660a5d986b210d658964c3b",
                "sha256:83494456c79f0d5020adafb438fb7b8934788383e217104a422cc0798fe2415c",
                "sha256:84f0fcaf506e3c5b4acdcef5e2664c7312a82d642e9d49043ea5f2668d84d434",
                "sha256:971ee091bb4d18ffb848c076c9d6bead4147d7bf1ae0b522816014c373fe0e37",
                "sha256:9cda5fe88500843f7887566e2fb05a17d2a12cdd4b931df8edd80fa48fa12968",
                "sha256:74fce02e83f0384c41d7c0c554d97c434756d28325e57fd1863914236e4b7cd5",
                "sha256:7037bbe3a3edb881af6e706de930b88765514481551c463a182aebf4bfbbb9a3",
                "sha256:b4d75676cfa5bead90c974977193f90bc8501dd4ce1a1bf3ca13d5e66fdd7f71"
            ]
        },
        "Metadata": {
            "LastTagTime": "0001-01-01T00:00:00Z"
        }
    }
]

Steps to reproduce

  1. Prepare the docker image following How to Set Up MoveIt 2 Docker Containers in Ubuntu tutorial (keep default rolling-tutorial as target image).
  2. Open src/mtc_tutorial/src/mtc_node.cpp. Comment out / delete its content and replace by the initial code available in the "Pick and Place with MoveIt2 Task Constructor" tutorial at 4.2. The code. Note: keep the default interpolation_planner.
  3. Run the demo according to 5. Running the demo : 
    colcon build --mixin debug

    (The tutorial says --mixin release but the debug info might be useful...)

    source ~/ws_moveit/install/setup.bash
ros2 launch moveit2_tutorials mtc_demo.launch.py

    (Check that RViz configuration matches "5.2. RViz configuration"... this should be OK since we pulled the docker image)

  4. Run the MTC tutorial node in another terminal : 
    docker exec -it moveit2_container bash
source ~/ws_moveit/install/setup.bash
ros2 launch mtc_tutorial pick_place_demo.launch.py

Expected behaviour

Should run as described by the tutorial to the end.

Backtrace or Console output

Demo (mtc_demo_launch.py) crashes at the execution step (full verbosity below) : 

[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [rviz2-1]: process started with pid [281901]
[INFO] [static_transform_publisher-2]: process started with pid [281903]
[INFO] [robot_state_publisher-3]: process started with pid [281905]
[INFO] [move_group-4]: process started with pid [281907]
[INFO] [ros2_control_node-5]: process started with pid [281909]
[INFO] [ros2 run controller_manager spawner panda_arm_controller-6]: process started with pid [281911]
[INFO] [ros2 run controller_manager spawner panda_hand_controller-7]: process started with pid [281913]
[INFO] [ros2 run controller_manager spawner joint_state_broadcaster-8]: process started with pid [281919]
[static_transform_publisher-2] [INFO] [1698678622.765226501] [static_transform_publisher]: Spinning until stopped - publishing transform
[static_transform_publisher-2] translation: ('0.000000', '0.000000', '0.000000')
[static_transform_publisher-2] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-2] from 'world' to 'panda_link0'
[robot_state_publisher-3] [WARN] [1698678622.784648091] [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-3] [INFO] [1698678622.785380778] [robot_state_publisher]: Robot initialized
[ros2_control_node-5] [WARN] [1698678622.884541836] [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-5] [INFO] [1698678622.885440622] [resource_manager]: Loading hardware 'PandaFakeSystem' 
[ros2_control_node-5] [INFO] [1698678622.887694454] [resource_manager]: Initialize hardware 'PandaFakeSystem' 
[ros2_control_node-5] [WARN] [1698678622.888265838] [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-5] <state_interface name="velocity"> 
[ros2_control_node-5]   <param name="initial_value">0.0</param> 
[ros2_control_node-5] </state_interface>
[ros2_control_node-5] [INFO] [1698678622.889079613] [resource_manager]: Successful initialization of hardware 'PandaFakeSystem'
[ros2_control_node-5] [INFO] [1698678622.889788841] [resource_manager]: Loading hardware 'PandaHandFakeSystem' 
[ros2_control_node-5] [INFO] [1698678622.890262740] [resource_manager]: Initialize hardware 'PandaHandFakeSystem' 
[ros2_control_node-5] [INFO] [1698678622.890742624] [resource_manager]: Successful initialization of hardware 'PandaHandFakeSystem'
[ros2_control_node-5] [INFO] [1698678622.891438513] [resource_manager]: 'configure' hardware 'PandaHandFakeSystem' 
[ros2_control_node-5] [INFO] [1698678622.892195892] [resource_manager]: Successful 'configure' of hardware 'PandaHandFakeSystem'
[ros2_control_node-5] [INFO] [1698678622.892849857] [resource_manager]: 'activate' hardware 'PandaHandFakeSystem' 
[ros2_control_node-5] [INFO] [1698678622.893337864] [resource_manager]: Successful 'activate' of hardware 'PandaHandFakeSystem'
[ros2_control_node-5] [INFO] [1698678622.893815180] [resource_manager]: 'configure' hardware 'PandaFakeSystem' 
[ros2_control_node-5] [INFO] [1698678622.894300789] [resource_manager]: Successful 'configure' of hardware 'PandaFakeSystem'
[ros2_control_node-5] [INFO] [1698678622.894870813] [resource_manager]: 'activate' hardware 'PandaFakeSystem' 
[ros2_control_node-5] [INFO] [1698678622.895749556] [resource_manager]: Successful 'activate' of hardware 'PandaFakeSystem'
[rviz2-1] QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-root'
[ros2_control_node-5] [INFO] [1698678622.915501307] [controller_manager]: update rate is 100 Hz
[ros2_control_node-5] [INFO] [1698678622.926301370] [controller_manager]: RT kernel is recommended for better performance
[move_group-4] [INFO] [1698678623.055632259] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00528751 seconds
[move_group-4] [INFO] [1698678623.075683409] [moveit_robot_model.robot_model]: Loading robot model 'panda'...
[move_group-4] [WARN] [1698678623.226879789] [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.
[move_group-4] [INFO] [1698678623.227618824] [moveit_kinematics_base.kinematics_base]: Joint weights for group 'panda_arm': 1 1 1 1 1 1 1
[move_group-4] [INFO] [1698678623.484100903] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-4] [INFO] [1698678623.485683149] [moveit.ros_planning_interface.moveit_cpp]: Listening to 'joint_states' for joint states
[move_group-4] [INFO] [1698678623.492597336] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[move_group-4] [INFO] [1698678623.499922253] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[move_group-4] [INFO] [1698678623.500518206] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[move_group-4] [INFO] [1698678623.516101510] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[move_group-4] [INFO] [1698678623.516960528] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[move_group-4] [INFO] [1698678623.519841684] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[move_group-4] [INFO] [1698678623.523102751] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[move_group-4] [WARN] [1698678623.530706536] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-4] [ERROR] [1698678623.531550666] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates
[ros2_control_node-5] [INFO] [1698678623.607403234] [controller_manager]: Loading controller 'panda_hand_controller'
[ros2_control_node-5] [INFO] [1698678623.649681552] [controller_manager]: Loading controller 'panda_arm_controller'
[move_group-4] [INFO] [1698678623.658786079] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL'
[ros2 run controller_manager spawner panda_hand_controller-7] [INFO] [1698678623.669623145] [spawner_panda_hand_controller]: Loaded panda_hand_controller
[ros2_control_node-5] [WARN] [1698678623.675236170] [panda_arm_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[move_group-4] [INFO] [1698678623.679109050] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[move_group-4] [INFO] [1698678623.679187985] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Resolve constraint frames to robot links'
[move_group-4] [INFO] [1698678623.679196600] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-4] [INFO] [1698678623.679202607] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-4] [INFO] [1698678623.679207571] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-4] [INFO] [1698678623.679213136] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[ros2_control_node-5] [INFO] [1698678623.683962300] [controller_manager]: Configuring controller 'panda_hand_controller'
[ros2_control_node-5] [INFO] [1698678623.684075082] [panda_hand_controller]: Action status changes will be monitored at 20Hz.
[ros2_control_node-5] [INFO] [1698678623.686422621] [controller_manager]: Loading controller 'joint_state_broadcaster'
[ros2 run controller_manager spawner panda_arm_controller-6] [INFO] [1698678623.710527479] [spawner_panda_arm_controller]: Loaded panda_arm_controller
[ros2_control_node-5] [INFO] [1698678623.729514252] [controller_manager]: Configuring controller 'panda_arm_controller'
[ros2_control_node-5] [INFO] [1698678623.729820359] [panda_arm_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ros2_control_node-5] [INFO] [1698678623.729881932] [panda_arm_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[ros2_control_node-5] [INFO] [1698678623.729911831] [panda_arm_controller]: Using 'splines' interpolation method.
[ros2_control_node-5] [INFO] [1698678623.731953153] [panda_arm_controller]: Action status changes will be monitored at 20.00 Hz.
[ros2 run controller_manager spawner panda_hand_controller-7] [INFO] [1698678623.735617519] [spawner_panda_hand_controller]: Configured and activated panda_hand_controller
[move_group-4] [INFO] [1698678623.749060108] [moveit.ros_planning.planning_pipeline]: Using planning interface 'CHOMP'
[ros2 run controller_manager spawner joint_state_broadcaster-8] [INFO] [1698678623.750325501] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ros2_control_node-5] [INFO] [1698678623.756837904] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[ros2_control_node-5] [INFO] [1698678623.756915861] [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] [1698678623.757513051] [spawner_panda_arm_controller]: Configured and activated panda_arm_controller
[move_group-4] [INFO] [1698678623.765912030] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[move_group-4] [INFO] [1698678623.765945346] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-4] [INFO] [1698678623.765952527] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-4] [INFO] [1698678623.765958327] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-4] [INFO] [1698678623.765963515] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[ros2 run controller_manager spawner joint_state_broadcaster-8] [INFO] [1698678623.779307879] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[rviz2-1] [INFO] [1698678623.781596672] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-1] [INFO] [1698678623.781748566] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[move_group-4] [INFO] [1698678623.797279968] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[move_group-4] [WARN] [1698678623.801328463] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Multi-DOF-Joint 'virtual_joint' not supported.
[move_group-4] [WARN] [1698678623.801376439] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Multi-DOF-Joint 'virtual_joint' not supported.
[move_group-4] [INFO] [1698678623.822655618] [moveit.pilz_industrial_motion_planner]: Available plugins: pilz_industrial_motion_planner/PlanningContextLoaderCIRC pilz_industrial_motion_planner/PlanningContextLoaderLIN pilz_industrial_motion_planner/PlanningContextLoaderPTP 
[move_group-4] [INFO] [1698678623.822680872] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderCIRC
[move_group-4] [INFO] [1698678623.839506392] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [CIRC]
[move_group-4] [INFO] [1698678623.839621879] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderLIN
[move_group-4] [INFO] [1698678623.845174534] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [LIN]
[move_group-4] [INFO] [1698678623.845922791] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderPTP
[rviz2-1] [INFO] [1698678623.849476016] [rviz2]: Stereo is NOT SUPPORTED
[move_group-4] [INFO] [1698678623.851762909] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [PTP]
[move_group-4] [INFO] [1698678623.851795316] [moveit.ros_planning.planning_pipeline]: Using planning interface 'Pilz Industrial Motion Planner'
[move_group-4] [INFO] [1698678623.861305012] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-4] [INFO] [1698678623.861332022] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-4] [INFO] [1698678623.861339138] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-4] [INFO] [1698678623.861344903] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[move_group-4] [INFO] [1698678623.926326707] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for panda_arm_controller
[move_group-4] [INFO] [1698678623.928246223] [moveit.plugins.moveit_simple_controller_manager]: Max effort set to 0.0
[move_group-4] [INFO] [1698678623.932432837] [moveit.plugins.moveit_simple_controller_manager]: Added GripperCommand controller for panda_hand_controller
[move_group-4] [INFO] [1698678623.933684592] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1698678623.934529671] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1698678623.937580740] [moveit_ros.trajectory_execution_manager]: Trajectory execution is managing controllers
[move_group-4] [INFO] [1698678623.938252765] [move_group.move_group]: MoveGroup debug mode is ON
[INFO] [ros2 run controller_manager spawner panda_hand_controller-7]: process has finished cleanly [pid 281913]
[INFO] [ros2 run controller_manager spawner panda_arm_controller-6]: process has finished cleanly [pid 281911]
[move_group-4] [INFO] [1698678624.052394213] [moveit.pilz_industrial_motion_planner.move_group_sequence_action]: initialize move group sequence action
[move_group-4] [INFO] [1698678624.059991919] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[move_group-4] [WARN] [1698678624.060056062] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Multi-DOF-Joint 'virtual_joint' not supported.
[move_group-4] [WARN] [1698678624.060071178] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Multi-DOF-Joint 'virtual_joint' not supported.
[move_group-4] [INFO] [1698678624.061178292] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[move_group-4] [WARN] [1698678624.061233653] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Multi-DOF-Joint 'virtual_joint' not supported.
[move_group-4] [WARN] [1698678624.061245610] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Multi-DOF-Joint 'virtual_joint' not supported.
[move_group-4] [INFO] [1698678624.065165180] [move_group.move_group]: 
[move_group-4] 
[move_group-4] ********************************************************
[move_group-4] * MoveGroup using: 
[move_group-4] *     - ApplyPlanningSceneService
[move_group-4] *     - ClearOctomapService
[move_group-4] *     - ExecuteTaskSolution
[move_group-4] *     - CartesianPathService
[move_group-4] *     - ExecuteTrajectoryAction
[move_group-4] *     - GetPlanningSceneService
[move_group-4] *     - KinematicsService
[move_group-4] *     - MoveAction
[move_group-4] *     - MotionPlanService
[move_group-4] *     - QueryPlannersService
[move_group-4] *     - StateValidationService
[move_group-4] *     - SequenceAction
[move_group-4] *     - SequenceService
[move_group-4] ********************************************************
[move_group-4] 
[move_group-4] [INFO] [1698678624.065236072] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[move_group-4] [INFO] [1698678624.065248300] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context initialization complete
[move_group-4] Loading 'move_group/ApplyPlanningSceneService'...
[move_group-4] Loading 'move_group/ClearOctomapService'...
[move_group-4] Loading 'move_group/ExecuteTaskSolutionCapability'...
[move_group-4] Loading 'move_group/MoveGroupCartesianPathService'...
[move_group-4] Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[move_group-4] Loading 'move_group/MoveGroupGetPlanningSceneService'...
[move_group-4] Loading 'move_group/MoveGroupKinematicsService'...
[move_group-4] Loading 'move_group/MoveGroupMoveAction'...
[move_group-4] Loading 'move_group/MoveGroupPlanService'...
[move_group-4] Loading 'move_group/MoveGroupQueryPlannersService'...
[move_group-4] Loading 'move_group/MoveGroupStateValidationService'...
[move_group-4] Loading 'pilz_industrial_motion_planner/MoveGroupSequenceAction'...
[move_group-4] Loading 'pilz_industrial_motion_planner/MoveGroupSequenceService'...
[move_group-4] 
[move_group-4] You can start planning now!
[move_group-4] 
[INFO] [ros2 run controller_manager spawner joint_state_broadcaster-8]: process has finished cleanly [pid 281919]
[rviz2-1] [INFO] [1698678624.179423908] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00595184 seconds
[rviz2-1] [INFO] [1698678624.179911481] [moveit_robot_model.robot_model]: Loading robot model 'panda'...
[rviz2-1] [INFO] [1698678624.721227126] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00316323 seconds
[rviz2-1] [INFO] [1698678624.721512753] [moveit_robot_model.robot_model]: Loading robot model 'panda'...
[rviz2-1] [WARN] [1698678624.777961256] [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.
[rviz2-1] [INFO] [1698678624.779247798] [moveit_kinematics_base.kinematics_base]: Joint weights for group 'panda_arm': 1 1 1 1 1 1 1
[rviz2-1] [INFO] [1698678624.906124688] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[rviz2-1] [INFO] [1698678624.910734363] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/monitored_planning_scene'
[move_group-4] [INFO] [1698678636.105194004] [moveit_task_constructor_visualization.execute_task_solution]: Executing TaskSolution
[move_group-4] [INFO] [1698678636.106019445] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1698678636.106095981] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] terminate called after throwing an instance of 'std::invalid_argument'
[move_group-4]   what():  rate must be greater than 0
[move_group-4] Stack trace (most recent call last) in thread 282082:
[move_group-4] #31   Object "/opt/ros/rolling/lib/librclcpp_action.so", at 0x7fc1992ef1b5, in rclcpp_action::ServerBase::execute_goal_request_received(std::shared_ptr<void>&)
[move_group-4] #30   Source "/opt/ros/rolling/include/rclcpp_action/rclcpp_action/server.hpp", line 513, in call_goal_accepted_callback [0x7fc1703e6f43]
[move_group-4]         510:       std::lock_guard<std::mutex> lock(goal_handles_mutex_);
[move_group-4]         511:       goal_handles_[uuid] = goal_handle;
[move_group-4]         512:     }
[move_group-4]       > 513:     handle_accepted_(goal_handle);
[move_group-4]         514:   }
[move_group-4]         515: 
[move_group-4]         516:   /// \internal
[move_group-4] #29   Source "/usr/include/c++/11/bits/std_function.h", line 590, in operator() [0x7fc1703e80f6]
[move_group-4]         587:       {
[move_group-4]         588:     if (_M_empty())
[move_group-4]         589:       __throw_bad_function_call();
[move_group-4]       > 590:     return _M_invoker(_M_functor, std::forward<_ArgTypes>(__args)...);
[move_group-4]         591:       }
[move_group-4]         592: 
[move_group-4]         593: #if __cpp_rtti
[move_group-4] #28   Source "/usr/include/c++/11/bits/std_function.h", line 290, in _M_invoke [0x7fc1703cec5e]
[move_group-4]         287:       static _Res
[move_group-4]         288:       _M_invoke(const _Any_data& __functor, _ArgTypes&&... __args)
[move_group-4]         289:       {
[move_group-4]       > 290:     return std::__invoke_r<_Res>(*_Base::_M_get_pointer(__functor),
[move_group-4]         291:                      std::forward<_ArgTypes>(__args)...);
[move_group-4]         292:       }
[move_group-4] #27   Source "/usr/include/c++/11/bits/invoke.h", line 111, in __invoke_r<void, std::_Bind<void (move_group::ExecuteTaskSolutionCapability::*(move_group::ExecuteTaskSolutionCapability*, std::_Placeholder<1>))(const std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> >&)>&, std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > > [0x7fc1703d4eae]
[move_group-4]         108:       using __type = typename __result::type;
[move_group-4]         109:       using __tag = typename __result::__invoke_type;
[move_group-4]         110:       if constexpr (is_void_v<_Res>)
[move_group-4]       > 111:     std::__invoke_impl<__type>(__tag{}, std::forward<_Callable>(__fn),
[move_group-4]         112:                     std::forward<_Args>(__args)...);
[move_group-4]         113:       else
[move_group-4]         114:     return std::__invoke_impl<__type>(__tag{},
[move_group-4] #26   Source "/usr/include/c++/11/bits/invoke.h", line 61, in __invoke_impl<void, std::_Bind<void (move_group::ExecuteTaskSolutionCapability::*(move_group::ExecuteTaskSolutionCapability*, std::_Placeholder<1>))(const std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> >&)>&, std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > > [0x7fc1703da71c]
[move_group-4]          58:   template<typename _Res, typename _Fn, typename... _Args>
[move_group-4]          59:     constexpr _Res
[move_group-4]          60:     __invoke_impl(__invoke_other, _Fn&& __f, _Args&&... __args)
[move_group-4]       >  61:     { return std::forward<_Fn>(__f)(std::forward<_Args>(__args)...); }
[move_group-4]          62: 
[move_group-4]          63:   template<typename _Res, typename _MemFun, typename _Tp, typename... _Args>
[move_group-4]          64:     constexpr _Res
[move_group-4] #25   Source "/usr/include/c++/11/functional", line 503, in operator()<std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > > [0x7fc1703de411]
[move_group-4]         500:     _Result
[move_group-4]         501:     operator()(_Args&&... __args)
[move_group-4]         502:     {
[move_group-4]       > 503:       return this->__call<_Result>(
[move_group-4]         504:           std::forward_as_tuple(std::forward<_Args>(__args)...),
[move_group-4]         505:           _Bound_indexes());
[move_group-4]         506:     }
[move_group-4] #24   Source "/usr/include/c++/11/functional", line 420, in __call<void, std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> >&&, 0, 1> [0x7fc1703e0f0f]
[move_group-4]         417:     _Result
[move_group-4]         418:     __call(tuple<_Args...>&& __args, _Index_tuple<_Indexes...>)
[move_group-4]         419:     {
[move_group-4]       > 420:       return std::__invoke(_M_f,
[move_group-4]         421:           _Mu<_Bound_args>()(std::get<_Indexes>(_M_bound_args), __args)...
[move_group-4]         422:           );
[move_group-4]         423:     }
[move_group-4] #23   Source "/usr/include/c++/11/bits/invoke.h", line 96, in __invoke<void (move_group::ExecuteTaskSolutionCapability::*&)(const std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> >&), move_group::ExecuteTaskSolutionCapability*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > > [0x7fc1703e28f2]
[move_group-4]          93:       using __result = __invoke_result<_Callable, _Args...>;
[move_group-4]          94:       using __type = typename __result::type;
[move_group-4]          95:       using __tag = typename __result::__invoke_type;
[move_group-4]       >  96:       return std::__invoke_impl<__type>(__tag{}, std::forward<_Callable>(__fn),
[move_group-4]          97:                     std::forward<_Args>(__args)...);
[move_group-4]          98:     }
[move_group-4] #22   Source "/usr/include/c++/11/bits/invoke.h", line 74, in __invoke_impl<void, void (move_group::ExecuteTaskSolutionCapability::*&)(const std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> >&), move_group::ExecuteTaskSolutionCapability*&, std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > > [0x7fc1703e3d5d]
[move_group-4]          71:     __invoke_impl(__invoke_memfun_deref, _MemFun&& __f, _Tp&& __t,
[move_group-4]          72:           _Args&&... __args)
[move_group-4]          73:     {
[move_group-4]       >  74:       return ((*std::forward<_Tp>(__t)).*__f)(std::forward<_Args>(__args)...);
[move_group-4]          75:     }
[move_group-4]          76: 
[move_group-4]          77:   template<typename _Res, typename _MemPtr, typename _Tp>
[move_group-4] #21   Source "/root/ws_moveit/src/moveit_task_constructor/capabilities/src/execute_task_solution_capability.cpp", line 117, in goalCallback [0x7fc1703baa94]
[move_group-4]         114:         result->error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
[move_group-4]         115:     else {
[move_group-4]         116:         RCLCPP_INFO(LOGGER, "Executing TaskSolution");
[move_group-4]       > 117:         result->error_code = context_->plan_execution_->executeAndMonitor(plan);
[move_group-4]         118:     }
[move_group-4]         119: 
[move_group-4]         120:     if (result->error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
[move_group-4] #20   Source "/root/ws_moveit/src/moveit2/moveit_ros/planning/plan_execution/src/plan_execution.cpp", line 414, in executeAndMonitor [0x7fc19cf35149]
[move_group-4]         411:     // Pass current value of reconfigurable parameter plan_execution/record_trajectory_state_frequency
[move_group-4]         412:     double sampling_frequency = 0.0;
[move_group-4]         413:     node_->get_parameter_or("plan_execution.record_trajectory_state_frequency", sampling_frequency, 0.0);
[move_group-4]       > 414:     trajectory_monitor_ = std::make_shared<planning_scene_monitor::TrajectoryMonitor>(
[move_group-4]         415:         planning_scene_monitor_->getStateMonitor(), sampling_frequency);
[move_group-4]         416:   }
[move_group-4] #19   Source "/usr/include/c++/11/bits/shared_ptr.h", line 879, in make_shared<planning_scene_monitor::TrajectoryMonitor, const std::shared_ptr<planning_scene_monitor::CurrentStateMonitor>&, double&> [0x7fc19cf3bac8]
[move_group-4]         876:     {
[move_group-4]         877:       typedef typename std::remove_cv<_Tp>::type _Tp_nc;
[move_group-4]         878:       return std::allocate_shared<_Tp>(std::allocator<_Tp_nc>(),
[move_group-4]       > 879:                        std::forward<_Args>(__args)...);
[move_group-4]         880:     }
[move_group-4]         881: 
[move_group-4]         882:   /// std::hash specialization for shared_ptr.
[move_group-4] #18   Source "/usr/include/c++/11/bits/shared_ptr.h", line 863, in allocate_shared<planning_scene_monitor::TrajectoryMonitor, std::allocator<planning_scene_monitor::TrajectoryMonitor>, const std::shared_ptr<planning_scene_monitor::CurrentStateMonitor>&, double&> [0x7fc19cf3d574]
[move_group-4]         860:       static_assert(!is_array<_Tp>::value, "make_shared<T[]> not supported");
[move_group-4]         861: 
[move_group-4]         862:       return shared_ptr<_Tp>(_Sp_alloc_shared_tag<_Alloc>{__a},
[move_group-4]       > 863:                  std::forward<_Args>(__args)...);
[move_group-4]         864:     }
[move_group-4]         865: 
[move_group-4]         866:   /**
[move_group-4] #17   Source "/usr/include/c++/11/bits/shared_ptr.h", line 409, in shared_ptr<std::allocator<planning_scene_monitor::TrajectoryMonitor>, const std::shared_ptr<planning_scene_monitor::CurrentStateMonitor>&, double&> [0x7fc19cf3e5aa]
[move_group-4]         406:       // This constructor is non-standard, it is used by allocate_shared.
[move_group-4]         407:       template<typename _Alloc, typename... _Args>
[move_group-4]         408:     shared_ptr(_Sp_alloc_shared_tag<_Alloc> __tag, _Args&&... __args)
[move_group-4]       > 409:     : __shared_ptr<_Tp>(__tag, std::forward<_Args>(__args)...)
[move_group-4]         410:     { }
[move_group-4]         411: 
[move_group-4]         412:       template<typename _Yp, typename _Alloc, typename... _Args>
[move_group-4] #16   Source "/usr/include/c++/11/bits/shared_ptr_base.h", line 1342, in __shared_ptr<std::allocator<planning_scene_monitor::TrajectoryMonitor>, const std::shared_ptr<planning_scene_monitor::CurrentStateMonitor>&, double&> [0x7fc19cf3f11d]
[move_group-4]        1339:       // This constructor is non-standard, it is used by allocate_shared.
[move_group-4]        1340:       template<typename _Alloc, typename... _Args>
[move_group-4]        1341:     __shared_ptr(_Sp_alloc_shared_tag<_Alloc> __tag, _Args&&... __args)
[move_group-4]       >1342:     : _M_ptr(), _M_refcount(_M_ptr, __tag, std::forward<_Args>(__args)...)
[move_group-4]        1343:     { _M_enable_shared_from_this_with(_M_ptr); }
[move_group-4]        1344: 
[move_group-4]        1345:       template<typename _Tp1, _Lock_policy _Lp1, typename _Alloc,
[move_group-4] #15   Source "/usr/include/c++/11/bits/shared_ptr_base.h", line 650, in __shared_count<planning_scene_monitor::TrajectoryMonitor, std::allocator<planning_scene_monitor::TrajectoryMonitor>, const std::shared_ptr<planning_scene_monitor::CurrentStateMonitor>&, double&> [0x7fc19cf3fa6a]
[move_group-4]         647:       typename _Sp_cp_type::__allocator_type __a2(__a._M_a);
[move_group-4]         648:       auto __guard = std::__allocate_guarded(__a2);
[move_group-4]         649:       _Sp_cp_type* __mem = __guard.get();
[move_group-4]       > 650:       auto __pi = ::new (__mem)
[move_group-4]         651:         _Sp_cp_type(__a._M_a, std::forward<_Args>(__args)...);
[move_group-4]         652:       __guard = nullptr;
[move_group-4]         653:       _M_pi = __pi;
[move_group-4] #14   Source "/usr/include/c++/11/bits/shared_ptr_base.h", line 519, in _Sp_counted_ptr_inplace<const std::shared_ptr<planning_scene_monitor::CurrentStateMonitor>&, double&> [0x7fc19cf40740]
[move_group-4]         516:     {
[move_group-4]         517:       // _GLIBCXX_RESOLVE_LIB_DEFECTS
[move_group-4]         518:       // 2070.  allocate_shared should use allocator_traits<A>::construct
[move_group-4]       > 519:       allocator_traits<_Alloc>::construct(__a, _M_ptr(),
[move_group-4]         520:           std::forward<_Args>(__args)...); // might throw
[move_group-4]         521:     }
[move_group-4] #13   Source "/usr/include/c++/11/bits/alloc_traits.h", line 516, in construct<planning_scene_monitor::TrajectoryMonitor, const std::shared_ptr<planning_scene_monitor::CurrentStateMonitor>&, double&> [0x7fc19cf4105d]
[move_group-4]         513:     noexcept(std::is_nothrow_constructible<_Up, _Args...>::value)
[move_group-4]         514:     {
[move_group-4]         515: #if __cplusplus <= 201703L
[move_group-4]       > 516:       __a.construct(__p, std::forward<_Args>(__args)...);
[move_group-4]         517: #else
[move_group-4]         518:       std::construct_at(__p, std::forward<_Args>(__args)...);
[move_group-4]         519: #endif
[move_group-4] #12   Source "/usr/include/c++/11/ext/new_allocator.h", line 162, in construct<planning_scene_monitor::TrajectoryMonitor, const std::shared_ptr<planning_scene_monitor::CurrentStateMonitor>&, double&> [0x7fc19cf41820]
[move_group-4]         159:     void
[move_group-4]         160:     construct(_Up* __p, _Args&&... __args)
[move_group-4]         161:     noexcept(std::is_nothrow_constructible<_Up, _Args...>::value)
[move_group-4]       > 162:     { ::new((void *)__p) _Up(std::forward<_Args>(__args)...); }
[move_group-4]         163: 
[move_group-4]         164:       template<typename _Up>
[move_group-4]         165:     void
[move_group-4] #11   Source "/root/ws_moveit/src/moveit2/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp", line 47, in TrajectoryMonitor [0x7fc19e2b8b77]
[move_group-4]          45: planning_scene_monitor::TrajectoryMonitor::TrajectoryMonitor(const CurrentStateMonitorConstPtr& state_monitor,
[move_group-4]          46:                                                              double sampling_frequency)
[move_group-4]       >  47:   : TrajectoryMonitor(state_monitor, std::make_unique<TrajectoryMonitorMiddlewareHandle>(sampling_frequency),
[move_group-4]          48:                       sampling_frequency)
[move_group-4]          49: {
[move_group-4]          50: }
[move_group-4] #10   Source "/usr/include/c++/11/bits/unique_ptr.h", line 962, in make_unique<planning_scene_monitor::TrajectoryMonitorMiddlewareHandle, double&> [0x7fc19e2ba722]
[move_group-4]         959:   template<typename _Tp, typename... _Args>
[move_group-4]         960:     inline typename _MakeUniq<_Tp>::__single_object
[move_group-4]         961:     make_unique(_Args&&... __args)
[move_group-4]       > 962:     { return unique_ptr<_Tp>(new _Tp(std::forward<_Args>(__args)...)); }
[move_group-4]         963: 
[move_group-4]         964:   /// std::make_unique for arrays of unknown bound
[move_group-4]         965:   template<typename _Tp>
[move_group-4] #9    Source "/root/ws_moveit/src/moveit2/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor_middleware_handle.cpp", line 42, in TrajectoryMonitorMiddlewareHandle [0x7fc19e2bd39d]
[move_group-4]          39: namespace planning_scene_monitor
[move_group-4]          40: {
[move_group-4]          41: planning_scene_monitor::TrajectoryMonitorMiddlewareHandle::TrajectoryMonitorMiddlewareHandle(double sampling_frequency)
[move_group-4]       >  42:   : rate_{ std::make_unique<rclcpp::Rate>(sampling_frequency) }
[move_group-4]          43: {
[move_group-4]          44: }
[move_group-4] #8    Source "/usr/include/c++/11/bits/unique_ptr.h", line 962, in make_unique<rclcpp::Rate, double&> [0x7fc19e2bd6c4]
[move_group-4]         959:   template<typename _Tp, typename... _Args>
[move_group-4]         960:     inline typename _MakeUniq<_Tp>::__single_object
[move_group-4]         961:     make_unique(_Args&&... __args)
[move_group-4]       > 962:     { return unique_ptr<_Tp>(new _Tp(std::forward<_Args>(__args)...)); }
[move_group-4]         963: 
[move_group-4]         964:   /// std::make_unique for arrays of unknown bound
[move_group-4]         965:   template<typename _Tp>
[move_group-4] #7    Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7fc19d562405, in 
[move_group-4] #6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fc19d2c64d7, in __cxa_throw
[move_group-4] #5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fc19d2c6276, in std::terminate()
[move_group-4] #4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fc19d2c620b, in 
[move_group-4] #3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fc19d2bab9d, in 
[move_group-4] #2    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fc19cff87f2, in abort
[move_group-4] #1    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fc19d012475, in raise
[move_group-4] #0    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fc19d0669fc, in pthread_kill
[move_group-4] Aborted (Signal sent by tkill() 281907 0)
[ERROR] [move_group-4]: process has died [pid 281907, exit code -6, cmd '/root/ws_moveit/install/moveit_ros_move_group/lib/moveit_ros_move_group/move_group --ros-args --params-file /tmp/launch_params_6idegugl --params-file /tmp/launch_params_p8lkx9dq'].

In RViz, the planned path is displayed (transparent object) but not executed:

image

Note: if trying to replace the interpolation planner by the sampling or cartesian planner, the demo crashes even sooner (either during planning or during task initialisation).

I tried to Google "rate must be greater than 0" but I failed to find anything related to MoveIt.

I have checked for other Issues and I don't thing that this has already been reported...

Thank you for any help you can provide!

VincentDelort commented 1 year ago

UPDATE : if you move forward and add at least the "PICK" container stage with all its sub-stages, the demo works and the task can be executed...

Have I missed something?

rhaschke commented 1 year ago

Looks like the sampling_frequency passed to TrajectoryMonitor is zero:

[move_group-4]         412:     double sampling_frequency = 0.0;
[move_group-4]         413:     node_->get_parameter_or("plan_execution.record_trajectory_state_frequency", sampling_frequency, 0.0);
[move_group-4]       > 414:     trajectory_monitor_ = std::make_shared<planning_scene_monitor::TrajectoryMonitor>(
[move_group-4]         415:         planning_scene_monitor_->getStateMonitor(), sampling_frequency);
sea-bass commented 11 months ago

I actually resolved this on main in https://github.com/ros-planning/moveit2/pull/2423 -- I guess the Docker image must not yet have this change?