How to add custom planner in moveit2

[LiangXin-gugu]

Hello moveit community, I want to add a custom motion planner in moveit2.
My environment:

1. Ubuntu22.04;
2. ros2-humble;
3. moveit2(from source).

I decide to take the first approach from this: Install OMPL from source and add custom planner to OMPL, and then change the corresponding MoveIt OMPL package to recognize this new planner.

I follow the steps as bellow (refer to 1,2):

1. Delete the OMPL binaries using sudo apt remove ros-humble-ompl.
2. Download OMPL source code using git clone https://github.com/ompl/ompl.git in custom dir "~/gugu_workspace/ompl_ws/src/".
3. Modify OMPL source code. I just copy a RRT algorithm planner(copy RRT.h and RRT.cpp and change the name).
4. Change OMPL install target folder, which involves changing both .so binary and .h files install path.
   • "~/gugu_workspace/ompl_ws/src/ompl/src/ompl/CMakeList.txt". On line 196, I change DESTINATION ${CMAKE_INSTALL_LIBDIR} to DESTINATION "/opt/ros/humble/lib/x86_64-linux-gnu", which is the path where the OMPL .so binaries are original installed.
   • "/home/gugu/gugu_workspace/ompl_ws/src/ompl/src/CMakeLists.txt". On line 14, I change DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/ompl" to DESTINATION "/opt/ros/humble/include/ompl-1.6/ompl", which is the path where the OMPL .h headers are original installed.
5. Install OMPL from source code followed the installation tutorial in this.
6. Modify MoveIt source code. Modify planning_context_manager.cpp: Include the header file that defines your planner at the top and register my planner in the registerDefaultPlanners() function as the existing ones are.
7. Rebuild MoveIt from source. cd ~/ws_moveit2 and colcon build.

After completing the above steps and compiling successfully, I try to test the planner through “MotionPlanning" Plugin in Rviz.

But once I set the goal state and click plan, the move_group node collapse. The messeages in terminal are as follows:

