Exception when using persistent roadmaps

[brownbrowny]

Description

I am currently using Moveit2 in combination with ROS Foxy to control a Universal Robot UR10e. The purpose is to try different planning algorithms for trajectory generation and validate them.
So far everything worked fine more or less (had some build problems). Then I found the feature of persistent roadmaps which came with OMPL 1.5.0 which I wanted to try of course. After adapting the ompl_planning.yaml accordingly I tried planning with the new planner but got a screen full of exceptions produced by Moveit.

This is my first report, so please excuse any mistakes or missing information and let me know. Also tips for improvements are welcome.

My environment

• ROS Distro: Foxy
• OS Version: Ubuntu 20.04.4 LTS
• Source or Binary build?: Source
• If source, which branch?: https://github.com/ros-planning/moveit2/releases/tag/2.2.2

Steps to reproduce

1. For using the planners with persistent roadmaps I followed this tutorial: https://moveit.picknik.ai/humble/doc/examples/ompl_interface/ompl_interface_tutorial.html#persistent-roadmaps
   So my ompl_planning.yaml looks like:

planner_configs:
  PersistentLazyPRMstar: # use this with a representative environment to create a roadmap
    type: geometric::LazyPRMstar
    multi_query_planning_enabled: true
    store_planner_data: true
    load_planner_data: false
    planner_data_path: /tmp/roadmap.graph
  PersistentLazyPRM: # use this to load a previously created roadmap
    type: geometric::LazyPRM
    multi_query_planning_enabled: true
    store_planner_data: false
    load_planner_data: true
    planner_data_path: /tmp/roadmap.graph
  SemiPersistentLazyPRMstar: # reuses roadmap during lifetime of node but doesn't save/load roadmap to/from disk
    type: geometric::LazyPRMstar
    multi_query_planning_enabled: true
    store_planner_data: false
    load_planner_data: false
  SemiPersistentLazyPRM: # reuses roadmap during lifetime of node but doesn't save/load roadmap to/from disk
    type: geometric::LazyPRM
    multi_query_planning_enabled: true
    store_planner_data: false
    load_planner_data: false
ur_manipulator:
  planner_configs:
    - PersistentLazyPRMstar
    - PersistentLazyPRM
    - SemiPersistentLazyPRMstar
    - SemiPersistentLazyPRM

2. Rebuilding the corresponding package ur_moveit_config

3. Start my Robot Control driver in simulation mode which I have from: https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver
   ros2 launch ur_bringup ur_control.launch.py ur_type:=ur10e use_fake_hardware:=true launch_rviz:=false

4. Start Moveit with Rviz:
   ros2 launch ur_bringup ur_moveit.launch.py ur_type:=ur10e use_fake_hardware:=true launch_rviz:=true

Expected behaviour

• I should be able to select the four planners I configured in the ompl_planning.yaml
• Then when planning to a goal pose by using the interactive marker, the planner should create a plan and visualize the computed trajectory

Actual behaviour

• I can see the planners in the dropdown in the Context tab, also selecting works. Then I can see the correct planner parameters
• As soon as I press Plan with default options, nothing happens but the error log in the Moveit console window
• Other planners without persistent roadmaps work fine

Backtrace or Console output

[move_group-1] [INFO] [1654170492.092594822] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-1] [INFO] [1654170492.092757147] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-1] [INFO] [1654170492.095270354] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-1] [INFO] [1654170492.095502904] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'move_group'
[move_group-1] [WARN] [1654170492.098723264] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.099248489] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.109191894] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.109621648] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Fix Start State In Collision': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.119616905] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.120040266] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.130033769] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.130550152] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Fix Start State Bounds': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.140525590] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.141049362] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.151041410] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.151358023] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Fix Start State In Collision': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.161418396] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.162057996] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.172091646] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.172337430] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Fix Workspace Bounds': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.182398749] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.182722149] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.192815169] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.193557909] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Fix Start State In Collision': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.203601439] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.203870689] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.213940967] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.214160167] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Fix Start State Bounds': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.224240383] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.224905289] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.234916072] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.235234773] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Fix Start State In Collision': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.245307466] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.245650953] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.255688559] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.256672061] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Add Time Optimal Parameterization': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.256959378] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.257184572] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.267224728] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.267510911] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Fix Start State In Collision': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.277549987] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.277818771] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.287949758] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.288509263] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Fix Start State Bounds': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.298441651] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.298751354] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.308780321] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.309058639] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Fix Start State In Collision': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.319395678] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.319930717] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.330090204] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.330928308] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Fix Workspace Bounds': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.340975820] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.341256139] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.351365765] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.351938652] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Fix Start State In Collision': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.361945409] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.362227436] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.372289402] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.372544073] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Fix Start State Bounds': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.382592776] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.383675491] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.383750946] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.383965975] [moveit.planning_request_adapter]: Exception caught executing *next* adapter 'Fix Start State In Collision': bad lexical cast: source type value could not be interpreted as target
[move_group-1] [WARN] [1654170492.384161326] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.384332856] [moveit.planning_request_adapter]: Exception caught executing *final* adapter 'Fix Start State Path Constraints': bad lexical cast: source type value could not be interpreted as target
[rviz2-3] [INFO] [1654170492.392889362] [move_group_interface]: Planning request accepted
[move_group-1] [WARN] [1654170492.394382513] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/base/goals/src/GoalLazySamples.cpp:129 - Goal sampling thread never did any work. Space information not set up.
[move_group-1] [ERROR] [1654170492.394663001] [moveit.ros_planning.planning_pipeline]: Exception caught: 'bad lexical cast: source type value could not be interpreted as target'
[move_group-1] [INFO] [1654170492.394695924] [moveit_move_group_default_capabilities.move_action_capability]: Catastrophic failure
[rviz2-3] [INFO] [1654170492.693611583] [move_group_interface]: Planning request aborted
[rviz2-3] [ERROR] [1654170492.793817319] [move_group_interface]: MoveGroupInterface::plan() failed or timeout reached

If you wonder why there are no other planners in the ompl_planning.yaml, I removed them all just to be sure there is no error there. They work without any problems at all.

I also just kept:

PersistentLazyPRMstar: # use this with a representative environment to create a roadmap
    type: geometric::LazyPRMstar
    multi_query_planning_enabled: true

But the error still occurs, so it must be the option multi_query_planning_enabled

Any ideas?

[FarisHamdi]

I am also facing a similar issue, but while attempting to use AnytimePathShortening

[FarisHamdi]

I figured it out!

Config parsing in the planning pipeline is unable to parse true or false due to the lexical cast in the line below. This issue affect any feature that were previously enabled via booleans including multi_query_planning
https://github.com/ros-planning/moveit2/blob/949facee1889bf2ba2dd76711051ab01bd307f51/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp#L359

This can be gotten around by using 1 for true or 0 for false

[AndyZe]

The documentation was fixed here: moveit/moveit2_tutorials#460

I'm going to close the issue now. Thanks for tracking this down!

[brownbrowny]

Sorry for replying so late. I tried your solution @FarisHamdi and it works perfectly. Thank you!