S-Path - Situationally-aware Path Planning
Exploiting 3D Scene Graphs

Introduction

S-Path is a hierarchical path-planning framework that combines semantic information from indoor 3D Scene Graphs with geometric motion planning. Semantic structure such as rooms, doors, and their connectivity is used to guide and decompose the geometric planning problem.

Planning is performed in two stages. First, a semantic plan is computed by searching over a semantic graph derived from the scene graph, yielding a sequence of rooms and doorways that represent the intended traversal. Second, this semantic plan constrains and decomposes the geometric planning task into smaller subproblems, each solved using a standard geometric planner within a constrained configuration space.

If a geometric subproblem is infeasible or a change in the scene graph occurs (for e.g., a closed doorway), the semantic plan is updated using information from previous attempts, allowing replanning without restarting the full planning process.

Installation

S-Path works with ROS Noetic and has been tested with Ubuntu 20.04. S-Path uses an older version of S-Graphs informed with BIM maps, which is already included in this repository. To install, make a new workspace, clone this repo and build using catkin.

mkdir workspace && cd workspace
git clone git@github.com:snt-arg/spath_ros.git
mv spath_ros src
catkin build -j <num-cores>

As S-Path contains many packages, it is recommended to use mprocs, the configuration file of which is in the base directory of this repo.

# install mprocs if not already installed
mprocs -c src/mprocs.yaml

Package overview:

• Carta:
  • Map generation S-Graphs data
  • Map generation from BIM data
  • Realtime map generation from sensor data
  • Ability to load/save SDF maps
  • Ability to export maps to .stl
• Navigator (semantic-geometric path planning)
  • NagivatorHL: high level global semantic-geometric planning
  • NagivatorSGraphs: semantic level global planning
• snav common:
  • Shared code
  • Various geometric primitives and algorithms
  • Graph structure
• rviz config:
  • The Rviz config files
• rviz plugins:
  • Planner widget for Rviz
• Utils
  • Floorplan:
    • .svg to .csv (map parser) converter tool
    • example floorplans
  • Benchmark analyzer
    • Plotting/analyzing of rosbags with benchmark data

Run a synthetic dataset

There are two synthetic environments present in this repository (SF1 and SF2 in the paper). The floorplans/BIM for these environments were generated manually and the package map_parser uses this information to generate the required scene graph. S-Path uses widgets in Rviz interactively provide queries to plan in an environment. Follow the steps below to run planning queries on one of the synthetic datasets.

1. Setup map_parser: specify the csv files (wall_file_path, room_file_path, door_file_path) corresponding to your dataset in the map_parser.launch file.

   For SF1 use:

   <param name="wall_file_path" value="$(find map_parser)/csv/floorplan/floorplan_office_walls.csv"/>
   <param name="room_file_path" value="$(find map_parser)/csv/floorplan/floorplan_office_rooms.csv" />
   <param name="door_file_path" value="$(find map_parser)/csv/floorplan/floorplan_office_doors.csv" />
   

   For SF2 use:

   <param name="wall_file_path" value="$(find map_parser)/csv/floorplan/floorplan_walls.csv"/>
   <param name="room_file_path" value="$(find map_parser)/csv/floorplan/floorplan_rooms.csv" />
   <param name="door_file_path" value="$(find map_parser)/csv/floorplan/floorplan_doors.csv" />
   

2. Launch the mprocs config and then the following packages in order:

   • rviz for visualization and setting planning queries interactively
   • map_parser to generate semantic graph from the definitions of walls, rooms, and doors in Step 1.
   • carta to generate an ESDF from the semantic graph.
   • navigator_sgraphs for the semantic planning on the semantic graph.
   • navigator_hl for the geometric planning via an interface with the OMPL planning library.

3. In rviz, go to the Planning tab, pick a room for start and goal from the comboboxes. Call:

   • The geometric planner only on the entire configuration space (Ablation I in the paper) by clicking on Planner Service.
   • The hierarchical planner on the restricted configuration space (Ablation II in the paper) by clicking on Planner Hierarchical.
   • The hierarchical planner with subproblems (S-Path) by clicking on Planner Hierarchical+SPS.
   • Optionally, you can provide a blocked door in the path, that simulates this door being blocked after the initial plan was computed (Replanning) by clicking on Planner Hierachical+SPS+Replanning.

Run a real dataset

Real datasets need both the semantic graph and the metric map obtained from the scene graph and the metric mesh from LIDAR measurements (using voxblox) respectively. S-Path provides this data for two real environments (RF1 and RF2 in the paper). Since S-Graphs does not have the capability to detect doorways, the semantic information was manually produced by overlaying over the metric mesh. The procedure for this will be released at a later time. Follow the steps below to load the RF1 environment and run S-Path on it.

