search

moveit / moveit_drake

Experimental repository for Moveit2 - Drake integration
BSD 3-Clause "New" or "Revised" License
11 stars 4 forks source link

Demo launch file debug #17

Open kamiradi opened 3 days ago

kamiradi commented 3 days ago

@sea-bass @sjahr I am trying to run the demo with the current state of the build (as of the latest commit in #8 ). I have removed all the default planning pipelines https://github.com/moveit/moveit_drake/pull/8/commits/4a31afe54127577419ba4c04fa970d5cc3ab622a#r1665337870. When running the demo script, I see that the pipeline bench node still loads ompl and stomp. Also, clearly the drake ktopt_planning plugin is not being loaded. I have been trying to follow the messages on the ros console but didnt know where to start with this. I may be missing something very silly. Feel free to build and run the demo at the linked commit. Any tips on debugging this?

sjahr commented 3 days ago

Can you share the log?

kamiradi commented 3 days ago 
[INFO] [launch]: All log files can be found below /root/.ros/log/2024-07-04-08-08-58-878309-aditya-HP-EliteBook-840-G6-2064
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [static_transform_publisher-1]: process started with pid [2065]
[INFO] [robot_state_publisher-2]: process started with pid [2067]
[INFO] [rviz2-3]: process started with pid [2069]
[INFO] [pipeline_testbench_example-4]: process started with pid [2071]
[INFO] [ros2_control_node-5]: process started with pid [2073]
[INFO] [spawner-6]: process started with pid [2075]
[INFO] [spawner-7]: process started with pid [2077]
[INFO] [spawner-8]: process started with pid [2079]
[static_transform_publisher-1] [WARN] [1720080539.578710195] []: Old-style arguments are deprecated; see --help for new-style arguments
[static_transform_publisher-1] [INFO] [1720080539.598940048] [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] [1720080539.614121819] [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] [1720080539.616272443] [robot_state_publisher]: got segment panda_hand
[robot_state_publisher-2] [INFO] [1720080539.618882651] [robot_state_publisher]: got segment panda_leftfinger
[robot_state_publisher-2] [INFO] [1720080539.621191685] [robot_state_publisher]: got segment panda_link0
[robot_state_publisher-2] [INFO] [1720080539.623255023] [robot_state_publisher]: got segment panda_link1
[robot_state_publisher-2] [INFO] [1720080539.624956216] [robot_state_publisher]: got segment panda_link2
[robot_state_publisher-2] [INFO] [1720080539.627430970] [robot_state_publisher]: got segment panda_link3
[robot_state_publisher-2] [INFO] [1720080539.630091888] [robot_state_publisher]: got segment panda_link4
[robot_state_publisher-2] [INFO] [1720080539.630138322] [robot_state_publisher]: got segment panda_link5
[robot_state_publisher-2] [INFO] [1720080539.630171590] [robot_state_publisher]: got segment panda_link6
[robot_state_publisher-2] [INFO] [1720080539.630192695] [robot_state_publisher]: got segment panda_link7
[robot_state_publisher-2] [INFO] [1720080539.630219181] [robot_state_publisher]: got segment panda_link8
[robot_state_publisher-2] [INFO] [1720080539.630239943] [robot_state_publisher]: got segment panda_rightfinger
[ros2_control_node-5] [WARN] [1720080539.639751195] [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] [1720080539.640419796] [resource_manager]: Loading hardware 'PandaFakeSystem' 
[ros2_control_node-5] [INFO] [1720080539.642181830] [resource_manager]: Initialize hardware 'PandaFakeSystem' 
[ros2_control_node-5] [INFO] [1720080539.642311188] [resource_manager]: Successful initialization of hardware 'PandaFakeSystem'
[ros2_control_node-5] [INFO] [1720080539.642366743] [resource_manager]: Loading hardware 'PandaHandFakeSystem' 
[ros2_control_node-5] [INFO] [1720080539.642400531] [resource_manager]: Initialize hardware 'PandaHandFakeSystem' 
[ros2_control_node-5] [INFO] [1720080539.642437365] [resource_manager]: Successful initialization of hardware 'PandaHandFakeSystem'
[ros2_control_node-5] [INFO] [1720080539.642541637] [resource_manager]: 'configure' hardware 'PandaHandFakeSystem' 
[ros2_control_node-5] [INFO] [1720080539.642557548] [resource_manager]: Successful 'configure' of hardware 'PandaHandFakeSystem'
[ros2_control_node-5] [INFO] [1720080539.642569884] [resource_manager]: 'activate' hardware 'PandaHandFakeSystem' 
[ros2_control_node-5] [INFO] [1720080539.642583014] [resource_manager]: Successful 'activate' of hardware 'PandaHandFakeSystem'
[ros2_control_node-5] [INFO] [1720080539.642595596] [resource_manager]: 'configure' hardware 'PandaFakeSystem' 
[ros2_control_node-5] [INFO] [1720080539.642607164] [resource_manager]: Successful 'configure' of hardware 'PandaFakeSystem'
[ros2_control_node-5] [INFO] [1720080539.642624740] [resource_manager]: 'activate' hardware 'PandaFakeSystem' 
[ros2_control_node-5] [INFO] [1720080539.642636748] [resource_manager]: Successful 'activate' of hardware 'PandaFakeSystem'
[ros2_control_node-5] [INFO] [1720080539.649025168] [controller_manager]: update rate is 100 Hz
[ros2_control_node-5] [INFO] [1720080539.649508259] [controller_manager]: RT kernel is recommended for better performance
[rviz2-3] QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-root'
[pipeline_testbench_example-4] [INFO] [1720080539.669254086] [pipeline_testbench]: Initialize node
[pipeline_testbench_example-4] [WARN] [1720080539.726051739] [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.
[pipeline_testbench_example-4] [INFO] [1720080539.740280732] [pipeline_testbench_example.moveit.ros.rdf_loader]: Loaded robot model in 0.00673243 seconds
[pipeline_testbench_example-4] [INFO] [1720080539.740346061] [pipeline_testbench_example.moveit.core.robot_model]: Loading robot model 'panda'...
[pipeline_testbench_example-4] [WARN] [1720080539.768188819] [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.
[pipeline_testbench_example-4] [INFO] [1720080539.768483478] [pipeline_testbench_example.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'panda_arm': 1 1 1 1 1 1 1
[pipeline_testbench_example-4] [INFO] [1720080539.827992571] [pipeline_testbench_example.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[pipeline_testbench_example-4] [INFO] [1720080539.828319941] [pipeline_testbench_example.moveit.ros.moveit_cpp]: Listening to '/joint_states' for joint states
[pipeline_testbench_example-4] [INFO] [1720080539.830853267] [pipeline_testbench_example.moveit.ros.current_state_monitor]: Listening to joint states on topic '/joint_states'
[pipeline_testbench_example-4] [INFO] [1720080539.832069448] [pipeline_testbench_example.moveit.ros.planning_scene_monitor]: Listening to '/moveit_cpp/planning_scene_monitor' for attached collision objects
[pipeline_testbench_example-4] [INFO] [1720080539.832102308] [pipeline_testbench_example.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher.
[pipeline_testbench_example-4] [INFO] [1720080539.832361437] [pipeline_testbench_example.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene.
[pipeline_testbench_example-4] [INFO] [1720080539.834855107] [pipeline_testbench_example.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on '/moveit_cpp/monitored_planning_scene'
[pipeline_testbench_example-4] [INFO] [1720080539.835002148] [pipeline_testbench_example.moveit.ros.planning_scene_monitor]: Starting planning scene monitor
[pipeline_testbench_example-4] [INFO] [1720080539.837406503] [pipeline_testbench_example.moveit.ros.planning_scene_monitor]: Listening to '/moveit_cpp/publish_planning_scene'
[pipeline_testbench_example-4] [INFO] [1720080539.837446976] [pipeline_testbench_example.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[pipeline_testbench_example-4] [INFO] [1720080539.838285783] [pipeline_testbench_example.moveit.ros.planning_scene_monitor]: Listening to 'collision_object'
[pipeline_testbench_example-4] [INFO] [1720080539.839238492] [pipeline_testbench_example.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[pipeline_testbench_example-4] [WARN] [1720080539.842037778] [pipeline_testbench_example.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[pipeline_testbench_example-4] [ERROR] [1720080539.842134205] [pipeline_testbench_example.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates
[rviz2-3] MESA: error: Failed to query drm device.
[rviz2-3] libGL error: glx: failed to create dri3 screen
[rviz2-3] libGL error: failed to load driver: iris
[rviz2-3] libGL error: failed to open /dev/dri/card0: No such file or directory
[rviz2-3] libGL error: failed to load driver: iris
[rviz2-3] MESA: error: Failed to query drm device.
[rviz2-3] libGL error: glx: failed to create dri3 screen
[rviz2-3] libGL error: failed to load driver: iris
[rviz2-3] libGL error: failed to open /dev/dri/card0: No such file or directory
[rviz2-3] libGL error: failed to load driver: iris
[ros2_control_node-5] [INFO] [1720080540.094560751] [controller_manager]: Loading controller 'panda_arm_controller'
[ros2_control_node-5] [WARN] [1720080540.124181196] [panda_arm_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[ros2_control_node-5] [INFO] [1720080540.132094024] [controller_manager]: Loading controller 'joint_state_broadcaster'
[ros2_control_node-5] [INFO] [1720080540.159085512] [controller_manager]: Loading controller 'panda_hand_controller'
[spawner-7] [INFO] [1720080540.178670774] [spawner_panda_arm_controller]: Loaded panda_arm_controller
[ros2_control_node-5] [INFO] [1720080540.180120226] [controller_manager]: Configuring controller 'panda_arm_controller'
[ros2_control_node-5] [INFO] [1720080540.180503461] [panda_arm_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ros2_control_node-5] [INFO] [1720080540.180556267] [panda_arm_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[ros2_control_node-5] [INFO] [1720080540.180582517] [panda_arm_controller]: Using 'splines' interpolation method.
[ros2_control_node-5] [INFO] [1720080540.182539464] [panda_arm_controller]: Controller state will be published at 50.00 Hz.
[ros2_control_node-5] [INFO] [1720080540.187895044] [panda_arm_controller]: Action status changes will be monitored at 20.00 Hz.
[spawner-6] [INFO] [1720080540.196373218] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ros2_control_node-5] [INFO] [1720080540.200154451] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[ros2_control_node-5] [INFO] [1720080540.200271792] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-8] [INFO] [1720080540.215740091] [spawner_panda_hand_controller]: Loaded panda_hand_controller
[ros2_control_node-5] [INFO] [1720080540.230165094] [controller_manager]: Configuring controller 'panda_hand_controller'
[ros2_control_node-5] [INFO] [1720080540.230292445] [panda_hand_controller]: Action status changes will be monitored at 20Hz.
[spawner-6] [INFO] [1720080540.231576018] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[pipeline_testbench_example-4] [INFO] [1720080540.249264948] [pipeline_testbench_example.moveit.ros.planning_pipeline]: Successfully loaded planner 'STOMP'
[pipeline_testbench_example-4] [INFO] [1720080540.258230742] [pipeline_testbench_example]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[pipeline_testbench_example-4] [INFO] [1720080540.260669886] [pipeline_testbench_example]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[pipeline_testbench_example-4] [INFO] [1720080540.260734675] [pipeline_testbench_example]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[pipeline_testbench_example-4] [INFO] [1720080540.261234169] [pipeline_testbench_example]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[pipeline_testbench_example-4] [INFO] [1720080540.261276777] [pipeline_testbench_example]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds'
[pipeline_testbench_example-4] [INFO] [1720080540.261377963] [pipeline_testbench_example]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds'
[pipeline_testbench_example-4] [INFO] [1720080540.261479477] [pipeline_testbench_example]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision'
[pipeline_testbench_example-4] [INFO] [1720080540.261533816] [pipeline_testbench_example]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision'
[spawner-8] [INFO] [1720080540.261506926] [spawner_panda_hand_controller]: Configured and activated panda_hand_controller
[pipeline_testbench_example-4] [INFO] [1720080540.270726094] [pipeline_testbench_example]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization'
[pipeline_testbench_example-4] [INFO] [1720080540.275053895] [pipeline_testbench_example]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization'
[pipeline_testbench_example-4] [INFO] [1720080540.275112087] [pipeline_testbench_example]: Try loading adapter 'default_planning_response_adapters/ValidateSolution'
[pipeline_testbench_example-4] [INFO] [1720080540.277139784] [pipeline_testbench_example]: Loaded adapter 'default_planning_response_adapters/ValidateSolution'
[pipeline_testbench_example-4] [INFO] [1720080540.277194130] [pipeline_testbench_example]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath'
[pipeline_testbench_example-4] [INFO] [1720080540.278834175] [pipeline_testbench_example]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath'
[spawner-7] [INFO] [1720080540.321841301] [spawner_panda_arm_controller]: Configured and activated panda_arm_controller
[pipeline_testbench_example-4] [INFO] [1720080540.331293376] [pipeline_testbench_example.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL'
[pipeline_testbench_example-4] [INFO] [1720080540.339093239] [pipeline_testbench_example]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[pipeline_testbench_example-4] [INFO] [1720080540.339762874] [pipeline_testbench_example]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[pipeline_testbench_example-4] [INFO] [1720080540.339812940] [pipeline_testbench_example]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[pipeline_testbench_example-4] [INFO] [1720080540.340450936] [pipeline_testbench_example]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[pipeline_testbench_example-4] [INFO] [1720080540.340497228] [pipeline_testbench_example]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds'
[pipeline_testbench_example-4] [INFO] [1720080540.340579857] [pipeline_testbench_example]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds'
[pipeline_testbench_example-4] [INFO] [1720080540.340608224] [pipeline_testbench_example]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision'
[pipeline_testbench_example-4] [INFO] [1720080540.340652983] [pipeline_testbench_example]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision'
[pipeline_testbench_example-4] [INFO] [1720080540.351919717] [pipeline_testbench_example]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization'
[pipeline_testbench_example-4] [INFO] [1720080540.355016455] [pipeline_testbench_example]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization'
[pipeline_testbench_example-4] [INFO] [1720080540.355120392] [pipeline_testbench_example]: Try loading adapter 'default_planning_response_adapters/ValidateSolution'
[pipeline_testbench_example-4] [INFO] [1720080540.356359501] [pipeline_testbench_example]: Loaded adapter 'default_planning_response_adapters/ValidateSolution'
[pipeline_testbench_example-4] [INFO] [1720080540.356449469] [pipeline_testbench_example]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath'
[pipeline_testbench_example-4] [INFO] [1720080540.358718846] [pipeline_testbench_example]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath'
[pipeline_testbench_example-4] terminate called after throwing an instance of 'std::runtime_error'
[pipeline_testbench_example-4]   what():  Planning plugin name is empty or not defined in namespace 'ompl_stomp'. Please choose one of the available plugins: chomp_interface/CHOMPPlanner, ompl_interface/OMPLPlanner, pilz_industrial_motion_planner/CommandPlanner, planning_pipeline_test/DummyPlannerManager, stomp_moveit/StompPlanner
[pipeline_testbench_example-4] Stack trace (most recent call last):
[pipeline_testbench_example-4] #18   Object "", at 0xffffffffffffffff, in 
[pipeline_testbench_example-4] #17   Object "/root/workspace/install/moveit_drake/lib/moveit_drake/pipeline_testbench_example", at 0x55dbea540b44, in _start
[pipeline_testbench_example-4] #16   Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f3dcf97ae3f, in __libc_start_main
[pipeline_testbench_example-4] #15   Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f3dcf97ad8f, in 
[pipeline_testbench_example-4] #14   Object "/root/workspace/install/moveit_drake/lib/moveit_drake/pipeline_testbench_example", at 0x55dbea5404ba, in main
[pipeline_testbench_example-4] #13   Object "/root/workspace/install/moveit_drake/lib/moveit_drake/pipeline_testbench_example", at 0x55dbea5487a0, in pipeline_testbench::Demo::Demo(std::shared_ptr<rclcpp::Node>)
[pipeline_testbench_example-4] #12   Object "/root/workspace/install/moveit_ros_planning/lib/libmoveit_cpp.so.2.10.0", at 0x7f3dd039c95a, in moveit_cpp::MoveItCpp::MoveItCpp(std::shared_ptr<rclcpp::Node> const&)
[pipeline_testbench_example-4] #11   Object "/root/workspace/install/moveit_ros_planning/lib/libmoveit_cpp.so.2.10.0", at 0x7f3dd039b287, in moveit_cpp::MoveItCpp::MoveItCpp(std::shared_ptr<rclcpp::Node> const&, moveit_cpp::MoveItCpp::Options const&)
[pipeline_testbench_example-4] #10   Object "/root/workspace/install/moveit_ros_planning/lib/libmoveit_cpp.so.2.10.0", at 0x7f3dd039a28b, in moveit_cpp::MoveItCpp::loadPlanningPipelines(moveit_cpp::MoveItCpp::PlanningPipelineOptions const&)
[pipeline_testbench_example-4] #9    Object "/root/workspace/install/moveit_ros_planning/lib/libmoveit_planning_pipeline_interfaces.so.2.10.0", at 0x7f3dcef71c76, in moveit::planning_pipeline_interfaces::createPlanningPipelineMap(std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, std::shared_ptr<moveit::core::RobotModel const> const&, std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[pipeline_testbench_example-4] #8    Object "/root/workspace/install/moveit_ros_planning/lib/libmoveit_planning_pipeline.so.2.10.0", at 0x7f3dcedeb03d, in planning_pipeline::PlanningPipeline::PlanningPipeline(std::shared_ptr<moveit::core::RobotModel const> const&, std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[pipeline_testbench_example-4] #7    Object "/root/workspace/install/moveit_ros_planning/lib/libmoveit_planning_pipeline.so.2.10.0", at 0x7f3dcede220e, in planning_pipeline::PlanningPipeline::configure() [clone .cold]
[pipeline_testbench_example-4] #6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f3dcfc484d7, in __cxa_throw
[pipeline_testbench_example-4] #5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f3dcfc48276, in std::terminate()
[pipeline_testbench_example-4] #4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f3dcfc4820b, in 
[pipeline_testbench_example-4] #3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f3dcfc3cb9d, in 
[pipeline_testbench_example-4] #2    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f3dcf9797f2, in abort
[pipeline_testbench_example-4] #1    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f3dcf993475, in raise
[pipeline_testbench_example-4] #0    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f3dcf9e79fc, in pthread_kill
[pipeline_testbench_example-4] Aborted (Signal sent by tkill() 2071 0)
[rviz2-3] [INFO] [1720080540.444930858] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-3] [INFO] [1720080540.445150905] [rviz2]: OpenGl version: 4.5 (GLSL 4.5)
[rviz2-3] [INFO] [1720080540.543686987] [rviz2]: Stereo is NOT SUPPORTED
[ERROR] [pipeline_testbench_example-4]: process has died [pid 2071, exit code -6, cmd '/root/workspace/install/moveit_drake/lib/moveit_drake/pipeline_testbench_example --ros-args -r __node:=pipeline_testbench_example --params-file /tmp/launch_params_zktthtf1 --params-file /tmp/launch_params_0dqna9fh --params-file /tmp/launch_params_ba4qwhez'].
[rviz2-3] [WARN] [1720080540.717234716] [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.
[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
[INFO] [spawner-6]: process has finished cleanly [pid 2075]
[INFO] [spawner-8]: process has finished cleanly [pid 2079]
[INFO] [spawner-7]: process has finished cleanly [pid 2077]
[rviz2-3] [ERROR] [1720080543.792018196] [rviz2.moveit.ros.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-3] [INFO] [1720080543.833289507] [rviz2.moveit.ros.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-3] [INFO] [1720080543.856010388] [rviz2.moveit.ros.rdf_loader]: Loaded robot model in 0.00681365 seconds
[rviz2-3] [INFO] [1720080543.856086845] [rviz2.moveit.core.robot_model]: Loading robot model 'panda'...
[rviz2-3] [WARN] [1720080544.287005563] [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.
[rviz2-3] [INFO] [1720080544.483018441] [rviz2.moveit.ros.rdf_loader]: Loaded robot model in 0.00402822 seconds
[rviz2-3] [INFO] [1720080544.483090301] [rviz2.moveit.core.robot_model]: Loading robot model 'panda'...
[rviz2-3] [WARN] [1720080544.536350076] [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-3] [INFO] [1720080544.536771046] [rviz2.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'panda_arm': 1 1 1 1 1 1 1
[rviz2-3] [INFO] [1720080544.641760321] [rviz2.moveit.ros.planning_scene_monitor]: Starting planning scene monitor
[rviz2-3] [INFO] [1720080544.645366774] [rviz2.moveit.ros.planning_scene_monitor]: Listening to '/monitored_planning_scene'
[rviz2-3] [INFO] [1720080544.673839297] [interactive_marker_display_94517832453472]: Connected on namespace: /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic
[rviz2-3] [WARN] [1720080544.684834204] [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-3] [INFO] [1720080544.685422624] [rviz2.moveit.ros.robot_interaction]: No active joints or end effectors found for group 'hand'. Make sure that kinematics.yaml is loaded in this node's namespace.
[rviz2-3] [INFO] [1720080545.101671281] [interactive_marker_display_94517832453472]: Sending request for interactive markers
[rviz2-3] [INFO] [1720080545.304404137] [interactive_marker_display_94517832453472]: Service response received for initialization
^B][rviz2-3] [INFO] [1720080549.693336609] [rviz2.moveit.ros.planning_scene_monitor]: Failed to call service get_planning_scene, have you launched move_group or called psm.providePlanningSceneService()?
[rviz2-3] [INFO] [1720080549.693515240] [rviz2.moveit.ros.motion_planning_frame]: group hand
[rviz2-3] [INFO] [1720080549.693529548] [rviz2.moveit.ros.motion_planning_frame]: Constructing new MoveGroup connection for group 'hand' in namespace ''
^[[A^[[A^B[^[[A^[[A^[[A^[^[[B[rviz2-3] [INFO] [1720080609.714497459] [rviz2.moveit.ros.move_group_interface]: Ready to take commands for planning group hand.
kamiradi commented 3 days ago

Ok I think I know what maybe happening, it has something to do with the planning_pipeline options set in the moveit_config builder in the launch file. I see that over there the config options still point to stomp, ompl, stomp_ompl. This needs to be changed to drake.

kamiradi commented 3 days ago

I get the following after readding the ktopt_interface/KTOptPlanner to the ompl_stomp pipeline. The rest of them load, however the plugin is not being found. Where else am I supposed to expose the ktopt_plugin, any clues?

[pipeline_testbench_example-4]   what():  According to the loaded plugin descriptions the class ktopt_interface/KTOptPlanner with base class type planning_interface::PlannerManager does not exist. Declared types are  chomp_interface/CHOMPPlanner ompl_interface/OMPLPlanner pilz_industrial_motion_planner/CommandPlanner planning_pipeline_test/DummyPlannerManager stomp_moveit/StompPlanner

This might have to do with adding the drake_planning.yaml file. Could you confirm.

sea-bass commented 3 days ago

Yep, seems like the Drake plugin is not visible.

Usually, this means there is an inconsistency in either the plugin definition in the .cpp file or in the YAML file. Or, something missing in the CMakeLists.txt.

kamiradi commented 3 days ago

Thanks @sea-bass, which YAML file would this be?

sea-bass commented 3 days ago

Thanks @sea-bass, which YAML file would this be?

Sorry, the plugin description XML* file

sjahr commented 3 days ago

As Sebastian said, you'll need to verify that all namespace and the plugin definition in general is consistent. I'd reiterate with instructions again: https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Pluginlib.html Unfortunately this is very error prone without good error messaging :disappointed:

sea-bass commented 3 days ago

@kamiradi I made a small typo fix in your PR. Now when I run the demo the KTOpt plugin does show!

[pipeline_testbench_example-4]   what():  
Planning plugin name is empty or not defined in namespace 'ompl_stomp'. 
Please choose one of the available plugins: 
chomp_interface/CHOMPPlanner, ktopt_interface/KTOptPlanner, 
ompl_interface/OMPLPlanner, pilz_industrial_motion_planner/CommandPlanner, 
planning_pipeline_test/DummyPlannerManager, stomp_moveit/StompPlanner
sjahr commented 2 days ago

@kamiradi You'll need to update the plugin name here too: https://github.com/moveit/moveit_drake/blob/d80644c55e6afaabaf4a3eb81af0b1b568b52641/demo/config/testbench_moveit_cpp.yaml#L13

The demo tries to load the pipelines from this list