[INFO] [launch]: All log files can be found below /home/gugu/.ros/log/2024-11-17-08-49-36-152595-GuoGuo-284881
[INFO] [launch]: Default logging verbosity is set to INFO
WARNING:root:Cannot infer SRDF from `/home/gugu/gugu_workspace/simplified_ur10e_ws/install/simplified_ur10e_moveit_config_manual_setup/share/simplified_ur10e_moveit_config_manual_setup`. -- using config/ur10e.srdf
[INFO] [move_group-1]: process started with pid [284882]
[INFO] [rviz2-2]: process started with pid [284884]
[rviz2-2] QStandardPaths: wrong permissions on runtime directory /run/user/1000/, 0755 instead of 0700
[move_group-1] [INFO] [1731804576.501583939] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0 seconds
[move_group-1] [INFO] [1731804576.501684159] [moveit_robot_model.robot_model]: Loading robot model 'ur10e'...
[move_group-1] [INFO] [1731804576.501694045] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[move_group-1] [INFO] [1731804576.533358618] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-1] [INFO] [1731804576.533596528] [moveit.ros_planning_interface.moveit_cpp]: Listening to 'joint_states' for joint states
[move_group-1] [INFO] [1731804576.534706727] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[move_group-1] [INFO] [1731804576.535451315] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[move_group-1] [INFO] [1731804576.535474916] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[move_group-1] [INFO] [1731804576.536201910] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[move_group-1] [INFO] [1731804576.536237015] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[move_group-1] [INFO] [1731804576.536724227] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[move_group-1] [INFO] [1731804576.537168789] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[move_group-1] [WARN] [1731804576.538114288] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-1] [ERROR] [1731804576.538149741] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates
[move_group-1] [INFO] [1731804576.539581807] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'ompl'
[move_group-1] [INFO] [1731804576.567914082] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL'
[move_group-1] [INFO] [1731804576.574660182] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.path_tolerance' was not set. Using default value: 0.100000
[move_group-1] [INFO] [1731804576.574736800] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.resample_dt' was not set. Using default value: 0.100000
[move_group-1] [INFO] [1731804576.574743525] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.min_angle_change' was not set. Using default value: 0.001000
[move_group-1] [INFO] [1731804576.574778794] [moveit_ros.fix_workspace_bounds]: Param 'ompl.default_workspace_bounds' was not set. Using default value: 10.000000
[move_group-1] [INFO] [1731804576.574794556] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_bounds_error' was set to 0.100000
[move_group-1] [INFO] [1731804576.574800001] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-1] [INFO] [1731804576.574810266] [moveit_ros.fix_start_state_collision]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-1] [INFO] [1731804576.574815292] [moveit_ros.fix_start_state_collision]: Param 'ompl.jiggle_fraction' was not set. Using default value: 0.020000
[move_group-1] [INFO] [1731804576.574834369] [moveit_ros.fix_start_state_collision]: Param 'ompl.max_sampling_attempts' was not set. Using default value: 100
[move_group-1] [INFO] [1731804576.574844952] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[move_group-1] [INFO] [1731804576.574851646] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Resolve constraint frames to robot links'
[move_group-1] [INFO] [1731804576.574854645] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-1] [INFO] [1731804576.574857674] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-1] [INFO] [1731804576.574860560] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-1] [INFO] [1731804576.574863365] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[move_group-1] [INFO] [1731804576.579468887] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'pilz_industrial_motion_planner'
[move_group-1] [INFO] [1731804576.591777385] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[move_group-1] [INFO] [1731804576.597728437] [moveit.pilz_industrial_motion_planner]: Available plugins: pilz_industrial_motion_planner/PlanningContextLoaderCIRC pilz_industrial_motion_planner/PlanningContextLoaderLIN pilz_industrial_motion_planner/PlanningContextLoaderPTP 
[move_group-1] [INFO] [1731804576.597790992] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderCIRC
[move_group-1] [INFO] [1731804576.600146448] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [CIRC]
[move_group-1] [INFO] [1731804576.600220231] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderLIN
[move_group-1] [INFO] [1731804576.601434671] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [LIN]
[move_group-1] [INFO] [1731804576.601493317] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderPTP
[move_group-1] [INFO] [1731804576.602734450] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [PTP]
[move_group-1] [INFO] [1731804576.602806851] [moveit.ros_planning.planning_pipeline]: Using planning interface 'Pilz Industrial Motion Planner'
[move_group-1] [INFO] [1731804576.606775825] [moveit_ros.fix_workspace_bounds]: Param 'pilz_industrial_motion_planner.default_workspace_bounds' was not set. Using default value: 10.000000
[move_group-1] [INFO] [1731804576.606871644] [moveit_ros.fix_start_state_bounds]: Param 'pilz_industrial_motion_planner.start_state_max_bounds_error' was not set. Using default value: 0.050000
[move_group-1] [INFO] [1731804576.606879587] [moveit_ros.fix_start_state_bounds]: Param 'pilz_industrial_motion_planner.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-1] [INFO] [1731804576.606891920] [moveit_ros.fix_start_state_collision]: Param 'pilz_industrial_motion_planner.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-1] [INFO] [1731804576.606898480] [moveit_ros.fix_start_state_collision]: Param 'pilz_industrial_motion_planner.jiggle_fraction' was not set. Using default value: 0.020000
[move_group-1] [INFO] [1731804576.606904263] [moveit_ros.fix_start_state_collision]: Param 'pilz_industrial_motion_planner.max_sampling_attempts' was not set. Using default value: 100
[move_group-1] [INFO] [1731804576.606912533] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-1] [INFO] [1731804576.606916739] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-1] [INFO] [1731804576.606918715] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-1] [INFO] [1731804576.606920997] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[rviz2-2] [INFO] [1731804576.647123840] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-2] [INFO] [1731804576.647235646] [rviz2]: OpenGl version: 4.5 (GLSL 4.5)
[move_group-1] [INFO] [1731804576.658886346] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for arm_controller
[move_group-1] [INFO] [1731804576.658993618] [moveit.plugins.moveit_simple_controller_manager]: Max effort set to 0.0
[move_group-1] [INFO] [1731804576.662659589] [moveit.plugins.moveit_simple_controller_manager]: Added GripperCommand controller for grip_controller
[move_group-1] [INFO] [1731804576.663074132] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-1] [INFO] [1731804576.663162490] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-1] [INFO] [1731804576.663744486] [moveit_ros.trajectory_execution_manager]: Trajectory execution is managing controllers
[move_group-1] [INFO] [1731804576.663759900] [move_group.move_group]: MoveGroup debug mode is ON
[move_group-1] [INFO] [1731804576.699485334] [move_group.move_group]: 
[move_group-1] 
[move_group-1] ********************************************************
[move_group-1] * MoveGroup using: 
[move_group-1] *     - ApplyPlanningSceneService
[move_group-1] *     - ClearOctomapService
[move_group-1] *     - ExecuteTaskSolution
[move_group-1] *     - CartesianPathService
[move_group-1] *     - ExecuteTrajectoryAction
[move_group-1] *     - GetPlanningSceneService
[move_group-1] *     - KinematicsService
[move_group-1] *     - MoveAction
[move_group-1] *     - MotionPlanService
[move_group-1] *     - QueryPlannersService
[move_group-1] *     - StateValidationService
[move_group-1] ********************************************************
[move_group-1] 
[move_group-1] [INFO] [1731804576.699572811] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[move_group-1] [INFO] [1731804576.699582985] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context initialization complete
[move_group-1] Loading 'move_group/ApplyPlanningSceneService'...
[move_group-1] Loading 'move_group/ClearOctomapService'...
[move_group-1] Loading 'move_group/ExecuteTaskSolutionCapability'...
[move_group-1] Loading 'move_group/MoveGroupCartesianPathService'...
[move_group-1] Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[move_group-1] Loading 'move_group/MoveGroupGetPlanningSceneService'...
[move_group-1] Loading 'move_group/MoveGroupKinematicsService'...
[move_group-1] Loading 'move_group/MoveGroupMoveAction'...
[move_group-1] Loading 'move_group/MoveGroupPlanService'...
[move_group-1] Loading 'move_group/MoveGroupQueryPlannersService'...
[move_group-1] Loading 'move_group/MoveGroupStateValidationService'...
[move_group-1] 
[move_group-1] You can start planning now!
[move_group-1] 
[rviz2-2] [INFO] [1731804576.708815564] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-2] 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-2]          at line 253 in /opt/ros/humble/include/class_loader/class_loader/class_loader_core.hpp
[rviz2-2] [INFO] [1731804576.861425960] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0 seconds
[rviz2-2] [INFO] [1731804576.861592451] [moveit_robot_model.robot_model]: Loading robot model 'ur10e'...
[rviz2-2] [INFO] [1731804576.861616052] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-2] [ERROR] [1731804579.895690511] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-2] [INFO] [1731804579.912523531] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-2] [INFO] [1731804579.973407498] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0 seconds
[rviz2-2] [INFO] [1731804579.973463436] [moveit_robot_model.robot_model]: Loading robot model 'ur10e'...
[rviz2-2] [INFO] [1731804579.973469712] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-2] [INFO] [1731804580.039911332] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[rviz2-2] [INFO] [1731804580.040947741] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/monitored_planning_scene'
[rviz2-2] [INFO] [1731804580.057806533] [interactive_marker_display_94303568574112]: Connected on namespace: /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic
[rviz2-2] [INFO] [1731804580.070238071] [moveit_ros_visualization.motion_planning_frame]: group ur_arm
[rviz2-2] [INFO] [1731804580.070289525] [moveit_ros_visualization.motion_planning_frame]: Constructing new MoveGroup connection for group 'ur_arm' in namespace ''
[rviz2-2] [INFO] [1731804580.114581375] [move_group_interface]: Ready to take commands for planning group ur_arm.
[rviz2-2] [INFO] [1731804580.116212481] [moveit_ros_visualization.motion_planning_frame]: group ur_arm
[rviz2-2] [INFO] [1731804580.154779645] [interactive_marker_display_94303568574112]: Sending request for interactive markers
[rviz2-2] [INFO] [1731804580.178427351] [interactive_marker_display_94303568574112]: Service response received for initialization
[rviz2-2] [INFO] [1731804584.129247569] [move_group_interface]: MoveGroup action client/server ready
[move_group-1] [INFO] [1731804584.130179353] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[rviz2-2] [INFO] [1731804584.130491539] [move_group_interface]: Planning request accepted
[move_group-1] [INFO] [1731804584.130565501] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-1] [INFO] [1731804584.130779887] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-1] [INFO] [1731804584.130844505] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'ompl'
[move_group-1] [INFO] [1731804584.132113527] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'ur_arm' will use planner 'geometric::RRT'. Additional configuration parameters will be set when the planner is constructed.
[ERROR] [move_group-1]: process has died [pid 284882, exit code -11, cmd '/home/gugu/gugu_workspace/ws_moveit2/install/moveit_ros_move_group/lib/moveit_ros_move_group/move_group --ros-args --params-file /tmp/launch_params_avu9n8n4 --params-file /tmp/launch_params_x1s34s5v --params-file /tmp/launch_params_d6_v9y7n --params-file /tmp/launch_params_cqjha9bi'].
[move_group-1] 

I suspect that there is something wrong with the steps of installing OMPL from source code, which caused the problem, because when I try to install the .so and .h files directly in the original path without modifying the OMPL source code, the same problem still occurs.

Besides, when I install OMPL from source, the terminal messages are as follows:

-- Installing: /usr/local/lib/libompl.so.1.6.0
-- Installing: /usr/local/lib/libompl.so.17
-- Set runtime path of "/usr/local/lib/libompl.so.1.6.0" to "/usr/local/lib"
-- Installing: /usr/local/lib/libompl.so

Is it possible that the problem is caused by incorrect run time path settings?

My questions are:

1. How to add custom motion planner in OMPL and then called by moveit2?
2. Are there better ways to add custom motion planner in moveit2?

Any input is appreciated.