1. Configure the map_parser.launch

<param name="wall_file_path" value="$(find map_parser)/csv/floorplan/floorplan_urchet_walls.csv"/>
<param name="room_file_path" value="$(find map_parser)/csv/floorplan/floorplan_urchet_rooms.csv" />
<param name="door_file_path" value="$(find map_parser)/csv/floorplan/floorplan_urchet_doors.csv" />

2. Launch mprocs and then the following packages in order: rviz, map_parser, carta_rt, navigator_sgraphs, and navigator_hl.

3. Now in rviz you will see the contours of the metric-semantic graph (you may have to zoom), but without the metric map. Let's load it by calling the /carta/load_map_and_update pointing it to our map e.g., /<workspace>/src/s-nav/snav_assets/voxblox_maps/urchet_f2.voxblox. Now you should have both the semantic-metric layer and the metric map.

4. Running queries using the planner panel is the same as described in the previous section. Notice that the quality of the metric map isn't very high (partially because they were exported every keyframe instead of frame during the LIDAR SLAM). If a path cannot be found, chances are that the start or endpoint are occupied. You can use the gizmos to adjust the positions (by clicking on Edit).

Run a TTP/Efficiency benchmark as in Table 1

The results in Table 1 of the paper can be reproduced as follows:

1. mprocs: launch rviz
2. mprocs: launch sgraphs_IS (this provides the metric-semantic graph)
   • Specify the correct dataset in the launch file (src/map_parser/launch/map_parser.launch)
   • e.g. for SF2:

   <launch>
      [...]
       <param name="wall_file_path" value="$(find map_parser)/csv/floorplan/floorplan_walls.csv"/>
       <param name="room_file_path"  value="$(find map_parser)/csv/floorplan/floorplan_rooms.csv" />
       <param name="door_file_path"  value="$(find map_parser)/csv/floorplan/floorplan_doors.csv" />
      [...]
   </launch>
   

3. For synthetic datasets we run mprocs carta_sgraphs (this will build the metric map from the floorplan data), for real datasets we run carta_rt as described in Run a real dataset
4. mprocs: launch navigator_hl, navigator_sgraphs
5. Verify that you see the map and graph in rviz
6. Configure navigator_hl:
   • Set the geometric_planner to either PRM*, RRT* or BitStar (notice that benchmark_planners is only used by Benchmark TTP)
   • Set sps_multithreaded to false to disable multithreading (not used for Table 1)
   • Set benchmark_rep_count to something reasonable, this is the number of repetitions per ttp (e.g., 100)
   • Set benchmark_max_seconds_to_plan to 6, benchmark_min_seconds_to_plan to 0.01 and benchmark_slices to 10 (see Figure 5). The values depend on the scenario, but this defines the ttp range used for each run. The number of runs is defined by benchmark_slices which 'slices' the interval defined by [benchmark_min_seconds_to_plan;benchmark_max_seconds_to_plan] logarithmically, i.e., with more data points towards the lower end of the ttp range (these are the datapoints you see in Fig 5). 10 is a good starting point.
   • Some of these parameters require iterative tuning to cover the range of interest properly
   • Don't forget to restart navigator_hl if you do changes
7. Go back to rviz
   • Set the start and end positions (x [m], y [m] etc.). For SF2 this would be from [0.5,2,0] to [42.5,36.5,0] (long) and [0.5,2,0] to [4.5,18,0] (short).
   • Verify that the path computes by clicking Planner hierarchical
   • Start recording the data (rosbag record /spot/navigator_hl/benchmark_data), you may also record everything, but this is the topic we are interested in
   • Click Benchmark TTP button, and get a coffee, this might take a while as it runs benchmark_rep_count * benchmark_slices iterations * 3 iterations (the 3 for ompl, ompl+sgraphs, ompl+sgraphs+sps). Check the output of navigator_hl to see when it has finished its work. During that time you should see many paths generated in rviz with different planners.
   • Stop recording and give the file a descriptive name, e.g., benchmark_<geometric_planner>.bag
   • You will want to repeat 7. and 8. for all relevant planners
8. After recording the results, we can generate the table and graphs using the src/s-nav/utils/benchmark_analyzer/run.py script
   • Read through the script, but in short you specify the datasets (the rosbags you generated) and run plot_successrate_over_secs_to_plan2 resp. analyze_wall_time on the data.
   • Run python3 run.py from the script's directory. This will generate the graphs *.pdf and also the tables *.tex in the results directory
   • You may have to go back to 7. to get satisfactory coverage of the domain of interest

Start and End points are also part of benchmark_data msg. Check out the original rosbags to find the values for certain parameters.