Initial public release

Author: hjlevy

CMakeLists.txt

@@ -0,0 +1,71 @@
+cmake_minimum_required(VERSION 3.10)
+
+# set the project name
+project(stitcher_planner)
+
+add_compile_options(-std=c++17)
+set(CMAKE_BUILD_TYPE "Release")
+
+#including eigen dir
+find_package(Eigen3 3.3 REQUIRED)
+include_directories(${EIGEN3_INCLUDE_DIR})
+
+find_package(PCL REQUIRED)
+include_directories(${PCL_INCLUDE_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+link_directories(${PCL_LIBRARY_DIRS})
+
+find_package(PkgConfig)
+pkg_check_modules(YAMLCPP REQUIRED yaml-cpp>=0.5)
+include_directories(${YAMLCPP_INCLUDE_DIRS})
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  pcl_ros
+  visualization_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS 
+	  std_msgs
+    nav_msgs 
+    sensor_msgs
+  LIBRARIES trajGenerator
+)
+
+include_directories(
+   ${PROJECT_SOURCE_DIR}
+   ${catkin_INCLUDE_DIRS}
+)
+
+add_library(trajGenerator src/primitives/lqmt/LQMTPrimitive.cpp
+                          src/primitives/min_time/MinTimePrimitive.cpp
+                          src/collision_checker/CollisionChecker.cc src/collision_checker/CollisionChecker.tpp
+                          src/feasibility_checker/FeasibilityChecker.cpp src/feasibility_checker/FeasibilityChecker.tpp
+                          src/geometric_planner/astar/Astar.cpp src/geometric_planner/jps/Jps3d.cpp src/geometric_planner/GeometricPlanner.cpp
+                          src/reference_path_utils/ReferencePath.cpp 
+                          src/planners/heuristic_generator/HeuristicDP.cpp
+                          src/planners/stitcher/STITCHER.tpp
+                          src/planners/stitcher/LqmtSTITCHER.cpp)
+            
+target_include_directories(trajGenerator PUBLIC src src/primitives/lqmt 
+                                                src/primitives/min_time 
+                                                src/collision_checker 
+                                                src/geometric_planner/astar 
+                                                src/geometric_planner/jps
+                                                src/geometric_planner 
+                                                src/reference_path_utils 
+                                                src/feasibility_checker
+                                                src/planners/heuristic_generator
+                                                src/planners/stitcher)
+
+################# EXECUTABLES FOR PATH PLOTTING NODES ##########################
+# planning with goal click for start and goal
+add_executable(goal_click_node  src/node/ROS_Goal_Click_Planning_Main.cc src/node/ROS_Goal_Click_Planning.cc)
+add_dependencies(goal_click_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(goal_click_node trajGenerator ${catkin_LIBRARIES})
+############################

LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2025 Helene J. Levy and Brett T. Lopez
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

README.md

@@ -0,0 +1,84 @@
+# STITCHER: Constrained Trajectory Planning in Complex Environments with Real-Time Motion Primitive Search
+
+#### [[ArXiv](https://arxiv.org/pdf/2510.14893)] [[Code](https://github.com/vectr-ucla/stitcher.git)]
+<!-- #### [[ArXiv]()] [[Video]()] [[Code]()] -->
+
+STITCHER is an optimization-free trajectory planning framework that stitches together motion primitives (i.e. short trajectory segments) to compute long-range, expressive, and near-optimal trajectories in real time. STITCHER has provable performance with guaranteed hard constraint satisfaction and a priori time/memory bounds. The framework has been extensively tested in both simulation and hardware on a custom quadrotor. 
+
+
+<br>
+<p align='center'>
+    <img src="./doc/STITCHER.png" alt="drawing" width="720"/>
+</p>
+
+### Dependencies
+- Ubuntu 20.04
+- [ROS Noetic](http://wiki.ros.org/noetic/Installation) 
+- C++ 17
+- [catkin tools](https://catkin-tools.readthedocs.io/en/latest/installing.html)
+- CMake >= `3.16.3`
+- Point Cloud Library >= `1.10.0`
+- Eigen >= `3.3.7`
+- LibYAML >= `0.6.2`
+
+```sh
+sudo apt install python3-catkin-tools cmake libpcl-dev libeigen3-dev libyaml-cpp-dev
+```
+
+### Compiling
+Create a catkin workspace, clone the `stitcher` repository into the `src` folder, and compile via the [`catkin_tools`](https://catkin-tools.readthedocs.io/en/latest/) package:
+```sh
+mkdir ws && cd ws && mkdir src && catkin init && cd src
+git clone https://github.com/vectr-ucla/stitcher.git
+catkin build
+```
+
+### Execution
+After sourcing the catkin workspace, launch the STITCHER planner ros node. 
+
+```sh
+cd ws
+source devel/setup.bash
+roslaunch stitcher_planner goal_click.launch
+```
+
+If successful, RViz will open with the loaded environment. To generate a path, use the 2D Nav Goal tool in RViz to click a start and goal point.
+
+<p align='center'>
+    <img src="./doc/stitcher_rviz.gif" alt="drawing" width="720"/>
+</p>
+
+### Test Environments
+
+<p align='center'>
+    <img src="./doc/env_types_labeled.png" alt="drawing" width="720"/>
+</p>
+
+
+We include two test environments in `data`: `perlin_map.pcd`, and `office.pcd` (referred to as Willow Garage in the manuscript). Edit `goal_click.launch` with the environment you would like to test in. 
+
+Note that the office environment requires a smaller resolution for the geometric path planner (e.g. A*, JPS) in order to move through doorways, while the perlin map may use a larger resolution for faster computation of the geometric path. The resolution can be changed in `cfg/params.yaml`.
+
+The following are values that work well. 
+- Perlin Noise: `resolution = 0.5`
+- Office: `resolution = 0.15`     
+
+## Quadrotor Flight
+Our framework was tested on hardware and shown to generate dynamically feasible paths. Below, is one experiment of our quadrotor flying a path generated by STITCHER. 
+
+<br>
+<p align='center'>
+    <img src="./doc/stitcher_flight.gif" alt="drawing" width="720"/>
+</p>
+
+## Citation
+If you found this work useful, please cite our manuscript:
+
+```bibtex
+@article{Levy25:STITCHER,
+  author={Levy, Helene J. and Lopez, Brett T. },
+  journal={ArXiv}, 
+  title={STITCHER: Constrained trajectory planning in complex environments with real-time motion primitive search}, 
+  year={2025}
+}
+```

cfg/params.yaml

@@ -0,0 +1,40 @@
+Robot:
+  thrust_max: 15. #N max total thrust (4*motor thrust max) equiv to fmax 18.75 m/s^2 mass normalized by 0.8kg 
+  thrust_min: 0.65 #N min total thrust (4*motor thrust min) equiv to 0.85 m/s^2 mass normalized by 0.8kg 
+  omega_max: 6 # rad/s max angular velocity
+  theta_max: 1. # rad max tilt acos(mg/Tmax), this will influence your fxy_max
+  mass: 0.8 #kg
+  grav: 9.8 #m/s^2
+  arm_length: 0.125 #m distance between center of quadrotor and rotor blade 
+  drag: 0.2 #propeller drag coefficient for quadrotor
+  motor_thrust_max: 3.75 #N per motor 
+  motor_thrust_min: 0.163 #N per motor
+  Jxx: 0.01 #moment of inertia of quadrotor
+  Jyy: 0.01
+  Jzz: 0.01
+
+Primitives:
+  vf_zenith: [90.] # sampled zenith angles in degrees
+  vf_azimuth: [-10., 0., 10.] # sampled azimuth angles in degrees
+  vf_magnitude: [0.25, 0.5, 0.75, 1.] # sampled magnitudes as fraction of v_max, increase number of magnitudes for denser sampling
+  v_max: 10.
+  # for admissible heuristic need to ensure that MinTimePrims have accel bounds that maximally bound thrust limits
+  a_max: 15.8 # calculated by mass norm thrust in xy minus grav: fxy_max - g
+  az_max: 9.  # calculated by mass norm thrust in xy minus grav: fz_max - g
+  j_max: 60.  
+  rho: 1000. # weighting on time in cost function for primitive generation
+
+Planner:
+  map_collision_buffer: 0.15
+  collision_check: true # turn on if you want to collision check
+  verbose: false
+
+GeometricPlanner:
+  geometric_planner_type: "jps"  # astar or jps (default jps)
+  resolution: 0.15 #for office environment need smaller resolution like 0.1 or 0.15 to fit through doorways
+  map_ub: [29, 29, 30] # these will update after running A* based on the map
+  map_lb: [-29, -29, 0] 
+  max_dist_btwn_wps: 12.5 # max distance between waypoints
+
+
+