add humble-navigation2

navigation2/nav2_smac_planner/CMakeLists.txt

@@ -0,0 +1,164 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_smac_planner)
+
+set(CMAKE_BUILD_TYPE Release) #Debug, Release
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(angles REQUIRED)
+find_package(ompl REQUIRED)
+find_package(OpenMP REQUIRED)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+
+if(MSVC)
+  add_compile_definitions(_USE_MATH_DEFINES)
+else()
+  add_compile_options(-O3 -Wextra -Wdeprecated -fPIC)
+endif()
+
+include_directories(
+  include
+  ${OMPL_INCLUDE_DIRS}
+  ${OpenMP_INCLUDE_DIRS}
+)
+
+find_package(OpenMP)
+if(OPENMP_FOUND)
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+    set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+endif()
+
+set(library_name nav2_smac_planner)
+
+set(dependencies
+  rclcpp
+  rclcpp_action
+  rclcpp_lifecycle
+  std_msgs
+  visualization_msgs
+  nav2_util
+  nav2_msgs
+  nav_msgs
+  geometry_msgs
+  builtin_interfaces
+  tf2_ros
+  nav2_costmap_2d
+  nav2_core
+  pluginlib
+  angles
+  eigen3_cmake_module
+)
+
+# Hybrid plugin
+add_library(${library_name} SHARED
+  src/smac_planner_hybrid.cpp
+  src/a_star.cpp
+  src/collision_checker.cpp
+  src/smoother.cpp
+  src/analytic_expansion.cpp
+  src/node_hybrid.cpp
+  src/node_lattice.cpp
+  src/costmap_downsampler.cpp
+  src/node_2d.cpp
+  src/node_basic.cpp
+)
+
+target_link_libraries(${library_name} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES}  OpenMP::OpenMP_CXX)
+target_include_directories(${library_name} PUBLIC ${Eigen3_INCLUDE_DIRS})
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+# 2D plugin
+add_library(${library_name}_2d SHARED
+  src/smac_planner_2d.cpp
+  src/a_star.cpp
+  src/smoother.cpp
+  src/collision_checker.cpp
+  src/analytic_expansion.cpp
+  src/node_hybrid.cpp
+  src/node_lattice.cpp
+  src/costmap_downsampler.cpp
+  src/node_2d.cpp
+  src/node_basic.cpp
+)
+
+target_link_libraries(${library_name}_2d ${OMPL_LIBRARIES})
+target_include_directories(${library_name}_2d PUBLIC ${Eigen3_INCLUDE_DIRS})
+
+ament_target_dependencies(${library_name}_2d
+  ${dependencies}
+)
+
+# Lattice plugin
+add_library(${library_name}_lattice SHARED
+  src/smac_planner_lattice.cpp
+  src/a_star.cpp
+  src/smoother.cpp
+  src/collision_checker.cpp
+  src/analytic_expansion.cpp
+  src/node_hybrid.cpp
+  src/node_lattice.cpp
+  src/costmap_downsampler.cpp
+  src/node_2d.cpp
+  src/node_basic.cpp
+)
+
+target_link_libraries(${library_name}_lattice ${OMPL_LIBRARIES})
+target_include_directories(${library_name}_lattice PUBLIC ${Eigen3_INCLUDE_DIRS})
+
+ament_target_dependencies(${library_name}_lattice
+  ${dependencies}
+)
+
+pluginlib_export_plugin_description_file(nav2_core smac_plugin_hybrid.xml)
+pluginlib_export_plugin_description_file(nav2_core smac_plugin_2d.xml)
+pluginlib_export_plugin_description_file(nav2_core smac_plugin_lattice.xml)
+
+install(TARGETS ${library_name} ${library_name}_2d ${library_name}_lattice
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+install(DIRECTORY lattice_primitives/sample_primitives DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(AMENT_LINT_AUTO_FILE_EXCLUDE include/nav2_smac_planner/thirdparty/robin_hood.h)
+  ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(test)
+endif()
+
+ament_export_include_directories(include ${OMPL_INCLUDE_DIRS})
+ament_export_libraries(${library_name} ${library_name}_2d ${library_name}_lattice)
+ament_export_dependencies(${dependencies})
+ament_package()

navigation2/nav2_smac_planner/README.md

@@ -0,0 +1,232 @@
+# Smac Planner
+
+The SmacPlanner is a plugin for the Nav2 Planner server. It includes currently 3 distinct plugins:
+- `SmacPlannerHybrid`: a highly optimized fully reconfigurable Hybrid-A* implementation supporting Dubin and Reeds-Shepp models (legged, ackermann and car models).
+ - `SmacPlannerLattice`: a highly optimized fully reconfigurable State Lattice implementation supporting configurable minimum control sets, with provided control sets for Ackermann, Legged, Differential and Omnidirectional models.
+- `SmacPlanner2D`: a highly optimized fully reconfigurable grid-based A* implementation supporting 8-connected neighborhood models.
+
+It also introduces the following basic building blocks:
+- `CostmapDownsampler`: A library to take in a costmap object and downsample it to another resolution.
+- `AStar`: A generic and highly optimized A* template library used by the planning plugins to search. Additional template for  planning also could be made available using it.
+- `CollisionChecker`: Collision check based on a robot's radius or footprint.
+- `Smoother`: A simple path smoother to smooth out 2D, Hybrid-A\*, and State Lattice paths.
+
+We have users reporting using this on:
+- Delivery robots
+- Industrial robots
+- Vertical farming
+- Solar farms
+
+See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smac-planner.html) for additional parameter descriptions.
+
+## Introduction
+
+The `nav2_smac_planner` package contains an optimized templated A* search algorithm used to create multiple A\*-based planners for multiple types of robot platforms. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). We support **circular** differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support **legged, cars, car-like, and ackermann vehicles** using the `SmacPlannerHybrid` plugin which implements a Hybrid-A\* planner.  We support **non-circular, arbitrary shaped, any model vehicles** using the `SmacPlannerLattice` plugin which implements a State Lattice planner. It contains control sets and generators for ackermann, legged, differential drive and omnidirectional vehicles, but you may provide your own for another robot type or to have different planning behaviors. The last two plugins are also useful for curvature constrained or kinematically feasible planning, like when planning robot at high speeds to make sure they don't flip over or otherwise skid out of control. It is also applicable to non-round robots (such as large rectangular or arbitrary shaped robots of differential/omnidirectional drivetrains) that need pose-based collision checking.
+
+The `SmacPlannerHybrid` implements the Hybrid-A* planner as proposed in [Practical Search Techniques in Path Planning for Autonomous Driving](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf), with modifications to the heuristic, traversal functions to increase path quality without needing expensive optimization-based smoothing.
+
+The `SmacPlannerLattice` implements the State Lattice planner. While we do not implement it precisely the same way as [Optimal, Smooth, Nonholonomic MobileRobot Motion Planning in State Lattices](https://www.ri.cmu.edu/pub_files/pub4/pivtoraiko_mihail_2007_1/pivtoraiko_mihail_2007_1.pdf) (with control sets found using [Generating Near Minimal Spanning Control Sets for Constrained Motion Planning in Discrete State Spaces](https://www.ri.cmu.edu/pub_files/pub4/pivtoraiko_mihail_2005_1/pivtoraiko_mihail_2005_1.pdf)), it is sufficiently similar it may be used as a good reference. Additional optimizations for on-approach analytic expansions and improved heuristic functions were used, largely matching those of Hybrid-A\* to allow them to share these optimized implementations to drive search towards the goal, faster.
+
+In summary...
+
+The `SmacPlannerHybrid` is designed to work with:
+- Ackermann, car, and car-like robots
+- High speed or curvature constrained robots (as to not flip over, skid, or dump load at high speeds)
+- Arbitrary shaped, non-circular differential or omnidirectional robots requiring kinematically feasible planning with SE2 collision checking
+- Legged robots
+
+The `SmacPlannerLattice` is designed to work with:
+- Arbitrary shaped, non-circular robots requiring kinematically feasible planning with SE2 collision checking using the full capabilities of the drivetrain
+- Flexibility to use other robot model types or with provided non-circular differential, ackermann, and omni support
+
+The `SmacPlanner2D` is designed to work with:
+- Circular, differential or omnidirectional robots
+- Relatively small robots with respect to environment size (e.g. RC car in a hallway or large robot in a convention center) that can be approximated by circular footprint.
+
+## Features 
+
+We further improve on Hybrid-A\* in the following ways:
+- Remove need for upsampling by searching with 10x smaller motion primitives (same as their upsampling ratio).
+- Multi-resolution search allowing planning to occur at a coarser resolution for wider spaces (O(N^2) faster).
+- Cost-aware penalty functions in search resulting in far smoother plans (further reducing requirement to smooth).
+- Gradient-descent, basic but fast smoother
+- Faster planning than original paper by highly optimizing the template A\* algorithm.
+- Faster planning via custom precomputed heuristic, motion primitive, and other functions.
+- Automatically adjusted search motion model sizes by motion model, costmap resolution, and bin sizing.
+- Closest path on approach within tolerance if exact path cannot be found or in invalid space.
+- Multi-model hybrid searching including Dubin and Reeds-Shepp models. More models may be trivially added.
+- High unit and integration test coverage, doxygen documentation.
+- Uses modern C++14 language features and individual components are easily reusable.
+- Speed optimizations: no data structure graph lookups in main loop, near-zero copy main loop, dynamically generated graph and dynamic programming-based obstacle heuristic, optional recomputation of heuristics for subsequent planning requests of the same goal, etc.
+- Templated Nodes and A\* implementation to support additional robot extensions.
+- Selective re-evaluation of the obstacle heuristic per goal/map or each iteration, which can speed up subsequent replanning 20x or more.
+
+Most of these features (multi-resolution, models, smoother, etc) are also available in the `SmacPlanner2D` and `SmacPlannerLattice` plugins.
+
+The 2D A\* implementation also does not have any of the weird artifacts introduced by the gradient wavefront-based 2D A\* implementation in the NavFn Planner. While this 2D A\* planner is slightly slower, I believe it's well worth the increased quality in paths.
+
+Note: In prior releases, a CG smoother largely implementing the original Hybrid-A\* paper's. However, this smoother failed to consistently provide useful results, took too much compute time, and was deprecated. While smoothing a path 95% of the time seems like a "good" solution, we need something far more reliable for practical use. Since we are working with mobile robots and not autonomous cars at 60 mph, we can take some different liberties in smoothing knowing that our local trajectory planners are pretty smart. If you are looking for it, it now lives in the new Smoothing Server as the Cost-aware smoother. This smoother has been replaced by a simpler optimization inspired solution which is faster, far more consistent, and simpler to understand. While this smoother is **not** cost-aware, we have added cost-aware penalty functions in the planners themselves to push the plans away from high-cost spaces and we do check for validity of smoothed sections to ensure feasibility. It will through terminate when paths become in collision with the environment. If you would like to use this smoother, however, it is available in the smoother server, though it will take some additional compute time.
+
+## Metrics
+
+The original Hybrid-A\* implementation boasted planning times of 50-300ms for planning across 102,400 cell maps with 72 angular bins. We see much faster results in our evaluations:
+
+- **2-20ms** for planning across 147,456 (1.4x larger) cell maps with 72 angular bins.
+- **30-200ms** for planning across 344,128 (3.3x larger) cell map with 72 angular bins.
+
+An example of the 3 planners can be seen below, planning a roughly 75 m path.
+- 2D A* computed the path in 243ms (Panel 1)
+- Hybrid-A* computed the path in 144ms (Panel 2)
+- State Lattice computed the path in 113ms (Panel 3)
+- For reference: NavFn compute the path in 146ms, including some nasty path discontinuity artifacts
+
+![alt text](test/3planners.png)
+
+## Design
+
+The basic design centralizes a templated A\* implementation that handles the search of a graph of nodes. The implementation is templated by the nodes, `NodeT`, which contain the methods needed to compute the hueristics, travel costs, and search neighborhoods. The outcome of this design is then a standard A\* implementation that can be used to traverse any type of graph as long as a node template can be created for it.
+
+We provide 3 nodes by default currently. The 2D node template (`Node2D`) which does 2D grid-search with either 4 or 8-connected neighborhoods. We also provide a Hybrid A\* node template (`NodeHybrid`) which does SE2 (X, Y, theta) search and collision checking on Dubin or Reeds-Shepp motion models. We also provide the Lattice (`NodeLattice`) node for state lattice planning making use of the wider range of velocity options available to differential and omnidirectional robots. Additional templates could be easily made and included for 3D grid search and non-grid base searching like routing.
+
+In the ROS2 facing plugin, we take in the global goals and pre-process the data to feed into the templated A\* used. This includes processing any requests to downsample the costmap to another resolution to speed up search and smoothing the resulting A\* path (not available for State Lattice due to the lattices generated are dependent on costmap resolution). For the `SmacPlannerHybrid` and `SmacPlannerLattice` plugins, the path is promised to be kinematically feasible due to the kinematically valid models used in branching search. The 2D A\* is also promised to be feasible for differential and omni-directional robots.
+
+We isolated the A\*, costmap downsampler, smoother, and Node template objects from ROS2 to allow them to be easily testable independently of ROS or the planner. The only place ROS is used is in the planner plugins themselves. 
+
+## Parameters
+
+See inline description of parameters in the `SmacPlanner`. This includes comments as specific parameters apply to `SmacPlanner2D` and `SmacPlanner` in place.
+
+```
+planner_server:
+  ros__parameters:
+    planner_plugins: ["GridBased"]
+    use_sim_time: True
+
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      tolerance: 0.5                      # tolerance for planning if unable to reach exact pose, in meters
+      downsample_costmap: false           # whether or not to downsample the map
+      downsampling_factor: 1              # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
+      allow_unknown: false                # allow traveling in unknown space
+      max_iterations: 1000000             # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
+      max_on_approach_iterations: 1000    # maximum number of iterations to attempt to reach goal once in tolerance
+      max_planning_time: 3.5              # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
+      motion_model_for_search: "DUBIN"    # For Hybrid Dubin, Redds-Shepp
+      cost_travel_multiplier: 2.0         # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
+      angle_quantization_bins: 64         # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
+      analytic_expansion_ratio: 3.5       # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach.
+      analytic_expansion_max_length: 3.0    # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius
+      minimum_turning_radius: 0.40        # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle
+      reverse_penalty: 2.1                # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
+      change_penalty: 0.0                 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0
+      non_straight_penalty: 1.20          # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1
+      cost_penalty: 2.0                   # For Hybrid nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
+      retrospective_penalty: 0.025        # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier along the path. Saves search time since earlier nodes are not expanded until it is necessary. Must be >= 0.0 and <= 1.0
+      rotation_penalty: 5.0               # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings.
+      lookup_table_size: 20.0               # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters.
+      cache_obstacle_heuristic: True      # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.  
+      allow_reverse_expansion: False      # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step.   
+      smooth_path: True                   # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes.
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.3
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: true               # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further
+```
+
+## Topics
+
+| Topic           | Type              |
+|-----------------|-------------------|
+| unsmoothed_path | nav_msgs/Path     |
+
+
+## Install
+
+```
+sudo apt-get install ros-<ros2-distro>-nav2-smac-planner
+```
+
+## Etc (Important Side Notes)
+
+### Potential Fields
+
+Many users and default navigation configuration files I find are really missing the point of the inflation layer. While it's true that you can simply inflate a small radius around the walls, the _true_ value of the inflation layer is creating a consistent potential field around the entire map. 
+
+Some of the most popular tuning guides for Navigation / Nav2 even [call this out specifically](https://arxiv.org/pdf/1706.09068.pdf) that there's substantial benefit to creating a gentle potential field across the width of the map - after inscribed costs are applied - yet very few users do this. 
+
+This habit actually results in paths produced by NavFn, Global Planner, and now SmacPlanner to be somewhat suboptimal. They really want to look for a smooth potential field rather than wide open 0-cost spaces in order to stay in the middle of spaces and deal with close-by moving obstacles better.
+
+So it is my recommendation in using this package, as well as all other cost-aware search planners available in ROS, to increase your inflation layer cost scale in order to adequately produce a smooth potential across the entire map. For very large open spaces, its fine to have 0-cost areas in the middle, but for halls, aisles, and similar; **please create a smooth potential to provide the best performance**. 
+
+### Hybrid-A* and State Lattice Turning Radius'
+
+A very reasonable and logical assumption would be to set the `minimum_turning_radius` to the kinematic limits of your vehicle. For an ackermann car, that's a physical quantity; while for differential or omni robots, its a bit of a dance around what kind of turns you'd like your robot to be able to make. Obviously setting this to something insanely small (like 20 cm) means you have alot of options, but also probably means the raw output plans won't be very straight and smooth when you have 2+ meter wide aisles to work in.
+
+I assert that you should also consider the environment you operate within when setting this. While you should **absolutely not** set this to be any smaller than the actual limits of your vehicle, there are some useful side effects of increasing this value in practical use. If you work in an area wider than the turning circle of your robot, you have some options that will ultimately improve the performance of the planner (in terms of CPU and compute time) as well as generate paths that are more "smooth" directly out of the planner -- not requiring any explicit path smoothing. 
+
+By default, `0.4m` is the setting which I think is "reasonable" for the smaller scale industrial grade robots (think Simbe, the small Fetch, or Locus robots) resulting in faster plans and less "wobbly" motions that do not require post-smoothing -- further improving CPU performance. I selected `0.4m` as a trade off between practical robots mentioned above and hobbyist users with a tiny-little-turtlebot-3 which might still need to navigate around some smaller cavities.
+
+### Costmap Resolutions
+
+We provide for the Hybrid-A\*, State Lattice, and 2D A\* implementations a costmap downsampler option. This can be **incredible** beneficial when planning very long paths in larger spaces. The motion models for SE2 planning and neighborhood search in 2D planning is proportional to the costmap resolution. By downsampling it, you can N^2 reduce the number of expansions required to achieve a particular goal. However, the lower the resolution, the larger small obstacles appear and you won't be able to get super close to obstacles. This is a trade-off to make and test. Some numbers I've seen are 2-4x drops in planning CPU time for a 2-3x downsample rate. For long and complex paths, I was able to get it << 100ms at only a 2x downsample rate from a plan that otherwise took upward of 400ms.
+
+I recommend users using a 5cm resolution costmap and playing with the different values of downsampling rate until they achieve what they think is optimal performance (lowest number of expansions vs. necessity to achieve fine goal poses). Then, I would recommend to change the global costmap resolution to this new value. That way you don't own the compute of downsampling and maintaining a higher-resolution costmap that isn't used.
+
+Remember, the global costmap is **only** there to provide an environment for the planner to work in. It is not there for human-viewing even if a more fine resolution costmap is more human "pleasing". If you use multiple planners in the planner server, then you will want to use the highest resolution for the most needed planner and then use the downsampler to downsample to the Hybrid-A* resolution. 
+
+### Penalty Function Tuning
+
+The penalty function defaults are tuned for all of the planners based on a 5cm costmap. While some change in this should not largely impact default behavior, it may be good to tune for your specific application and needs. The default values were tuned to have decent out of the box behavior for a large number of platforms and resolutions. In most situations, you should not need to play with them. 
+
+**However**, due to the nature of the State Lattice planner being able to use any number of custom generated minimum control sets, this planner may require more tuning to get good behavior. The defaults for State Lattice were generated using the 5cm Ackermann files you can find in this package as initial examples. After a change in formulation for the Hybrid-A* planner, the default of change penalty off seems to produce good results, but please tune to your application need and run-time speed requirements.
+
+When tuning, the "reasonable" range for each penalty is listed below. While you may obviously tune outside of these ranges, I've found that they offer a good trade-off and outside of these ranges behaviors get suboptimal quickly.
+- Cost: 1.7 - 6.0
+- Non-Straight: 1.0 - 1.3
+- Change: 0.0 - 0.3
+- Reverse: 1.3 - 5.0
+
+Note that change penalty must be greater than 0.0. The non-straight, reverse, and cost penalties must be greater than 1.0, strictly.
+
+### No path found for clearly valid goals or long compute times
+
+Before addressing the section below, make sure you have an appropriately set max iterations parameter. If you have a 1 km2 sized warehouse, clearly 5000 expansions will be insufficient. Try increasing this value if you're unable to achieve goals or disable it with the `-1` value to see if you are now able to plan within a reasonable time period. If you still have issues, there is a secondary effect which could be happening that is good to be aware of.
+
+In maps with small gaps or holes, you may see an issue planning to certain regions. If there are gaps small enough to be untraversible yet large enough that inflation doesn't close it up with inflated costs, then it is recommended to lightly touch up the map or increase your inflation to remove those spaces from non-lethal space. 
+
+Seeing the figures below, you'll see an attempt to plan into a "U" shaped region across the map. The first figure shows the small gap in the map (from an imperfect SLAM session) which is nearly traversible, but not quite. From the starting location, that gap yeilds the shortest path to the goal, so the heuristics will attempt to route the paths in that direction. However, it is not possible to actually pass with a kinematically valid path with the footprint set. As a result, the planner expands all of its maximum 1,000,000 iterations attempting to fit through it (visualized in red). If an infinite number of iterations were allowed, eventually a valid path would be found, but might take significant time.
+
+By simply increasing the footprint (a bit hackier, the best solution is to edit the map to make this area impassible), then that gap is now properly blocked as un-navigable. In the second figure, you can see that the heuristics influence the expansion down a navigable route and is able to find a path in less than 10,000 iterations (or about 110ms). It is easy now!
+
+As such, it is recommended if you have sparse SLAM maps, gaps or holes in your map, that you lightly post-process them to fill those gaps or increasing your footprint's padding or radius to make these areas invalid. Without it, it might waste expansions on this small corridor that: A) you dont want your robot actually using B) probably isnt actually valid and a SLAM artifact and C) if there's a more open space, you'd rather it use that. 
+
+![](media/A.png)
+![](media/B.png)
+
+One interesting thing to note from the second figure is that you see a number of expansions in open space. This is due to travel / heuristic values being so similar, tuning values of the penalty weights can have a decent impact there. The defaults are set as a good middle ground between large open spaces and confined aisles (environment specific tuning could be done to reduce the number of expansions for a specific map, speeding up the planner). The planner actually runs substantially faster the more confined the areas of search / environments are -- but still plenty fast for even wide open areas!
+
+Sometimes visualizing the expansions is very useful to debug potential concerns (why does this goal take longer to compute, why can't I find a path, etc), should you on rare occasion run into an issue. The following snippet is what I used to visualize the expansion in the images above which may help you in future endevours.
+
+``` cpp
+// In createPath()
+static auto node = std::make_shared<rclcpp::Node>("test");
+static auto pub = node->create_publisher<geometry_msgs::msg::PoseArray>("expansions", 1);
+geometry_msgs::msg::PoseArray msg;
+geometry_msgs::msg::Pose msg_pose;
+msg.header.stamp = node->now();
+msg.header.frame_id = "map";
+
+...
+
+// Each time we expand a new node 
+msg_pose.position.x = _costmap->getOriginX() + (current_node->pose.x * _costmap->getResolution());
+msg_pose.position.y = _costmap->getOriginY() + (current_node->pose.y * _costmap->getResolution());
+msg.poses.push_back(msg_pose);
+
+... 
+
+// On backtrace or failure
+pub->publish(msg);
+```

navigation2/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -0,0 +1,265 @@
+// Copyright (c) 2020, Samsung Research America
+// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__A_STAR_HPP_
+#define NAV2_SMAC_PLANNER__A_STAR_HPP_
+
+#include <vector>
+#include <iostream>
+#include <unordered_map>
+#include <memory>
+#include <queue>
+#include <utility>
+#include "Eigen/Core"
+
+#include "nav2_costmap_2d/costmap_2d.hpp"
+
+#include "nav2_smac_planner/thirdparty/robin_hood.h"
+#include "nav2_smac_planner/analytic_expansion.hpp"
+#include "nav2_smac_planner/node_2d.hpp"
+#include "nav2_smac_planner/node_hybrid.hpp"
+#include "nav2_smac_planner/node_lattice.hpp"
+#include "nav2_smac_planner/node_basic.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/constants.hpp"
+
+namespace nav2_smac_planner
+{
+
+/**
+ * @class nav2_smac_planner::AStarAlgorithm
+ * @brief An A* implementation for planning in a costmap. Templated based on the Node type.
+ */
+template<typename NodeT>
+class AStarAlgorithm
+{
+public:
+  typedef NodeT * NodePtr;
+  typedef robin_hood::unordered_node_map<unsigned int, NodeT> Graph;
+  typedef std::vector<NodePtr> NodeVector;
+  typedef std::pair<float, NodeBasic<NodeT>> NodeElement;
+  typedef typename NodeT::Coordinates Coordinates;
+  typedef typename NodeT::CoordinateVector CoordinateVector;
+  typedef typename NodeVector::iterator NeighborIterator;
+  typedef std::function<bool (const unsigned int &, NodeT * &)> NodeGetter;
+
+  /**
+   * @struct nav2_smac_planner::NodeComparator
+   * @brief Node comparison for priority queue sorting
+   */
+  struct NodeComparator
+  {
+    bool operator()(const NodeElement & a, const NodeElement & b) const
+    {
+      return a.first > b.first;
+    }
+  };
+
+  typedef std::priority_queue<NodeElement, std::vector<NodeElement>, NodeComparator> NodeQueue;
+
+  /**
+   * @brief A constructor for nav2_smac_planner::AStarAlgorithm
+   */
+  explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info);
+
+  /**
+   * @brief A destructor for nav2_smac_planner::AStarAlgorithm
+   */
+  ~AStarAlgorithm();
+
+  /**
+   * @brief Initialization of the planner with defaults
+   * @param allow_unknown Allow search in unknown space, good for navigation while mapping
+   * @param max_iterations Maximum number of iterations to use while expanding search
+   * @param max_on_approach_iterations Maximum number of iterations before returning a valid
+   * path once within thresholds to refine path
+   * comes at more compute time but smoother paths.
+   * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns
+   * false after this timeout
+   */
+  void initialize(
+    const bool & allow_unknown,
+    int & max_iterations,
+    const int & max_on_approach_iterations,
+    const double & max_planning_time,
+    const float & lookup_table_size,
+    const unsigned int & dim_3_size);
+
+  /**
+   * @brief Creating path from given costmap, start, and goal
+   * @param path Reference to a vector of indicies of generated path
+   * @param num_iterations Reference to number of iterations to create plan
+   * @param tolerance Reference to tolerance in costmap nodes
+   * @return if plan was successful
+   */
+  bool createPath(CoordinateVector & path, int & num_iterations, const float & tolerance);
+
+  /**
+   * @brief Sets the collision checker to use
+   * @param collision_checker Collision checker to use for checking state validity
+   */
+  void setCollisionChecker(GridCollisionChecker * collision_checker);
+
+  /**
+   * @brief Set the goal for planning, as a node index
+   * @param mx The node X index of the goal
+   * @param my The node Y index of the goal
+   * @param dim_3 The node dim_3 index of the goal
+   */
+  void setGoal(
+    const unsigned int & mx,
+    const unsigned int & my,
+    const unsigned int & dim_3);
+
+  /**
+   * @brief Set the starting pose for planning, as a node index
+   * @param mx The node X index of the goal
+   * @param my The node Y index of the goal
+   * @param dim_3 The node dim_3 index of the goal
+   */
+  void setStart(
+    const unsigned int & mx,
+    const unsigned int & my,
+    const unsigned int & dim_3);
+
+  /**
+   * @brief Get maximum number of iterations to plan
+   * @return Reference to Maximum iterations parameter
+   */
+  int & getMaxIterations();
+
+  /**
+   * @brief Get pointer reference to starting node
+   * @return Node pointer reference to starting node
+   */
+  NodePtr & getStart();
+
+  /**
+   * @brief Get pointer reference to goal node
+   * @return Node pointer reference to goal node
+   */
+  NodePtr & getGoal();
+
+  /**
+   * @brief Get maximum number of on-approach iterations after within threshold
+   * @return Reference to Maximum on-appraoch iterations parameter
+   */
+  int & getOnApproachMaxIterations();
+
+  /**
+   * @brief Get tolerance, in node nodes
+   * @return Reference to tolerance parameter
+   */
+  float & getToleranceHeuristic();
+
+  /**
+   * @brief Get size of graph in X
+   * @return Size in X
+   */
+  unsigned int & getSizeX();
+
+  /**
+   * @brief Get size of graph in Y
+   * @return Size in Y
+   */
+  unsigned int & getSizeY();
+
+  /**
+   * @brief Get number of angle quantization bins (SE2) or Z coordinate  (XYZ)
+   * @return Number of angle bins / Z dimension
+   */
+  unsigned int & getSizeDim3();
+
+protected:
+  /**
+   * @brief Get pointer to next goal in open set
+   * @return Node pointer reference to next heuristically scored node
+   */
+  inline NodePtr getNextNode();
+
+  /**
+   * @brief Add a node to the open set
+   * @param cost The cost to sort into the open set of the node
+   * @param node Node pointer reference to add to open set
+   */
+  inline void addNode(const float & cost, NodePtr & node);
+
+  /**
+   * @brief Adds node to graph
+   * @param index Node index to add
+   */
+  inline NodePtr addToGraph(const unsigned int & index);
+
+  /**
+   * @brief Check if this node is the goal node
+   * @param node Node pointer to check if its the goal node
+   * @return if node is goal
+   */
+  inline bool isGoal(NodePtr & node);
+
+  /**
+   * @brief Get cost of heuristic of node
+   * @param node Node pointer to get heuristic for
+   * @return Heuristic cost for node
+   */
+  inline float getHeuristicCost(const NodePtr & node);
+
+  /**
+   * @brief Check if inputs to planner are valid
+   * @return Are valid
+   */
+  inline bool areInputsValid();
+
+  /**
+   * @brief Clear hueristic queue of nodes to search
+   */
+  inline void clearQueue();
+
+  /**
+   * @brief Clear graph of nodes searched
+   */
+  inline void clearGraph();
+
+  int _timing_interval = 5000;
+
+  bool _traverse_unknown;
+  bool _is_initialized;
+  int _max_iterations;
+  int _max_on_approach_iterations;
+  double _max_planning_time;
+  float _tolerance;
+  unsigned int _x_size;
+  unsigned int _y_size;
+  unsigned int _dim3_size;
+  SearchInfo _search_info;
+
+  Coordinates _goal_coordinates;
+  NodePtr _start;
+  NodePtr _goal;
+
+  Graph _graph;
+  NodeQueue _queue;
+
+  MotionModel _motion_model;
+  NodeHeuristicPair _best_heuristic_node;
+
+  GridCollisionChecker * _collision_checker;
+  nav2_costmap_2d::Costmap2D * _costmap;
+  std::unique_ptr<AnalyticExpansion<NodeT>> _expander;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__A_STAR_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp

@@ -0,0 +1,133 @@
+// Copyright (c) 2021, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
+#define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
+
+#include <string>
+#include <vector>
+#include <list>
+#include <memory>
+
+#include "nav2_smac_planner/node_2d.hpp"
+#include "nav2_smac_planner/node_hybrid.hpp"
+#include "nav2_smac_planner/node_lattice.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/constants.hpp"
+
+namespace nav2_smac_planner
+{
+
+template<typename NodeT>
+class AnalyticExpansion
+{
+public:
+  typedef NodeT * NodePtr;
+  typedef typename NodeT::Coordinates Coordinates;
+  typedef std::function<bool (const unsigned int &, NodeT * &)> NodeGetter;
+
+  /**
+   * @struct nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes
+   * @brief Analytic expansion nodes and associated metadata
+   */
+  struct AnalyticExpansionNode
+  {
+    AnalyticExpansionNode(
+      NodePtr & node_in,
+      Coordinates & initial_coords_in,
+      Coordinates & proposed_coords_in)
+    : node(node_in),
+      initial_coords(initial_coords_in),
+      proposed_coords(proposed_coords_in)
+    {
+    }
+
+    NodePtr node;
+    Coordinates initial_coords;
+    Coordinates proposed_coords;
+  };
+
+  typedef std::vector<AnalyticExpansionNode> AnalyticExpansionNodes;
+
+  /**
+   * @brief Constructor for analytic expansion object
+   */
+  AnalyticExpansion(
+    const MotionModel & motion_model,
+    const SearchInfo & search_info,
+    const bool & traverse_unknown,
+    const unsigned int & dim_3_size);
+
+  /**
+   * @brief Sets the collision checker and costmap to use in expansion validation
+   * @param collision_checker Collision checker to use
+   */
+  void setCollisionChecker(GridCollisionChecker * collision_checker);
+
+  /**
+   * @brief Attempt an analytic path completion
+   * @param node The node to start the analytic path from
+   * @param goal The goal node to plan to
+   * @param getter Gets a node at a set of coordinates
+   * @param iterations Iterations to run over
+   * @param best_cost Best heuristic cost to propertionally expand more closer to the goal
+   * @return Node pointer reference to goal node if successful, else
+   * return nullptr
+   */
+  NodePtr tryAnalyticExpansion(
+    const NodePtr & current_node,
+    const NodePtr & goal_node,
+    const NodeGetter & getter, int & iterations, int & best_cost);
+
+  /**
+   * @brief Perform an analytic path expansion to the goal
+   * @param node The node to start the analytic path from
+   * @param goal The goal node to plan to
+   * @param getter The function object that gets valid nodes from the graph
+   * @return A set of analytically expanded nodes to the goal from current node, if possible
+   */
+  AnalyticExpansionNodes getAnalyticPath(
+    const NodePtr & node, const NodePtr & goal,
+    const NodeGetter & getter);
+
+  /**
+   * @brief Takes final analytic expansion and appends to current expanded node
+   * @param node The node to start the analytic path from
+   * @param goal The goal node to plan to
+   * @param expanded_nodes Expanded nodes to append to end of current search path
+   * @return Node pointer to goal node if successful, else return nullptr
+   */
+  NodePtr setAnalyticPath(
+    const NodePtr & node, const NodePtr & goal,
+    const AnalyticExpansionNodes & expanded_nodes);
+
+  /**
+   * @brief Takes an expanded nodes to clean up, if necessary, of any state
+   * information that may be poluting it from a prior search iteration
+   * @param expanded_nodes Expanded node to clean up from search
+   */
+  void cleanNode(const NodePtr & nodes);
+
+protected:
+  MotionModel _motion_model;
+  SearchInfo _search_info;
+  bool _traverse_unknown;
+  unsigned int _dim_3_size;
+  GridCollisionChecker * _collision_checker;
+  std::list<std::unique_ptr<NodeT>> _detached_nodes;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp

@@ -0,0 +1,129 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+#include <vector>
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
+#include "nav2_smac_planner/constants.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+#ifndef NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_
+#define NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_
+
+namespace nav2_smac_planner
+{
+
+/**
+ * @class nav2_smac_planner::GridCollisionChecker
+ * @brief A costmap grid collision checker
+ */
+class GridCollisionChecker
+  : public nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
+{
+public:
+  /**
+   * @brief A constructor for nav2_smac_planner::GridCollisionChecker
+   * for use when regular bin intervals are appropriate
+   * @param costmap The costmap to collision check against
+   * @param num_quantizations The number of quantizations to precompute footprint
+   * @param node Node to extract clock and logger from
+   * orientations for to speed up collision checking
+   */
+  GridCollisionChecker(
+    nav2_costmap_2d::Costmap2D * costmap,
+    unsigned int num_quantizations,
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node);
+
+  /**
+   * @brief A constructor for nav2_smac_planner::GridCollisionChecker
+   * for use when irregular bin intervals are appropriate
+   * @param costmap The costmap to collision check against
+   * @param angles The vector of possible angle bins to precompute for
+   * orientations for to speed up collision checking, in radians
+   */
+  // GridCollisionChecker(
+  //   nav2_costmap_2d::Costmap2D * costmap,
+  //   std::vector<float> & angles);
+
+  /**
+   * @brief Set the footprint to use with collision checker
+   * @param footprint The footprint to collision check against
+   * @param radius Whether or not the footprint is a circle and use radius collision checking
+   */
+  void setFootprint(
+    const nav2_costmap_2d::Footprint & footprint,
+    const bool & radius,
+    const double & possible_inscribed_cost);
+
+  /**
+   * @brief Check if in collision with costmap and footprint at pose
+   * @param x X coordinate of pose to check against
+   * @param y Y coordinate of pose to check against
+   * @param theta Angle bin number of pose to check against (NOT radians)
+   * @param traverse_unknown Whether or not to traverse in unknown space
+   * @return boolean if in collision or not.
+   */
+  bool inCollision(
+    const float & x,
+    const float & y,
+    const float & theta,
+    const bool & traverse_unknown);
+
+  /**
+   * @brief Check if in collision with costmap and footprint at pose
+   * @param i Index to search collision status of
+   * @param traverse_unknown Whether or not to traverse in unknown space
+   * @return boolean if in collision or not.
+   */
+  bool inCollision(
+    const unsigned int & i,
+    const bool & traverse_unknown);
+
+  /**
+   * @brief Get cost at footprint pose in costmap
+   * @return the cost at the pose in costmap
+   */
+  float getCost();
+
+  /**
+   * @brief Get the angles of the precomputed footprint orientations
+   * @return the ordered vector of angles corresponding to footprints
+   */
+  std::vector<float> & getPrecomputedAngles()
+  {
+    return angles_;
+  }
+
+private:
+  /**
+   * @brief Check if value outside the range
+   * @param min Minimum value of the range
+   * @param max Maximum value of the range
+   * @param value the value to check if it is within the range
+   * @return boolean if in range or not
+   */
+  bool outsideRange(const unsigned int & max, const float & value);
+
+protected:
+  std::vector<nav2_costmap_2d::Footprint> oriented_footprints_;
+  nav2_costmap_2d::Footprint unoriented_footprint_;
+  double footprint_cost_;
+  bool footprint_is_radius_;
+  std::vector<float> angles_;
+  double possible_inscribed_cost_{-1};
+  rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")};
+  rclcpp::Clock::SharedPtr clock_;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/constants.hpp

@@ -0,0 +1,70 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__CONSTANTS_HPP_
+#define NAV2_SMAC_PLANNER__CONSTANTS_HPP_
+
+#include <string>
+
+namespace nav2_smac_planner
+{
+enum class MotionModel
+{
+  UNKNOWN = 0,
+  TWOD = 1,
+  DUBIN = 2,
+  REEDS_SHEPP = 3,
+  STATE_LATTICE = 4,
+};
+
+inline std::string toString(const MotionModel & n)
+{
+  switch (n) {
+    case MotionModel::TWOD:
+      return "2D";
+    case MotionModel::DUBIN:
+      return "Dubin";
+    case MotionModel::REEDS_SHEPP:
+      return "Reeds-Shepp";
+    case MotionModel::STATE_LATTICE:
+      return "State Lattice";
+    default:
+      return "Unknown";
+  }
+}
+
+inline MotionModel fromString(const std::string & n)
+{
+  if (n == "2D") {
+    return MotionModel::TWOD;
+  } else if (n == "DUBIN") {
+    return MotionModel::DUBIN;
+  } else if (n == "REEDS_SHEPP") {
+    return MotionModel::REEDS_SHEPP;
+  } else if (n == "STATE_LATTICE") {
+    return MotionModel::STATE_LATTICE;
+  } else {
+    return MotionModel::UNKNOWN;
+  }
+}
+
+const float UNKNOWN = 255.0;
+const float OCCUPIED = 254.0;
+const float INSCRIBED = 253.0;
+const float MAX_NON_OBSTACLE = 252.0;
+const float FREE = 0;
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__CONSTANTS_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp

@@ -0,0 +1,118 @@
+// Copyright (c) 2020, Carlos Luis
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
+#define NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
+
+#include <algorithm>
+#include <string>
+#include <memory>
+
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_smac_planner/constants.hpp"
+
+namespace nav2_smac_planner
+{
+
+/**
+ * @class nav2_smac_planner::CostmapDownsampler
+ * @brief A costmap downsampler for more efficient path planning
+ */
+class CostmapDownsampler
+{
+public:
+  /**
+   * @brief A constructor for CostmapDownsampler
+   */
+  CostmapDownsampler();
+
+  /**
+   * @brief A destructor for CostmapDownsampler
+   */
+  ~CostmapDownsampler();
+
+  /**
+   * @brief Configure the downsampled costmap object and the ROS publisher
+   * @param node Lifecycle node pointer
+   * @param global_frame The ID of the global frame used by the costmap
+   * @param topic_name The name of the topic to publish the downsampled costmap
+   * @param costmap The costmap we want to downsample
+   * @param downsampling_factor Multiplier for the costmap resolution
+   * @param use_min_cost_neighbor If true, min function is used instead of max for downsampling
+   */
+  void on_configure(
+    const nav2_util::LifecycleNode::WeakPtr & node,
+    const std::string & global_frame,
+    const std::string & topic_name,
+    nav2_costmap_2d::Costmap2D * const costmap,
+    const unsigned int & downsampling_factor,
+    const bool & use_min_cost_neighbor = false);
+
+  /**
+   * @brief Activate the publisher of the downsampled costmap
+   */
+  void on_activate();
+
+  /**
+   * @brief Deactivate the publisher of the downsampled costmap
+   */
+  void on_deactivate();
+
+  /**
+   * @brief Cleanup the publisher of the downsampled costmap
+   */
+  void on_cleanup();
+
+  /**
+   * @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap
+   * @param downsampling_factor Multiplier for the costmap resolution
+   * @return A ptr to the downsampled costmap
+   */
+  nav2_costmap_2d::Costmap2D * downsample(const unsigned int & downsampling_factor);
+
+  /**
+   * @brief Resize the downsampled costmap. Used in case the costmap changes and we need to update the downsampled version
+   */
+  void resizeCostmap();
+
+protected:
+  /**
+   * @brief Update the sizes X-Y of the costmap and its downsampled version
+   */
+  void updateCostmapSize();
+
+  /**
+   * @brief Explore all subcells of the original costmap and assign the max cost to the new (downsampled) cell
+   * @param new_mx The X-coordinate of the cell in the new costmap
+   * @param new_my The Y-coordinate of the cell in the new costmap
+   */
+  void setCostOfCell(
+    const unsigned int & new_mx,
+    const unsigned int & new_my);
+
+  unsigned int _size_x;
+  unsigned int _size_y;
+  unsigned int _downsampled_size_x;
+  unsigned int _downsampled_size_y;
+  unsigned int _downsampling_factor;
+  bool _use_min_cost_neighbor;
+  float _downsampled_resolution;
+  nav2_costmap_2d::Costmap2D * _costmap;
+  std::unique_ptr<nav2_costmap_2d::Costmap2D> _downsampled_costmap;
+  std::unique_ptr<nav2_costmap_2d::Costmap2DPublisher> _downsampled_costmap_pub;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp

@@ -0,0 +1,284 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__NODE_2D_HPP_
+#define NAV2_SMAC_PLANNER__NODE_2D_HPP_
+
+#include <math.h>
+#include <vector>
+#include <iostream>
+#include <memory>
+#include <queue>
+#include <limits>
+#include <utility>
+#include <functional>
+
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/constants.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/node_hybrid.hpp"
+
+namespace nav2_smac_planner
+{
+
+/**
+ * @class nav2_smac_planner::Node2D
+ * @brief Node2D implementation for graph
+ */
+class Node2D
+{
+public:
+  typedef Node2D * NodePtr;
+  typedef std::unique_ptr<std::vector<Node2D>> Graph;
+  typedef std::vector<NodePtr> NodeVector;
+
+  /**
+   * @class nav2_smac_planner::Node2D::Coordinates
+   * @brief Node2D implementation of coordinate structure
+   */
+  struct Coordinates
+  {
+    Coordinates() {}
+    Coordinates(const float & x_in, const float & y_in)
+    : x(x_in), y(y_in)
+    {}
+
+    float x, y;
+  };
+  typedef std::vector<Coordinates> CoordinateVector;
+
+  /**
+   * @brief A constructor for nav2_smac_planner::Node2D
+   * @param index The index of this node for self-reference
+   */
+  explicit Node2D(const unsigned int index);
+
+  /**
+   * @brief A destructor for nav2_smac_planner::Node2D
+   */
+  ~Node2D();
+
+  /**
+   * @brief operator== for comparisons
+   * @param Node2D right hand side node reference
+   * @return If cell indicies are equal
+   */
+  bool operator==(const Node2D & rhs)
+  {
+    return this->_index == rhs._index;
+  }
+
+  /**
+   * @brief Reset method for new search
+   */
+  void reset();
+  /**
+   * @brief Gets the accumulated cost at this node
+   * @return accumulated cost
+   */
+  inline float & getAccumulatedCost()
+  {
+    return _accumulated_cost;
+  }
+
+  /**
+   * @brief Sets the accumulated cost at this node
+   * @param reference to accumulated cost
+   */
+  inline void setAccumulatedCost(const float & cost_in)
+  {
+    _accumulated_cost = cost_in;
+  }
+
+  /**
+   * @brief Gets the costmap cost at this node
+   * @return costmap cost
+   */
+  inline float & getCost()
+  {
+    return _cell_cost;
+  }
+
+  /**
+   * @brief Gets the costmap cost at this node
+   * @return costmap cost
+   */
+  inline void setCost(const float & cost)
+  {
+    _cell_cost = cost;
+  }
+
+  /**
+   * @brief Gets if cell has been visited in search
+   * @param If cell was visited
+   */
+  inline bool & wasVisited()
+  {
+    return _was_visited;
+  }
+
+  /**
+   * @brief Sets if cell has been visited in search
+   */
+  inline void visited()
+  {
+    _was_visited = true;
+    _is_queued = false;
+  }
+
+  /**
+   * @brief Gets if cell is currently queued in search
+   * @param If cell was queued
+   */
+  inline bool & isQueued()
+  {
+    return _is_queued;
+  }
+
+  /**
+   * @brief Sets if cell is currently queued in search
+   */
+  inline void queued()
+  {
+    _is_queued = true;
+  }
+
+  /**
+   * @brief Gets cell index
+   * @return Reference to cell index
+   */
+  inline unsigned int & getIndex()
+  {
+    return _index;
+  }
+
+  /**
+   * @brief Check if this node is valid
+   * @param traverse_unknown If we can explore unknown nodes on the graph
+   * @param collision_checker Pointer to collision checker object
+   * @return whether this node is valid and collision free
+   */
+  bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker * collision_checker);
+
+  /**
+   * @brief get traversal cost from this node to child node
+   * @param child Node pointer to this node's child
+   * @return traversal cost
+   */
+  float getTraversalCost(const NodePtr & child);
+
+  /**
+   * @brief Get index
+   * @param x x coordinate of point to get index of
+   * @param y y coordinate of point to get index of
+   * @param width width of costmap
+   * @return index
+   */
+  static inline unsigned int getIndex(
+    const unsigned int & x, const unsigned int & y, const unsigned int & width)
+  {
+    return x + y * width;
+  }
+
+  /**
+   * @brief Get index
+   * @param Index Index of point
+   * @param width width of costmap
+   * @param angles angle bins to use (must be 1 or throws exception)
+   * @return coordinates of point
+   */
+  static inline Coordinates getCoords(
+    const unsigned int & index, const unsigned int & width, const unsigned int & angles)
+  {
+    if (angles != 1) {
+      throw std::runtime_error("Node type Node2D does not have a valid angle quantization.");
+    }
+
+    return Coordinates(index % width, index / width);
+  }
+
+  /**
+   * @brief Get index
+   * @param Index Index of point
+   * @return coordinates of point
+   */
+  static inline Coordinates getCoords(const unsigned int & index)
+  {
+    const unsigned int & size_x = _neighbors_grid_offsets[3];
+    return Coordinates(index % size_x, index / size_x);
+  }
+
+  /**
+   * @brief Get cost of heuristic of node
+   * @param node Node index current
+   * @param node Node index of new
+   * @param costmap Costmap ptr to use
+   * @return Heuristic cost between the nodes
+   */
+  static float getHeuristicCost(
+    const Coordinates & node_coords,
+    const Coordinates & goal_coordinates,
+    const nav2_costmap_2d::Costmap2D * costmap);
+
+  /**
+   * @brief Initialize the neighborhood to be used in A*
+   * We support 4-connect (VON_NEUMANN) and 8-connect (MOORE)
+   * @param neighborhood The desired neighborhood type
+   * @param x_size_uint The total x size to find neighbors
+   * @param y_size The total y size to find neighbors
+   * @param num_angle_quantization Number of quantizations, must be 0
+   * @param search_info Search parameters, unused by 2D node
+   */
+  static void initMotionModel(
+    const MotionModel & motion_model,
+    unsigned int & size_x,
+    unsigned int & size_y,
+    unsigned int & num_angle_quantization,
+    SearchInfo & search_info);
+
+  /**
+   * @brief Retrieve all valid neighbors of a node.
+   * @param validity_checker Functor for state validity checking
+   * @param collision_checker Collision checker to use
+   * @param traverse_unknown If unknown costs are valid to traverse
+   * @param neighbors Vector of neighbors to be filled
+   */
+  void getNeighbors(
+    std::function<bool(const unsigned int &, nav2_smac_planner::Node2D * &)> & validity_checker,
+    GridCollisionChecker * collision_checker,
+    const bool & traverse_unknown,
+    NodeVector & neighbors);
+
+  /**
+   * @brief Set the starting pose for planning, as a node index
+   * @param path Reference to a vector of indicies of generated path
+   * @return whether the path was able to be backtraced
+   */
+  bool backtracePath(CoordinateVector & path);
+
+  Node2D * parent;
+  static float cost_travel_multiplier;
+  static std::vector<int> _neighbors_grid_offsets;
+
+private:
+  float _cell_cost;
+  float _accumulated_cost;
+  unsigned int _index;
+  bool _was_visited;
+  bool _is_queued;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__NODE_2D_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp

@@ -0,0 +1,82 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__NODE_BASIC_HPP_
+#define NAV2_SMAC_PLANNER__NODE_BASIC_HPP_
+
+#include <math.h>
+#include <vector>
+#include <cmath>
+#include <iostream>
+#include <functional>
+#include <queue>
+#include <memory>
+#include <utility>
+#include <limits>
+
+#include "ompl/base/StateSpace.h"
+
+#include "nav2_smac_planner/constants.hpp"
+#include "nav2_smac_planner/node_hybrid.hpp"
+#include "nav2_smac_planner/node_lattice.hpp"
+#include "nav2_smac_planner/node_2d.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+
+namespace nav2_smac_planner
+{
+
+/**
+ * @class nav2_smac_planner::NodeBasic
+ * @brief NodeBasic implementation for priority queue insertion
+ */
+template<typename NodeT>
+class NodeBasic
+{
+public:
+  /**
+   * @brief A constructor for nav2_smac_planner::NodeBasic
+   * @param index The index of this node for self-reference
+   */
+  explicit NodeBasic(const unsigned int index)
+  : index(index),
+    graph_node_ptr(nullptr)
+  {
+  }
+
+  /**
+   * @brief Take a NodeBasic and populate it with any necessary state
+   * cached in the queue for NodeT.
+   * @param node NodeT ptr to populate metadata into NodeBasic
+   */
+  void populateSearchNode(NodeT * & node);
+
+  /**
+   * @brief Take a NodeBasic and populate it with any necessary state
+   * cached in the queue for NodeTs.
+   * @param node Search node (basic) object to initialize internal node
+   * with state
+   */
+  void processSearchNode();
+
+  typename NodeT::Coordinates pose;  // Used by NodeHybrid and NodeLattice
+  NodeT * graph_node_ptr;
+  MotionPrimitive * prim_ptr;  // Used by NodeLattice
+  unsigned int index, motion_index;
+  bool backward;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__NODE_BASIC_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp

@@ -0,0 +1,467 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_
+#define NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_
+
+#include <math.h>
+#include <vector>
+#include <cmath>
+#include <iostream>
+#include <functional>
+#include <queue>
+#include <memory>
+#include <utility>
+#include <limits>
+
+#include "ompl/base/StateSpace.h"
+
+#include "nav2_smac_planner/constants.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/costmap_downsampler.hpp"
+
+namespace nav2_smac_planner
+{
+
+typedef std::vector<float> LookupTable;
+typedef std::pair<double, double> TrigValues;
+
+typedef std::pair<float, unsigned int> ObstacleHeuristicElement;
+struct ObstacleHeuristicComparator
+{
+  bool operator()(const ObstacleHeuristicElement & a, const ObstacleHeuristicElement & b) const
+  {
+    return a.first > b.first;
+  }
+};
+
+typedef std::vector<ObstacleHeuristicElement> ObstacleHeuristicQueue;
+
+// Must forward declare
+class NodeHybrid;
+
+/**
+ * @struct nav2_smac_planner::HybridMotionTable
+ * @brief A table of motion primitives and related functions
+ */
+struct HybridMotionTable
+{
+  /**
+   * @brief A constructor for nav2_smac_planner::HybridMotionTable
+   */
+  HybridMotionTable() {}
+
+  /**
+   * @brief Initializing using Dubin model
+   * @param size_x_in Size of costmap in X
+   * @param size_y_in Size of costmap in Y
+   * @param angle_quantization_in Size of costmap in bin sizes
+   * @param search_info Parameters for searching
+   */
+  void initDubin(
+    unsigned int & size_x_in,
+    unsigned int & size_y_in,
+    unsigned int & angle_quantization_in,
+    SearchInfo & search_info);
+
+  /**
+   * @brief Initializing using Reeds-Shepp model
+   * @param size_x_in Size of costmap in X
+   * @param size_y_in Size of costmap in Y
+   * @param angle_quantization_in Size of costmap in bin sizes
+   * @param search_info Parameters for searching
+   */
+  void initReedsShepp(
+    unsigned int & size_x_in,
+    unsigned int & size_y_in,
+    unsigned int & angle_quantization_in,
+    SearchInfo & search_info);
+
+  /**
+   * @brief Get projections of motion models
+   * @param node Ptr to NodeHybrid
+   * @return A set of motion poses
+   */
+  MotionPoses getProjections(const NodeHybrid * node);
+
+  /**
+   * @brief Get the angular bin to use from a raw orientation
+   * @param theta Angle in radians
+   * @return bin index of closest angle to request
+   */
+  unsigned int getClosestAngularBin(const double & theta);
+
+  /**
+   * @brief Get the raw orientation from an angular bin
+   * @param bin_idx Index of the bin
+   * @return Raw orientation in radians
+   */
+  float getAngleFromBin(const unsigned int & bin_idx);
+
+  MotionModel motion_model = MotionModel::UNKNOWN;
+  MotionPoses projections;
+  unsigned int size_x;
+  unsigned int num_angle_quantization;
+  float num_angle_quantization_float;
+  float min_turning_radius;
+  float bin_size;
+  float change_penalty;
+  float non_straight_penalty;
+  float cost_penalty;
+  float reverse_penalty;
+  float travel_distance_reward;
+  ompl::base::StateSpacePtr state_space;
+  std::vector<std::vector<double>> delta_xs;
+  std::vector<std::vector<double>> delta_ys;
+  std::vector<TrigValues> trig_values;
+};
+
+/**
+ * @class nav2_smac_planner::NodeHybrid
+ * @brief NodeHybrid implementation for graph, Hybrid-A*
+ */
+class NodeHybrid
+{
+public:
+  typedef NodeHybrid * NodePtr;
+  typedef std::unique_ptr<std::vector<NodeHybrid>> Graph;
+  typedef std::vector<NodePtr> NodeVector;
+
+  /**
+   * @class nav2_smac_planner::NodeHybrid::Coordinates
+   * @brief NodeHybrid implementation of coordinate structure
+   */
+  struct Coordinates
+  {
+    /**
+     * @brief A constructor for nav2_smac_planner::NodeHybrid::Coordinates
+     */
+    Coordinates() {}
+
+    /**
+     * @brief A constructor for nav2_smac_planner::NodeHybrid::Coordinates
+     * @param x_in X coordinate
+     * @param y_in Y coordinate
+     * @param theta_in Theta coordinate
+     */
+    Coordinates(const float & x_in, const float & y_in, const float & theta_in)
+    : x(x_in), y(y_in), theta(theta_in)
+    {}
+
+    inline bool operator==(const Coordinates & rhs)
+    {
+      return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta;
+    }
+
+    inline bool operator!=(const Coordinates & rhs)
+    {
+      return !(*this == rhs);
+    }
+
+    float x, y, theta;
+  };
+
+  typedef std::vector<Coordinates> CoordinateVector;
+
+  /**
+   * @brief A constructor for nav2_smac_planner::NodeHybrid
+   * @param index The index of this node for self-reference
+   */
+  explicit NodeHybrid(const unsigned int index);
+
+  /**
+   * @brief A destructor for nav2_smac_planner::NodeHybrid
+   */
+  ~NodeHybrid();
+
+  /**
+   * @brief operator== for comparisons
+   * @param NodeHybrid right hand side node reference
+   * @return If cell indicies are equal
+   */
+  bool operator==(const NodeHybrid & rhs)
+  {
+    return this->_index == rhs._index;
+  }
+
+  /**
+   * @brief setting continuous coordinate search poses (in partial-cells)
+   * @param Pose pose
+   */
+  inline void setPose(const Coordinates & pose_in)
+  {
+    pose = pose_in;
+  }
+
+  /**
+   * @brief Reset method for new search
+   */
+  void reset();
+
+  /**
+   * @brief Gets the accumulated cost at this node
+   * @return accumulated cost
+   */
+  inline float & getAccumulatedCost()
+  {
+    return _accumulated_cost;
+  }
+
+  /**
+   * @brief Sets the accumulated cost at this node
+   * @param reference to accumulated cost
+   */
+  inline void setAccumulatedCost(const float & cost_in)
+  {
+    _accumulated_cost = cost_in;
+  }
+
+  /**
+   * @brief Sets the motion primitive index used to achieve node in search
+   * @param reference to motion primitive idx
+   */
+  inline void setMotionPrimitiveIndex(const unsigned int & idx)
+  {
+    _motion_primitive_index = idx;
+  }
+
+  /**
+   * @brief Gets the motion primitive index used to achieve node in search
+   * @return reference to motion primitive idx
+   */
+  inline unsigned int & getMotionPrimitiveIndex()
+  {
+    return _motion_primitive_index;
+  }
+
+  /**
+   * @brief Gets the costmap cost at this node
+   * @return costmap cost
+   */
+  inline float & getCost()
+  {
+    return _cell_cost;
+  }
+
+  /**
+   * @brief Gets if cell has been visited in search
+   * @param If cell was visited
+   */
+  inline bool & wasVisited()
+  {
+    return _was_visited;
+  }
+
+  /**
+   * @brief Sets if cell has been visited in search
+   */
+  inline void visited()
+  {
+    _was_visited = true;
+  }
+
+  /**
+   * @brief Gets cell index
+   * @return Reference to cell index
+   */
+  inline unsigned int & getIndex()
+  {
+    return _index;
+  }
+
+  /**
+   * @brief Check if this node is valid
+   * @param traverse_unknown If we can explore unknown nodes on the graph
+   * @return whether this node is valid and collision free
+   */
+  bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker * collision_checker);
+
+  /**
+   * @brief Get traversal cost of parent node to child node
+   * @param child Node pointer to child
+   * @return traversal cost
+   */
+  float getTraversalCost(const NodePtr & child);
+
+  /**
+   * @brief Get index at coordinates
+   * @param x X coordinate of point
+   * @param y Y coordinate of point
+   * @param angle Theta coordinate of point
+   * @param width Width of costmap
+   * @param angle_quantization Number of theta bins
+   * @return Index
+   */
+  static inline unsigned int getIndex(
+    const unsigned int & x, const unsigned int & y, const unsigned int & angle,
+    const unsigned int & width, const unsigned int & angle_quantization)
+  {
+    return angle + x * angle_quantization + y * width * angle_quantization;
+  }
+
+  /**
+   * @brief Get index at coordinates
+   * @param x X coordinate of point
+   * @param y Y coordinate of point
+   * @param angle Theta coordinate of point
+   * @return Index
+   */
+  static inline unsigned int getIndex(
+    const unsigned int & x, const unsigned int & y, const unsigned int & angle)
+  {
+    return getIndex(
+      x, y, angle, motion_table.size_x,
+      motion_table.num_angle_quantization);
+  }
+
+  /**
+   * @brief Get coordinates at index
+   * @param index Index of point
+   * @param width Width of costmap
+   * @param angle_quantization Theta size of costmap
+   * @return Coordinates
+   */
+  static inline Coordinates getCoords(
+    const unsigned int & index,
+    const unsigned int & width, const unsigned int & angle_quantization)
+  {
+    return Coordinates(
+      (index / angle_quantization) % width,    // x
+      index / (angle_quantization * width),    // y
+      index % angle_quantization);    // theta
+  }
+
+  /**
+   * @brief Get cost of heuristic of node
+   * @param node Node index current
+   * @param node Node index of new
+   * @param costmap Costmap ptr to use
+   * @return Heuristic cost between the nodes
+   */
+  static float getHeuristicCost(
+    const Coordinates & node_coords,
+    const Coordinates & goal_coordinates,
+    const nav2_costmap_2d::Costmap2D * costmap);
+
+  /**
+   * @brief Initialize motion models
+   * @param motion_model Motion model enum to use
+   * @param size_x Size of X of graph
+   * @param size_y Size of y of graph
+   * @param angle_quantization Size of theta bins of graph
+   * @param search_info Search info to use
+   */
+  static void initMotionModel(
+    const MotionModel & motion_model,
+    unsigned int & size_x,
+    unsigned int & size_y,
+    unsigned int & angle_quantization,
+    SearchInfo & search_info);
+
+  /**
+   * @brief Compute the SE2 distance heuristic
+   * @param lookup_table_dim Size, in costmap pixels, of the
+   * each lookup table dimension to populate
+   * @param motion_model Motion model to use for state space
+   * @param dim_3_size Number of quantization bins for caching
+   * @param search_info Info containing minimum radius to use
+   */
+  static void precomputeDistanceHeuristic(
+    const float & lookup_table_dim,
+    const MotionModel & motion_model,
+    const unsigned int & dim_3_size,
+    const SearchInfo & search_info);
+
+  /**
+   * @brief Compute the Obstacle heuristic
+   * @param node_coords Coordinates to get heuristic at
+   * @param goal_coords Coordinates to compute heuristic to
+   * @return heuristic Heuristic value
+   */
+  static float getObstacleHeuristic(
+    const Coordinates & node_coords,
+    const Coordinates & goal_coords,
+    const double & cost_penalty);
+
+  /**
+   * @brief Compute the Distance heuristic
+   * @param node_coords Coordinates to get heuristic at
+   * @param goal_coords Coordinates to compute heuristic to
+   * @param obstacle_heuristic Value of the obstacle heuristic to compute
+   * additional motion heuristics if required
+   * @return heuristic Heuristic value
+   */
+  static float getDistanceHeuristic(
+    const Coordinates & node_coords,
+    const Coordinates & goal_coords,
+    const float & obstacle_heuristic);
+
+  /**
+   * @brief reset the obstacle heuristic state
+   * @param costmap Costmap to use
+   * @param goal_coords Coordinates to start heuristic expansion at
+   */
+  static void resetObstacleHeuristic(
+    nav2_costmap_2d::Costmap2D * costmap,
+    const unsigned int & start_x, const unsigned int & start_y,
+    const unsigned int & goal_x, const unsigned int & goal_y);
+
+  /**
+   * @brief Retrieve all valid neighbors of a node.
+   * @param validity_checker Functor for state validity checking
+   * @param collision_checker Collision checker to use
+   * @param traverse_unknown If unknown costs are valid to traverse
+   * @param neighbors Vector of neighbors to be filled
+   */
+  void getNeighbors(
+    std::function<bool(const unsigned int &, nav2_smac_planner::NodeHybrid * &)> & validity_checker,
+    GridCollisionChecker * collision_checker,
+    const bool & traverse_unknown,
+    NodeVector & neighbors);
+
+  /**
+   * @brief Set the starting pose for planning, as a node index
+   * @param path Reference to a vector of indicies of generated path
+   * @return whether the path was able to be backtraced
+   */
+  bool backtracePath(CoordinateVector & path);
+
+  NodeHybrid * parent;
+  Coordinates pose;
+
+  // Constants required across all nodes but don't want to allocate more than once
+  static double travel_distance_cost;
+  static HybridMotionTable motion_table;
+  // Wavefront lookup and queue for continuing to expand as needed
+  static LookupTable obstacle_heuristic_lookup_table;
+  static ObstacleHeuristicQueue obstacle_heuristic_queue;
+
+  static nav2_costmap_2d::Costmap2D * sampled_costmap;
+  static CostmapDownsampler downsampler;
+  // Dubin / Reeds-Shepp lookup and size for dereferencing
+  static LookupTable dist_heuristic_lookup_table;
+  static float size_lookup;
+
+private:
+  float _cell_cost;
+  float _accumulated_cost;
+  unsigned int _index;
+  bool _was_visited;
+  unsigned int _motion_primitive_index;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp

@@ -0,0 +1,433 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_
+#define NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_
+
+#include <math.h>
+
+#include <vector>
+#include <cmath>
+#include <iostream>
+#include <functional>
+#include <queue>
+#include <memory>
+#include <utility>
+#include <limits>
+#include <string>
+
+#include "nlohmann/json.hpp"
+#include "ompl/base/StateSpace.h"
+#include "angles/angles.h"
+
+#include "nav2_smac_planner/constants.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/node_hybrid.hpp"
+#include "nav2_smac_planner/utils.hpp"
+
+namespace nav2_smac_planner
+{
+
+// forward declare
+class NodeLattice;
+class NodeHybrid;
+
+/**
+ * @struct nav2_smac_planner::LatticeMotionTable
+ * @brief A table of motion primitives and related functions
+ */
+struct LatticeMotionTable
+{
+  /**
+   * @brief A constructor for nav2_smac_planner::LatticeMotionTable
+   */
+  LatticeMotionTable() {}
+
+  /**
+   * @brief Initializing state lattice planner's motion model
+   * @param size_x_in Size of costmap in X
+   * @param search_info Parameters for searching
+   */
+  void initMotionModel(
+    unsigned int & size_x_in,
+    SearchInfo & search_info);
+
+  /**
+   * @brief Get projections of motion models
+   * @param node Ptr to NodeLattice
+   * @return A set of motion poses
+   */
+  MotionPrimitivePtrs getMotionPrimitives(const NodeLattice * node);
+
+  /**
+   * @brief Get file metadata needed
+   * @param lattice_filepath Filepath to the lattice file
+   * @return A set of metadata containing the number of angular bins
+   * and the global coordinates minimum turning radius of the primitives
+   * for use in analytic expansion and heuristic calculation.
+   */
+  static LatticeMetadata getLatticeMetadata(const std::string & lattice_filepath);
+
+  /**
+   * @brief Get the angular bin to use from a raw orientation
+   * @param theta Angle in radians
+   * @return bin index of closest angle to request
+   */
+  unsigned int getClosestAngularBin(const double & theta);
+
+  /**
+   * @brief Get the raw orientation from an angular bin
+   * @param bin_idx Index of the bin
+   * @return Raw orientation in radians
+   */
+  float & getAngleFromBin(const unsigned int & bin_idx);
+
+  unsigned int size_x;
+  unsigned int num_angle_quantization;
+  float change_penalty;
+  float non_straight_penalty;
+  float cost_penalty;
+  float reverse_penalty;
+  float travel_distance_reward;
+  float rotation_penalty;
+  bool allow_reverse_expansion;
+  std::vector<std::vector<MotionPrimitive>> motion_primitives;
+  ompl::base::StateSpacePtr state_space;
+  std::vector<TrigValues> trig_values;
+  std::string current_lattice_filepath;
+  LatticeMetadata lattice_metadata;
+};
+
+/**
+ * @class nav2_smac_planner::NodeLattice
+ * @brief NodeLattice implementation for graph, Hybrid-A*
+ */
+class NodeLattice
+{
+public:
+  typedef NodeLattice * NodePtr;
+  typedef std::unique_ptr<std::vector<NodeLattice>> Graph;
+  typedef std::vector<NodePtr> NodeVector;
+  typedef NodeHybrid::Coordinates Coordinates;
+  typedef NodeHybrid::CoordinateVector CoordinateVector;
+
+  /**
+   * @brief A constructor for nav2_smac_planner::NodeLattice
+   * @param index The index of this node for self-reference
+   */
+  explicit NodeLattice(const unsigned int index);
+
+  /**
+   * @brief A destructor for nav2_smac_planner::NodeLattice
+   */
+  ~NodeLattice();
+
+  /**
+   * @brief operator== for comparisons
+   * @param NodeLattice right hand side node reference
+   * @return If cell indicies are equal
+   */
+  bool operator==(const NodeLattice & rhs)
+  {
+    return this->_index == rhs._index;
+  }
+
+  /**
+   * @brief setting continuous coordinate search poses (in partial-cells)
+   * @param Pose pose
+   */
+  inline void setPose(const Coordinates & pose_in)
+  {
+    pose = pose_in;
+  }
+
+  /**
+   * @brief Reset method for new search
+   */
+  void reset();
+
+  /**
+   * @brief Sets the motion primitive used to achieve node in search
+   * @param pointer to motion primitive
+   */
+  inline void setMotionPrimitive(MotionPrimitive * prim)
+  {
+    _motion_primitive = prim;
+  }
+
+  /**
+   * @brief Gets the motion primitive used to achieve node in search
+   * @return pointer to motion primitive
+   */
+  inline MotionPrimitive * & getMotionPrimitive()
+  {
+    return _motion_primitive;
+  }
+
+  /**
+   * @brief Gets the accumulated cost at this node
+   * @return accumulated cost
+   */
+  inline float & getAccumulatedCost()
+  {
+    return _accumulated_cost;
+  }
+
+  /**
+   * @brief Sets the accumulated cost at this node
+   * @param reference to accumulated cost
+   */
+  inline void setAccumulatedCost(const float & cost_in)
+  {
+    _accumulated_cost = cost_in;
+  }
+
+  /**
+   * @brief Gets the costmap cost at this node
+   * @return costmap cost
+   */
+  inline float & getCost()
+  {
+    return _cell_cost;
+  }
+
+  /**
+   * @brief Gets if cell has been visited in search
+   * @param If cell was visited
+   */
+  inline bool & wasVisited()
+  {
+    return _was_visited;
+  }
+
+  /**
+   * @brief Sets if cell has been visited in search
+   */
+  inline void visited()
+  {
+    _was_visited = true;
+  }
+
+  /**
+   * @brief Gets cell index
+   * @return Reference to cell index
+   */
+  inline unsigned int & getIndex()
+  {
+    return _index;
+  }
+
+  /**
+   * @brief Sets that this primitive is moving in reverse
+   */
+  inline void backwards(bool back = true)
+  {
+    _backwards = back;
+  }
+
+  /**
+   * @brief Gets if this primitive is moving in reverse
+   * @return backwards If moving in reverse
+   */
+  inline bool isBackward()
+  {
+    return _backwards;
+  }
+
+  /**
+   * @brief Check if this node is valid
+   * @param traverse_unknown If we can explore unknown nodes on the graph
+   * @param collision_checker Collision checker object to aid in validity checking
+   * @param primitive Optional argument if needing to check over a primitive
+   * not only a terminal pose
+   * @param is_backwards Optional argument if needed to check if prim expansion is
+   * in reverse
+   * @return whether this node is valid and collision free
+   */
+  bool isNodeValid(
+    const bool & traverse_unknown,
+    GridCollisionChecker * collision_checker,
+    MotionPrimitive * primitive = nullptr,
+    bool is_backwards = false);
+
+  /**
+   * @brief Get traversal cost of parent node to child node
+   * @param child Node pointer to child
+   * @return traversal cost
+   */
+  float getTraversalCost(const NodePtr & child);
+
+  /**
+   * @brief Get index at coordinates
+   * @param x X coordinate of point
+   * @param y Y coordinate of point
+   * @param angle Theta coordinate of point
+   * @return Index
+   */
+  static inline unsigned int getIndex(
+    const unsigned int & x, const unsigned int & y, const unsigned int & angle)
+  {
+    // Hybrid-A* and State Lattice share a coordinate system
+    return NodeHybrid::getIndex(
+      x, y, angle, motion_table.size_x,
+      motion_table.num_angle_quantization);
+  }
+
+  /**
+   * @brief Get coordinates at index
+   * @param index Index of point
+   * @param width Width of costmap
+   * @param angle_quantization Theta size of costmap
+   * @return Coordinates
+   */
+  static inline Coordinates getCoords(
+    const unsigned int & index,
+    const unsigned int & width, const unsigned int & angle_quantization)
+  {
+    // Hybrid-A* and State Lattice share a coordinate system
+    return NodeHybrid::Coordinates(
+      (index / angle_quantization) % width,    // x
+      index / (angle_quantization * width),    // y
+      index % angle_quantization);    // theta
+  }
+
+  /**
+   * @brief Get cost of heuristic of node
+   * @param node Node index current
+   * @param node Node index of new
+   * @param costmap Costmap ptr to use
+   * @return Heuristic cost between the nodes
+   */
+  static float getHeuristicCost(
+    const Coordinates & node_coords,
+    const Coordinates & goal_coordinates,
+    const nav2_costmap_2d::Costmap2D * costmap);
+
+  /**
+   * @brief Initialize motion models
+   * @param motion_model Motion model enum to use
+   * @param size_x Size of X of graph
+   * @param size_y Size of y of graph
+   * @param angle_quantization Size of theta bins of graph
+   * @param search_info Search info to use
+   */
+  static void initMotionModel(
+    const MotionModel & motion_model,
+    unsigned int & size_x,
+    unsigned int & size_y,
+    unsigned int & angle_quantization,
+    SearchInfo & search_info);
+
+  /**
+   * @brief Compute the SE2 distance heuristic
+   * @param lookup_table_dim Size, in costmap pixels, of the
+   * each lookup table dimension to populate
+   * @param motion_model Motion model to use for state space
+   * @param dim_3_size Number of quantization bins for caching
+   * @param search_info Info containing minimum radius to use
+   */
+  static void precomputeDistanceHeuristic(
+    const float & lookup_table_dim,
+    const MotionModel & motion_model,
+    const unsigned int & dim_3_size,
+    const SearchInfo & search_info);
+
+  /**
+   * @brief Compute the wavefront heuristic
+   * @param costmap Costmap to use
+   * @param goal_coords Coordinates to start heuristic expansion at
+   */
+  static void resetObstacleHeuristic(
+    nav2_costmap_2d::Costmap2D * costmap,
+    const unsigned int & start_x, const unsigned int & start_y,
+    const unsigned int & goal_x, const unsigned int & goal_y)
+  {
+    // State Lattice and Hybrid-A* share this heuristics
+    NodeHybrid::resetObstacleHeuristic(costmap, start_x, start_y, goal_x, goal_y);
+  }
+
+  /**
+   * @brief Compute the Obstacle heuristic
+   * @param node_coords Coordinates to get heuristic at
+   * @param goal_coords Coordinates to compute heuristic to
+   * @return heuristic Heuristic value
+   */
+  static float getObstacleHeuristic(
+    const Coordinates & node_coords,
+    const Coordinates & goal_coords,
+    const double & cost_penalty)
+  {
+    return NodeHybrid::getObstacleHeuristic(node_coords, goal_coords, cost_penalty);
+  }
+
+  /**
+   * @brief Compute the Distance heuristic
+   * @param node_coords Coordinates to get heuristic at
+   * @param goal_coords Coordinates to compute heuristic to
+   * @param obstacle_heuristic Value of the obstacle heuristic to compute
+   * additional motion heuristics if required
+   * @return heuristic Heuristic value
+   */
+  static float getDistanceHeuristic(
+    const Coordinates & node_coords,
+    const Coordinates & goal_coords,
+    const float & obstacle_heuristic);
+
+  /**
+   * @brief Retrieve all valid neighbors of a node.
+   * @param validity_checker Functor for state validity checking
+   * @param collision_checker Collision checker to use
+   * @param traverse_unknown If unknown costs are valid to traverse
+   * @param neighbors Vector of neighbors to be filled
+   */
+  void getNeighbors(
+    std::function<bool(const unsigned int &,
+    nav2_smac_planner::NodeLattice * &)> & validity_checker,
+    GridCollisionChecker * collision_checker,
+    const bool & traverse_unknown,
+    NodeVector & neighbors);
+
+  /**
+   * @brief Set the starting pose for planning, as a node index
+   * @param path Reference to a vector of indicies of generated path
+   * @return whether the path was able to be backtraced
+   */
+  bool backtracePath(CoordinateVector & path);
+
+  /**
+   * \brief add node to the path
+   * \param current_node
+   */
+  void addNodeToPath(NodePtr current_node, CoordinateVector & path);
+
+  NodeLattice * parent;
+  Coordinates pose;
+  static LatticeMotionTable motion_table;
+  // Dubin / Reeds-Shepp lookup and size for dereferencing
+  static LookupTable dist_heuristic_lookup_table;
+  static float size_lookup;
+
+private:
+  float _cell_cost;
+  float _accumulated_cost;
+  unsigned int _index;
+  bool _was_visited;
+  MotionPrimitive * _motion_primitive;
+  bool _backwards;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp

@@ -0,0 +1,128 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_
+#define NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_
+
+#include <memory>
+#include <vector>
+#include <string>
+#include <mutex>
+
+#include "nav2_smac_planner/a_star.hpp"
+#include "nav2_smac_planner/smoother.hpp"
+#include "nav2_smac_planner/utils.hpp"
+#include "nav2_smac_planner/costmap_downsampler.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav2_core/global_planner.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "tf2/utils.h"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+
+namespace nav2_smac_planner
+{
+
+class SmacPlanner2D : public nav2_core::GlobalPlanner
+{
+public:
+  /**
+   * @brief constructor
+   */
+  SmacPlanner2D();
+
+  /**
+   * @brief destructor
+   */
+  ~SmacPlanner2D();
+
+  /**
+   * @brief Configuring plugin
+   * @param parent Lifecycle node pointer
+   * @param name Name of plugin map
+   * @param tf Shared ptr of TF2 buffer
+   * @param costmap_ros Costmap2DROS object
+   */
+  void configure(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
+
+  /**
+   * @brief Cleanup lifecycle node
+   */
+  void cleanup() override;
+
+  /**
+   * @brief Activate lifecycle node
+   */
+  void activate() override;
+
+  /**
+   * @brief Deactivate lifecycle node
+   */
+  void deactivate() override;
+
+  /**
+   * @brief Creating a plan from start and goal poses
+   * @param start Start pose
+   * @param goal Goal pose
+   * @return nav2_msgs::Path of the generated path
+   */
+  nav_msgs::msg::Path createPlan(
+    const geometry_msgs::msg::PoseStamped & start,
+    const geometry_msgs::msg::PoseStamped & goal) override;
+
+protected:
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  std::unique_ptr<AStarAlgorithm<Node2D>> _a_star;
+  GridCollisionChecker _collision_checker;
+  std::unique_ptr<Smoother> _smoother;
+  nav2_costmap_2d::Costmap2D * _costmap;
+  std::unique_ptr<CostmapDownsampler> _costmap_downsampler;
+  rclcpp::Clock::SharedPtr _clock;
+  rclcpp::Logger _logger{rclcpp::get_logger("SmacPlanner2D")};
+  std::string _global_frame, _name;
+  float _tolerance;
+  int _downsampling_factor;
+  bool _downsample_costmap;
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
+  double _max_planning_time;
+  bool _allow_unknown;
+  int _max_iterations;
+  int _max_on_approach_iterations;
+  bool _use_final_approach_orientation;
+  SearchInfo _search_info;
+  std::string _motion_model_for_search;
+  MotionModel _motion_model;
+  std::mutex _mutex;
+  rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
+
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr _dyn_params_handler;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

@@ -0,0 +1,131 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_
+#define NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_
+
+#include <memory>
+#include <vector>
+#include <string>
+
+#include "nav2_smac_planner/a_star.hpp"
+#include "nav2_smac_planner/smoother.hpp"
+#include "nav2_smac_planner/utils.hpp"
+#include "nav2_smac_planner/costmap_downsampler.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav2_core/global_planner.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "tf2/utils.h"
+
+namespace nav2_smac_planner
+{
+
+class SmacPlannerHybrid : public nav2_core::GlobalPlanner
+{
+public:
+  /**
+   * @brief constructor
+   */
+  SmacPlannerHybrid();
+
+  /**
+   * @brief destructor
+   */
+  ~SmacPlannerHybrid();
+
+  /**
+   * @brief Configuring plugin
+   * @param parent Lifecycle node pointer
+   * @param name Name of plugin map
+   * @param tf Shared ptr of TF2 buffer
+   * @param costmap_ros Costmap2DROS object
+   */
+  void configure(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
+
+  /**
+   * @brief Cleanup lifecycle node
+   */
+  void cleanup() override;
+
+  /**
+   * @brief Activate lifecycle node
+   */
+  void activate() override;
+
+  /**
+   * @brief Deactivate lifecycle node
+   */
+  void deactivate() override;
+
+  /**
+   * @brief Creating a plan from start and goal poses
+   * @param start Start pose
+   * @param goal Goal pose
+   * @return nav2_msgs::Path of the generated path
+   */
+  nav_msgs::msg::Path createPlan(
+    const geometry_msgs::msg::PoseStamped & start,
+    const geometry_msgs::msg::PoseStamped & goal) override;
+
+protected:
+  /**
+   * @brief Callback executed when a paramter change is detected
+   * @param parameters list of changed parameters
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  std::unique_ptr<AStarAlgorithm<NodeHybrid>> _a_star;
+  GridCollisionChecker _collision_checker;
+  std::unique_ptr<Smoother> _smoother;
+  rclcpp::Clock::SharedPtr _clock;
+  rclcpp::Logger _logger{rclcpp::get_logger("SmacPlannerHybrid")};
+  nav2_costmap_2d::Costmap2D * _costmap;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> _costmap_ros;
+  std::unique_ptr<CostmapDownsampler> _costmap_downsampler;
+  std::string _global_frame, _name;
+  float _lookup_table_dim;
+  float _tolerance;
+  bool _downsample_costmap;
+  int _downsampling_factor;
+  double _angle_bin_size;
+  unsigned int _angle_quantizations;
+  bool _allow_unknown;
+  int _max_iterations;
+  int _max_on_approach_iterations;
+  SearchInfo _search_info;
+  double _max_planning_time;
+  double _lookup_table_size;
+  double _minimum_turning_radius_global_coords;
+  std::string _motion_model_for_search;
+  MotionModel _motion_model;
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
+  std::mutex _mutex;
+  rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
+
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr _dyn_params_handler;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp

@@ -0,0 +1,123 @@
+// Copyright (c) 2021, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_LATTICE_HPP_
+#define NAV2_SMAC_PLANNER__SMAC_PLANNER_LATTICE_HPP_
+
+#include <memory>
+#include <vector>
+#include <string>
+
+#include "nav2_smac_planner/a_star.hpp"
+#include "nav2_smac_planner/smoother.hpp"
+#include "nav2_smac_planner/utils.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav2_core/global_planner.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "tf2/utils.h"
+
+namespace nav2_smac_planner
+{
+
+class SmacPlannerLattice : public nav2_core::GlobalPlanner
+{
+public:
+  /**
+   * @brief constructor
+   */
+  SmacPlannerLattice();
+
+  /**
+   * @brief destructor
+   */
+  ~SmacPlannerLattice();
+
+  /**
+   * @brief Configuring plugin
+   * @param parent Lifecycle node pointer
+   * @param name Name of plugin map
+   * @param tf Shared ptr of TF2 buffer
+   * @param costmap_ros Costmap2DROS object
+   */
+  void configure(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
+
+  /**
+   * @brief Cleanup lifecycle node
+   */
+  void cleanup() override;
+
+  /**
+   * @brief Activate lifecycle node
+   */
+  void activate() override;
+
+  /**
+   * @brief Deactivate lifecycle node
+   */
+  void deactivate() override;
+
+  /**
+   * @brief Creating a plan from start and goal poses
+   * @param start Start pose
+   * @param goal Goal pose
+   * @return nav2_msgs::Path of the generated path
+   */
+  nav_msgs::msg::Path createPlan(
+    const geometry_msgs::msg::PoseStamped & start,
+    const geometry_msgs::msg::PoseStamped & goal) override;
+
+protected:
+  /**
+   * @brief Callback executed when a paramter change is detected
+   * @param parameters list of changed parameters
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  std::unique_ptr<AStarAlgorithm<NodeLattice>> _a_star;
+  GridCollisionChecker _collision_checker;
+  std::unique_ptr<Smoother> _smoother;
+  rclcpp::Clock::SharedPtr _clock;
+  rclcpp::Logger _logger{rclcpp::get_logger("SmacPlannerLattice")};
+  nav2_costmap_2d::Costmap2D * _costmap;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> _costmap_ros;
+  MotionModel _motion_model;
+  LatticeMetadata _metadata;
+  std::string _global_frame, _name;
+  SearchInfo _search_info;
+  bool _allow_unknown;
+  int _max_iterations;
+  int _max_on_approach_iterations;
+  float _tolerance;
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
+  double _max_planning_time;
+  double _lookup_table_size;
+  std::mutex _mutex;
+  rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
+
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr _dyn_params_handler;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__SMAC_PLANNER_LATTICE_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp

@@ -0,0 +1,243 @@
+// Copyright (c) 2021, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__SMOOTHER_HPP_
+#define NAV2_SMAC_PLANNER__SMOOTHER_HPP_
+
+#include <cmath>
+#include <vector>
+#include <iostream>
+#include <memory>
+#include <queue>
+#include <utility>
+
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/constants.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "angles/angles.h"
+#include "tf2/utils.h"
+#include "ompl/base/StateSpace.h"
+#include "ompl/base/spaces/DubinsStateSpace.h"
+
+namespace nav2_smac_planner
+{
+
+/**
+ * @class nav2_smac_planner::PathSegment
+ * @brief A segment of a path in start/end indices
+ */
+struct PathSegment
+{
+  unsigned int start;
+  unsigned int end;
+};
+
+/**
+ * @struct nav2_smac_planner::BoundaryPoints
+ * @brief Set of boundary condition points from expansion
+ */
+struct BoundaryPoints
+{
+  /**
+   * @brief A constructor for BoundaryPoints
+   */
+  BoundaryPoints(double & x_in, double & y_in, double & theta_in)
+  : x(x_in), y(y_in), theta(theta_in)
+  {}
+
+  double x;
+  double y;
+  double theta;
+};
+
+/**
+ * @struct nav2_smac_planner::BoundaryExpansion
+ * @brief Boundary expansion state
+ */
+struct BoundaryExpansion
+{
+  double path_end_idx{0.0};
+  double expansion_path_length{0.0};
+  double original_path_length{0.0};
+  std::vector<BoundaryPoints> pts;
+  bool in_collision{false};
+};
+
+typedef std::vector<BoundaryExpansion> BoundaryExpansions;
+typedef std::vector<geometry_msgs::msg::PoseStamped>::iterator PathIterator;
+typedef std::vector<geometry_msgs::msg::PoseStamped>::reverse_iterator ReversePathIterator;
+
+/**
+ * @class nav2_smac_planner::Smoother
+ * @brief A path smoother implementation
+ */
+class Smoother
+{
+public:
+  /**
+   * @brief A constructor for nav2_smac_planner::Smoother
+   */
+  explicit Smoother(const SmootherParams & params);
+
+  /**
+   * @brief A destructor for nav2_smac_planner::Smoother
+   */
+  ~Smoother() {}
+
+  /**
+   * @brief Initialization of the smoother
+   * @param min_turning_radius Minimum turning radius (m)
+   * @param motion_model Motion model type
+   */
+  void initialize(
+    const double & min_turning_radius);
+
+  /**
+   * @brief Smoother API method
+   * @param path Reference to path
+   * @param costmap Pointer to minimal costmap
+   * @param max_time Maximum time to compute, stop early if over limit
+   * @return If smoothing was successful
+   */
+  bool smooth(
+    nav_msgs::msg::Path & path,
+    const nav2_costmap_2d::Costmap2D * costmap,
+    const double & max_time);
+
+protected:
+  /**
+   * @brief Smoother method - does the smoothing on a segment
+   * @param path Reference to path
+   * @param reversing_segment Return if this is a reversing segment
+   * @param costmap Pointer to minimal costmap
+   * @param max_time Maximum time to compute, stop early if over limit
+   * @return If smoothing was successful
+   */
+  bool smoothImpl(
+    nav_msgs::msg::Path & path,
+    bool & reversing_segment,
+    const nav2_costmap_2d::Costmap2D * costmap,
+    const double & max_time);
+
+  /**
+   * @brief Get the field value for a given dimension
+   * @param msg Current pose to sample
+   * @param dim Dimension ID of interest
+   * @return dim value
+   */
+  inline double getFieldByDim(
+    const geometry_msgs::msg::PoseStamped & msg,
+    const unsigned int & dim);
+
+  /**
+   * @brief Set the field value for a given dimension
+   * @param msg Current pose to sample
+   * @param dim Dimension ID of interest
+   * @param value to set the dimention to for the pose
+   */
+  inline void setFieldByDim(
+    geometry_msgs::msg::PoseStamped & msg, const unsigned int dim,
+    const double & value);
+
+  /**
+   * @brief Finds the starting and end indices of path segments where
+   * the robot is traveling in the same direction (e.g. forward vs reverse)
+   * @param path Path in which to look for cusps
+   * @return Set of index pairs for each segment of the path in a given direction
+   */
+  std::vector<PathSegment> findDirectionalPathSegments(const nav_msgs::msg::Path & path);
+
+  /**
+   * @brief Enforced minimum curvature boundary conditions on plan output
+   * the robot is traveling in the same direction (e.g. forward vs reverse)
+   * @param start_pose Start pose of the feasible path to maintain
+   * @param path Path to modify for curvature constraints on start / end of path
+   * @param costmap Costmap to check for collisions
+   * @param reversing_segment Whether this path segment is in reverse
+   */
+  void enforceStartBoundaryConditions(
+    const geometry_msgs::msg::Pose & start_pose,
+    nav_msgs::msg::Path & path,
+    const nav2_costmap_2d::Costmap2D * costmap,
+    const bool & reversing_segment);
+
+  /**
+   * @brief Enforced minimum curvature boundary conditions on plan output
+   * the robot is traveling in the same direction (e.g. forward vs reverse)
+   * @param end_pose End pose of the feasible path to maintain
+   * @param path Path to modify for curvature constraints on start / end of path
+   * @param costmap Costmap to check for collisions
+   * @param reversing_segment Whether this path segment is in reverse
+   */
+  void enforceEndBoundaryConditions(
+    const geometry_msgs::msg::Pose & end_pose,
+    nav_msgs::msg::Path & path,
+    const nav2_costmap_2d::Costmap2D * costmap,
+    const bool & reversing_segment);
+
+  /**
+   * @brief Given a set of boundary expansion, find the one which is shortest
+   * such that it is least likely to contain a loop-de-loop when working with
+   * close-by primitive markers. Instead, select a further away marker which
+   * generates a shorter `
+   * @param boundary_expansions Set of boundary expansions
+   * @return Idx of the shorest boundary expansion option
+   */
+  unsigned int findShortestBoundaryExpansionIdx(const BoundaryExpansions & boundary_expansions);
+
+  /**
+   * @brief Populate a motion model expansion from start->end into expansion
+   * @param start Start pose of the feasible path to maintain
+   * @param end End pose of the feasible path to maintain
+   * @param expansion Expansion object to populate
+   * @param costmap Costmap to check for collisions
+   * @param reversing_segment Whether this path segment is in reverse
+   */
+  void findBoundaryExpansion(
+    const geometry_msgs::msg::Pose & start,
+    const geometry_msgs::msg::Pose & end,
+    BoundaryExpansion & expansion,
+    const nav2_costmap_2d::Costmap2D * costmap);
+
+  /**
+   * @brief Generates boundary expansions with end idx at least strategic
+   * distances away, using either Reverse or (Forward) Path Iterators.
+   * @param start iterator to start search in path for
+   * @param end iterator to end search for
+   * @return Boundary expansions with end idxs populated
+   */
+  template<typename IteratorT>
+  BoundaryExpansions generateBoundaryExpansionPoints(IteratorT start, IteratorT end);
+
+  /**
+   * @brief For a given path, update the path point orientations based on smoothing
+   * @param path Path to approximate the path orientation in
+   * @param reversing_segment Return if this is a reversing segment
+   */
+  inline void updateApproximatePathOrientations(
+    nav_msgs::msg::Path & path,
+    bool & reversing_segment);
+
+  double min_turning_rad_, tolerance_, data_w_, smooth_w_;
+  int max_its_, refinement_ctr_;
+  bool is_holonomic_, do_refinement_;
+  MotionModel motion_model_;
+  ompl::base::StateSpacePtr state_space_;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__SMOOTHER_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/types.hpp

@@ -0,0 +1,169 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__TYPES_HPP_
+#define NAV2_SMAC_PLANNER__TYPES_HPP_
+
+#include <vector>
+#include <utility>
+#include <string>
+#include <memory>
+
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_smac_planner
+{
+
+typedef std::pair<float, unsigned int> NodeHeuristicPair;
+
+/**
+ * @struct nav2_smac_planner::SearchInfo
+ * @brief Search properties and penalties
+ */
+struct SearchInfo
+{
+  float minimum_turning_radius;
+  float non_straight_penalty;
+  float change_penalty;
+  float reverse_penalty;
+  float cost_penalty;
+  float retrospective_penalty;
+  float rotation_penalty;
+  float analytic_expansion_ratio;
+  float analytic_expansion_max_length;
+  std::string lattice_filepath;
+  bool cache_obstacle_heuristic;
+  bool allow_reverse_expansion;
+};
+
+/**
+ * @struct nav2_smac_planner::SmootherParams
+ * @brief Parameters for the smoother
+ */
+struct SmootherParams
+{
+  /**
+   * @brief A constructor for nav2_smac_planner::SmootherParams
+   */
+  SmootherParams()
+  : holonomic_(false)
+  {
+  }
+
+  /**
+   * @brief Get params from ROS parameter
+   * @param node Ptr to node
+   * @param name Name of plugin
+   */
+  void get(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, const std::string & name)
+  {
+    std::string local_name = name + std::string(".smoother.");
+
+    // Smoother params
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "tolerance", rclcpp::ParameterValue(1e-10));
+    node->get_parameter(local_name + "tolerance", tolerance_);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "max_iterations", rclcpp::ParameterValue(1000));
+    node->get_parameter(local_name + "max_iterations", max_its_);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "w_data", rclcpp::ParameterValue(0.2));
+    node->get_parameter(local_name + "w_data", w_data_);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "w_smooth", rclcpp::ParameterValue(0.3));
+    node->get_parameter(local_name + "w_smooth", w_smooth_);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "do_refinement", rclcpp::ParameterValue(true));
+    node->get_parameter(local_name + "do_refinement", do_refinement_);
+  }
+
+  double tolerance_;
+  int max_its_;
+  double w_data_;
+  double w_smooth_;
+  bool holonomic_;
+  bool do_refinement_;
+};
+
+/**
+ * @struct nav2_smac_planner::MotionPose
+ * @brief A struct for poses in motion primitives
+ */
+struct MotionPose
+{
+  /**
+   * @brief A constructor for nav2_smac_planner::MotionPose
+   */
+  MotionPose() {}
+
+  /**
+   * @brief A constructor for nav2_smac_planner::MotionPose
+   * @param x X pose
+   * @param y Y pose
+   * @param theta Angle of pose
+   */
+  MotionPose(const float & x, const float & y, const float & theta)
+  : _x(x), _y(y), _theta(theta)
+  {}
+
+  MotionPose operator-(const MotionPose & p2)
+  {
+    return MotionPose(this->_x - p2._x, this->_y - p2._y, this->_theta - p2._theta);
+  }
+
+  float _x;
+  float _y;
+  float _theta;
+};
+
+typedef std::vector<MotionPose> MotionPoses;
+
+/**
+ * @struct nav2_smac_planner::LatticeMetadata
+ * @brief A struct of all lattice metadata
+ */
+struct LatticeMetadata
+{
+  float min_turning_radius;
+  float grid_resolution;
+  unsigned int number_of_headings;
+  std::vector<float> heading_angles;
+  unsigned int number_of_trajectories;
+  std::string motion_model;
+};
+
+/**
+ * @struct nav2_smac_planner::MotionPrimitive
+ * @brief A struct of all motion primitive data
+ */
+struct MotionPrimitive
+{
+  unsigned int trajectory_id;
+  float start_angle;
+  float end_angle;
+  float turning_radius;
+  float trajectory_length;
+  float arc_length;
+  float straight_length;
+  bool left_turn;
+  MotionPoses poses;
+};
+
+typedef std::vector<MotionPrimitive> MotionPrimitives;
+typedef std::vector<MotionPrimitive *> MotionPrimitivePtrs;
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__TYPES_HPP_

navigation2/nav2_smac_planner/include/nav2_smac_planner/utils.hpp

@@ -0,0 +1,159 @@
+// Copyright (c) 2021, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef NAV2_SMAC_PLANNER__UTILS_HPP_
+#define NAV2_SMAC_PLANNER__UTILS_HPP_
+
+#include <vector>
+#include <memory>
+
+#include "Eigen/Core"
+#include "geometry_msgs/msg/quaternion.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "tf2/utils.h"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_costmap_2d/inflation_layer.hpp"
+
+namespace nav2_smac_planner
+{
+
+/**
+* @brief Create an Eigen Vector2D of world poses from continuous map coords
+* @param mx float of map X coordinate
+* @param my float of map Y coordinate
+* @param costmap Costmap pointer
+* @return Eigen::Vector2d eigen vector of the generated path
+*/
+inline geometry_msgs::msg::Pose getWorldCoords(
+  const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap)
+{
+  geometry_msgs::msg::Pose msg;
+  msg.position.x =
+    static_cast<float>(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution();
+  msg.position.y =
+    static_cast<float>(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution();
+  return msg;
+}
+
+/**
+* @brief Create quaternion from radians
+* @param theta continuous bin coordinates angle
+* @return quaternion orientation in map frame
+*/
+inline geometry_msgs::msg::Quaternion getWorldOrientation(
+  const float & theta)
+{
+  // theta is in radians already
+  tf2::Quaternion q;
+  q.setEuler(0.0, 0.0, theta);
+  return tf2::toMsg(q);
+}
+
+/**
+* @brief Find the min cost of the inflation decay function for which the robot MAY be
+* in collision in any orientation
+* @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation)
+* @return double circumscribed cost, any higher than this and need to do full footprint collision checking
+* since some element of the robot could be in collision
+*/
+inline double findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
+{
+  double result = -1.0;
+  bool inflation_layer_found = false;
+  std::vector<std::shared_ptr<nav2_costmap_2d::Layer>>::iterator layer;
+
+  // check if the costmap has an inflation layer
+  for (layer = costmap->getLayeredCostmap()->getPlugins()->begin();
+    layer != costmap->getLayeredCostmap()->getPlugins()->end();
+    ++layer)
+  {
+    std::shared_ptr<nav2_costmap_2d::InflationLayer> inflation_layer =
+      std::dynamic_pointer_cast<nav2_costmap_2d::InflationLayer>(*layer);
+    if (!inflation_layer) {
+      continue;
+    }
+
+    inflation_layer_found = true;
+    double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
+    double resolution = costmap->getCostmap()->getResolution();
+    result = static_cast<double>(inflation_layer->computeCost(circum_radius / resolution));
+  }
+
+  if (!inflation_layer_found) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("computeCircumscribedCost"),
+      "No inflation layer found in costmap configuration. "
+      "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+      "field to speed up collision checking by only checking the full footprint "
+      "when robot is within possibly-inscribed radius of an obstacle. This may "
+      "significantly slow down planning times!");
+  }
+
+  return result;
+}
+
+/**
+ * @brief convert json to lattice metadata
+ * @param[in] json json object
+ * @param[out] lattice meta data
+ */
+inline void fromJsonToMetaData(const nlohmann::json & json, LatticeMetadata & lattice_metadata)
+{
+  json.at("turning_radius").get_to(lattice_metadata.min_turning_radius);
+  json.at("grid_resolution").get_to(lattice_metadata.grid_resolution);
+  json.at("num_of_headings").get_to(lattice_metadata.number_of_headings);
+  json.at("heading_angles").get_to(lattice_metadata.heading_angles);
+  json.at("number_of_trajectories").get_to(lattice_metadata.number_of_trajectories);
+  json.at("motion_model").get_to(lattice_metadata.motion_model);
+}
+
+/**
+ * @brief convert json to pose
+ * @param[in] json json object
+ * @param[out] pose
+ */
+inline void fromJsonToPose(const nlohmann::json & json, MotionPose & pose)
+{
+  pose._x = json[0];
+  pose._y = json[1];
+  pose._theta = json[2];
+}
+
+/**
+ * @brief convert json to motion primitive
+ * @param[in] json json object
+ * @param[out] motion primitive
+ */
+inline void fromJsonToMotionPrimitive(
+  const nlohmann::json & json, MotionPrimitive & motion_primitive)
+{
+  json.at("trajectory_id").get_to(motion_primitive.trajectory_id);
+  json.at("start_angle_index").get_to(motion_primitive.start_angle);
+  json.at("end_angle_index").get_to(motion_primitive.end_angle);
+  json.at("trajectory_radius").get_to(motion_primitive.turning_radius);
+  json.at("trajectory_length").get_to(motion_primitive.trajectory_length);
+  json.at("arc_length").get_to(motion_primitive.arc_length);
+  json.at("straight_length").get_to(motion_primitive.straight_length);
+  json.at("left_turn").get_to(motion_primitive.left_turn);
+
+  for (unsigned int i = 0; i < json["poses"].size(); i++) {
+    MotionPose pose;
+    fromJsonToPose(json["poses"][i], pose);
+    motion_primitive.poses.push_back(pose);
+  }
+}
+
+}  // namespace nav2_smac_planner
+
+#endif  // NAV2_SMAC_PLANNER__UTILS_HPP_

navigation2/nav2_smac_planner/lattice_primitives/README.md

@@ -0,0 +1,168 @@
+# Lattice Primitive Generator
+## Contents
+
+- **[About](#about)**
+- **[Setup](#setup)**
+- **[Usage](#usage)**
+- **[Parameters](#parameters)**
+- **[Output file structure](#output-file-structure)**
+- **[How it Works](#how-it-works)**
+</br>
+
+## About
+The scripts in this folder are used to generate the minimum control set for the state lattice planner. This work is based on [Generating Near Minimal Control Sets for Constrained Motion Planning in Discrete State Spaces](https://www.ri.cmu.edu/pub_files/pub4/pivtoraiko_mihail_2005_1/pivtoraiko_mihail_2005_1.pdf). An example of the trajectories for a grid resolution of 5cm and turning radius of 0.5m is shown below.
+
+![ ](docs/all_trajectories.png)
+
+## Setup
+To install the required python packages run the following command
+
+```
+pip install -r requirements.txt
+```
+
+## Usage
+Run the primitive generator by using the following command
+```
+python3 generate_motion_primitives.py [--config] [--output] [--visualizations]
+```
+
+To adjust the settings to fit your particular needs you can edit the parameters in the [config.json](config.json) file. Alternatively, you can create your own file and pass it in using the --config flag.
+
+The output file can be specified by passing in a path with the --output flag. The default is set to save in a file called output.json in the same directory as this README.
+
+The directory to save the visualizations can be specified by passing in a path with the --visualizations flag.
+
+## Parameters ##
+Note: None of these parameters have defaults. They all must be specified through the [config.json](config.json) file.
+
+**motion_model** (string)
+    
+The type of motion model used. Accepted values:
+- `ackermann`: Only forward and reversing trajectories
+- `diff`: Forward moving trajectories + rotation in place by a single angular bin
+- `omni`: Forward moving trajectories + rotation in place by a single angular bin + sideways sliding motions
+</br>
+</br>
+
+**turning_radius** (float)
+
+The minimum turning radius of the robot (in meters). Typical values for a service robot range from 0.4 to 1.0m.
+</br>
+</br>
+
+**grid_resolution** (float)
+
+The resolution of the grid (in meters) used to create the lattice. If the grid resolution is too large proportionally to the turning radius then the generator will not return a good result. This should be the same as your costmap resolution.
+</br>
+</br>
+
+**stopping_threshold** (float)
+
+Number of consecutive iterations without a valid trajectory before stopping the search. A value too low may mean that you stop searching too early. A value too high will cause the search to take longer. We found that stopping after 5 iterations produced the best results.
+</br>
+</br>
+
+**num_of_headings** (float)
+
+Number of discrete angular headings used. Due to the way discretization is done this number should be restricted to multiples of 8. Angles are not generated uniformly but instead generated in a way to facilitate straight lines. See [angle discretization](#angle-discretization) for more details. We believe 16 headings is a good number for most use cases.
+</br>
+</br>
+
+## Output file structure
+The output file is a JSON file and contains the following fields:
+
+**version**
+
+The version number of the lattice generator that created the output file
+
+**date_generated**
+
+The date the output file was generated. Format: YYYY-MM-DD
+
+**lattice_metadata**
+
+A dictionary that contains information about the generated lattice. Most of this data comes from the config file used when generating the primitives. More information on each field is given in the [Parameters](#parameters) section. Includes the following fields:
+- **motion_model**
+- **turning_radius** (meters)
+- **grid_resolution** (meters)
+- **stopping_threshold**
+- **num_of_headings**
+- **heading_angles**
+    - A list of the heading angles (in radians) that are used in the primitives
+- **number_of_trajectories**
+    - The total number of trajectories contained in the output file
+
+**primitives**
+
+A list of dictionaries where each dictionary represents an individual motion primitive. Each motion primitive contains the following fields:
+- **trajectory_id**
+    - The id associated with the primitive
+- **start_angle_index**
+    - The start angle of the primitive represented as an index for the heading_angle list given in the lattice_metadata
+- **end_angle_index**
+    - The end angle of the primitive represented as an index for the heading_angle list given in the lattice_metadata
+- **left_turn**
+    - A boolean value that is true if the path curves to the left. Straight paths default to true.
+- **trajectory_radius** (meters)
+    - The radius of the circle that was used to create the arc portion of a primitive. trajectory_radius is 0 if the primitive is purely a straight line
+- **trajectory_length** (meters)
+    - The length of the primitive
+- **arc_length** (meters)
+    - The length of the arc portion of a primitive. arc_length is 0 if the primitive is a straight line
+- **straight_length** (meters)
+    - The length of the straight line portion of a primitive. straight_length is 0 if the primitive is a pure arc.
+- **poses**
+    - A list where each entry is a list containing three values: x, y, and yaw (radians)
+
+## How it works
+This section describes how the various portions of the generation algorithm works.
+
+### Angle Discretization
+Dividing a full turn into uniform angular headings presents several problems. The biggest problem is that it will create angles for which a straight trajectory does not land nicely on an endpoint that aligns with the grid. Instead we discretize the angles in a way that ensures straight paths will land on endpoints aligned with the grid. 
+
+![ ](docs/angle_discretization.png)
+
+The image shows how the angular headings are generated. The same idea can be extended to a higher number of headings. As a result, the number of headings parameter is restricted to multiples of 8.  
+
+### Trajectory Generator
+1. Create two lines. Line 1 passes through start point with angle of start angle, and line 2 passes through the end point with angle of end angle
+
+2. Find the intersection point I of lines 1 and 2
+
+3. Calculate the distance beween I and the origin (let this be d1). Also calculate the distance between I and the end point (let this be d2)
+
+4. If d1 and d2 are equal then proceed to step 5. Otherwise, create intermediate points for each line that are min(d1, d2) distance away along the lines from I. So there should be an intermediate point on line 1 and an intermediate point on line 2. One of these intermediate points should align with either the origin or the end point by nature of how the distance was calculated.
+
+5. Create perpindicular lines for line 1 and line 2 that pass through the respective intermediate points. The intersection of these perpindicular lines is the centre of the circle whose arc represents the curved portion of the trajectory.
+
+6. Finally, if needed append straight segments to the path to ensure we start at the origin and end at the end point.
+
+
+There are several checks we need to make to ensure a valid trajectory is generated:
+- If the start and end angles are parallel then the lines must overlap
+- The intersection point must occur before the end point on line 2 and after the origin on line 1
+- The radius of the generated trajectory must be less than the user supplied minimum turning radius
+
+### Lattice Generator
+The lattice generator is generally based on the generation of the control set as described in [Generating Near Minimal Control Sets for Constrained Motion Planning in Discrete State Spaces](https://www.ri.cmu.edu/pub_files/pub4/pivtoraiko_mihail_2005_1/pivtoraiko_mihail_2005_1.pdf). However, some changes were made to the above method. A brief outline of the implemented method is given below:
+
+1. Create a wavefront that begins a minimum trajectory length away from the origin.
+
+    - The minimum trajectory length is defined as the length a trajectory needs to move from one discrete heading to the next. (Since the headings are not separated equally we use the smallest heading change)
+
+2. Generate paths to all points on this wavefront for all possible end heading angles.
+
+3. When a path is generated it is checked to ensure it does not pass "close" to another path. If it does it is removed, otherwise it remains in the set
+
+    - "Close" is defined to be within half the grid resolution for length and half the average angular bin size for angular rotation
+
+4. Steps 2-3 are repeated for the next wavefront which is a grid resolution step further away from the origin.
+
+5. Steps 1-4 are repeated untill all trajectories are being removed. The generator will continue for a few more wavefront steps until N wavefronts have been searched with no new trajectories. At this point the generator terminates and returns the computed minimal set.
+
+    - The number N is the stopping_threshold parameter
+
+6. Steps 1-5 are repeated for all possible start angles between 0 and 90.
+
+7. The resulting control set will only contain trajectories in quadrant 1. To get the final control set we exploit symmetry across the axess and flip the trajectories in different ways.

navigation2/nav2_smac_planner/lattice_primitives/config.json

@@ -0,0 +1,7 @@
+{
+    "motion_model": "ackermann",
+    "turning_radius": 0.5,
+    "grid_resolution": 0.05,
+    "stopping_threshold": 5,
+    "num_of_headings": 16
+}

navigation2/nav2_smac_planner/lattice_primitives/constants.py

@@ -0,0 +1,15 @@
+# Copyright (c) 2021, Matthew Booker
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License. Reserved.
+
+VERSION = 1.0

navigation2/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py

@@ -0,0 +1,260 @@
+# Copyright (c) 2021, Matthew Booker
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License. Reserved.
+
+import argparse
+from datetime import datetime
+import json
+import logging
+from pathlib import Path
+import time
+
+import constants
+from lattice_generator import LatticeGenerator
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+logging.basicConfig(level=logging.INFO)
+logger = logging.getLogger(__name__)
+
+
+def handle_arg_parsing():
+    """
+    Handle the parsing of arguments.
+
+    Returns
+    -------
+    argparse.Namespace
+        An object containing all parsed arguments
+
+    """
+    parser = argparse.ArgumentParser(description='Generate motionprimitives '
+                                                 "for Nav2's State "
+                                                 'Lattice Planner')
+    parser.add_argument('--config',
+                        type=Path,
+                        default='./config.json',
+                        help='The config file containing the '
+                        'parameters to be used')
+    parser.add_argument('--output',
+                        type=Path,
+                        default='./output.json',
+                        help='The output file containing the '
+                        'trajectory data')
+    parser.add_argument('--visualizations',
+                        type=Path,
+                        default='./visualizations',
+                        help='The output folder where the '
+                        'visualizations of the trajectories will be saved')
+
+    return parser.parse_args()
+
+
+def create_heading_angle_list(minimal_set_trajectories: dict) -> list:
+    """
+    Create a sorted list of heading angles from the minimal trajectory set.
+
+    Args:
+    ----
+    minimal_set_trajectories: dict
+        The minimal spanning set
+
+    Returns
+    -------
+    list
+        A sorted list of heading angles
+
+    """
+    heading_angles = set(minimal_set_trajectories.keys())
+    return sorted(heading_angles, key=lambda x: (x < 0, x))
+
+
+def read_config(config_path) -> dict:
+    """
+    Read in the user defined parameters via JSON.
+
+    Args:
+    ----
+    config_path: Path
+        Path to the config file
+
+    Returns
+    -------
+    dict
+        Dictionary containing the user defined parameters
+
+    """
+    with open(config_path) as config_file:
+        config = json.load(config_file)
+
+    return config
+
+
+def create_header(config: dict, minimal_set_trajectories: dict) -> dict:
+    """
+    Create a dict containing all the fields to populate the header with.
+
+    Args:
+    ----
+    config: dict
+        The dict containing user specified parameters
+    minimal_set_trajectories: dict
+        The minimal spanning set
+
+    Returns
+    -------
+    dict
+        A dictionary containing the fields to populate the header with
+
+    """
+    header_dict = {
+        'version': constants.VERSION,
+        'date_generated': datetime.today().strftime('%Y-%m-%d'),
+        'lattice_metadata': {},
+        'primitives': [],
+    }
+
+    for key, value in config.items():
+        header_dict['lattice_metadata'][key] = value
+
+    heading_angles = create_heading_angle_list(minimal_set_trajectories)
+    adjusted_heading_angles = [angle + 2*np.pi if angle < 0 else angle for angle in heading_angles]
+
+    header_dict['lattice_metadata']['heading_angles'] = adjusted_heading_angles
+
+    return header_dict
+
+
+def write_to_json(output_path: Path, minimal_set_trajectories: dict, config: dict) -> None:
+    """
+    Write the minimal spanning set to an output file.
+
+    Args:
+    ----
+    output_path: Path
+        The output file for the json data
+    minimal_set_trajectories: dict
+        The minimal spanning set
+    config: dict
+        The dict containing user specified parameters
+
+    """
+    output_dict = create_header(config, minimal_set_trajectories)
+
+    trajectory_start_angles = list(minimal_set_trajectories.keys())
+
+    heading_angle_list = create_heading_angle_list(minimal_set_trajectories)
+    heading_lookup = {angle: idx for idx, angle in
+                      enumerate(heading_angle_list)}
+
+    idx = 0
+    for start_angle in sorted(trajectory_start_angles,
+                              key=lambda x: (x < 0, x)):
+
+        for trajectory in sorted(
+            minimal_set_trajectories[start_angle],
+            key=lambda x: x.parameters.end_angle
+        ):
+
+            traj_info = {}
+            traj_info['trajectory_id'] = idx
+            traj_info['start_angle_index'] = heading_lookup[trajectory.parameters.start_angle]
+            traj_info['end_angle_index'] = heading_lookup[trajectory.parameters.end_angle]
+            traj_info['left_turn'] = bool(trajectory.parameters.left_turn)
+            traj_info['trajectory_radius'] = \
+                trajectory.parameters.turning_radius
+            traj_info['trajectory_length'] = round(
+                trajectory.parameters.total_length, 5
+            )
+            traj_info['arc_length'] = round(
+                trajectory.parameters.arc_length,
+                5
+            )
+            traj_info['straight_length'] = round(
+                trajectory.parameters.start_straight_length
+                + trajectory.parameters.end_straight_length,
+                5,
+            )
+            traj_info['poses'] = trajectory.path.to_output_format()
+
+            output_dict['primitives'].append(traj_info)
+            idx += 1
+
+    output_dict['lattice_metadata']['number_of_trajectories'] = idx
+
+    with open(output_path, 'w') as output_file:
+        json.dump(output_dict, output_file, indent='\t')
+
+
+def save_visualizations(visualizations_folder: Path, minimal_set_trajectories: dict) -> None:
+    """
+    Draw the visualizations for every trajectory and save it as an image.
+
+    Args:
+    ----
+    visualizations_folder: Path
+        The path to the folder for where to save the images
+    minimal_set_trajectories: dict
+        The minimal spanning set
+
+    """
+    # Create the directory if it doesnt exist
+    visualizations_folder.mkdir(exist_ok=True)
+
+    for start_angle in minimal_set_trajectories.keys():
+
+        for trajectory in minimal_set_trajectories[start_angle]:
+            plt.plot(trajectory.path.xs, trajectory.path.ys, 'b')
+
+    plt.grid(True)
+    plt.axis('square')
+    left_x, right_x = plt.xlim()
+    left_y, right_y = plt.ylim()
+
+    output_path = visualizations_folder / 'all_trajectories.png'
+    plt.savefig(output_path)
+    plt.clf()
+
+    for start_angle in minimal_set_trajectories.keys():
+
+        angle_in_deg = np.rad2deg(start_angle)
+
+        if start_angle < 0 or start_angle > np.pi / 2:
+            continue
+
+        for trajectory in minimal_set_trajectories[start_angle]:
+            plt.plot(trajectory.path.xs, trajectory.path.ys, 'b')
+            plt.xlim(left_x, right_x)
+            plt.ylim(left_y, right_y)
+
+        plt.grid(True)
+
+        output_path = visualizations_folder / f'{angle_in_deg}.png'
+        plt.savefig(output_path)
+        plt.clf()
+
+
+if __name__ == '__main__':
+
+    args = handle_arg_parsing()
+    config = read_config(args.config)
+
+    start = time.time()
+    lattice_gen = LatticeGenerator(config)
+    minimal_set_trajectories = lattice_gen.run()
+    print(f'Finished Generating. Took {time.time() - start} seconds')
+
+    write_to_json(args.output, minimal_set_trajectories, config)
+    save_visualizations(args.visualizations, minimal_set_trajectories)

navigation2/nav2_smac_planner/lattice_primitives/helper.py

@@ -0,0 +1,127 @@
+# Copyright (c) 2021, Matthew Booker
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License. Reserved.
+
+import numpy as np
+
+
+def normalize_angle(angle):
+    """
+    Normalize the angle to between [0, 2pi).
+
+    Args:
+    angle: float
+        The angle to normalize in radians
+
+    Returns
+    -------
+    The normalized angle in the range [0,2pi)
+
+    """
+    while angle >= 2*np.pi:
+        angle -= 2*np.pi
+
+    while angle < 0:
+        angle += 2*np.pi
+
+    return angle
+
+
+def angle_difference(angle_1, angle_2, left_turn=None):
+    """
+    Calculate the difference between two angles based on a given direction.
+
+    Args:
+    angle_1: float
+        The starting angle in radians
+    angle_2: float
+        The ending angle in radians
+    left_turn: bool
+        The direction of turn. True if left, false if right
+        and None if smallest angular difference should be
+        returned
+
+    Returns
+    -------
+    The angular difference between the two angles according to
+    the specified turn direction
+
+    """
+    if left_turn is None:
+        dif = abs(angle_1 - angle_2)
+
+        return dif if dif <= np.pi else 2 * np.pi - dif
+
+    elif left_turn:
+
+        if angle_2 >= angle_1:
+            return abs(angle_1 - angle_2)
+        else:
+            return 2 * np.pi - abs(angle_1 - angle_2)
+
+    else:
+        if angle_1 >= angle_2:
+            return abs(angle_1 - angle_2)
+        else:
+            return 2 * np.pi - abs(angle_1 - angle_2)
+
+
+def interpolate_yaws(start_angle, end_angle, left_turn, steps):
+    """
+    Create equally spaced yaws between two angles.
+
+    Args:
+    start_angle: float
+        The starting angle
+    end_angle: float
+        The ending angle
+    left_turn: bool
+        The direction of turn. True if left, False otherwise
+    steps: int
+        The number of yaws to generate between start and end
+        angle
+
+    Returns
+    -------
+    An array of yaws starting at start angle and ending at end
+    angle with steps number of angles between them
+
+    """
+    if left_turn:
+        if start_angle > end_angle:
+            end_angle += 2 * np.pi
+    else:
+        if end_angle > start_angle:
+            end_angle -= 2 * np.pi
+
+    yaws = np.linspace(start_angle, end_angle, steps)
+    yaws = np.vectorize(normalize_angle)(yaws)
+
+    return yaws
+
+
+def get_rotation_matrix(angle):
+    """
+    Return a rotation matrix that is equivalent to a 2D rotation of angle.
+
+    Args:
+    angle: float
+        The angle to create a rotation matrix for
+
+    Returns
+    -------
+    A 2x2 matrix representing a 2D rotation by angle
+
+    """
+    return np.array([[np.cos(angle), -np.sin(angle)],
+                    [np.sin(angle), np.cos(angle)]])

navigation2/nav2_smac_planner/lattice_primitives/lattice_generator.py

@@ -0,0 +1,745 @@
+# Copyright (c) 2021, Matthew Booker
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License. Reserved.
+
+from collections import defaultdict
+from enum import Enum
+
+from helper import angle_difference, interpolate_yaws
+
+import numpy as np
+
+from rtree import index
+
+from trajectory import Path, Trajectory, TrajectoryParameters
+
+from trajectory_generator import TrajectoryGenerator
+
+
+class LatticeGenerator:
+    """
+    Handles all the logic for computing the minimal control set.
+
+    Computes the minimal control set for a vehicle given its parameters.
+    Includes handling the propogating and searching along wavefronts as
+    well as determining if a trajectory is part of the minimal set based
+    on previously added trajectories.
+    """
+
+    class MotionModel(Enum):
+        """An Enum used for determining the motion model to use."""
+
+        ACKERMANN = 1
+        DIFF = 2
+        OMNI = 3
+
+    class Flip(Enum):
+        """An Enum used for determining how a trajectory should be flipped."""
+
+        X = 1
+        Y = 2
+        BOTH = 3
+
+    def __init__(self, config: dict):
+        """Init the lattice generator from the user supplied config."""
+        self.trajectory_generator = TrajectoryGenerator(config)
+        self.grid_resolution = config['grid_resolution']
+        self.turning_radius = config['turning_radius']
+        self.stopping_threshold = config['stopping_threshold']
+        self.num_of_headings = config['num_of_headings']
+        self.headings = \
+            self._get_heading_discretization(config['num_of_headings'])
+
+        self.motion_model = self.MotionModel[config['motion_model'].upper()]
+
+        self.DISTANCE_THRESHOLD = 0.5 * self.grid_resolution
+        self.ROTATION_THRESHOLD = 0.5 * (2 * np.pi / self.num_of_headings)
+
+    def _get_wave_front_points(self, pos: int) -> np.array:
+        """
+        Calculate the end points that lie on the wave front.
+
+        Uses the user supplied grid resolution to calculate the
+        valid end points that lie on a wave front at a discrete
+        interval away from the origin.
+
+        Args:
+        pos: int
+            The number of discrete intervals of grid resolution
+            away from the origin to generate the wave points at
+
+        Returns
+        -------
+        np.array
+            An array of coordinates
+
+        """
+        positions = []
+
+        max_point_coord = self.grid_resolution * pos
+
+        for i in range(pos):
+            varying_point_coord = self.grid_resolution * i
+
+            # Change the y and keep x at max
+            positions.append((max_point_coord, varying_point_coord))
+
+            # Change the x and keep y at max
+            positions.append((varying_point_coord, max_point_coord))
+
+        # Append the corner
+        positions.append((max_point_coord, max_point_coord))
+
+        return np.array(positions)
+
+    def _get_heading_discretization(self, number_of_headings: int) -> list:
+        """
+        Calculate the heading discretization based on the number of headings.
+
+        Does not uniformly generate headings but instead generates a set of
+        discrete headings that is better suited for straight line trajectories.
+
+        Args:
+        number_of_headings: int
+            The number of headings to discretize a 360 degree turn into
+
+        Returns
+        -------
+        list
+            A list of headings in radians
+
+        """
+        max_val = int(number_of_headings / 8)
+
+        outer_edge_x = []
+        outer_edge_y = []
+
+        # Generate points that lie on the perimeter of the surface
+        # of a square with sides of length max_val
+        for i in range(-max_val, max_val + 1):
+            outer_edge_x.extend([i, i])
+            outer_edge_y.extend([-max_val, max_val])
+
+            if i != max_val and i != -max_val:
+                outer_edge_x.extend([-max_val, max_val])
+                outer_edge_y.extend([i, i])
+
+        return sorted([np.arctan2(j, i) for i, j in zip(outer_edge_x, outer_edge_y)])
+
+    def _point_to_line_distance(self, p1: np.array, p2: np.array, q: np.array) -> float:
+        """
+        Return the shortest distance from a point to a line segment.
+
+        Args:
+        p1: np.array(2,)
+            Start point of line segment
+        p2: np.array(2,)
+            End point of line segment
+        q: np.array(2,)
+            Point to get distance away from line of
+
+        Returns
+        -------
+        float
+            The shortest distance between q and line segment p1p2
+
+        """
+        # Get back the l2-norm without the square root
+        l2 = np.inner(p1 - p2, p1 - p2)
+
+        if l2 == 0:
+            return np.linalg.norm(p1 - q)
+
+        # Ensure t lies in [0, 1]
+        t = max(0, min(1, np.dot(q - p1, p2 - p1) / l2))
+        projected_point = p1 + t * (p2 - p1)
+
+        return np.linalg.norm(q - projected_point)
+
+    def _is_minimal_trajectory(
+        self, trajectory: Trajectory, prior_end_poses: index.Rtree
+    ) -> bool:
+        """
+        Determine wheter a trajectory is a minimal trajectory.
+
+        Uses an RTree for speedup.
+
+        Args:
+        trajectory: Trajectory
+            The trajectory to check
+        prior_end_poses: RTree
+            An RTree holding the current minimal set of trajectories
+
+        Returns
+        -------
+        bool
+            True if the trajectory is a minimal trajectory otherwise false
+
+        """
+        # Iterate over line segments in the trajectory
+        for x1, y1, x2, y2, yaw in zip(
+            trajectory.path.xs[:-1],
+            trajectory.path.ys[:-1],
+            trajectory.path.xs[1:],
+            trajectory.path.ys[1:],
+            trajectory.path.yaws[:-1],
+        ):
+
+            p1 = np.array([x1, y1])
+            p2 = np.array([x2, y2])
+
+            # Create a bounding box search region
+            # around the line segment
+            left_bb = min(x1, x2) - self.DISTANCE_THRESHOLD
+            right_bb = max(x1, x2) + self.DISTANCE_THRESHOLD
+            top_bb = max(y1, y2) + self.DISTANCE_THRESHOLD
+            bottom_bb = min(y1, y2) - self.DISTANCE_THRESHOLD
+
+            # For any previous end points in the search region we
+            # check the distance to that point and the angle
+            # difference. If they are within threshold then this
+            # trajectory can be composed from a previous trajectory
+            for prior_end_pose in prior_end_poses.intersection(
+                (left_bb, bottom_bb, right_bb, top_bb), objects='raw'
+            ):
+                if (
+                    self._point_to_line_distance(p1, p2, prior_end_pose[:-1])
+                    < self.DISTANCE_THRESHOLD
+                    and angle_difference(yaw, prior_end_pose[-1])
+                    < self.ROTATION_THRESHOLD
+                ):
+                    return False
+
+        return True
+
+    def _compute_min_trajectory_length(self) -> float:
+        """
+        Compute the minimum trajectory length for the given parameters.
+
+        The minimum trajectory length is defined as the length needed
+        for the sharpest possible turn to move from 0 degrees to the next
+        discrete heading. Since the distance between headings is not uniform
+        we take the smallest possible difference.
+
+        Returns
+        -------
+        float
+            The minimal length of a trajectory
+
+        """
+        # Compute arc length for a turn that moves from 0 degrees to
+        # the minimum heading difference
+        heading_diff = [
+            abs(self.headings[i + 1] - self.headings[i])
+            for i in range(len(self.headings) - 1)
+        ]
+
+        return self.turning_radius * min(heading_diff)
+
+    def _generate_minimal_spanning_set(self) -> dict:
+        """
+        Generate the minimal spanning set.
+
+        Iteratves over all possible trajectories and keeps only those that
+        are part of the minimal set.
+
+        Returns
+        -------
+        dict
+            A dictionary where the key is the start_angle and the value is
+            a list of trajectories that begin at that angle
+
+        """
+        quadrant1_end_poses = defaultdict(list)
+
+        # Since we only compute for quadrant 1 we only need headings between
+        # 0 and 90 degrees
+        initial_headings = sorted(
+            filter(lambda x: 0 <= x and x <= np.pi / 2, self.headings)
+        )
+
+        # Use the minimum trajectory length to find the starting wave front
+        min_trajectory_length = self._compute_min_trajectory_length()
+        wave_front_start_pos = int(
+            np.round(min_trajectory_length / self.grid_resolution)
+        )
+
+        for start_heading in initial_headings:
+            iterations_without_trajectory = 0
+
+            prior_end_poses = index.Index()
+
+            wave_front_cur_pos = wave_front_start_pos
+
+            # To get target headings: sort headings radially and remove those
+            # that are more than 90 degrees away
+            target_headings = sorted(
+                self.headings, key=lambda x: (abs(x - start_heading), -x)
+            )
+            target_headings = list(
+                filter(lambda x: abs(start_heading - x) <= np.pi / 2, target_headings)
+            )
+
+            while iterations_without_trajectory < self.stopping_threshold:
+                iterations_without_trajectory += 1
+
+                # Generate x,y coordinates for current wave front
+                positions = self._get_wave_front_points(wave_front_cur_pos)
+
+                for target_point in positions:
+                    for target_heading in target_headings:
+                        # Use 10% of grid separation for finer granularity
+                        # when checking if trajectory overlaps another already
+                        # seen trajectory
+                        trajectory = self.trajectory_generator.generate_trajectory(
+                            target_point,
+                            start_heading,
+                            target_heading,
+                            0.1 * self.grid_resolution,
+                        )
+
+                        if trajectory is not None:
+                            # Check if path overlaps something in minimal
+                            # spanning set
+                            if self._is_minimal_trajectory(trajectory, prior_end_poses):
+
+                                # Add end pose to minimal set
+                                new_end_pose = np.array(
+                                    [target_point[0], target_point[1], target_heading]
+                                )
+
+                                quadrant1_end_poses[start_heading].append(
+                                    (target_point, target_heading)
+                                )
+
+                                # Create a new bounding box in the RTree
+                                # for this trajectory
+                                left_bb = target_point[0] - self.DISTANCE_THRESHOLD
+                                right_bb = target_point[0] + self.DISTANCE_THRESHOLD
+                                bottom_bb = target_point[1] - self.DISTANCE_THRESHOLD
+                                top_bb = target_point[1] + self.DISTANCE_THRESHOLD
+
+                                prior_end_poses.insert(
+                                    0,
+                                    (left_bb, bottom_bb, right_bb, top_bb),
+                                    new_end_pose,
+                                )
+
+                                iterations_without_trajectory = 0
+
+                wave_front_cur_pos += 1
+
+        # Once we have found the minimal trajectory set for quadrant 1
+        # we can leverage symmetry to create the complete minimal set
+        return self._create_complete_minimal_spanning_set(quadrant1_end_poses)
+
+    def _flip_angle(self, angle: float, flip_type: Flip) -> float:
+        """
+        Return the the appropriate flip of the angle in self.headings.
+
+        Args:
+        angle: float
+            The angle to flip
+        flip_type: Flip
+            Whether to flip acrpss X axis, Y axis, or both
+
+        Returns
+        -------
+        float
+            The angle in self.heading that is the appropriate flip
+
+        """
+        angle_idx = self.headings.index(angle)
+
+        if flip_type == self.Flip.X:
+            heading_idx = (self.num_of_headings / 2 - 1) - angle_idx - 1
+        elif flip_type == self.Flip.Y:
+            heading_idx = self.num_of_headings - angle_idx - 2
+        elif flip_type == self.Flip.BOTH:
+            heading_idx = (
+                angle_idx - (self.num_of_headings / 2)
+            ) % self.num_of_headings
+        else:
+            raise Exception(f'Unsupported flip type: {flip_type}')
+
+        return self.headings[int(heading_idx)]
+
+    def _create_complete_minimal_spanning_set(
+        self, single_quadrant_minimal_set: dict
+    ) -> dict:
+        """
+        Create the full minimal spanning set from a single quadrant set.
+
+        Exploits the symmetry between the quadrants to create the full set.
+        This is done by flipping every trajectory in the first quadrant across
+        either the X-axis, Y-axis, or both axes.
+
+        Args:
+        single_quadrant_minimal_set: dict
+            The minimal set for quadrant 1 (positive x and positive y)
+
+        Returns
+        -------
+        dict
+            The complete minimal spanning set containing the trajectories
+            in all quadrants
+
+        """
+        all_trajectories = defaultdict(list)
+
+        for start_angle in single_quadrant_minimal_set.keys():
+
+            for end_point, end_angle in single_quadrant_minimal_set[start_angle]:
+
+                x, y = end_point
+
+                # Prevent double adding trajectories that lie on axes
+                # (i.e. start and end angle are either both 0 or both pi/2)
+                if start_angle == 0 and end_angle == 0:
+                    unflipped_start_angle = 0.0
+                    flipped_x_start_angle = np.pi
+
+                    unflipped_end_angle = 0.0
+                    flipped_x_end_angle = np.pi
+
+                    # Generate trajectories from calculated parameters
+                    unflipped_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([x, y]),
+                            unflipped_start_angle,
+                            unflipped_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+                    flipped_x_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([-x, -y]),
+                            flipped_x_start_angle,
+                            flipped_x_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+
+                    all_trajectories[
+                        unflipped_trajectory.parameters.start_angle
+                    ].append(unflipped_trajectory)
+
+                    all_trajectories[
+                        flipped_x_trajectory.parameters.start_angle
+                    ].append(flipped_x_trajectory)
+
+                elif abs(start_angle) == np.pi / 2 and abs(end_angle) == np.pi / 2:
+                    unflipped_start_angle = np.pi / 2
+                    flipped_y_start_angle = -np.pi / 2
+
+                    unflipped_end_angle = np.pi / 2
+                    flipped_y_end_angle = -np.pi / 2
+
+                    # Generate trajectories from calculated parameters
+                    unflipped_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([-x, y]),
+                            unflipped_start_angle,
+                            unflipped_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+
+                    flipped_y_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([x, -y]),
+                            flipped_y_start_angle,
+                            flipped_y_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+
+                    all_trajectories[
+                        unflipped_trajectory.parameters.start_angle
+                    ].append(unflipped_trajectory)
+                    all_trajectories[
+                        flipped_y_trajectory.parameters.start_angle
+                    ].append(flipped_y_trajectory)
+
+                else:
+                    unflipped_start_angle = start_angle
+                    flipped_x_start_angle = self._flip_angle(start_angle, self.Flip.X)
+                    flipped_y_start_angle = self._flip_angle(start_angle, self.Flip.Y)
+                    flipped_xy_start_angle = self._flip_angle(
+                        start_angle, self.Flip.BOTH
+                    )
+
+                    unflipped_end_angle = end_angle
+                    flipped_x_end_angle = self._flip_angle(end_angle, self.Flip.X)
+                    flipped_y_end_angle = self._flip_angle(end_angle, self.Flip.Y)
+                    flipped_xy_end_angle = self._flip_angle(end_angle, self.Flip.BOTH)
+
+                    # Generate trajectories from calculated parameters
+                    unflipped_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([x, y]),
+                            unflipped_start_angle,
+                            unflipped_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+                    flipped_x_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([-x, y]),
+                            flipped_x_start_angle,
+                            flipped_x_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+                    flipped_y_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([x, -y]),
+                            flipped_y_start_angle,
+                            flipped_y_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+                    flipped_xy_trajectory = (
+                        self.trajectory_generator.generate_trajectory(
+                            np.array([-x, -y]),
+                            flipped_xy_start_angle,
+                            flipped_xy_end_angle,
+                            self.grid_resolution,
+                        )
+                    )
+
+                    all_trajectories[
+                        unflipped_trajectory.parameters.start_angle
+                    ].append(unflipped_trajectory)
+                    all_trajectories[
+                        flipped_x_trajectory.parameters.start_angle
+                    ].append(flipped_x_trajectory)
+                    all_trajectories[
+                        flipped_y_trajectory.parameters.start_angle
+                    ].append(flipped_y_trajectory)
+                    all_trajectories[
+                        flipped_xy_trajectory.parameters.start_angle
+                    ].append(flipped_xy_trajectory)
+
+        return all_trajectories
+
+    def _handle_motion_model(self, spanning_set: dict) -> dict:
+        """
+        Add the appropriate motions for the user supplied motion model.
+
+        Ackerman: No additional trajectories
+
+        Diff: In place turns to the right and left
+
+        Omni: Diff + Sliding movements to right and left
+
+        Args:
+        spanning set: dict
+            The minimal spanning set
+
+        Returns
+        -------
+        dict
+            The minimal spanning set with additional trajectories based
+            on the motion model
+
+        """
+        if self.motion_model == self.MotionModel.ACKERMANN:
+            return spanning_set
+
+        elif self.motion_model == self.MotionModel.DIFF:
+            diff_spanning_set = self._add_in_place_turns(spanning_set)
+            return diff_spanning_set
+
+        elif self.motion_model == self.MotionModel.OMNI:
+            omni_spanning_set = self._add_in_place_turns(spanning_set)
+            omni_spanning_set = self._add_horizontal_motions(omni_spanning_set)
+            return omni_spanning_set
+
+        else:
+            print('No handling implemented for Motion Model: ' +
+                  f'{self.motion_model}')
+            raise NotImplementedError
+
+    def _add_in_place_turns(self, spanning_set: dict) -> dict:
+        """
+        Add in place turns to the spanning set.
+
+        In place turns are trajectories with only a rotational component and
+        only shift a single angular heading step
+
+        Args:
+        spanning_set: dict
+            The minimal spanning set
+
+        Returns
+        -------
+        dict
+            The minimal spanning set containing additional in place turns
+            for each start angle
+
+        """
+        all_angles = sorted(spanning_set.keys())
+
+        for idx, start_angle in enumerate(all_angles):
+            prev_angle_idx = idx - 1 if idx - 1 >= 0 else len(all_angles) - 1
+            next_angle_idx = idx + 1 if idx + 1 < len(all_angles) else 0
+
+            prev_angle = all_angles[prev_angle_idx]
+            next_angle = all_angles[next_angle_idx]
+
+            left_turn_params = TrajectoryParameters.no_arc(
+                end_point=np.array([0, 0]),
+                start_angle=start_angle,
+                end_angle=next_angle,
+            )
+            right_turn_params = TrajectoryParameters.no_arc(
+                end_point=np.array([0, 0]),
+                start_angle=start_angle,
+                end_angle=prev_angle,
+            )
+
+            # Calculate number of steps needed to rotate by roughly 10 degrees
+            # for each pose
+            angle_dif = angle_difference(start_angle, next_angle)
+            steps = int(round(angle_dif / np.deg2rad(10))) + 1
+
+            position = np.full(steps, 0)
+            left_yaws = interpolate_yaws(start_angle, next_angle, True, steps)
+            right_yaws = interpolate_yaws(start_angle, prev_angle, False, steps)
+
+            left_turn_path = Path(xs=position, ys=position, yaws=left_yaws)
+            right_turn_path = Path(xs=position, ys=position, yaws=right_yaws)
+
+            left_turn = Trajectory(parameters=left_turn_params, path=left_turn_path)
+            right_turn = Trajectory(parameters=right_turn_params, path=right_turn_path)
+
+            spanning_set[start_angle].append(left_turn)
+            spanning_set[start_angle].append(right_turn)
+
+        return spanning_set
+
+    def _add_horizontal_motions(self, spanning_set: dict) -> dict:
+        """
+        Add horizontal sliding motions to the spanning set.
+
+        The horizontal sliding motions are simply straight line trajectories
+        at 90 degrees to every start angle in the spanning set. The yaw of these
+        trajectories is the same as the start angle for which it is generated.
+
+        Args:
+        spanning_set: dict
+            The minimal spanning set
+
+        Returns
+        -------
+        dict
+            The minimal spanning set containing additional sliding motions
+            for each start angle
+
+        """
+        # Calculate the offset in the headings list that represents an
+        # angle change of 90 degrees
+        idx_offset = int(self.num_of_headings / 4)
+
+        for idx, angle in enumerate(self.headings):
+
+            # Copy the straight line trajectory for the start angle that
+            # is 90 degrees to the left
+            left_angle_idx = int((idx + idx_offset) % self.num_of_headings)
+            left_angle = self.headings[left_angle_idx]
+            left_trajectories = spanning_set[left_angle]
+            left_straight_trajectory = next(
+                t for t in left_trajectories if t.parameters.end_angle == left_angle
+            )
+
+            # Copy the straight line trajectory for the start angle that
+            # is 90 degrees to the right
+            right_angle_idx = int((idx - idx_offset) % self.num_of_headings)
+            right_angle = self.headings[right_angle_idx]
+            right_trajectories = spanning_set[right_angle]
+            right_straight_trajectory = next(
+                t for t in right_trajectories if t.parameters.end_angle == right_angle
+            )
+
+            yaws = np.full(
+                len(left_straight_trajectory.path.xs), angle, dtype=np.float64
+            )
+
+            # Create a new set of parameters that represents
+            # the left sliding motion
+            parmas_l = left_straight_trajectory.parameters
+            left_motion_parameters = TrajectoryParameters(
+                parmas_l.turning_radius,
+                parmas_l.x_offset,
+                parmas_l.y_offset,
+                parmas_l.end_point,
+                angle,
+                angle,
+                parmas_l.left_turn,
+                parmas_l.arc_start_point,
+                parmas_l.arc_end_point,
+            )
+
+            # Create a new set of parameters that represents
+            # the right sliding motion
+            params_r = right_straight_trajectory.parameters
+            right_motion_parameters = TrajectoryParameters(
+                params_r.turning_radius,
+                params_r.x_offset,
+                params_r.y_offset,
+                params_r.end_point,
+                angle,
+                angle,
+                params_r.left_turn,
+                parmas_l.arc_start_point,
+                parmas_l.arc_end_point,
+            )
+
+            left_motion = Trajectory(
+                parameters=left_motion_parameters,
+                path=Path(
+                    xs=left_straight_trajectory.path.xs,
+                    ys=left_straight_trajectory.path.ys,
+                    yaws=yaws,
+                ),
+            )
+
+            right_motion = Trajectory(
+                parameters=right_motion_parameters,
+                path=Path(
+                    xs=right_straight_trajectory.path.xs,
+                    ys=right_straight_trajectory.path.ys,
+                    yaws=yaws,
+                ),
+            )
+
+            spanning_set[angle].append(left_motion)
+            spanning_set[angle].append(right_motion)
+
+        return spanning_set
+
+    def run(self):
+        """
+        Run the lattice generator.
+
+        Returns
+        -------
+        dict
+            The minimal spanning set including additional motions for the
+            specified motion model
+
+        """
+        complete_spanning_set = self._generate_minimal_spanning_set()
+
+        return self._handle_motion_model(complete_spanning_set)

navigation2/nav2_smac_planner/lattice_primitives/requirements.txt

@@ -0,0 +1,3 @@
+numpy>=1.17.4
+matplotlib>=3.1.2
+Rtree>=0.9.7

navigation2/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py

@@ -0,0 +1,90 @@
+# Copyright (c) 2021, Matthew Booker
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License. Reserved.
+
+import unittest
+
+from lattice_generator import LatticeGenerator
+import numpy as np
+
+MOTION_MODEL = 'ackermann'
+TURNING_RADIUS = 0.5
+GRID_RESOLUTION = 0.05
+STOPPING_THRESHOLD = 5
+NUM_OF_HEADINGS = 16
+
+
+class TestLatticeGenerator(unittest.TestCase):
+    """Contains the unit tests for the TrajectoryGenerator."""
+
+    def setUp(self) -> None:
+        config = {'motion_model': MOTION_MODEL,
+                  'turning_radius': TURNING_RADIUS,
+                  'grid_resolution': GRID_RESOLUTION,
+                  'stopping_threshold': STOPPING_THRESHOLD,
+                  'num_of_headings': NUM_OF_HEADINGS}
+
+        lattice_gen = LatticeGenerator(config)
+
+        self.minimal_set = lattice_gen.run()
+
+    def test_minimal_set_lengths_are_positive(self):
+        # Test that lengths are all positive
+
+        for start_angle in self.minimal_set.keys():
+            for trajectory in self.minimal_set[start_angle]:
+
+                self.assertGreaterEqual(trajectory.parameters.arc_length, 0)
+                self.assertGreaterEqual(trajectory.parameters.start_straight_length, 0)
+                self.assertGreaterEqual(trajectory.parameters.end_straight_length, 0)
+                self.assertGreaterEqual(trajectory.parameters.total_length, 0)
+
+    def test_minimal_set_end_points_lie_on_grid(self):
+        # Test that end points lie on the grid resolution
+
+        for start_angle in self.minimal_set.keys():
+            for trajectory in self.minimal_set[start_angle]:
+
+                end_point_x = trajectory.path.xs[-1]
+                end_point_y = trajectory.path.ys[-1]
+
+                div_x = end_point_x / GRID_RESOLUTION
+                div_y = end_point_y / GRID_RESOLUTION
+
+                self.assertAlmostEqual(div_x, np.round(div_x), delta=0.00001)
+                self.assertAlmostEqual(div_y, np.round(div_y), delta=0.00001)
+
+    def test_minimal_set_end_angle_is_correct(self):
+        # Test that end angle agrees with the end angle parameter
+
+        for start_angle in self.minimal_set.keys():
+            for trajectory in self.minimal_set[start_angle]:
+
+                end_point_angle = trajectory.path.yaws[-1]
+
+                self.assertEqual(end_point_angle, trajectory.parameters.end_angle)
+
+    def test_output_angles_in_correct_range(self):
+        # Test that the outputted angles always lie within 0 to 2*pi
+
+        for start_angle in self.minimal_set.keys():
+            for trajectory in self.minimal_set[start_angle]:
+                output = trajectory.path.to_output_format()
+
+                for x, y, angle in output:
+                    self.assertGreaterEqual(angle, 0)
+                    self.assertLessEqual(angle, 2*np.pi)
+
+
+if __name__ == '__main__':
+    unittest.main()

navigation2/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py

@@ -0,0 +1,226 @@
+# Copyright (c) 2021, Matthew Booker
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License. Reserved.
+
+import unittest
+
+import numpy as np
+from trajectory_generator import TrajectoryGenerator
+
+TURNING_RADIUS = 1
+STEP_DISTANCE = 0.1
+
+
+class TestTrajectoryGenerator(unittest.TestCase):
+    """Contains the unit tests for the TrajectoryGenerator."""
+
+    def setUp(self) -> None:
+        config = {'turning_radius': TURNING_RADIUS}
+        self.trajectory_generator = TrajectoryGenerator(config)
+
+    def test_generate_trajectory_only_arc(self):
+        # Quadrant 1
+        end_point = np.array([1, 1])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+        # Quadrant 2
+        end_point = np.array([-1, 1])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+        # Quadrant 3
+        end_point = np.array([-1, -1])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+        # Quadrant 4
+        end_point = np.array([1, -1])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+    def test_generate_trajectory_only_line(self):
+        # Quadrant 1
+        end_point = np.array([1, 1])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, np.deg2rad(45), np.deg2rad(45), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+        # Quadrant 2
+        end_point = np.array([-1, 1])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, np.deg2rad(135), np.deg2rad(135), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+        # Quadrant 3
+        end_point = np.array([-1, -1])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, -np.deg2rad(135), -np.deg2rad(135), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+        # Quadrant 4
+        end_point = np.array([1, -1])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, -np.deg2rad(45), -np.deg2rad(45), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+    def test_generate_trajectory_line_to_arc(self):
+        # Quadrant 1
+        end_point = np.array([2, 1])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+        # Quadrant 2
+        end_point = np.array([-2, 1])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+        # Quadrant 3
+        end_point = np.array([-2, -1])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+        # Quadrant 1
+        end_point = np.array([2, -1])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+    def test_generate_trajectory_line_to_end(self):
+        # Quadrant 1
+        end_point = np.array([1, 2])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+        # Quadrant 2
+        end_point = np.array([-1, 2])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+        # Quadrant 3
+        end_point = np.array([-1, -2])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+        # Quadrant 4
+        end_point = np.array([1, -2])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+    def test_generate_trajectory_radius_too_small(self):
+        # Quadrant 1
+        end_point = np.array([.9, .9])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(trajectory, None)
+
+        # Quadrant 2
+        end_point = np.array([-.9, -.9])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(trajectory, None)
+
+        # Quadrant 3
+        end_point = np.array([-.9, -.9])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(trajectory, None)
+
+        # Quadrant 4
+        end_point = np.array([.9, -.9])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE)
+
+        self.assertEqual(trajectory, None)
+
+    def test_generate_trajectory_parallel_lines_coincident(self):
+        # Quadrant 1
+        end_point = np.array([5, 0])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+        # Quadrant 2
+        end_point = np.array([-5, 0])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE)
+
+        self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys))
+        self.assertGreater(len(trajectory.path.xs), 0)
+
+    def test_generate_trajectory_parallel_lines_not_coincident(self):
+        # Quadrant 1
+        end_point = np.array([0, 3])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE)
+
+        self.assertEqual(trajectory, None)
+
+        # Quadrant 2
+        end_point = np.array([0, 3])
+        trajectory = self.trajectory_generator.generate_trajectory(
+            end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE)
+
+        self.assertEqual(trajectory, None)
+
+
+if __name__ == '__main__':
+    unittest.main()

navigation2/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py

@@ -0,0 +1,98 @@
+# Copyright (c) 2021, Matthew Booker
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License. Reserved.
+
+"""
+This script is used visualize each trajectory individually.
+
+This helps to better understand how a single trajectory looks and
+to ensure that the x, y, and yaw values are correct. This is mainly
+used for debugging when making changes to parts of the code.
+However, if you would like to see how each trajectory in your
+ouput file looks then you can run this script.
+"""
+
+import json
+from pathlib import Path
+
+import matplotlib.pyplot as plt
+
+import numpy as np
+
+
+def plot_arrow(x, y, yaw, length=1.0, fc='r', ec='k'):
+    """Plot arrow."""
+    plt.arrow(x, y, length * np.cos(yaw), length *
+              np.sin(yaw), width=0.05*length, length_includes_head=True)
+    plt.plot(x, y)
+    plt.plot(0, 0)
+
+
+def read_trajectories_data(file_path):
+
+    with open(file_path) as data_file:
+        trajectory_data = json.load(data_file)
+
+    return trajectory_data
+
+
+cur_file_path = Path(__file__)
+trajectory_file_path = cur_file_path.parent.parent / 'output.json'
+
+
+trajectory_data = read_trajectories_data(trajectory_file_path)
+min_x = min([min([pose[0] for pose in primitive['poses']])
+            for primitive in trajectory_data['primitives']])
+max_x = max([max([pose[0] for pose in primitive['poses']])
+            for primitive in trajectory_data['primitives']])
+
+min_y = min([min([pose[1] for pose in primitive['poses']])
+            for primitive in trajectory_data['primitives']])
+max_y = max([max([pose[1] for pose in primitive['poses']])
+            for primitive in trajectory_data['primitives']])
+
+heading_angles = trajectory_data['lattice_metadata']['heading_angles']
+
+for primitive in trajectory_data['primitives']:
+    arrow_length = (primitive['arc_length'] +
+                    primitive['straight_length']) / len(primitive['poses'])
+
+    if arrow_length == 0:
+        arrow_length = max_x / len(primitive['poses'])
+
+    xs = np.array([pose[0] for pose in primitive['poses']])
+    ys = np.array([pose[1] for pose in primitive['poses']])
+
+    lengths = np.sqrt((xs[1:] - xs[:-1])**2 + (ys[1:] - ys[:-1])**2)
+    print('Distances between points: ', lengths)
+
+    for x, y, yaw in primitive['poses']:
+        plot_arrow(x, y, yaw, length=arrow_length)
+
+    plt.scatter(xs, ys)
+    plt.grid(True)
+    plt.axis('square')
+
+    left_x, right_x = plt.xlim()
+    left_y, right_y = plt.ylim()
+    plt.xlim(1.2*min_x, 1.2*max_x)
+    plt.ylim(1.2*min_y, 1.2*max_y)
+
+    start_angle = np.rad2deg(heading_angles[primitive['start_angle_index']])
+    end_angle = np.rad2deg(heading_angles[primitive['end_angle_index']])
+
+    plt.title(f"Trajectory ID: {primitive['trajectory_id']}")
+    plt.figtext(
+        0.7, 0.9, f'Start: {start_angle}\nEnd: {end_angle}')
+
+    plt.show()

navigation2/nav2_smac_planner/lattice_primitives/trajectory.py

@@ -0,0 +1,148 @@
+# Copyright (c) 2021, Matthew Booker
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License. Reserved.
+
+from dataclasses import dataclass
+
+from helper import angle_difference, normalize_angle
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class TrajectoryParameters:
+    """
+    A dataclass that holds the data needed to create the path for a trajectory.
+
+    turning_radius: The radius of the circle used to generate
+        the arc of the path
+    x_offset: The x coordinate of the circle used to generate
+        the arc of the path
+    y_offset: They y coordinate of the circle used to generate
+        the arc of the path
+    end_point: The end coordinate of the path
+    start_angle: The starting angle of the path
+        - given in radians from -pi to pi where 0 radians is along
+            the positive x axis
+    end_angle: The end angle of the path
+        - given in radians from -pi to pi where 0 radians is along
+            the positive x axis
+    left_turn: Whether the arc in the path turns to the left
+    arc_start_point: Coordinates of the starting position of the arc
+    arc_end_point: Coordinates of the ending position of the arc
+    """
+
+    turning_radius: float
+    x_offset: float
+    y_offset: float
+    end_point: np.array
+    start_angle: float
+    end_angle: float
+    left_turn: bool
+
+    arc_start_point: float
+    arc_end_point: float
+
+    @property
+    def arc_length(self):
+        """Arc length of the trajectory."""
+        return self.turning_radius * angle_difference(
+            self.start_angle, self.end_angle, self.left_turn
+        )
+
+    @property
+    def start_straight_length(self):
+        """Length of the straight line from start to arc."""
+        return np.linalg.norm(self.arc_start_point)
+
+    @property
+    def end_straight_length(self):
+        """Length of the straight line from arc to end."""
+        return np.linalg.norm(self.end_point - self.arc_end_point)
+
+    @property
+    def total_length(self):
+        """Total length of trajectory."""
+        return self.arc_length + self.start_straight_length + \
+            self.end_straight_length
+
+    @staticmethod
+    def no_arc(end_point, start_angle, end_angle):
+        """Create the parameters for a trajectory with no arc."""
+        return TrajectoryParameters(
+            turning_radius=0.0,
+            x_offset=0.0,
+            y_offset=0.0,
+            end_point=end_point,
+            start_angle=start_angle,
+            end_angle=end_angle,
+            left_turn=True,
+            arc_start_point=end_point,
+            arc_end_point=end_point,
+        )
+
+
+@dataclass(frozen=True)
+class Path:
+    """
+    A dataclass that holds the generated poses for a given trajectory.
+
+    xs: X coordinates of poses along trajectory
+    ys: Y coordinates of poses along trajectory
+    yaws: Yaws of poses along trajectory
+    """
+
+    xs: np.array
+    ys: np.array
+    yaws: np.array
+
+    def __add__(self, rhs):
+        """Add two paths together by concatenating them."""
+        if self.xs is None:
+            return rhs
+
+        xs = np.concatenate((self.xs, rhs.xs))
+        ys = np.concatenate((self.ys, rhs.ys))
+        yaws = np.concatenate((self.yaws, rhs.yaws))
+
+        return Path(xs, ys, yaws)
+
+    def to_output_format(self):
+        """Return the path data in a format suitable for outputting."""
+        output_xs = self.xs.round(5)
+        output_ys = self.ys.round(5)
+
+        # A bit of a hack but it removes any -0.0
+        output_xs = output_xs + 0.0
+        output_ys = output_ys + 0.0
+        output_yaws = self.yaws + 0.0
+
+        vectorized_normalize_angle = np.vectorize(normalize_angle)
+        output_yaws = vectorized_normalize_angle(output_yaws)
+
+        stacked = np.vstack([output_xs, output_ys, output_yaws]).transpose()
+
+        return stacked.tolist()
+
+
+@dataclass(frozen=True)
+class Trajectory:
+    """
+    A dataclass that holds the path and parameters for a trajectory.
+
+    path: The Path that represents the trajectory
+    parameters: The TrajectoryParameters that represent the trajectory
+    """
+
+    path: Path
+    parameters: TrajectoryParameters

navigation2/nav2_smac_planner/lattice_primitives/trajectory_generator.py

@@ -0,0 +1,568 @@
+# Copyright (c) 2021, Matthew Booker
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License. Reserved.
+
+import logging
+from typing import Tuple, Union
+
+import numpy as np
+
+from trajectory import Path, Trajectory, TrajectoryParameters
+
+logger = logging.getLogger(__name__)
+
+
+class TrajectoryGenerator:
+    """Handles all the logic for generating trajectories."""
+
+    def __init__(self, config: dict):
+        """Init TrajectoryGenerator using the user supplied config."""
+        self.turning_radius = config['turning_radius']
+
+    def _get_arc_point(
+        self, trajectory_params: TrajectoryParameters, t: float
+    ) -> Tuple[float, float, float]:
+        """
+        Get point on the arc trajectory using the following parameterization.
+
+            r(t) = <R * cos(t - pi/2) + a, R * sin(t - pi/2) + b>
+
+            R = radius
+            a = x offset
+            b = y offset
+
+        Args
+        ----
+        trajectory_params: TrajectoryParameters
+            The parameters that describe the arc to create
+        t: float
+            A value between 0 - 1 that denotes where along the arc
+            to calculate the point
+
+        Returns
+        -------
+        x: float
+            x coordinate of generated point
+        y: float
+            y coordinate of generated point
+        yaw: float
+            angle of tangent line to arc at point (x,y)
+
+        """
+        start_angle = trajectory_params.start_angle
+
+        arc_dist = t * trajectory_params.arc_length
+        angle_step = arc_dist / trajectory_params.turning_radius
+
+        if trajectory_params.left_turn:
+            # Calculate points using
+            # r(t) = <R * cos(t - pi/2) + a, R * sin(t - pi/2) + b>
+            t = start_angle + angle_step
+            x = (
+                trajectory_params.turning_radius * np.cos(t - np.pi / 2)
+                + trajectory_params.x_offset
+            )
+            y = (
+                trajectory_params.turning_radius * np.sin(t - np.pi / 2)
+                + trajectory_params.y_offset
+            )
+
+            yaw = t
+
+        else:
+            # Right turns go the opposite way across the arc, so we
+            # need to invert the angles and adjust the parametrization
+            start_angle = -start_angle
+
+            # Calculate points using
+            # r(t) = <R * -cos(t + pi/2) + a, R * sin(t + pi/2) + b>
+            t = start_angle + angle_step
+            x = (
+                trajectory_params.turning_radius * -np.cos(t + np.pi / 2)
+                + trajectory_params.x_offset
+            )
+            y = (
+                trajectory_params.turning_radius * np.sin(t + np.pi / 2)
+                + trajectory_params.y_offset
+            )
+
+            yaw = -t
+
+        return x, y, yaw
+
+    def _get_line_point(
+        self, start_point: np.array, end_point: np.array, t: float
+    ) -> Tuple[float, float]:
+        """
+        Get point on a line segment using the following parameterization.
+
+            r(t) = p + t * (q - p)
+
+            p = start point
+            q = end point
+
+        Args
+        ----
+        start_point: np.array(2,)
+            Starting point of the line segment
+        end_point: np.array(2,)
+            End point of the line segment
+        t: float
+            A value between 0 - 1 that denotes where along the segment
+            to calculate the point
+
+        Returns
+        -------
+        x: float
+            x coordinate of generated point
+        y: float
+            y coordinate of generated point
+
+        """
+        return start_point + t * (end_point - start_point)
+
+    def _create_path(
+        self, trajectory_params: TrajectoryParameters, primitive_resolution: float
+    ) -> Path:
+        """
+        Create the full trajectory path from the given trajectory parameters.
+
+        Args
+        ----
+        trajectory_params: TrajectoryParameters
+            The parameters that describe the trajectory to create
+        primitive_resolution: float
+            The desired distance between sampled points along the line.
+            This value is not strictly adhered to as the path may not
+            be neatly divisible, however the spacing will be as close
+            as possible.
+
+        Returns
+        -------
+        TrajectoryPath
+            The trajectory path described by the trajectory parameters
+
+        """
+        number_of_steps = np.round(
+            trajectory_params.total_length / primitive_resolution
+        ).astype(int)
+        t_step = 1 / number_of_steps
+
+        start_to_arc_dist = np.linalg.norm(trajectory_params.arc_start_point)
+
+        transition_points = [
+            start_to_arc_dist / trajectory_params.total_length,
+            (start_to_arc_dist + trajectory_params.arc_length)
+            / trajectory_params.total_length,
+        ]
+
+        cur_t = t_step
+
+        xs = []
+        ys = []
+        yaws = []
+
+        for i in range(1, number_of_steps + 1):
+
+            # This prevents cur_t going over 1 due to rounding issues in t_step
+            cur_t = min(cur_t, 1)
+
+            # Handle the initial straight line segment
+            if cur_t <= transition_points[0]:
+                line_t = cur_t / transition_points[0]
+                x, y = self._get_line_point(
+                    np.array([0, 0]), trajectory_params.arc_start_point, line_t
+                )
+                yaw = trajectory_params.start_angle
+
+            # Handle the arc
+            elif cur_t <= transition_points[1]:
+                arc_t = (cur_t - transition_points[0]) / (
+                    transition_points[1] - transition_points[0]
+                )
+                x, y, yaw = self._get_arc_point(trajectory_params, arc_t)
+
+            # Handle the end straight line segment
+            else:
+                line_t = (cur_t - transition_points[1]) / (1 - transition_points[1])
+                x, y = self._get_line_point(
+                    trajectory_params.arc_end_point, trajectory_params.end_point, line_t
+                )
+                yaw = trajectory_params.end_angle
+
+            xs.append(x)
+            ys.append(y)
+            yaws.append(yaw)
+
+            cur_t += t_step
+
+        # Convert to numpy arrays
+        xs = np.array(xs)
+        ys = np.array(ys)
+        yaws = np.array(yaws)
+
+        # The last point may be slightly off due to rounding issues
+        # so we correct the last point to be exactly the end point
+        xs[-1], ys[-1] = trajectory_params.end_point
+        yaws[-1] = trajectory_params.end_angle
+
+        return Path(xs, ys, yaws)
+
+    def _get_intersection_point(
+        self, m1: float, c1: float, m2: float, c2: float
+    ) -> np.array:
+        """
+        Get the intersection point of two lines.
+
+        The two lines are described by m1 * x + c1 and m2 * x + c2.
+
+        Args
+        ----
+        m1: float
+            Gradient of line 1
+        c1: float
+            y-intercept of line 1
+        m2: float
+            Gradient of line 2
+        c2: float
+            y-intercept of line2
+
+        Returns
+        -------
+        np.array (2,)
+            The intersection point of line 1 and 2
+
+        """
+        def line1(x):
+            return m1 * x + c1
+
+        x_point = (c2 - c1) / (m1 - m2)
+
+        return np.array([x_point, line1(x_point)])
+
+    def _is_left_turn(self, intersection_point: np.array, end_point: np.array) -> bool:
+        """
+        Determine if a trajectory will be a left turn.
+
+        Uses the determinant to determine whether the arc formed by the
+        intersection and end point turns left or right.
+
+        Args
+        ----
+        intersection_point: np.array(2,)
+            The intersection point of the lines formed from the start
+            and end angles
+        end_point: np.array(2,)
+            The chosen end point of the trajectory
+
+        Returns
+        -------
+        bool
+            True if curve turns left, false otherwise
+
+        """
+        matrix = np.vstack([intersection_point, end_point])
+        det = np.linalg.det(matrix)
+
+        return det >= 0
+
+    def _is_dir_vec_correct(
+        self, point1: np.array, point2: np.array, line_angle: float
+    ) -> bool:
+        """
+        Check that the direction vector agrees with the line angle.
+
+        The direction vector is defined as the vector from point 1 to
+        point 2.
+
+        Args
+        ----
+        point1: np.array(2,)
+            The start point of the vector
+        point2: np.array(2,)
+            The end point of the vector
+        line_angle: float
+            The angle of a line to compare against the vector
+
+        Returns
+        -------
+        bool
+            True if both line and vector point in same direction
+
+        """
+        # Need to round to prevent very small values for 0
+        m = abs(np.tan(line_angle).round(5))
+
+        if line_angle < 0:
+            m *= -1
+
+        direction_vec_from_points = point2 - point1
+
+        direction_vec_from_gradient = np.array([1, m])
+
+        # Handle when line angle is in quadrant 2 or 3 and when angle is 90
+        if abs(line_angle) > np.pi / 2:
+            direction_vec_from_gradient = np.array([-1, m])
+        elif abs(line_angle) == np.pi / 2:
+            direction_vec_from_gradient = np.array([0, m])
+
+        direction_vec_from_gradient = direction_vec_from_gradient.round(5)
+        direction_vec_from_points = direction_vec_from_points.round(5)
+
+        if np.all(
+            np.sign(direction_vec_from_points) == np.sign(direction_vec_from_gradient)
+        ):
+            return True
+        else:
+            return False
+
+    def _calculate_trajectory_params(
+        self, end_point: np.array, start_angle: float, end_angle: float
+    ) -> Union[TrajectoryParameters, None]:
+        """
+        Calculate the parameters for a trajectory with the desired constraints.
+
+        The trajectory may consist of an arc and at most two line segments.
+        A straight trajectory will consist of a single line segment. Similarly,
+        a purely curving trajectory will only consist of an arc.
+
+        Idea:
+            1. Extend a line from (0,0) with angle of start_angle
+            2. Extend a line from end_point with angle of end_angle
+            3. Compute their intersection point, I
+            4. Check that the intersection point leads to a
+            valid trajectory
+                - If I is too close to (0,0) or the end point then
+                no arc greater than the turning radius will reach
+                from (0,0) to end point
+
+        If two segments from the same exterior point are tangent to
+        a circle then they are congruent
+
+        Args
+        ----
+        end_point: np.array(2,)
+            The desired end point of the trajectory
+        start_angle: float
+            The start angle of the trajectory in radians
+        end_angle: float
+            The end angle of the trajectory in radians
+
+        Returns
+        -------
+        TrajectoryParameters or None
+            If a valid trajectory exists then the Trajectory parameters
+            are returned, otherwise None
+
+        """
+        x2, y2 = end_point
+        arc_start_point = np.array([0, 0])
+        arc_end_point = end_point
+
+        # Find gradient of line 1 passing through (0,0) that makes an angle
+        # of start_angle with x_axis
+        m1 = np.tan(start_angle).round(5)
+
+        # Find gradient of line 2 passing through end point that makes an angle
+        # of end_angle with x-axis
+        m2 = np.tan(end_angle).round(5)
+
+        # Deal with lines that are parallel
+        if m1 == m2:
+            # If they are coincident (i.e. y-intercept is same) then simply
+            # return a circle with infinite radius
+            if round(-m2 * x2 + y2, 5) == 0:
+                return TrajectoryParameters.no_arc(
+                    end_point=end_point, start_angle=start_angle, end_angle=end_angle
+                )
+
+            # Deal with edge case of 90
+            elif (
+                abs(start_angle) == np.pi / 2 and arc_end_point[0] == arc_start_point[0]
+            ):
+                return TrajectoryParameters.no_arc(
+                    end_point=end_point,
+                    start_angle=start_angle,
+                    end_angle=end_angle,
+                )
+
+            else:
+                logger.debug(
+                    'No trajectory possible for equivalent start and '
+                    + f'end angles that also passes through p = {x2, y2}'
+                )
+                return None
+
+        # Find intersection point of lines 1 and 2
+        intersection_point = self._get_intersection_point(m1, 0, m2, -m2 * x2 + y2)
+
+        # Check that the vector from (0,0) to intersection point agrees
+        # with the angle of line 1
+        if not self._is_dir_vec_correct(
+            arc_start_point, intersection_point, start_angle
+        ):
+            logger.debug(
+                'No trajectory possible since intersection point occurs '
+                + 'before start point on line 1'
+            )
+            return None
+
+        # Check that the vector from intersection point to arc start point agrees with
+        # the angle of line 2
+        if not self._is_dir_vec_correct(intersection_point, arc_end_point, end_angle):
+            logger.debug(
+                'No trajectory possible since intersection point occurs '
+                + 'after end point on line 2'
+            )
+            return None
+
+        # Calculate distance between arc start point and intersection point
+        dist_a = round(np.linalg.norm(arc_start_point - intersection_point), 5)
+
+        # Calculate distance between arc end point and intersection point
+        dist_b = round(np.linalg.norm(arc_end_point - intersection_point), 5)
+
+        # Calculate the angle between start angle and end angle lines
+        angle_between_lines = np.pi - abs(end_angle - start_angle)
+
+        # The closer the arc start and end points are to the intersection point the
+        # smaller the turning radius will be. However, we have a constraint on how
+        # small this turning radius can get. To calculate the minimum allowed
+        # distance we draw a right triangle with height equal to the constrained
+        # radius and angle opposite the height leg as half the angle between the
+        # start and end angle lines. The minimum valid distance is then the
+        # base of this triangle.
+        min_valid_distance = round(
+            self.turning_radius / np.tan(angle_between_lines / 2), 5
+        )
+
+        # Both the distance of p along line 2 and intersection point along
+        # line 1 must be greater than the minimum valid distance
+        if dist_a < min_valid_distance or dist_b < min_valid_distance:
+            logger.debug(
+                'No trajectory possible where radius is larger than '
+                + 'minimum turning radius'
+            )
+            return None
+
+        if dist_a < dist_b:
+            # Find new point on line 2 that is equidistant away from
+            # intersection point as arc start point is on line 1
+            vec_line2 = arc_end_point - intersection_point
+            vec_line2 /= np.linalg.norm(vec_line2)
+            arc_end_point = intersection_point + dist_a * vec_line2
+
+        elif dist_a > dist_b:
+            # Find new point on line 1 that is equidistant away from
+            # intersection point as arc end point is on line 2
+            vec_line1 = arc_start_point - intersection_point
+            vec_line1 /= np.linalg.norm(vec_line1)
+
+            arc_start_point = intersection_point + dist_b * vec_line1
+
+        x1, y1 = arc_start_point
+        x2, y2 = arc_end_point
+
+        # Find intersection point of the perpindicular lines of line 1 and 2
+        # that pass through arc start and arc end point respectively
+        if m1 == 0:
+            # If line 1 has gradient 0 then it is the x-axis.
+
+            def perp_line2(x):
+                return -1 / m2 * (x - x2) + y2
+
+            circle_center = np.array([x1, perp_line2(x1)])
+        elif m2 == 0:
+
+            def perp_line1(x):
+                return -1 / m1 * (x - x1) + y1
+
+            circle_center = np.array([x2, perp_line1(x2)])
+        else:
+            perp_m1 = -1 / m1 if m1 != 0 else 0
+            perp_m2 = -1 / m2 if m2 != 0 else 0
+
+            circle_center = self._get_intersection_point(
+                perp_m1, -perp_m1 * x1 + y1, perp_m2, -perp_m2 * x2 + y2
+            )
+
+        # The circles radius is the length from the center to arc start/end point
+        # (both distances are the same)
+        radius = np.linalg.norm(circle_center - arc_end_point).round(5)
+        x_offset = circle_center[0].round(5)
+        y_offset = circle_center[1].round(5)
+
+        if radius < self.turning_radius:
+            logger.debug(
+                'Calculated circle radius is smaller than allowed turning '
+                + f'radius: r = {radius}, min_radius = {self.turning_radius}'
+            )
+            return None
+
+        left_turn = self._is_left_turn(intersection_point, end_point)
+
+        return TrajectoryParameters(
+            radius,
+            x_offset,
+            y_offset,
+            end_point,
+            start_angle,
+            end_angle,
+            left_turn,
+            arc_start_point,
+            arc_end_point,
+        )
+
+    def generate_trajectory(
+        self,
+        end_point: np.array,
+        start_angle: float,
+        end_angle: float,
+        primitive_resolution: float,
+    ) -> Union[Trajectory, None]:
+        """
+        Create a trajectory from (0,0, start_angle) to (end_point, end_angle).
+
+        The trajectory will consist of a path that contains discrete points
+        that are spaced primitive_resolution apart.
+
+        Args
+        ----
+        end_point: np.array(2,)
+            The desired end point of the trajectory
+        start_angle: float
+            The start angle of the trajectory in radians
+        end_angle: float
+            The end angle of the trajectory in radians
+        primitive_resolution: float
+            The spacing between points along the trajectory
+
+        Returns
+        -------
+        Trajectory or None
+            If a valid trajectory exists then the Trajectory is returned,
+            otherwise None
+
+        """
+        trajectory_params = self._calculate_trajectory_params(
+            end_point, start_angle, end_angle
+        )
+
+        if trajectory_params is None:
+            return None
+
+        logger.debug('Trajectory found')
+
+        trajectory_path = self._create_path(trajectory_params, primitive_resolution)
+
+        return Trajectory(trajectory_path, trajectory_params)

navigation2/nav2_smac_planner/package.xml

@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_smac_planner</name>
+  <version>1.1.18</version>
+  <description>Smac global planning plugin: A*, Hybrid-A*, State Lattice</description>
+  <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>visualization_msgs</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>builtin_interfaces</depend>
+  <depend>nav2_common</depend>
+  <depend>tf2_ros</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>nav2_core</depend>
+  <depend>pluginlib</depend>
+  <depend>eigen3_cmake_module</depend>
+  <depend>eigen</depend>
+  <depend>ompl</depend>
+  <depend>nlohmann-json-dev</depend>
+  <depend>angles</depend>
+
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <nav2_core plugin="${prefix}/smac_plugin_hybrid.xml" />
+    <nav2_core plugin="${prefix}/smac_plugin_2d.xml" />
+    <nav2_core plugin="${prefix}/smac_plugin_lattice.xml" />
+  </export>
+</package>

navigation2/nav2_smac_planner/smac_plugin_2d.xml

@@ -0,0 +1,5 @@
+<library path="nav2_smac_planner_2d">
+  <class name="nav2_smac_planner/SmacPlanner2D" type="nav2_smac_planner::SmacPlanner2D" base_class_type="nav2_core::GlobalPlanner">
+    <description>2D A* SMAC Planner</description>
+  </class>
+</library>

navigation2/nav2_smac_planner/smac_plugin_hybrid.xml

@@ -0,0 +1,5 @@
+<library path="nav2_smac_planner">
+	<class name="nav2_smac_planner/SmacPlannerHybrid" type="nav2_smac_planner::SmacPlannerHybrid" base_class_type="nav2_core::GlobalPlanner">
+	  <description>Hybrid-A* SMAC planner</description>
+	</class>
+</library>

navigation2/nav2_smac_planner/smac_plugin_lattice.xml

@@ -0,0 +1,5 @@
+<library path="nav2_smac_planner_lattice">
+	<class name="nav2_smac_planner/SmacPlannerLattice" type="nav2_smac_planner::SmacPlannerLattice" base_class_type="nav2_core::GlobalPlanner">
+	  <description>State Lattice SMAC planner</description>
+	</class>
+</library>

navigation2/nav2_smac_planner/src/a_star.cpp

@@ -0,0 +1,447 @@
+// Copyright (c) 2020, Samsung Research America
+// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <omp.h>
+#include <cmath>
+#include <stdexcept>
+#include <memory>
+#include <algorithm>
+#include <limits>
+#include <type_traits>
+#include <chrono>
+#include <thread>
+#include <utility>
+#include <vector>
+
+#include "nav2_smac_planner/a_star.hpp"
+using namespace std::chrono;  // NOLINT
+
+namespace nav2_smac_planner
+{
+
+template<typename NodeT>
+AStarAlgorithm<NodeT>::AStarAlgorithm(
+  const MotionModel & motion_model,
+  const SearchInfo & search_info)
+: _traverse_unknown(true),
+  _is_initialized(false),
+  _max_iterations(0),
+  _max_planning_time(0),
+  _x_size(0),
+  _y_size(0),
+  _search_info(search_info),
+  _goal_coordinates(Coordinates()),
+  _start(nullptr),
+  _goal(nullptr),
+  _motion_model(motion_model)
+{
+  _graph.reserve(100000);
+}
+
+template<typename NodeT>
+AStarAlgorithm<NodeT>::~AStarAlgorithm()
+{
+}
+
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::initialize(
+  const bool & allow_unknown,
+  int & max_iterations,
+  const int & max_on_approach_iterations,
+  const double & max_planning_time,
+  const float & lookup_table_size,
+  const unsigned int & dim_3_size)
+{
+  _traverse_unknown = allow_unknown;
+  _max_iterations = max_iterations;
+  _max_on_approach_iterations = max_on_approach_iterations;
+  _max_planning_time = max_planning_time;
+  if(!_is_initialized) {
+    NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info);
+  }
+  _is_initialized = true;
+  _dim3_size = dim_3_size;
+  _expander = std::make_unique<AnalyticExpansion<NodeT>>(
+    _motion_model, _search_info, _traverse_unknown, _dim3_size);
+}
+
+template<>
+void AStarAlgorithm<Node2D>::initialize(
+  const bool & allow_unknown,
+  int & max_iterations,
+  const int & max_on_approach_iterations,
+  const double & max_planning_time,
+  const float & /*lookup_table_size*/,
+  const unsigned int & dim_3_size)
+{
+  _traverse_unknown = allow_unknown;
+  _max_iterations = max_iterations;
+  _max_on_approach_iterations = max_on_approach_iterations;
+  _max_planning_time = max_planning_time;
+
+  if (dim_3_size != 1) {
+    throw std::runtime_error("Node type Node2D cannot be given non-1 dim 3 quantization.");
+  }
+  _dim3_size = dim_3_size;
+  _expander = std::make_unique<AnalyticExpansion<Node2D>>(
+    _motion_model, _search_info, _traverse_unknown, _dim3_size);
+}
+
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::setCollisionChecker(GridCollisionChecker * collision_checker)
+{
+  _collision_checker = collision_checker;
+  _costmap = collision_checker->getCostmap();
+  unsigned int x_size = _costmap->getSizeInCellsX();
+  unsigned int y_size = _costmap->getSizeInCellsY();
+
+  clearGraph();
+
+  if (getSizeX() != x_size || getSizeY() != y_size) {
+    _x_size = x_size;
+    _y_size = y_size;
+    NodeT::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info);
+  }
+  _expander->setCollisionChecker(collision_checker);
+}
+
+template<typename NodeT>
+typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::addToGraph(
+  const unsigned int & index)
+{
+  auto iter = _graph.find(index);
+  if (iter != _graph.end()) {
+    return &(iter->second);
+  }
+
+  return &(_graph.emplace(index, NodeT(index)).first->second);
+}
+
+template<>
+void AStarAlgorithm<Node2D>::setStart(
+  const unsigned int & mx,
+  const unsigned int & my,
+  const unsigned int & dim_3)
+{
+  if (dim_3 != 0) {
+    throw std::runtime_error("Node type Node2D cannot be given non-zero starting dim 3.");
+  }
+  _start = addToGraph(Node2D::getIndex(mx, my, getSizeX()));
+}
+
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::setStart(
+  const unsigned int & mx,
+  const unsigned int & my,
+  const unsigned int & dim_3)
+{
+  _start = addToGraph(NodeT::getIndex(mx, my, dim_3));
+  _start->setPose(
+    Coordinates(
+      static_cast<float>(mx),
+      static_cast<float>(my),
+      static_cast<float>(dim_3)));
+}
+
+template<>
+void AStarAlgorithm<Node2D>::setGoal(
+  const unsigned int & mx,
+  const unsigned int & my,
+  const unsigned int & dim_3)
+{
+  if (dim_3 != 0) {
+    throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3.");
+  }
+
+  _goal = addToGraph(Node2D::getIndex(mx, my, getSizeX()));
+  _goal_coordinates = Node2D::Coordinates(mx, my);
+}
+
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::setGoal(
+  const unsigned int & mx,
+  const unsigned int & my,
+  const unsigned int & dim_3)
+{
+  _goal = addToGraph(NodeT::getIndex(mx, my, dim_3));
+
+  typename NodeT::Coordinates goal_coords(
+    static_cast<float>(mx),
+    static_cast<float>(my),
+    static_cast<float>(dim_3));
+
+  if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) {
+    if (!_start) {
+      throw std::runtime_error("Start must be set before goal.");
+    }
+
+    NodeT::resetObstacleHeuristic(_costmap, _start->pose.x, _start->pose.y, mx, my);
+  }
+
+  _goal_coordinates = goal_coords;
+  _goal->setPose(_goal_coordinates);
+}
+
+template<typename NodeT>
+bool AStarAlgorithm<NodeT>::areInputsValid()
+{
+  // Check if graph was filled in
+  if (_graph.empty()) {
+    throw std::runtime_error("Failed to compute path, no costmap given.");
+  }
+
+  // Check if points were filled in
+  if (!_start || !_goal) {
+    throw std::runtime_error("Failed to compute path, no valid start or goal given.");
+  }
+
+  // Check if ending point is valid
+  if (getToleranceHeuristic() < 0.001 &&
+    !_goal->isNodeValid(_traverse_unknown, _collision_checker))
+  {
+    throw std::runtime_error("Failed to compute path, goal is occupied with no tolerance.");
+  }
+
+  // Check if starting point is valid
+  if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) {
+    throw std::runtime_error("Starting point in lethal space! Cannot create feasible plan.");
+  }
+
+  return true;
+}
+
+template<typename NodeT>
+bool AStarAlgorithm<NodeT>::createPath(
+  CoordinateVector & path, int & iterations,
+  const float & tolerance)
+{
+  steady_clock::time_point start_time = steady_clock::now();
+  _tolerance = tolerance;
+  _best_heuristic_node = {std::numeric_limits<float>::max(), 0};
+  clearQueue();
+
+  if (!areInputsValid()) {
+    return false;
+  }
+
+  // 0) Add starting point to the open set
+  addNode(0.0, getStart());
+  getStart()->setAccumulatedCost(0.0);
+
+  // Optimization: preallocate all variables
+  NodePtr current_node = nullptr;
+  NodePtr neighbor = nullptr;
+  NodePtr expansion_result = nullptr;
+  float g_cost = 0.0;
+  NodeVector neighbors;
+  int approach_iterations = 0;
+  NeighborIterator neighbor_iterator;
+  int analytic_iterations = 0;
+  int closest_distance = std::numeric_limits<int>::max();
+
+  // Given an index, return a node ptr reference if its collision-free and valid
+  const unsigned int max_index = getSizeX() * getSizeY() * getSizeDim3();
+  NodeGetter neighborGetter =
+    [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool
+    {
+      if (index >= max_index) {
+        return false;
+      }
+
+      neighbor_rtn = addToGraph(index);
+      return true;
+    };
+
+  while (iterations < getMaxIterations() && !_queue.empty()) {
+    // Check for planning timeout only on every Nth iteration
+    if (iterations % _timing_interval == 0) {
+      std::chrono::duration<double> planning_duration =
+        std::chrono::duration_cast<std::chrono::duration<double>>(steady_clock::now() - start_time);
+      if (static_cast<double>(planning_duration.count()) >= _max_planning_time) {
+        return false;
+      }
+    }
+
+    // 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue
+    current_node = getNextNode();
+
+    // We allow for nodes to be queued multiple times in case
+    // shorter paths result in it, but we can visit only once
+    if (current_node->wasVisited()) {
+      continue;
+    }
+
+    iterations++;
+
+    // 2) Mark Nbest as visited
+    current_node->visited();
+
+    // 2.1) Use an analytic expansion (if available) to generate a path
+    expansion_result = nullptr;
+    expansion_result = _expander->tryAnalyticExpansion(
+      current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance);
+    if (expansion_result != nullptr) {
+      current_node = expansion_result;
+    }
+
+    // 3) Check if we're at the goal, backtrace if required
+    if (isGoal(current_node)) {
+      return current_node->backtracePath(path);
+    } else if (_best_heuristic_node.first < getToleranceHeuristic()) {
+      // Optimization: Let us find when in tolerance and refine within reason
+      approach_iterations++;
+      if (approach_iterations >= getOnApproachMaxIterations()) {
+        return _graph.at(_best_heuristic_node.second).backtracePath(path);
+      }
+    }
+
+    // 4) Expand neighbors of Nbest not visited
+    neighbors.clear();
+    current_node->getNeighbors(neighborGetter, _collision_checker, _traverse_unknown, neighbors);
+
+    for (neighbor_iterator = neighbors.begin();
+      neighbor_iterator != neighbors.end(); ++neighbor_iterator)
+    {
+      neighbor = *neighbor_iterator;
+
+      // 4.1) Compute the cost to go to this node
+      g_cost = current_node->getAccumulatedCost() + current_node->getTraversalCost(neighbor);
+
+      // 4.2) If this is a lower cost than prior, we set this as the new cost and new approach
+      if (g_cost < neighbor->getAccumulatedCost()) {
+        neighbor->setAccumulatedCost(g_cost);
+        neighbor->parent = current_node;
+
+        // 4.3) Add to queue with heuristic cost
+        addNode(g_cost + getHeuristicCost(neighbor), neighbor);
+      }
+    }
+  }
+
+  if (_best_heuristic_node.first < getToleranceHeuristic()) {
+    // If we run out of serach options, return the path that is closest, if within tolerance.
+    return _graph.at(_best_heuristic_node.second).backtracePath(path);
+  }
+
+  return false;
+}
+
+template<typename NodeT>
+bool AStarAlgorithm<NodeT>::isGoal(NodePtr & node)
+{
+  return node == getGoal();
+}
+
+template<typename NodeT>
+typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getStart()
+{
+  return _start;
+}
+
+template<typename NodeT>
+typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getGoal()
+{
+  return _goal;
+}
+
+template<typename NodeT>
+typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::getNextNode()
+{
+  NodeBasic<NodeT> node = _queue.top().second;
+  _queue.pop();
+  node.processSearchNode();
+  return node.graph_node_ptr;
+}
+
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::addNode(const float & cost, NodePtr & node)
+{
+  NodeBasic<NodeT> queued_node(node->getIndex());
+  queued_node.populateSearchNode(node);
+  _queue.emplace(cost, queued_node);
+}
+
+template<typename NodeT>
+float AStarAlgorithm<NodeT>::getHeuristicCost(const NodePtr & node)
+{
+  const Coordinates node_coords =
+    NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3());
+  float heuristic = NodeT::getHeuristicCost(
+    node_coords, _goal_coordinates, _costmap);
+
+  if (heuristic < _best_heuristic_node.first) {
+    _best_heuristic_node = {heuristic, node->getIndex()};
+  }
+
+  return heuristic;
+}
+
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::clearQueue()
+{
+  NodeQueue q;
+  std::swap(_queue, q);
+}
+
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::clearGraph()
+{
+  Graph g;
+  std::swap(_graph, g);
+  _graph.reserve(100000);
+}
+
+template<typename NodeT>
+int & AStarAlgorithm<NodeT>::getMaxIterations()
+{
+  return _max_iterations;
+}
+
+template<typename NodeT>
+int & AStarAlgorithm<NodeT>::getOnApproachMaxIterations()
+{
+  return _max_on_approach_iterations;
+}
+
+template<typename NodeT>
+float & AStarAlgorithm<NodeT>::getToleranceHeuristic()
+{
+  return _tolerance;
+}
+
+template<typename NodeT>
+unsigned int & AStarAlgorithm<NodeT>::getSizeX()
+{
+  return _x_size;
+}
+
+template<typename NodeT>
+unsigned int & AStarAlgorithm<NodeT>::getSizeY()
+{
+  return _y_size;
+}
+
+template<typename NodeT>
+unsigned int & AStarAlgorithm<NodeT>::getSizeDim3()
+{
+  return _dim3_size;
+}
+
+// Instantiate algorithm for the supported template types
+template class AStarAlgorithm<Node2D>;
+template class AStarAlgorithm<NodeHybrid>;
+template class AStarAlgorithm<NodeLattice>;
+
+}  // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/analytic_expansion.cpp

@@ -0,0 +1,286 @@
+// Copyright (c) 2021, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <ompl/base/ScopedState.h>
+#include <ompl/base/spaces/DubinsStateSpace.h>
+#include <ompl/base/spaces/ReedsSheppStateSpace.h>
+
+#include <algorithm>
+#include <vector>
+#include <memory>
+
+#include "nav2_smac_planner/analytic_expansion.hpp"
+
+namespace nav2_smac_planner
+{
+
+template<typename NodeT>
+AnalyticExpansion<NodeT>::AnalyticExpansion(
+  const MotionModel & motion_model,
+  const SearchInfo & search_info,
+  const bool & traverse_unknown,
+  const unsigned int & dim_3_size)
+: _motion_model(motion_model),
+  _search_info(search_info),
+  _traverse_unknown(traverse_unknown),
+  _dim_3_size(dim_3_size),
+  _collision_checker(nullptr)
+{
+}
+
+template<typename NodeT>
+void AnalyticExpansion<NodeT>::setCollisionChecker(
+  GridCollisionChecker * collision_checker)
+{
+  _collision_checker = collision_checker;
+}
+
+template<typename NodeT>
+typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalyticExpansion(
+  const NodePtr & current_node, const NodePtr & goal_node,
+  const NodeGetter & getter, int & analytic_iterations,
+  int & closest_distance)
+{
+  // This must be a valid motion model for analytic expansion to be attempted
+  if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP ||
+    _motion_model == MotionModel::STATE_LATTICE)
+  {
+    // See if we are closer and should be expanding more often
+    auto costmap = _collision_checker->getCostmap();
+    const Coordinates node_coords =
+      NodeT::getCoords(current_node->getIndex(), costmap->getSizeInCellsX(), _dim_3_size);
+    closest_distance = std::min(
+      closest_distance,
+      static_cast<int>(NodeT::getHeuristicCost(node_coords, goal_node->pose, costmap)));
+
+    // We want to expand at a rate of d/expansion_ratio,
+    // but check to see if we are so close that we would be expanding every iteration
+    // If so, limit it to the expansion ratio (rounded up)
+    int desired_iterations = std::max(
+      static_cast<int>(closest_distance / _search_info.analytic_expansion_ratio),
+      static_cast<int>(std::ceil(_search_info.analytic_expansion_ratio)));
+
+    // If we are closer now, we should update the target number of iterations to go
+    analytic_iterations =
+      std::min(analytic_iterations, desired_iterations);
+
+    // Always run the expansion on the first run in case there is a
+    // trivial path to be found
+    if (analytic_iterations <= 0) {
+      // Reset the counter and try the analytic path expansion
+      analytic_iterations = desired_iterations;
+      AnalyticExpansionNodes analytic_nodes = getAnalyticPath(current_node, goal_node, getter);
+      if (!analytic_nodes.empty()) {
+        // If we have a valid path, attempt to refine it
+        NodePtr node = current_node;
+        NodePtr test_node = current_node;
+        AnalyticExpansionNodes refined_analytic_nodes;
+        for (int i = 0; i < 8; i++) {
+          // Attempt to create better paths in 5 node increments, need to make sure
+          // they exist for each in order to do so (maximum of 40 points back).
+          if (test_node->parent && test_node->parent->parent && test_node->parent->parent->parent &&
+            test_node->parent->parent->parent->parent &&
+            test_node->parent->parent->parent->parent->parent)
+          {
+            test_node = test_node->parent->parent->parent->parent->parent;
+            refined_analytic_nodes = getAnalyticPath(test_node, goal_node, getter);
+            if (refined_analytic_nodes.empty()) {
+              break;
+            }
+            analytic_nodes = refined_analytic_nodes;
+            node = test_node;
+          } else {
+            break;
+          }
+        }
+
+        return setAnalyticPath(node, goal_node, analytic_nodes);
+      }
+    }
+
+    analytic_iterations--;
+  }
+
+  // No valid motion model - return nullptr
+  return NodePtr(nullptr);
+}
+
+template<typename NodeT>
+typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<NodeT>::getAnalyticPath(
+  const NodePtr & node,
+  const NodePtr & goal,
+  const NodeGetter & node_getter)
+{
+  static ompl::base::ScopedState<> from(node->motion_table.state_space), to(
+    node->motion_table.state_space), s(node->motion_table.state_space);
+  from[0] = node->pose.x;
+  from[1] = node->pose.y;
+  from[2] = node->motion_table.getAngleFromBin(node->pose.theta);
+  to[0] = goal->pose.x;
+  to[1] = goal->pose.y;
+  to[2] = node->motion_table.getAngleFromBin(goal->pose.theta);
+
+  float d = node->motion_table.state_space->distance(from(), to());
+
+  // If the length is too far, exit. This prevents unsafe shortcutting of paths
+  // into higher cost areas far out from the goal itself, let search to the work of getting
+  // close before the analytic expansion brings it home. This should never be smaller than
+  // 4-5x the minimum turning radius being used, or planning times will begin to spike.
+  if (d > _search_info.analytic_expansion_max_length) {
+    return AnalyticExpansionNodes();
+  }
+
+  // A move of sqrt(2) is guaranteed to be in a new cell
+  static const float sqrt_2 = std::sqrt(2.);
+  unsigned int num_intervals = std::floor(d / sqrt_2);
+
+  AnalyticExpansionNodes possible_nodes;
+  // When "from" and "to" are zero or one cell away,
+  // num_intervals == 0
+  possible_nodes.reserve(num_intervals);  // We won't store this node or the goal
+  std::vector<double> reals;
+  double theta;
+
+  // Pre-allocate
+  NodePtr prev(node);
+  unsigned int index = 0;
+  NodePtr next(nullptr);
+  float angle = 0.0;
+  Coordinates proposed_coordinates;
+  bool failure = false;
+
+  // Check intermediary poses (non-goal, non-start)
+  for (float i = 1; i <= num_intervals; i++) {
+    node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, s());
+    reals = s.reals();
+    // Make sure in range [0, 2PI)
+    theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2];
+    theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta;
+    angle = node->motion_table.getClosestAngularBin(theta);
+
+    // Turn the pose into a node, and check if it is valid
+    index = NodeT::getIndex(
+      static_cast<unsigned int>(reals[0]),
+      static_cast<unsigned int>(reals[1]),
+      static_cast<unsigned int>(angle));
+    // Get the node from the graph
+    if (node_getter(index, next)) {
+      Coordinates initial_node_coords = next->pose;
+      proposed_coordinates = {static_cast<float>(reals[0]), static_cast<float>(reals[1]), angle};
+      next->setPose(proposed_coordinates);
+      if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) {
+        // Save the node, and its previous coordinates in case we need to abort
+        possible_nodes.emplace_back(next, initial_node_coords, proposed_coordinates);
+        prev = next;
+      } else {
+        // Abort
+        next->setPose(initial_node_coords);
+        failure = true;
+        break;
+      }
+    } else {
+      // Abort
+      failure = true;
+      break;
+    }
+  }
+
+  // Reset to initial poses to not impact future searches
+  for (const auto & node_pose : possible_nodes) {
+    const auto & n = node_pose.node;
+    n->setPose(node_pose.initial_coords);
+  }
+
+  if (failure) {
+    return AnalyticExpansionNodes();
+  }
+
+  return possible_nodes;
+}
+
+template<typename NodeT>
+typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::setAnalyticPath(
+  const NodePtr & node,
+  const NodePtr & goal_node,
+  const AnalyticExpansionNodes & expanded_nodes)
+{
+  _detached_nodes.clear();
+  // Legitimate final path - set the parent relationships, states, and poses
+  NodePtr prev = node;
+  for (const auto & node_pose : expanded_nodes) {
+    auto n = node_pose.node;
+    cleanNode(n);
+    if (n->getIndex() != goal_node->getIndex()) {
+      if (n->wasVisited()) {
+        _detached_nodes.push_back(std::make_unique<NodeT>(-1));
+        n = _detached_nodes.back().get();
+      }
+      n->parent = prev;
+      n->pose = node_pose.proposed_coords;
+      n->visited();
+      prev = n;
+    }
+  }
+  if (goal_node != prev) {
+    goal_node->parent = prev;
+    cleanNode(goal_node);
+    goal_node->visited();
+  }
+  return goal_node;
+}
+
+template<>
+void AnalyticExpansion<NodeLattice>::cleanNode(const NodePtr & node)
+{
+  node->setMotionPrimitive(nullptr);
+}
+
+template<typename NodeT>
+void AnalyticExpansion<NodeT>::cleanNode(const NodePtr & /*expanded_nodes*/)
+{
+}
+
+template<>
+typename AnalyticExpansion<Node2D>::AnalyticExpansionNodes AnalyticExpansion<Node2D>::
+getAnalyticPath(
+  const NodePtr & node,
+  const NodePtr & goal,
+  const NodeGetter & node_getter)
+{
+  return AnalyticExpansionNodes();
+}
+
+template<>
+typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::setAnalyticPath(
+  const NodePtr & node,
+  const NodePtr & goal_node,
+  const AnalyticExpansionNodes & expanded_nodes)
+{
+  return NodePtr(nullptr);
+}
+
+template<>
+typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::tryAnalyticExpansion(
+  const NodePtr & current_node, const NodePtr & goal_node,
+  const NodeGetter & getter, int & analytic_iterations,
+  int & closest_distance)
+{
+  return NodePtr(nullptr);
+}
+
+template class AnalyticExpansion<Node2D>;
+template class AnalyticExpansion<NodeHybrid>;
+template class AnalyticExpansion<NodeLattice>;
+
+}  // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/collision_checker.cpp

@@ -0,0 +1,196 @@
+// Copyright (c) 2021, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include "nav2_smac_planner/collision_checker.hpp"
+
+namespace nav2_smac_planner
+{
+
+GridCollisionChecker::GridCollisionChecker(
+  nav2_costmap_2d::Costmap2D * costmap,
+  unsigned int num_quantizations,
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+: FootprintCollisionChecker(costmap)
+{
+  if (node) {
+    clock_ = node->get_clock();
+    logger_ = node->get_logger();
+  }
+
+  // Convert number of regular bins into angles
+  float bin_size = 2 * M_PI / static_cast<float>(num_quantizations);
+  angles_.reserve(num_quantizations);
+  for (unsigned int i = 0; i != num_quantizations; i++) {
+    angles_.push_back(bin_size * i);
+  }
+}
+
+// GridCollisionChecker::GridCollisionChecker(
+//   nav2_costmap_2d::Costmap2D * costmap,
+//   std::vector<float> & angles)
+// : FootprintCollisionChecker(costmap),
+//   angles_(angles)
+// {
+// }
+
+void GridCollisionChecker::setFootprint(
+  const nav2_costmap_2d::Footprint & footprint,
+  const bool & radius,
+  const double & possible_inscribed_cost)
+{
+  possible_inscribed_cost_ = possible_inscribed_cost;
+  footprint_is_radius_ = radius;
+
+  // Use radius, no caching required
+  if (radius) {
+    return;
+  }
+
+  // No change, no updates required
+  if (footprint == unoriented_footprint_) {
+    return;
+  }
+
+  oriented_footprints_.clear();
+  oriented_footprints_.reserve(angles_.size());
+  double sin_th, cos_th;
+  geometry_msgs::msg::Point new_pt;
+  const unsigned int footprint_size = footprint.size();
+
+  // Precompute the orientation bins for checking to use
+  for (unsigned int i = 0; i != angles_.size(); i++) {
+    sin_th = sin(angles_[i]);
+    cos_th = cos(angles_[i]);
+    nav2_costmap_2d::Footprint oriented_footprint;
+    oriented_footprint.reserve(footprint_size);
+
+    for (unsigned int j = 0; j < footprint_size; j++) {
+      new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th;
+      new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th;
+      oriented_footprint.push_back(new_pt);
+    }
+
+    oriented_footprints_.push_back(oriented_footprint);
+  }
+
+  unoriented_footprint_ = footprint;
+}
+
+bool GridCollisionChecker::inCollision(
+  const float & x,
+  const float & y,
+  const float & angle_bin,
+  const bool & traverse_unknown)
+{
+  // Check to make sure cell is inside the map
+  if (outsideRange(costmap_->getSizeInCellsX(), x) ||
+    outsideRange(costmap_->getSizeInCellsY(), y))
+  {
+    return true;
+  }
+
+  // Assumes setFootprint already set
+  double wx, wy;
+  costmap_->mapToWorld(static_cast<double>(x), static_cast<double>(y), wx, wy);
+
+  if (!footprint_is_radius_) {
+    // if footprint, then we check for the footprint's points, but first see
+    // if the robot is even potentially in an inscribed collision
+    footprint_cost_ = costmap_->getCost(
+      static_cast<unsigned int>(x), static_cast<unsigned int>(y));
+
+    if (footprint_cost_ < possible_inscribed_cost_) {
+      if (possible_inscribed_cost_ > 0) {
+        return false;
+      } else {
+        RCLCPP_ERROR_THROTTLE(
+          logger_, *clock_, 1000,
+          "Inflation layer either not found or inflation is not set sufficiently for "
+          "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
+          " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
+          "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields"
+          " for full instructions. This will substantially impact run-time performance.");
+      }
+    }
+
+    // If its inscribed, in collision, or unknown in the middle,
+    // no need to even check the footprint, its invalid
+    if (footprint_cost_ == UNKNOWN && !traverse_unknown) {
+      return true;
+    }
+
+    if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) {
+      return true;
+    }
+
+    // if possible inscribed, need to check actual footprint pose.
+    // Use precomputed oriented footprints are done on initialization,
+    // offset by translation value to collision check
+    geometry_msgs::msg::Point new_pt;
+    const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin];
+    nav2_costmap_2d::Footprint current_footprint;
+    current_footprint.reserve(oriented_footprint.size());
+    for (unsigned int i = 0; i < oriented_footprint.size(); ++i) {
+      new_pt.x = wx + oriented_footprint[i].x;
+      new_pt.y = wy + oriented_footprint[i].y;
+      current_footprint.push_back(new_pt);
+    }
+
+    footprint_cost_ = footprintCost(current_footprint);
+
+    if (footprint_cost_ == UNKNOWN && traverse_unknown) {
+      return false;
+    }
+
+    // if occupied or unknown and not to traverse unknown space
+    return footprint_cost_ >= OCCUPIED;
+  } else {
+    // if radius, then we can check the center of the cost assuming inflation is used
+    footprint_cost_ = costmap_->getCost(
+      static_cast<unsigned int>(x), static_cast<unsigned int>(y));
+
+    if (footprint_cost_ == UNKNOWN && traverse_unknown) {
+      return false;
+    }
+
+    // if occupied or unknown and not to traverse unknown space
+    return static_cast<double>(footprint_cost_) >= INSCRIBED;
+  }
+}
+
+bool GridCollisionChecker::inCollision(
+  const unsigned int & i,
+  const bool & traverse_unknown)
+{
+  footprint_cost_ = costmap_->getCost(i);
+  if (footprint_cost_ == UNKNOWN && traverse_unknown) {
+    return false;
+  }
+
+  // if occupied or unknown and not to traverse unknown space
+  return footprint_cost_ >= INSCRIBED;
+}
+
+float GridCollisionChecker::getCost()
+{
+  // Assumes inCollision called prior
+  return static_cast<float>(footprint_cost_);
+}
+
+bool GridCollisionChecker::outsideRange(const unsigned int & max, const float & value)
+{
+  return value < 0.0f || value > max;
+}
+
+}  // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/costmap_downsampler.cpp

@@ -0,0 +1,154 @@
+// Copyright (c) 2020, Carlos Luis
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include "nav2_smac_planner/costmap_downsampler.hpp"
+
+#include <string>
+#include <memory>
+#include <algorithm>
+
+namespace nav2_smac_planner
+{
+
+CostmapDownsampler::CostmapDownsampler()
+: _costmap(nullptr),
+  _downsampled_costmap(nullptr),
+  _downsampled_costmap_pub(nullptr)
+{
+}
+
+CostmapDownsampler::~CostmapDownsampler()
+{
+}
+
+void CostmapDownsampler::on_configure(
+  const nav2_util::LifecycleNode::WeakPtr & node,
+  const std::string & global_frame,
+  const std::string & topic_name,
+  nav2_costmap_2d::Costmap2D * const costmap,
+  const unsigned int & downsampling_factor,
+  const bool & use_min_cost_neighbor)
+{
+  _costmap = costmap;
+  _downsampling_factor = downsampling_factor;
+  _use_min_cost_neighbor = use_min_cost_neighbor;
+  updateCostmapSize();
+
+  _downsampled_costmap = std::make_unique<nav2_costmap_2d::Costmap2D>(
+    _downsampled_size_x, _downsampled_size_y, _downsampled_resolution,
+    _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN);
+
+  if (!node.expired()) {
+    _downsampled_costmap_pub = std::make_unique<nav2_costmap_2d::Costmap2DPublisher>(
+      node, _downsampled_costmap.get(), global_frame, topic_name, false);
+  }
+}
+
+void CostmapDownsampler::on_activate()
+{
+  if (_downsampled_costmap_pub) {
+    _downsampled_costmap_pub->on_activate();
+  }
+}
+
+void CostmapDownsampler::on_deactivate()
+{
+  if (_downsampled_costmap_pub) {
+    _downsampled_costmap_pub->on_deactivate();
+  }
+}
+
+void CostmapDownsampler::on_cleanup()
+{
+  _costmap = nullptr;
+  _downsampled_costmap.reset();
+  _downsampled_costmap_pub.reset();
+}
+
+nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample(
+  const unsigned int & downsampling_factor)
+{
+  _downsampling_factor = downsampling_factor;
+  updateCostmapSize();
+
+  // Adjust costmap size if needed
+  if (_downsampled_costmap->getSizeInCellsX() != _downsampled_size_x ||
+    _downsampled_costmap->getSizeInCellsY() != _downsampled_size_y ||
+    _downsampled_costmap->getResolution() != _downsampled_resolution)
+  {
+    resizeCostmap();
+  }
+
+  // Assign costs
+  for (unsigned int i = 0; i < _downsampled_size_x; ++i) {
+    for (unsigned int j = 0; j < _downsampled_size_y; ++j) {
+      setCostOfCell(i, j);
+    }
+  }
+
+  if (_downsampled_costmap_pub) {
+    _downsampled_costmap_pub->publishCostmap();
+  }
+  return _downsampled_costmap.get();
+}
+
+void CostmapDownsampler::updateCostmapSize()
+{
+  _size_x = _costmap->getSizeInCellsX();
+  _size_y = _costmap->getSizeInCellsY();
+  _downsampled_size_x = ceil(static_cast<float>(_size_x) / _downsampling_factor);
+  _downsampled_size_y = ceil(static_cast<float>(_size_y) / _downsampling_factor);
+  _downsampled_resolution = _downsampling_factor * _costmap->getResolution();
+}
+
+void CostmapDownsampler::resizeCostmap()
+{
+  _downsampled_costmap->resizeMap(
+    _downsampled_size_x,
+    _downsampled_size_y,
+    _downsampled_resolution,
+    _costmap->getOriginX(),
+    _costmap->getOriginY());
+}
+
+void CostmapDownsampler::setCostOfCell(
+  const unsigned int & new_mx,
+  const unsigned int & new_my)
+{
+  unsigned int mx, my;
+  unsigned char cost = _use_min_cost_neighbor ? 255 : 0;
+  unsigned int x_offset = new_mx * _downsampling_factor;
+  unsigned int y_offset = new_my * _downsampling_factor;
+
+  for (unsigned int i = 0; i < _downsampling_factor; ++i) {
+    mx = x_offset + i;
+    if (mx >= _size_x) {
+      continue;
+    }
+    for (unsigned int j = 0; j < _downsampling_factor; ++j) {
+      my = y_offset + j;
+      if (my >= _size_y) {
+        continue;
+      }
+      cost = _use_min_cost_neighbor ?
+        std::min(cost, _costmap->getCost(mx, my)) :
+        std::max(cost, _costmap->getCost(mx, my));
+    }
+  }
+
+  _downsampled_costmap->setCost(new_mx, new_my, cost);
+}
+
+}  // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/node_2d.cpp

@@ -0,0 +1,171 @@
+// Copyright (c) 2020, Samsung Research America
+// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include "nav2_smac_planner/node_2d.hpp"
+
+#include <vector>
+#include <limits>
+
+namespace nav2_smac_planner
+{
+
+// defining static member for all instance to share
+std::vector<int> Node2D::_neighbors_grid_offsets;
+float Node2D::cost_travel_multiplier = 2.0;
+
+Node2D::Node2D(const unsigned int index)
+: parent(nullptr),
+  _cell_cost(std::numeric_limits<float>::quiet_NaN()),
+  _accumulated_cost(std::numeric_limits<float>::max()),
+  _index(index),
+  _was_visited(false),
+  _is_queued(false)
+{
+}
+
+Node2D::~Node2D()
+{
+  parent = nullptr;
+}
+
+void Node2D::reset()
+{
+  parent = nullptr;
+  _cell_cost = std::numeric_limits<float>::quiet_NaN();
+  _accumulated_cost = std::numeric_limits<float>::max();
+  _was_visited = false;
+  _is_queued = false;
+}
+
+bool Node2D::isNodeValid(
+  const bool & traverse_unknown,
+  GridCollisionChecker * collision_checker)
+{
+  if (collision_checker->inCollision(this->getIndex(), traverse_unknown)) {
+    return false;
+  }
+
+  _cell_cost = collision_checker->getCost();
+  return true;
+}
+
+float Node2D::getTraversalCost(const NodePtr & child)
+{
+  float normalized_cost = child->getCost() / 252.0;
+  const Coordinates A = getCoords(child->getIndex());
+  const Coordinates B = getCoords(this->getIndex());
+  const float & dx = A.x - B.x;
+  const float & dy = A.y - B.y;
+  static float sqrt_2 = sqrt(2);
+
+  // If a diagonal move, travel cost is sqrt(2) not 1.0.
+  if ((dx * dx + dy * dy) > 1.05) {
+    return sqrt_2 * (1.0 + cost_travel_multiplier * normalized_cost);
+  }
+
+  // Length = 1.0
+  return 1.0 + cost_travel_multiplier * normalized_cost;
+}
+
+float Node2D::getHeuristicCost(
+  const Coordinates & node_coords,
+  const Coordinates & goal_coordinates,
+  const nav2_costmap_2d::Costmap2D * /*costmap*/)
+{
+  // Using Moore distance as it more accurately represents the distances
+  // even a Van Neumann neighborhood robot can navigate.
+  auto dx = goal_coordinates.x - node_coords.x;
+  auto dy = goal_coordinates.y - node_coords.y;
+  return std::sqrt(dx * dx + dy * dy);
+}
+
+void Node2D::initMotionModel(
+  const MotionModel & motion_model,
+  unsigned int & x_size_uint,
+  unsigned int & /*size_y*/,
+  unsigned int & /*num_angle_quantization*/,
+  SearchInfo & search_info)
+{
+  if (motion_model != MotionModel::TWOD) {
+    throw std::runtime_error("Invalid motion model for 2D node.");
+  }
+
+  int x_size = static_cast<int>(x_size_uint);
+  cost_travel_multiplier = search_info.cost_penalty;
+  _neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1,
+    -x_size + 1, +x_size - 1, +x_size + 1};
+}
+
+void Node2D::getNeighbors(
+  std::function<bool(const unsigned int &, nav2_smac_planner::Node2D * &)> & NeighborGetter,
+  GridCollisionChecker * collision_checker,
+  const bool & traverse_unknown,
+  NodeVector & neighbors)
+{
+  // NOTE(stevemacenski): Irritatingly, the order here matters. If you start in free
+  // space and then expand 8-connected, the first set of neighbors will be all cost
+  // 1.0. Then its expansion will all be 2 * 1.0 but now multiple
+  // nodes are touching that node so the last cell to update the back pointer wins.
+  // Thusly, the ordering ends with the cardinal directions for both sets such that
+  // behavior is consistent in large free spaces between them.
+  // 100  50   0
+  // 100  50  50
+  // 100 100 100   where lower-middle '100' is visited with same cost by both bottom '50' nodes
+  // Therefore, it is valuable to have some low-potential across the entire map
+  // rather than a small inflation around the obstacles
+  int index;
+  NodePtr neighbor;
+  int node_i = this->getIndex();
+  const Coordinates parent = getCoords(this->getIndex());
+  Coordinates child;
+
+  for (unsigned int i = 0; i != _neighbors_grid_offsets.size(); ++i) {
+    index = node_i + _neighbors_grid_offsets[i];
+
+    // Check for wrap around conditions
+    child = getCoords(index);
+    if (fabs(parent.x - child.x) > 1 || fabs(parent.y - child.y) > 1) {
+      continue;
+    }
+
+    if (NeighborGetter(index, neighbor)) {
+      if (neighbor->isNodeValid(traverse_unknown, collision_checker) && !neighbor->wasVisited()) {
+        neighbors.push_back(neighbor);
+      }
+    }
+  }
+}
+
+bool Node2D::backtracePath(CoordinateVector & path)
+{
+  if (!this->parent) {
+    return false;
+  }
+
+  NodePtr current_node = this;
+
+  while (current_node->parent) {
+    path.push_back(
+      Node2D::getCoords(current_node->getIndex()));
+    current_node = current_node->parent;
+  }
+
+  // add the start pose
+  path.push_back(Node2D::getCoords(current_node->getIndex()));
+
+  return true;
+}
+
+}  // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/node_basic.cpp

@@ -0,0 +1,77 @@
+// Copyright (c) 2021, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include "nav2_smac_planner/node_basic.hpp"
+
+namespace nav2_smac_planner
+{
+
+template<typename Node2D>
+void NodeBasic<Node2D>::processSearchNode()
+{
+}
+
+template<>
+void NodeBasic<NodeHybrid>::processSearchNode()
+{
+  // We only want to override the node's pose if it has not yet been visited
+  // to prevent the case that a node has been queued multiple times and
+  // a new branch is overriding one of lower cost already visited.
+  if (!this->graph_node_ptr->wasVisited()) {
+    this->graph_node_ptr->pose = this->pose;
+    this->graph_node_ptr->setMotionPrimitiveIndex(this->motion_index);
+  }
+}
+
+template<>
+void NodeBasic<NodeLattice>::processSearchNode()
+{
+  // We only want to override the node's pose/primitive if it has not yet been visited
+  // to prevent the case that a node has been queued multiple times and
+  // a new branch is overriding one of lower cost already visited.
+  if (!this->graph_node_ptr->wasVisited()) {
+    this->graph_node_ptr->pose = this->pose;
+    this->graph_node_ptr->setMotionPrimitive(this->prim_ptr);
+    this->graph_node_ptr->backwards(this->backward);
+  }
+}
+
+template<>
+void NodeBasic<Node2D>::populateSearchNode(Node2D * & node)
+{
+  this->graph_node_ptr = node;
+}
+
+template<>
+void NodeBasic<NodeHybrid>::populateSearchNode(NodeHybrid * & node)
+{
+  this->pose = node->pose;
+  this->graph_node_ptr = node;
+  this->motion_index = node->getMotionPrimitiveIndex();
+}
+
+template<>
+void NodeBasic<NodeLattice>::populateSearchNode(NodeLattice * & node)
+{
+  this->pose = node->pose;
+  this->graph_node_ptr = node;
+  this->prim_ptr = node->getMotionPrimitive();
+  this->backward = node->isBackward();
+}
+
+template class NodeBasic<Node2D>;
+template class NodeBasic<NodeHybrid>;
+template class NodeBasic<NodeLattice>;
+
+}  // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/node_hybrid.cpp

@@ -0,0 +1,724 @@
+// Copyright (c) 2020, Samsung Research America
+// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <math.h>
+#include <chrono>
+#include <vector>
+#include <memory>
+#include <algorithm>
+#include <queue>
+#include <limits>
+#include <utility>
+
+#include "ompl/base/ScopedState.h"
+#include "ompl/base/spaces/DubinsStateSpace.h"
+#include "ompl/base/spaces/ReedsSheppStateSpace.h"
+
+#include "nav2_smac_planner/node_hybrid.hpp"
+
+using namespace std::chrono;  // NOLINT
+
+namespace nav2_smac_planner
+{
+
+// defining static member for all instance to share
+LookupTable NodeHybrid::obstacle_heuristic_lookup_table;
+double NodeHybrid::travel_distance_cost = sqrt(2);
+HybridMotionTable NodeHybrid::motion_table;
+float NodeHybrid::size_lookup = 25;
+LookupTable NodeHybrid::dist_heuristic_lookup_table;
+nav2_costmap_2d::Costmap2D * NodeHybrid::sampled_costmap = nullptr;
+CostmapDownsampler NodeHybrid::downsampler;
+ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue;
+
+// Each of these tables are the projected motion models through
+// time and space applied to the search on the current node in
+// continuous map-coordinates (e.g. not meters but partial map cells)
+// Currently, these are set to project *at minimum* into a neighboring
+// cell. Though this could be later modified to project a certain
+// amount of time or particular distance forward.
+
+// http://planning.cs.uiuc.edu/node821.html
+// Model for ackermann style vehicle with minimum radius restriction
+void HybridMotionTable::initDubin(
+  unsigned int & size_x_in,
+  unsigned int & /*size_y_in*/,
+  unsigned int & num_angle_quantization_in,
+  SearchInfo & search_info)
+{
+  size_x = size_x_in;
+  change_penalty = search_info.change_penalty;
+  non_straight_penalty = search_info.non_straight_penalty;
+  cost_penalty = search_info.cost_penalty;
+  reverse_penalty = search_info.reverse_penalty;
+  travel_distance_reward = 1.0f - search_info.retrospective_penalty;
+
+  // if nothing changed, no need to re-compute primitives
+  if (num_angle_quantization_in == num_angle_quantization &&
+    min_turning_radius == search_info.minimum_turning_radius &&
+    motion_model == MotionModel::DUBIN)
+  {
+    return;
+  }
+
+  num_angle_quantization = num_angle_quantization_in;
+  num_angle_quantization_float = static_cast<float>(num_angle_quantization);
+  min_turning_radius = search_info.minimum_turning_radius;
+  motion_model = MotionModel::DUBIN;
+
+  // angle must meet 3 requirements:
+  // 1) be increment of quantized bin size
+  // 2) chord length must be greater than sqrt(2) to leave current cell
+  // 3) maximum curvature must be respected, represented by minimum turning angle
+  // Thusly:
+  // On circle of radius minimum turning angle, we need select motion primatives
+  // with chord length > sqrt(2) and be an increment of our bin size
+  //
+  // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size
+  // Thusly: angle <= 2.0 * asin(sqrt(2) / (2 * R))
+  float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius));
+  // Now make sure angle is an increment of the quantized bin size
+  // And since its based on the minimum chord, we need to make sure its always larger
+  bin_size =
+    2.0f * static_cast<float>(M_PI) / static_cast<float>(num_angle_quantization);
+  float increments;
+  if (angle < bin_size) {
+    increments = 1.0f;
+  } else {
+    // Search dimensions are clean multiples of quantization - this prevents
+    // paths with loops in them
+    increments = ceil(angle / bin_size);
+  }
+  angle = increments * bin_size;
+
+  // find deflections
+  // If we make a right triangle out of the chord in circle of radius
+  // min turning angle, we can see that delta X = R * sin (angle)
+  float delta_x = min_turning_radius * sin(angle);
+  // Using that same right triangle, we can see that the complement
+  // to delta Y is R * cos (angle). If we subtract R, we get the actual value
+  float delta_y = min_turning_radius - (min_turning_radius * cos(angle));
+
+  projections.clear();
+  projections.reserve(3);
+  projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0);  // Forward
+  projections.emplace_back(delta_x, delta_y, increments);  // Left
+  projections.emplace_back(delta_x, -delta_y, -increments);  // Right
+
+  // Create the correct OMPL state space
+  state_space = std::make_unique<ompl::base::DubinsStateSpace>(min_turning_radius);
+
+  // Precompute projection deltas
+  delta_xs.resize(projections.size());
+  delta_ys.resize(projections.size());
+  trig_values.resize(num_angle_quantization);
+
+  for (unsigned int i = 0; i != projections.size(); i++) {
+    delta_xs[i].resize(num_angle_quantization);
+    delta_ys[i].resize(num_angle_quantization);
+
+    for (unsigned int j = 0; j != num_angle_quantization; j++) {
+      double cos_theta = cos(bin_size * j);
+      double sin_theta = sin(bin_size * j);
+      if (i == 0) {
+        // if first iteration, cache the trig values for later
+        trig_values[j] = {cos_theta, sin_theta};
+      }
+      delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta;
+      delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta;
+    }
+  }
+}
+
+// http://planning.cs.uiuc.edu/node822.html
+// Same as Dubin model but now reverse is valid
+// See notes in Dubin for explanation
+void HybridMotionTable::initReedsShepp(
+  unsigned int & size_x_in,
+  unsigned int & /*size_y_in*/,
+  unsigned int & num_angle_quantization_in,
+  SearchInfo & search_info)
+{
+  size_x = size_x_in;
+  change_penalty = search_info.change_penalty;
+  non_straight_penalty = search_info.non_straight_penalty;
+  cost_penalty = search_info.cost_penalty;
+  reverse_penalty = search_info.reverse_penalty;
+  travel_distance_reward = 1.0f - search_info.retrospective_penalty;
+
+  // if nothing changed, no need to re-compute primitives
+  if (num_angle_quantization_in == num_angle_quantization &&
+    min_turning_radius == search_info.minimum_turning_radius &&
+    motion_model == MotionModel::REEDS_SHEPP)
+  {
+    return;
+  }
+
+  num_angle_quantization = num_angle_quantization_in;
+  num_angle_quantization_float = static_cast<float>(num_angle_quantization);
+  min_turning_radius = search_info.minimum_turning_radius;
+  motion_model = MotionModel::REEDS_SHEPP;
+
+  float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius));
+  bin_size =
+    2.0f * static_cast<float>(M_PI) / static_cast<float>(num_angle_quantization);
+  float increments;
+  if (angle < bin_size) {
+    increments = 1.0f;
+  } else {
+    increments = ceil(angle / bin_size);
+  }
+  angle = increments * bin_size;
+
+  float delta_x = min_turning_radius * sin(angle);
+  float delta_y = min_turning_radius - (min_turning_radius * cos(angle));
+
+  projections.clear();
+  projections.reserve(6);
+  projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0);  // Forward
+  projections.emplace_back(delta_x, delta_y, increments);  // Forward + Left
+  projections.emplace_back(delta_x, -delta_y, -increments);  // Forward + Right
+  projections.emplace_back(-hypotf(delta_x, delta_y), 0.0, 0.0);  // Backward
+  projections.emplace_back(-delta_x, delta_y, -increments);  // Backward + Left
+  projections.emplace_back(-delta_x, -delta_y, increments);  // Backward + Right
+
+  // Create the correct OMPL state space
+  state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(min_turning_radius);
+
+  // Precompute projection deltas
+  delta_xs.resize(projections.size());
+  delta_ys.resize(projections.size());
+  trig_values.resize(num_angle_quantization);
+
+  for (unsigned int i = 0; i != projections.size(); i++) {
+    delta_xs[i].resize(num_angle_quantization);
+    delta_ys[i].resize(num_angle_quantization);
+
+    for (unsigned int j = 0; j != num_angle_quantization; j++) {
+      double cos_theta = cos(bin_size * j);
+      double sin_theta = sin(bin_size * j);
+      if (i == 0) {
+        // if first iteration, cache the trig values for later
+        trig_values[j] = {cos_theta, sin_theta};
+      }
+      delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta;
+      delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta;
+    }
+  }
+}
+
+MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node)
+{
+  MotionPoses projection_list;
+  projection_list.reserve(projections.size());
+
+  for (unsigned int i = 0; i != projections.size(); i++) {
+    const MotionPose & motion_model = projections[i];
+
+    // normalize theta, I know its overkill, but I've been burned before...
+    const float & node_heading = node->pose.theta;
+    float new_heading = node_heading + motion_model._theta;
+
+    if (new_heading < 0.0) {
+      new_heading += num_angle_quantization_float;
+    }
+
+    if (new_heading >= num_angle_quantization_float) {
+      new_heading -= num_angle_quantization_float;
+    }
+
+    projection_list.emplace_back(
+      delta_xs[i][node_heading] + node->pose.x,
+      delta_ys[i][node_heading] + node->pose.y,
+      new_heading);
+  }
+
+  return projection_list;
+}
+
+unsigned int HybridMotionTable::getClosestAngularBin(const double & theta)
+{
+  auto bin = static_cast<unsigned int>(round(static_cast<float>(theta) / bin_size));
+  return bin < num_angle_quantization ? bin : 0u;
+}
+
+float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx)
+{
+  return bin_idx * bin_size;
+}
+
+NodeHybrid::NodeHybrid(const unsigned int index)
+: parent(nullptr),
+  pose(0.0f, 0.0f, 0.0f),
+  _cell_cost(std::numeric_limits<float>::quiet_NaN()),
+  _accumulated_cost(std::numeric_limits<float>::max()),
+  _index(index),
+  _was_visited(false),
+  _motion_primitive_index(std::numeric_limits<unsigned int>::max())
+{
+}
+
+NodeHybrid::~NodeHybrid()
+{
+  parent = nullptr;
+}
+
+void NodeHybrid::reset()
+{
+  parent = nullptr;
+  _cell_cost = std::numeric_limits<float>::quiet_NaN();
+  _accumulated_cost = std::numeric_limits<float>::max();
+  _was_visited = false;
+  _motion_primitive_index = std::numeric_limits<unsigned int>::max();
+  pose.x = 0.0f;
+  pose.y = 0.0f;
+  pose.theta = 0.0f;
+}
+
+bool NodeHybrid::isNodeValid(
+  const bool & traverse_unknown,
+  GridCollisionChecker * collision_checker)
+{
+  if (collision_checker->inCollision(
+      this->pose.x, this->pose.y, this->pose.theta /*bin number*/, traverse_unknown))
+  {
+    return false;
+  }
+
+  _cell_cost = collision_checker->getCost();
+  return true;
+}
+
+float NodeHybrid::getTraversalCost(const NodePtr & child)
+{
+  const float normalized_cost = child->getCost() / 252.0;
+  if (std::isnan(normalized_cost)) {
+    throw std::runtime_error(
+            "Node attempted to get traversal "
+            "cost without a known SE2 collision cost!");
+  }
+
+  // this is the first node
+  if (getMotionPrimitiveIndex() == std::numeric_limits<unsigned int>::max()) {
+    return NodeHybrid::travel_distance_cost;
+  }
+
+  float travel_cost = 0.0;
+  float travel_cost_raw =
+    NodeHybrid::travel_distance_cost *
+    (motion_table.travel_distance_reward + motion_table.cost_penalty * normalized_cost);
+
+  if (child->getMotionPrimitiveIndex() == 0 || child->getMotionPrimitiveIndex() == 3) {
+    // New motion is a straight motion, no additional costs to be applied
+    travel_cost = travel_cost_raw;
+  } else {
+    if (getMotionPrimitiveIndex() == child->getMotionPrimitiveIndex()) {
+      // Turning motion but keeps in same direction: encourages to commit to turning if starting it
+      travel_cost = travel_cost_raw * motion_table.non_straight_penalty;
+    } else {
+      // Turning motion and changing direction: penalizes wiggling
+      travel_cost = travel_cost_raw *
+        (motion_table.non_straight_penalty + motion_table.change_penalty);
+    }
+  }
+
+  if (child->getMotionPrimitiveIndex() > 2) {
+    // reverse direction
+    travel_cost *= motion_table.reverse_penalty;
+  }
+
+  return travel_cost;
+}
+
+float NodeHybrid::getHeuristicCost(
+  const Coordinates & node_coords,
+  const Coordinates & goal_coords,
+  const nav2_costmap_2d::Costmap2D * /*costmap*/)
+{
+  const float obstacle_heuristic =
+    getObstacleHeuristic(node_coords, goal_coords, motion_table.cost_penalty);
+  const float dist_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic);
+  return std::max(obstacle_heuristic, dist_heuristic);
+}
+
+void NodeHybrid::initMotionModel(
+  const MotionModel & motion_model,
+  unsigned int & size_x,
+  unsigned int & size_y,
+  unsigned int & num_angle_quantization,
+  SearchInfo & search_info)
+{
+  // find the motion model selected
+  switch (motion_model) {
+    case MotionModel::DUBIN:
+      motion_table.initDubin(size_x, size_y, num_angle_quantization, search_info);
+      break;
+    case MotionModel::REEDS_SHEPP:
+      motion_table.initReedsShepp(size_x, size_y, num_angle_quantization, search_info);
+      break;
+    default:
+      throw std::runtime_error(
+              "Invalid motion model for Hybrid A*. Please select between"
+              " Dubin (Ackermann forward only),"
+              " Reeds-Shepp (Ackermann forward and back).");
+  }
+
+  travel_distance_cost = motion_table.projections[0]._x;
+}
+
+inline float distanceHeuristic2D(
+  const unsigned int idx, const unsigned int size_x,
+  const unsigned int target_x, const unsigned int target_y)
+{
+  int dx = static_cast<int>(idx % size_x) - static_cast<int>(target_x);
+  int dy = static_cast<int>(idx / size_x) - static_cast<int>(target_y);
+  return std::sqrt(dx * dx + dy * dy);
+}
+
+void NodeHybrid::resetObstacleHeuristic(
+  nav2_costmap_2d::Costmap2D * costmap,
+  const unsigned int & start_x, const unsigned int & start_y,
+  const unsigned int & goal_x, const unsigned int & goal_y)
+{
+  // Downsample costmap 2x to compute a sparse obstacle heuristic. This speeds up
+  // the planner considerably to search through 75% less cells with no detectable
+  // erosion of path quality after even modest smoothing. The error would be no more
+  // than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality
+  std::weak_ptr<nav2_util::LifecycleNode> ptr;
+  downsampler.on_configure(ptr, "fake_frame", "fake_topic", costmap, 2.0, true);
+  downsampler.on_activate();
+  sampled_costmap = downsampler.downsample(2.0);
+
+  // Clear lookup table
+  unsigned int size = sampled_costmap->getSizeInCellsX() * sampled_costmap->getSizeInCellsY();
+  if (obstacle_heuristic_lookup_table.size() == size) {
+    // must reset all values
+    std::fill(
+      obstacle_heuristic_lookup_table.begin(),
+      obstacle_heuristic_lookup_table.end(), 0.0);
+  } else {
+    unsigned int obstacle_size = obstacle_heuristic_lookup_table.size();
+    obstacle_heuristic_lookup_table.resize(size, 0.0);
+    // must reset values for non-constructed indices
+    std::fill_n(
+      obstacle_heuristic_lookup_table.begin(), obstacle_size, 0.0);
+  }
+
+  obstacle_heuristic_queue.clear();
+  obstacle_heuristic_queue.reserve(
+    sampled_costmap->getSizeInCellsX() * sampled_costmap->getSizeInCellsY());
+
+  // Set initial goal point to queue from. Divided by 2 due to downsampled costmap.
+  const unsigned int size_x = sampled_costmap->getSizeInCellsX();
+  const unsigned int goal_index = floor(goal_y / 2.0) * size_x + floor(goal_x / 2.0);
+  obstacle_heuristic_queue.emplace_back(
+    distanceHeuristic2D(goal_index, size_x, start_x, start_y), goal_index);
+
+  // initialize goal cell with a very small value to differentiate it from 0.0 (~uninitialized)
+  // the negative value means the cell is in the open set
+  obstacle_heuristic_lookup_table[goal_index] = -0.00001f;
+}
+
+float NodeHybrid::getObstacleHeuristic(
+  const Coordinates & node_coords,
+  const Coordinates & goal_coords,
+  const double & cost_penalty)
+{
+  // If already expanded, return the cost
+  const unsigned int size_x = sampled_costmap->getSizeInCellsX();
+  // Divided by 2 due to downsampled costmap.
+  const unsigned int start_y = floor(node_coords.y / 2.0);
+  const unsigned int start_x = floor(node_coords.x / 2.0);
+  const unsigned int start_index = start_y * size_x + start_x;
+  const float & requested_node_cost = obstacle_heuristic_lookup_table[start_index];
+  if (requested_node_cost > 0.0f) {
+    // costs are doubled due to downsampling
+    return 2.0 * requested_node_cost;
+  }
+
+  // If not, expand until it is included. This dynamic programming ensures that
+  // we only expand the MINIMUM spanning set of the costmap per planning request.
+  // Rather than naively expanding the entire (potentially massive) map for a limited
+  // path, we only expand to the extent required for the furthest expansion in the
+  // search-planning request that dynamically updates during search as needed.
+
+  // start_x and start_y have changed since last call
+  // we need to recompute 2D distance heuristic and reprioritize queue
+  for (auto & n : obstacle_heuristic_queue) {
+    n.first = -obstacle_heuristic_lookup_table[n.second] +
+      distanceHeuristic2D(n.second, size_x, start_x, start_y);
+  }
+  std::make_heap(
+    obstacle_heuristic_queue.begin(), obstacle_heuristic_queue.end(),
+    ObstacleHeuristicComparator{});
+
+  const int size_x_int = static_cast<int>(size_x);
+  const unsigned int size_y = sampled_costmap->getSizeInCellsY();
+  const float sqrt_2 = sqrt(2);
+  float c_cost, cost, travel_cost, new_cost, existing_cost;
+  unsigned int idx, mx, my, mx_idx, my_idx;
+  unsigned int new_idx = 0;
+
+  const std::vector<int> neighborhood = {1, -1,  // left right
+    size_x_int, -size_x_int,  // up down
+    size_x_int + 1, size_x_int - 1,  // upper diagonals
+    -size_x_int + 1, -size_x_int - 1};  // lower diagonals
+
+  while (!obstacle_heuristic_queue.empty()) {
+    idx = obstacle_heuristic_queue.front().second;
+    std::pop_heap(
+      obstacle_heuristic_queue.begin(), obstacle_heuristic_queue.end(),
+      ObstacleHeuristicComparator{});
+    obstacle_heuristic_queue.pop_back();
+    c_cost = obstacle_heuristic_lookup_table[idx];
+    if (c_cost > 0.0f) {
+      // cell has been processed and closed, no further cost improvements
+      // are mathematically possible thanks to euclidean distance heuristic consistency
+      continue;
+    }
+    c_cost = -c_cost;
+    obstacle_heuristic_lookup_table[idx] = c_cost;  // set a positive value to close the cell
+
+    my_idx = idx / size_x;
+    mx_idx = idx - (my_idx * size_x);
+
+    // find neighbors
+    for (unsigned int i = 0; i != neighborhood.size(); i++) {
+      new_idx = static_cast<unsigned int>(static_cast<int>(idx) + neighborhood[i]);
+
+      // if neighbor path is better and non-lethal, set new cost and add to queue
+      if (new_idx < size_x * size_y) {
+        cost = static_cast<float>(sampled_costmap->getCost(new_idx));
+        if (cost >= INSCRIBED) {
+          continue;
+        }
+
+        my = new_idx / size_x;
+        mx = new_idx - (my * size_x);
+
+        if (mx == 0 && mx_idx >= size_x - 1 || mx >= size_x - 1 && mx_idx == 0) {
+          continue;
+        }
+        if (my == 0 && my_idx >= size_y - 1 || my >= size_y - 1 && my_idx == 0) {
+          continue;
+        }
+
+        existing_cost = obstacle_heuristic_lookup_table[new_idx];
+        if (existing_cost <= 0.0f) {
+          travel_cost =
+            ((i <= 3) ? 1.0f : sqrt_2) * (1.0f + (cost_penalty * cost / 252.0f));
+          new_cost = c_cost + travel_cost;
+          if (existing_cost == 0.0f || -existing_cost > new_cost) {
+            // the negative value means the cell is in the open set
+            obstacle_heuristic_lookup_table[new_idx] = -new_cost;
+            obstacle_heuristic_queue.emplace_back(
+              new_cost + distanceHeuristic2D(new_idx, size_x, start_x, start_y), new_idx);
+            std::push_heap(
+              obstacle_heuristic_queue.begin(), obstacle_heuristic_queue.end(),
+              ObstacleHeuristicComparator{});
+          }
+        }
+      }
+    }
+
+    if (idx == start_index) {
+      break;
+    }
+  }
+
+  // return requested_node_cost which has been updated by the search
+  // costs are doubled due to downsampling
+  return 2.0 * requested_node_cost;
+}
+
+float NodeHybrid::getDistanceHeuristic(
+  const Coordinates & node_coords,
+  const Coordinates & goal_coords,
+  const float & obstacle_heuristic)
+{
+  // rotate and translate node_coords such that goal_coords relative is (0,0,0)
+  // Due to the rounding involved in exact cell increments for caching,
+  // this is not an exact replica of a live heuristic, but has bounded error.
+  // (Usually less than 1 cell)
+
+  // This angle is negative since we are de-rotating the current node
+  // by the goal angle; cos(-th) = cos(th) & sin(-th) = -sin(th)
+  const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta];
+  const float cos_th = trig_vals.first;
+  const float sin_th = -trig_vals.second;
+  const float dx = node_coords.x - goal_coords.x;
+  const float dy = node_coords.y - goal_coords.y;
+
+  double dtheta_bin = node_coords.theta - goal_coords.theta;
+  if (dtheta_bin < 0) {
+    dtheta_bin += motion_table.num_angle_quantization;
+  }
+  if (dtheta_bin > motion_table.num_angle_quantization) {
+    dtheta_bin -= motion_table.num_angle_quantization;
+  }
+
+  Coordinates node_coords_relative(
+    round(dx * cos_th - dy * sin_th),
+    round(dx * sin_th + dy * cos_th),
+    round(dtheta_bin));
+
+  // Check if the relative node coordinate is within the localized window around the goal
+  // to apply the distance heuristic. Since the lookup table is contains only the positive
+  // X axis, we mirror the Y and theta values across the X axis to find the heuristic values.
+  float motion_heuristic = 0.0;
+  const int floored_size = floor(size_lookup / 2.0);
+  const int ceiling_size = ceil(size_lookup / 2.0);
+  const float mirrored_relative_y = abs(node_coords_relative.y);
+  if (abs(node_coords_relative.x) < floored_size && mirrored_relative_y < floored_size) {
+    // Need to mirror angle if Y coordinate was mirrored
+    int theta_pos;
+    if (node_coords_relative.y < 0.0) {
+      theta_pos = motion_table.num_angle_quantization - node_coords_relative.theta;
+    } else {
+      theta_pos = node_coords_relative.theta;
+    }
+    const int x_pos = node_coords_relative.x + floored_size;
+    const int y_pos = static_cast<int>(mirrored_relative_y);
+    const int index =
+      x_pos * ceiling_size * motion_table.num_angle_quantization +
+      y_pos * motion_table.num_angle_quantization +
+      theta_pos;
+    motion_heuristic = dist_heuristic_lookup_table[index];
+  } else if (obstacle_heuristic <= 0.0) {
+    // If no obstacle heuristic value, must have some H to use
+    // In nominal situations, this should never be called.
+    static ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space);
+    to[0] = goal_coords.x;
+    to[1] = goal_coords.y;
+    to[2] = goal_coords.theta * motion_table.num_angle_quantization;
+    from[0] = node_coords.x;
+    from[1] = node_coords.y;
+    from[2] = node_coords.theta * motion_table.num_angle_quantization;
+    motion_heuristic = motion_table.state_space->distance(from(), to());
+  }
+
+  return motion_heuristic;
+}
+
+void NodeHybrid::precomputeDistanceHeuristic(
+  const float & lookup_table_dim,
+  const MotionModel & motion_model,
+  const unsigned int & dim_3_size,
+  const SearchInfo & search_info)
+{
+  // Dubin or Reeds-Shepp shortest distances
+  if (motion_model == MotionModel::DUBIN) {
+    motion_table.state_space = std::make_unique<ompl::base::DubinsStateSpace>(
+      search_info.minimum_turning_radius);
+  } else if (motion_model == MotionModel::REEDS_SHEPP) {
+    motion_table.state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(
+      search_info.minimum_turning_radius);
+  } else {
+    throw std::runtime_error(
+            "Node attempted to precompute distance heuristics "
+            "with invalid motion model!");
+  }
+
+  ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space);
+  to[0] = 0.0;
+  to[1] = 0.0;
+  to[2] = 0.0;
+  size_lookup = lookup_table_dim;
+  float motion_heuristic = 0.0;
+  unsigned int index = 0;
+  int dim_3_size_int = static_cast<int>(dim_3_size);
+  float angular_bin_size = 2 * M_PI / static_cast<float>(dim_3_size);
+
+  // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal
+  // to help drive the search towards admissible approaches. Deu to symmetries in the
+  // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror
+  // around the X axis any relative node lookup. This reduces memory overhead and increases
+  // the size of a window a platform can store in memory.
+  dist_heuristic_lookup_table.resize(size_lookup * ceil(size_lookup / 2.0) * dim_3_size_int);
+  for (float x = ceil(-size_lookup / 2.0); x <= floor(size_lookup / 2.0); x += 1.0) {
+    for (float y = 0.0; y <= floor(size_lookup / 2.0); y += 1.0) {
+      for (int heading = 0; heading != dim_3_size_int; heading++) {
+        from[0] = x;
+        from[1] = y;
+        from[2] = heading * angular_bin_size;
+        motion_heuristic = motion_table.state_space->distance(from(), to());
+        dist_heuristic_lookup_table[index] = motion_heuristic;
+        index++;
+      }
+    }
+  }
+}
+
+void NodeHybrid::getNeighbors(
+  std::function<bool(const unsigned int &, nav2_smac_planner::NodeHybrid * &)> & NeighborGetter,
+  GridCollisionChecker * collision_checker,
+  const bool & traverse_unknown,
+  NodeVector & neighbors)
+{
+  unsigned int index = 0;
+  NodePtr neighbor = nullptr;
+  Coordinates initial_node_coords;
+  const MotionPoses motion_projections = motion_table.getProjections(this);
+
+  for (unsigned int i = 0; i != motion_projections.size(); i++) {
+    index = NodeHybrid::getIndex(
+      static_cast<unsigned int>(motion_projections[i]._x),
+      static_cast<unsigned int>(motion_projections[i]._y),
+      static_cast<unsigned int>(motion_projections[i]._theta),
+      motion_table.size_x, motion_table.num_angle_quantization);
+
+    if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) {
+      // Cache the initial pose in case it was visited but valid
+      // don't want to disrupt continuous coordinate expansion
+      initial_node_coords = neighbor->pose;
+      neighbor->setPose(
+        Coordinates(
+          motion_projections[i]._x,
+          motion_projections[i]._y,
+          motion_projections[i]._theta));
+      if (neighbor->isNodeValid(traverse_unknown, collision_checker)) {
+        neighbor->setMotionPrimitiveIndex(i);
+        neighbors.push_back(neighbor);
+      } else {
+        neighbor->setPose(initial_node_coords);
+      }
+    }
+  }
+}
+
+bool NodeHybrid::backtracePath(CoordinateVector & path)
+{
+  if (!this->parent) {
+    return false;
+  }
+
+  NodePtr current_node = this;
+
+  while (current_node->parent) {
+    path.push_back(current_node->pose);
+    // Convert angle to radians
+    path.back().theta = NodeHybrid::motion_table.getAngleFromBin(path.back().theta);
+    current_node = current_node->parent;
+  }
+
+  // add the start pose
+  path.push_back(current_node->pose);
+  // Convert angle to radians
+  path.back().theta = NodeHybrid::motion_table.getAngleFromBin(path.back().theta);
+
+  return true;
+}
+
+}  // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/node_lattice.cpp

@@ -0,0 +1,591 @@
+// Copyright (c) 2021, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <math.h>
+#include <chrono>
+#include <vector>
+#include <memory>
+#include <algorithm>
+#include <queue>
+#include <limits>
+#include <string>
+#include <fstream>
+#include <cmath>
+
+#include "ompl/base/ScopedState.h"
+#include "ompl/base/spaces/DubinsStateSpace.h"
+#include "ompl/base/spaces/ReedsSheppStateSpace.h"
+
+#include "nav2_smac_planner/node_lattice.hpp"
+
+using namespace std::chrono;  // NOLINT
+
+namespace nav2_smac_planner
+{
+
+// defining static member for all instance to share
+LatticeMotionTable NodeLattice::motion_table;
+float NodeLattice::size_lookup = 25;
+LookupTable NodeLattice::dist_heuristic_lookup_table;
+
+// Each of these tables are the projected motion models through
+// time and space applied to the search on the current node in
+// continuous map-coordinates (e.g. not meters but partial map cells)
+// Currently, these are set to project *at minimum* into a neighboring
+// cell. Though this could be later modified to project a certain
+// amount of time or particular distance forward.
+void LatticeMotionTable::initMotionModel(
+  unsigned int & size_x_in,
+  SearchInfo & search_info)
+{
+  size_x = size_x_in;
+
+  if (current_lattice_filepath == search_info.lattice_filepath) {
+    return;
+  }
+
+  size_x = size_x_in;
+  change_penalty = search_info.change_penalty;
+  non_straight_penalty = search_info.non_straight_penalty;
+  cost_penalty = search_info.cost_penalty;
+  reverse_penalty = search_info.reverse_penalty;
+  travel_distance_reward = 1.0f - search_info.retrospective_penalty;
+  current_lattice_filepath = search_info.lattice_filepath;
+  allow_reverse_expansion = search_info.allow_reverse_expansion;
+  rotation_penalty = search_info.rotation_penalty;
+
+  // Get the metadata about this minimum control set
+  lattice_metadata = getLatticeMetadata(current_lattice_filepath);
+  std::ifstream latticeFile(current_lattice_filepath);
+  if (!latticeFile.is_open()) {
+    throw std::runtime_error("Could not open lattice file");
+  }
+  nlohmann::json json;
+  latticeFile >> json;
+  num_angle_quantization = lattice_metadata.number_of_headings;
+
+  if (!state_space) {
+    if (!allow_reverse_expansion) {
+      state_space = std::make_unique<ompl::base::DubinsStateSpace>(
+        lattice_metadata.min_turning_radius);
+    } else {
+      state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(
+        lattice_metadata.min_turning_radius);
+    }
+  }
+
+  // Populate the motion primitives at each heading angle
+  float prev_start_angle = 0.0;
+  std::vector<MotionPrimitive> primitives;
+  nlohmann::json json_primitives = json["primitives"];
+  for (unsigned int i = 0; i < json_primitives.size(); ++i) {
+    MotionPrimitive new_primitive;
+    fromJsonToMotionPrimitive(json_primitives[i], new_primitive);
+
+    if (prev_start_angle != new_primitive.start_angle) {
+      motion_primitives.push_back(primitives);
+      primitives.clear();
+      prev_start_angle = new_primitive.start_angle;
+    }
+    primitives.push_back(new_primitive);
+  }
+  motion_primitives.push_back(primitives);
+
+  // Populate useful precomputed values to be leveraged
+  trig_values.reserve(lattice_metadata.number_of_headings);
+  for (unsigned int i = 0; i < lattice_metadata.heading_angles.size(); ++i) {
+    trig_values.emplace_back(
+      cos(lattice_metadata.heading_angles[i]),
+      sin(lattice_metadata.heading_angles[i]));
+  }
+}
+
+MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives(const NodeLattice * node)
+{
+  MotionPrimitives & prims_at_heading = motion_primitives[node->pose.theta];
+  MotionPrimitivePtrs primitive_projection_list;
+  for (unsigned int i = 0; i != prims_at_heading.size(); i++) {
+    primitive_projection_list.push_back(&prims_at_heading[i]);
+  }
+
+  if (allow_reverse_expansion) {
+    // Find normalized heading bin of the reverse expansion
+    double reserve_heading = node->pose.theta - (num_angle_quantization / 2);
+    if (reserve_heading < 0) {
+      reserve_heading += num_angle_quantization;
+    }
+    if (reserve_heading > num_angle_quantization) {
+      reserve_heading -= num_angle_quantization;
+    }
+
+    MotionPrimitives & prims_at_reverse_heading = motion_primitives[reserve_heading];
+    for (unsigned int i = 0; i != prims_at_reverse_heading.size(); i++) {
+      primitive_projection_list.push_back(&prims_at_reverse_heading[i]);
+    }
+  }
+
+  return primitive_projection_list;
+}
+
+LatticeMetadata LatticeMotionTable::getLatticeMetadata(const std::string & lattice_filepath)
+{
+  std::ifstream lattice_file(lattice_filepath);
+  if (!lattice_file.is_open()) {
+    throw std::runtime_error("Could not open lattice file!");
+  }
+
+  nlohmann::json j;
+  lattice_file >> j;
+  LatticeMetadata metadata;
+  fromJsonToMetaData(j["lattice_metadata"], metadata);
+  return metadata;
+}
+
+unsigned int LatticeMotionTable::getClosestAngularBin(const double & theta)
+{
+  float min_dist = std::numeric_limits<float>::max();
+  unsigned int closest_idx = 0;
+  float dist = 0.0;
+  for (unsigned int i = 0; i != lattice_metadata.heading_angles.size(); i++) {
+    dist = fabs(angles::shortest_angular_distance(theta, lattice_metadata.heading_angles[i]));
+    if (dist < min_dist) {
+      min_dist = dist;
+      closest_idx = i;
+    }
+  }
+  return closest_idx;
+}
+
+float & LatticeMotionTable::getAngleFromBin(const unsigned int & bin_idx)
+{
+  return lattice_metadata.heading_angles[bin_idx];
+}
+
+NodeLattice::NodeLattice(const unsigned int index)
+: parent(nullptr),
+  pose(0.0f, 0.0f, 0.0f),
+  _cell_cost(std::numeric_limits<float>::quiet_NaN()),
+  _accumulated_cost(std::numeric_limits<float>::max()),
+  _index(index),
+  _was_visited(false),
+  _motion_primitive(nullptr),
+  _backwards(false)
+{
+}
+
+NodeLattice::~NodeLattice()
+{
+  parent = nullptr;
+}
+
+void NodeLattice::reset()
+{
+  parent = nullptr;
+  _cell_cost = std::numeric_limits<float>::quiet_NaN();
+  _accumulated_cost = std::numeric_limits<float>::max();
+  _was_visited = false;
+  pose.x = 0.0f;
+  pose.y = 0.0f;
+  pose.theta = 0.0f;
+  _motion_primitive = nullptr;
+  _backwards = false;
+}
+
+bool NodeLattice::isNodeValid(
+  const bool & traverse_unknown,
+  GridCollisionChecker * collision_checker,
+  MotionPrimitive * motion_primitive,
+  bool is_backwards)
+{
+  // Check primitive end pose
+  // Convert grid quantization of primitives to radians, then collision checker quantization
+  static const double bin_size = 2.0 * M_PI / collision_checker->getPrecomputedAngles().size();
+  const double & angle = motion_table.getAngleFromBin(this->pose.theta) / bin_size;
+  if (collision_checker->inCollision(
+      this->pose.x, this->pose.y, angle /*bin in collision checker*/, traverse_unknown))
+  {
+    return false;
+  }
+
+  // Set the cost of a node to the highest cost across the primitive
+  float max_cell_cost = collision_checker->getCost();
+
+  // If valid motion primitives are set, check intermediary poses > 1 cell apart
+  if (motion_primitive) {
+    const float & grid_resolution = motion_table.lattice_metadata.grid_resolution;
+    const float & resolution_diag_sq = 2.0 * grid_resolution * grid_resolution;
+    MotionPose last_pose(1e9, 1e9, 1e9), pose_dist(0.0, 0.0, 0.0);
+
+    // Back out the initial node starting point to move motion primitive relative to
+    MotionPose initial_pose, prim_pose;
+    initial_pose._x = this->pose.x - (motion_primitive->poses.back()._x / grid_resolution);
+    initial_pose._y = this->pose.y - (motion_primitive->poses.back()._y / grid_resolution);
+    initial_pose._theta = motion_table.getAngleFromBin(motion_primitive->start_angle);
+
+    for (auto it = motion_primitive->poses.begin(); it != motion_primitive->poses.end(); ++it) {
+      // poses are in metric coordinates from (0, 0), not grid space yet
+      pose_dist = *it - last_pose;
+      // Avoid square roots by (hypot(x, y) > res) == (x*x+y*y > diag*diag)
+      if (pose_dist._x * pose_dist._x + pose_dist._y * pose_dist._y > resolution_diag_sq) {
+        last_pose = *it;
+        // Convert primitive pose into grid space if it should be checked
+        prim_pose._x = initial_pose._x + (it->_x / grid_resolution);
+        prim_pose._y = initial_pose._y + (it->_y / grid_resolution);
+        // If reversing, invert the angle because the robot is backing into the primitive
+        // not driving forward with it
+        if (is_backwards) {
+          prim_pose._theta = std::fmod(it->_theta + M_PI, 2.0 * M_PI);
+        } else {
+          prim_pose._theta = it->_theta;
+        }
+        if (collision_checker->inCollision(
+            prim_pose._x,
+            prim_pose._y,
+            prim_pose._theta / bin_size /*bin in collision checker*/,
+            traverse_unknown))
+        {
+          return false;
+        }
+        max_cell_cost = std::max(max_cell_cost, collision_checker->getCost());
+      }
+    }
+  }
+
+  _cell_cost = max_cell_cost;
+  return true;
+}
+
+float NodeLattice::getTraversalCost(const NodePtr & child)
+{
+  const float normalized_cost = child->getCost() / 252.0;
+  if (std::isnan(normalized_cost)) {
+    throw std::runtime_error(
+            "Node attempted to get traversal "
+            "cost without a known collision cost!");
+  }
+
+  // this is the first node
+  MotionPrimitive * prim = this->getMotionPrimitive();
+  MotionPrimitive * transition_prim = child->getMotionPrimitive();
+  const float prim_length =
+    transition_prim->trajectory_length / motion_table.lattice_metadata.grid_resolution;
+  if (prim == nullptr) {
+    return prim_length;
+  }
+
+  // Pure rotation in place 1 angular bin in either direction
+  if (transition_prim->trajectory_length < 1e-4) {
+    return motion_table.rotation_penalty * (1.0 + motion_table.cost_penalty * normalized_cost);
+  }
+
+  float travel_cost = 0.0;
+  float travel_cost_raw = prim_length *
+    (motion_table.travel_distance_reward + motion_table.cost_penalty * normalized_cost);
+
+  if (transition_prim->arc_length < 0.001) {
+    // New motion is a straight motion, no additional costs to be applied
+    travel_cost = travel_cost_raw;
+  } else {
+    if (prim->left_turn == transition_prim->left_turn) {
+      // Turning motion but keeps in same general direction: encourages to commit to actions
+      travel_cost = travel_cost_raw * motion_table.non_straight_penalty;
+    } else {
+      // Turning motion and velocity directions: penalizes wiggling.
+      travel_cost = travel_cost_raw *
+        (motion_table.non_straight_penalty + motion_table.change_penalty);
+    }
+  }
+
+  // If backwards flag is set, this primitive is moving in reverse
+  if (child->isBackward()) {
+    // reverse direction
+    travel_cost *= motion_table.reverse_penalty;
+  }
+
+  return travel_cost;
+}
+
+float NodeLattice::getHeuristicCost(
+  const Coordinates & node_coords,
+  const Coordinates & goal_coords,
+  const nav2_costmap_2d::Costmap2D * costmap)
+{
+  // get obstacle heuristic value
+  const float obstacle_heuristic = getObstacleHeuristic(
+    node_coords, goal_coords, motion_table.cost_penalty);
+  const float distance_heuristic =
+    getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic);
+  return std::max(obstacle_heuristic, distance_heuristic);
+}
+
+void NodeLattice::initMotionModel(
+  const MotionModel & motion_model,
+  unsigned int & size_x,
+  unsigned int & /*size_y*/,
+  unsigned int & /*num_angle_quantization*/,
+  SearchInfo & search_info)
+{
+  if (motion_model != MotionModel::STATE_LATTICE) {
+    throw std::runtime_error(
+            "Invalid motion model for Lattice node. Please select"
+            " STATE_LATTICE and provide a valid lattice file.");
+  }
+
+  motion_table.initMotionModel(size_x, search_info);
+}
+
+float NodeLattice::getDistanceHeuristic(
+  const Coordinates & node_coords,
+  const Coordinates & goal_coords,
+  const float & obstacle_heuristic)
+{
+  // rotate and translate node_coords such that goal_coords relative is (0,0,0)
+  // Due to the rounding involved in exact cell increments for caching,
+  // this is not an exact replica of a live heuristic, but has bounded error.
+  // (Usually less than 1 cell length)
+
+  // This angle is negative since we are de-rotating the current node
+  // by the goal angle; cos(-th) = cos(th) & sin(-th) = -sin(th)
+  const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta];
+  const float cos_th = trig_vals.first;
+  const float sin_th = -trig_vals.second;
+  const float dx = node_coords.x - goal_coords.x;
+  const float dy = node_coords.y - goal_coords.y;
+
+  double dtheta_bin = node_coords.theta - goal_coords.theta;
+  if (dtheta_bin < 0) {
+    dtheta_bin += motion_table.num_angle_quantization;
+  }
+  if (dtheta_bin > motion_table.num_angle_quantization) {
+    dtheta_bin -= motion_table.num_angle_quantization;
+  }
+
+  Coordinates node_coords_relative(
+    round(dx * cos_th - dy * sin_th),
+    round(dx * sin_th + dy * cos_th),
+    round(dtheta_bin));
+
+  // Check if the relative node coordinate is within the localized window around the goal
+  // to apply the distance heuristic. Since the lookup table is contains only the positive
+  // X axis, we mirror the Y and theta values across the X axis to find the heuristic values.
+  float motion_heuristic = 0.0;
+  const int floored_size = floor(size_lookup / 2.0);
+  const int ceiling_size = ceil(size_lookup / 2.0);
+  const float mirrored_relative_y = abs(node_coords_relative.y);
+  if (abs(node_coords_relative.x) < floored_size && mirrored_relative_y < floored_size) {
+    // Need to mirror angle if Y coordinate was mirrored
+    int theta_pos;
+    if (node_coords_relative.y < 0.0) {
+      theta_pos = motion_table.num_angle_quantization - node_coords_relative.theta;
+    } else {
+      theta_pos = node_coords_relative.theta;
+    }
+    const int x_pos = node_coords_relative.x + floored_size;
+    const int y_pos = static_cast<int>(mirrored_relative_y);
+    const int index =
+      x_pos * ceiling_size * motion_table.num_angle_quantization +
+      y_pos * motion_table.num_angle_quantization +
+      theta_pos;
+    motion_heuristic = dist_heuristic_lookup_table[index];
+  } else if (obstacle_heuristic == 0.0) {
+    static ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space);
+    to[0] = goal_coords.x;
+    to[1] = goal_coords.y;
+    to[2] = motion_table.getAngleFromBin(goal_coords.theta);
+    from[0] = node_coords.x;
+    from[1] = node_coords.y;
+    from[2] = motion_table.getAngleFromBin(node_coords.theta);
+    motion_heuristic = motion_table.state_space->distance(from(), to());
+  }
+
+  return motion_heuristic;
+}
+
+void NodeLattice::precomputeDistanceHeuristic(
+  const float & lookup_table_dim,
+  const MotionModel & motion_model,
+  const unsigned int & dim_3_size,
+  const SearchInfo & search_info)
+{
+  // Dubin or Reeds-Shepp shortest distances
+  if (!search_info.allow_reverse_expansion) {
+    motion_table.state_space = std::make_unique<ompl::base::DubinsStateSpace>(
+      search_info.minimum_turning_radius);
+  } else {
+    motion_table.state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(
+      search_info.minimum_turning_radius);
+  }
+  motion_table.lattice_metadata =
+    LatticeMotionTable::getLatticeMetadata(search_info.lattice_filepath);
+
+  ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space);
+  to[0] = 0.0;
+  to[1] = 0.0;
+  to[2] = 0.0;
+  size_lookup = lookup_table_dim;
+  float motion_heuristic = 0.0;
+  unsigned int index = 0;
+  int dim_3_size_int = static_cast<int>(dim_3_size);
+
+  // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal
+  // to help drive the search towards admissible approaches. Deu to symmetries in the
+  // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror
+  // around the X axis any relative node lookup. This reduces memory overhead and increases
+  // the size of a window a platform can store in memory.
+  dist_heuristic_lookup_table.resize(size_lookup * ceil(size_lookup / 2.0) * dim_3_size_int);
+  for (float x = ceil(-size_lookup / 2.0); x <= floor(size_lookup / 2.0); x += 1.0) {
+    for (float y = 0.0; y <= floor(size_lookup / 2.0); y += 1.0) {
+      for (int heading = 0; heading != dim_3_size_int; heading++) {
+        from[0] = x;
+        from[1] = y;
+        from[2] = motion_table.getAngleFromBin(heading);
+        motion_heuristic = motion_table.state_space->distance(from(), to());
+        dist_heuristic_lookup_table[index] = motion_heuristic;
+        index++;
+      }
+    }
+  }
+}
+
+void NodeLattice::getNeighbors(
+  std::function<bool(const unsigned int &, nav2_smac_planner::NodeLattice * &)> & NeighborGetter,
+  GridCollisionChecker * collision_checker,
+  const bool & traverse_unknown,
+  NodeVector & neighbors)
+{
+  unsigned int index = 0;
+  bool backwards = false;
+  NodePtr neighbor = nullptr;
+  Coordinates initial_node_coords, motion_projection;
+  MotionPrimitivePtrs motion_primitives = motion_table.getMotionPrimitives(this);
+  const float & grid_resolution = motion_table.lattice_metadata.grid_resolution;
+
+  unsigned int direction_change_idx = 1e9;
+  for (unsigned int i = 0; i != motion_primitives.size(); i++) {
+    if (motion_primitives[0]->start_angle != motion_primitives[i]->start_angle) {
+      direction_change_idx = i;
+      break;
+    }
+  }
+
+  for (unsigned int i = 0; i != motion_primitives.size(); i++) {
+    const MotionPose & end_pose = motion_primitives[i]->poses.back();
+    motion_projection.x = this->pose.x + (end_pose._x / grid_resolution);
+    motion_projection.y = this->pose.y + (end_pose._y / grid_resolution);
+    motion_projection.theta = motion_primitives[i]->end_angle /*this is the ending angular bin*/;
+
+    // if i >= idx, then we're in a reversing primitive. In that situation,
+    // the orientation of the robot is mirrored from what it would otherwise
+    // appear to be from the motion primitives file. We want to take this into
+    // account in case the robot base footprint is asymmetric.
+    backwards = false;
+    if (i >= direction_change_idx) {
+      backwards = true;
+      float opposite_heading_theta =
+        motion_projection.theta - (motion_table.num_angle_quantization / 2);
+      if (opposite_heading_theta < 0) {
+        opposite_heading_theta += motion_table.num_angle_quantization;
+      }
+      if (opposite_heading_theta > motion_table.num_angle_quantization) {
+        opposite_heading_theta -= motion_table.num_angle_quantization;
+      }
+      motion_projection.theta = opposite_heading_theta;
+    }
+
+    index = NodeLattice::getIndex(
+      static_cast<unsigned int>(motion_projection.x),
+      static_cast<unsigned int>(motion_projection.y),
+      static_cast<unsigned int>(motion_projection.theta));
+
+    if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) {
+      // Cache the initial pose in case it was visited but valid
+      // don't want to disrupt continuous coordinate expansion
+      initial_node_coords = neighbor->pose;
+      neighbor->setPose(
+        Coordinates(
+          motion_projection.x,
+          motion_projection.y,
+          motion_projection.theta));
+
+      // Using a special isNodeValid API here, giving the motion primitive to use to
+      // validity check the transition of the current node to the new node over
+      if (neighbor->isNodeValid(
+          traverse_unknown, collision_checker, motion_primitives[i], backwards))
+      {
+        neighbor->setMotionPrimitive(motion_primitives[i]);
+        // Marking if this search was obtained in the reverse direction
+        neighbor->backwards(backwards);
+        neighbors.push_back(neighbor);
+      } else {
+        neighbor->setPose(initial_node_coords);
+      }
+    }
+  }
+}
+
+bool NodeLattice::backtracePath(CoordinateVector & path)
+{
+  if (!this->parent) {
+    return false;
+  }
+
+  NodePtr current_node = this;
+
+  while (current_node->parent) {
+    addNodeToPath(current_node, path);
+    current_node = current_node->parent;
+  }
+
+  // add start to path
+  addNodeToPath(current_node, path);
+
+  return true;
+}
+
+void NodeLattice::addNodeToPath(
+  NodeLattice::NodePtr current_node,
+  NodeLattice::CoordinateVector & path)
+{
+  Coordinates initial_pose, prim_pose;
+  MotionPrimitive * prim = nullptr;
+  const float & grid_resolution = NodeLattice::motion_table.lattice_metadata.grid_resolution;
+  prim = current_node->getMotionPrimitive();
+  // if motion primitive is valid, then was searched (rather than analytically expanded),
+  // include dense path of subpoints making up the primitive at grid resolution
+  if (prim) {
+    initial_pose.x = current_node->pose.x - (prim->poses.back()._x / grid_resolution);
+    initial_pose.y = current_node->pose.y - (prim->poses.back()._y / grid_resolution);
+    initial_pose.theta = NodeLattice::motion_table.getAngleFromBin(prim->start_angle);
+
+    for (auto it = prim->poses.crbegin(); it != prim->poses.crend(); ++it) {
+      // Convert primitive pose into grid space if it should be checked
+      prim_pose.x = initial_pose.x + (it->_x / grid_resolution);
+      prim_pose.y = initial_pose.y + (it->_y / grid_resolution);
+      // If reversing, invert the angle because the robot is backing into the primitive
+      // not driving forward with it
+      if (current_node->isBackward()) {
+        prim_pose.theta = std::fmod(it->_theta + M_PI, 2.0 * M_PI);
+      } else {
+        prim_pose.theta = it->_theta;
+      }
+      path.push_back(prim_pose);
+    }
+  } else {
+    // For analytic expansion nodes where there is no valid motion primitive
+    path.push_back(current_node->pose);
+    path.back().theta = NodeLattice::motion_table.getAngleFromBin(path.back().theta);
+  }
+}
+
+}  // namespace nav2_smac_planner

navigation2/nav2_smac_planner/src/smac_planner_2d.cpp

@@ -0,0 +1,419 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <string>
+#include <memory>
+#include <vector>
+#include <limits>
+#include <algorithm>
+
+#include "nav2_smac_planner/smac_planner_2d.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+// #define BENCHMARK_TESTING
+
+namespace nav2_smac_planner
+{
+using namespace std::chrono;  // NOLINT
+using rcl_interfaces::msg::ParameterType;
+using std::placeholders::_1;
+
+SmacPlanner2D::SmacPlanner2D()
+: _a_star(nullptr),
+  _collision_checker(nullptr, 1, nullptr),
+  _smoother(nullptr),
+  _costmap(nullptr),
+  _costmap_downsampler(nullptr)
+{
+}
+
+SmacPlanner2D::~SmacPlanner2D()
+{
+  RCLCPP_INFO(
+    _logger, "Destroying plugin %s of type SmacPlanner2D",
+    _name.c_str());
+}
+
+void SmacPlanner2D::configure(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  std::string name, std::shared_ptr<tf2_ros::Buffer>/*tf*/,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  _node = parent;
+  auto node = parent.lock();
+  _logger = node->get_logger();
+  _clock = node->get_clock();
+  _costmap = costmap_ros->getCostmap();
+  _name = name;
+  _global_frame = costmap_ros->getGlobalFrameID();
+
+  RCLCPP_INFO(_logger, "Configuring %s of type SmacPlanner2D", name.c_str());
+
+  // General planner params
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".tolerance", rclcpp::ParameterValue(0.125));
+  _tolerance = static_cast<float>(node->get_parameter(name + ".tolerance").as_double());
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".downsample_costmap", rclcpp::ParameterValue(false));
+  node->get_parameter(name + ".downsample_costmap", _downsample_costmap);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".downsampling_factor", rclcpp::ParameterValue(1));
+  node->get_parameter(name + ".downsampling_factor", _downsampling_factor);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".cost_travel_multiplier", rclcpp::ParameterValue(1.0));
+  node->get_parameter(name + ".cost_travel_multiplier", _search_info.cost_penalty);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".allow_unknown", rclcpp::ParameterValue(true));
+  node->get_parameter(name + ".allow_unknown", _allow_unknown);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".max_iterations", rclcpp::ParameterValue(1000000));
+  node->get_parameter(name + ".max_iterations", _max_iterations);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000));
+  node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false));
+  node->get_parameter(name + ".use_final_approach_orientation", _use_final_approach_orientation);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".max_planning_time", rclcpp::ParameterValue(2.0));
+  node->get_parameter(name + ".max_planning_time", _max_planning_time);
+
+  _motion_model = MotionModel::TWOD;
+
+  if (_max_on_approach_iterations <= 0) {
+    RCLCPP_INFO(
+      _logger, "On approach iteration selected as <= 0, "
+      "disabling tolerance and on approach iterations.");
+    _max_on_approach_iterations = std::numeric_limits<int>::max();
+  }
+
+  if (_max_iterations <= 0) {
+    RCLCPP_INFO(
+      _logger, "maximum iteration selected as <= 0, "
+      "disabling maximum iterations.");
+    _max_iterations = std::numeric_limits<int>::max();
+  }
+
+  // Initialize collision checker
+  _collision_checker = GridCollisionChecker(_costmap, 1 /*for 2D, most be 1*/, node);
+  _collision_checker.setFootprint(
+    costmap_ros->getRobotFootprint(),
+    true /*for 2D, most use radius*/,
+    0.0 /*for 2D cost at inscribed isn't relevent*/);
+
+  // Initialize A* template
+  _a_star = std::make_unique<AStarAlgorithm<Node2D>>(_motion_model, _search_info);
+  _a_star->initialize(
+    _allow_unknown,
+    _max_iterations,
+    _max_on_approach_iterations,
+    _max_planning_time,
+    0.0 /*unused for 2D*/,
+    1.0 /*unused for 2D*/);
+
+  // Initialize path smoother
+  SmootherParams params;
+  params.get(node, name);
+  params.holonomic_ = true;  // So smoother will treat this as a grid search
+  _smoother = std::make_unique<Smoother>(params);
+  _smoother->initialize(1e-50 /*No valid minimum turning radius for 2D*/);
+
+  // Initialize costmap downsampler
+  if (_downsample_costmap && _downsampling_factor > 1) {
+    std::string topic_name = "downsampled_costmap";
+    _costmap_downsampler = std::make_unique<CostmapDownsampler>();
+    _costmap_downsampler->on_configure(
+      node, _global_frame, topic_name, _costmap, _downsampling_factor);
+  }
+
+  _raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
+
+  RCLCPP_INFO(
+    _logger, "Configured plugin %s of type SmacPlanner2D with "
+    "tolerance %.2f, maximum iterations %i, "
+    "max on approach iterations %i, and %s.",
+    _name.c_str(), _tolerance, _max_iterations, _max_on_approach_iterations,
+    _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal");
+}
+
+void SmacPlanner2D::activate()
+{
+  RCLCPP_INFO(
+    _logger, "Activating plugin %s of type SmacPlanner2D",
+    _name.c_str());
+  _raw_plan_publisher->on_activate();
+  if (_costmap_downsampler) {
+    _costmap_downsampler->on_activate();
+  }
+  auto node = _node.lock();
+  // Add callback for dynamic parameters
+  _dyn_params_handler = node->add_on_set_parameters_callback(
+    std::bind(&SmacPlanner2D::dynamicParametersCallback, this, _1));
+}
+
+void SmacPlanner2D::deactivate()
+{
+  RCLCPP_INFO(
+    _logger, "Deactivating plugin %s of type SmacPlanner2D",
+    _name.c_str());
+  _raw_plan_publisher->on_deactivate();
+  if (_costmap_downsampler) {
+    _costmap_downsampler->on_deactivate();
+  }
+  _dyn_params_handler.reset();
+}
+
+void SmacPlanner2D::cleanup()
+{
+  RCLCPP_INFO(
+    _logger, "Cleaning up plugin %s of type SmacPlanner2D",
+    _name.c_str());
+  _a_star.reset();
+  _smoother.reset();
+  if (_costmap_downsampler) {
+    _costmap_downsampler->on_cleanup();
+    _costmap_downsampler.reset();
+  }
+  _raw_plan_publisher.reset();
+}
+
+nav_msgs::msg::Path SmacPlanner2D::createPlan(
+  const geometry_msgs::msg::PoseStamped & start,
+  const geometry_msgs::msg::PoseStamped & goal)
+{
+  std::lock_guard<std::mutex> lock_reinit(_mutex);
+  steady_clock::time_point a = steady_clock::now();
+
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(_costmap->getMutex()));
+
+  // Downsample costmap, if required
+  nav2_costmap_2d::Costmap2D * costmap = _costmap;
+  if (_costmap_downsampler) {
+    costmap = _costmap_downsampler->downsample(_downsampling_factor);
+    _collision_checker.setCostmap(costmap);
+  }
+
+  // Set collision checker and costmap information
+  _a_star->setCollisionChecker(&_collision_checker);
+
+  // Set starting point
+  unsigned int mx_start, my_start, mx_goal, my_goal;
+  costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start);
+  _a_star->setStart(mx_start, my_start, 0);
+
+  // Set goal point
+  costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal);
+  _a_star->setGoal(mx_goal, my_goal, 0);
+
+  // Setup message
+  nav_msgs::msg::Path plan;
+  plan.header.stamp = _clock->now();
+  plan.header.frame_id = _global_frame;
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header = plan.header;
+  pose.pose.position.z = 0.0;
+  pose.pose.orientation.x = 0.0;
+  pose.pose.orientation.y = 0.0;
+  pose.pose.orientation.z = 0.0;
+  pose.pose.orientation.w = 1.0;
+
+  // Corner case of start and goal beeing on the same cell
+  if (mx_start == mx_goal && my_start == my_goal) {
+    if (costmap->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) {
+      RCLCPP_WARN(_logger, "Failed to create a unique pose path because of obstacles");
+      return plan;
+    }
+    pose.pose = start.pose;
+    // if we have a different start and goal orientation, set the unique path pose to the goal
+    // orientation, unless use_final_approach_orientation=true where we need it to be the start
+    // orientation to avoid movement from the local planner
+    if (start.pose.orientation != goal.pose.orientation && !_use_final_approach_orientation) {
+      pose.pose.orientation = goal.pose.orientation;
+    }
+    plan.poses.push_back(pose);
+    return plan;
+  }
+
+  // Compute plan
+  Node2D::CoordinateVector path;
+  int num_iterations = 0;
+  std::string error;
+  try {
+    if (!_a_star->createPath(
+        path, num_iterations, _tolerance / static_cast<float>(costmap->getResolution())))
+    {
+      if (num_iterations < _a_star->getMaxIterations()) {
+        error = std::string("no valid path found");
+      } else {
+        error = std::string("exceeded maximum iterations");
+      }
+    }
+  } catch (const std::runtime_error & e) {
+    error = "invalid use: ";
+    error += e.what();
+  }
+
+  if (!error.empty()) {
+    RCLCPP_WARN(
+      _logger,
+      "%s: failed to create plan, %s.",
+      _name.c_str(), error.c_str());
+    return plan;
+  }
+
+  // Convert to world coordinates
+  plan.poses.reserve(path.size());
+  for (int i = path.size() - 1; i >= 0; --i) {
+    pose.pose = getWorldCoords(path[i].x, path[i].y, costmap);
+    plan.poses.push_back(pose);
+  }
+
+  // Publish raw path for debug
+  if (_raw_plan_publisher->get_subscription_count() > 0) {
+    _raw_plan_publisher->publish(plan);
+  }
+
+  // Find how much time we have left to do smoothing
+  steady_clock::time_point b = steady_clock::now();
+  duration<double> time_span = duration_cast<duration<double>>(b - a);
+  double time_remaining = _max_planning_time - static_cast<double>(time_span.count());
+
+#ifdef BENCHMARK_TESTING
+  std::cout << "It took " << time_span.count() * 1000 <<
+    " milliseconds with " << num_iterations << " iterations." << std::endl;
+#endif
+
+  // Smooth plan
+  _smoother->smooth(plan, costmap, time_remaining);
+
+  // If use_final_approach_orientation=true, interpolate the last pose orientation from the
+  // previous pose to set the orientation to the 'final approach' orientation of the robot so
+  // it does not rotate.
+  // And deal with corner case of plan of length 1
+  // If use_final_approach_orientation=false (default), override last pose orientation to match goal
+  size_t plan_size = plan.poses.size();
+  if (_use_final_approach_orientation) {
+    if (plan_size == 1) {
+      plan.poses.back().pose.orientation = start.pose.orientation;
+    } else if (plan_size > 1) {
+      double dx, dy, theta;
+      auto last_pose = plan.poses.back().pose.position;
+      auto approach_pose = plan.poses[plan_size - 2].pose.position;
+      dx = last_pose.x - approach_pose.x;
+      dy = last_pose.y - approach_pose.y;
+      theta = atan2(dy, dx);
+      plan.poses.back().pose.orientation =
+        nav2_util::geometry_utils::orientationAroundZAxis(theta);
+    }
+  } else if (plan_size > 0) {
+    plan.poses.back().pose.orientation = goal.pose.orientation;
+  }
+
+  return plan;
+}
+
+rcl_interfaces::msg::SetParametersResult
+SmacPlanner2D::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  std::lock_guard<std::mutex> lock_reinit(_mutex);
+
+  bool reinit_a_star = false;
+  bool reinit_downsampler = false;
+
+  for (auto parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == _name + ".tolerance") {
+        _tolerance = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".cost_travel_multiplier") {
+        reinit_a_star = true;
+        _search_info.cost_penalty = parameter.as_double();
+      } else if (name == _name + ".max_planning_time") {
+        reinit_a_star = true;
+        _max_planning_time = parameter.as_double();
+      }
+    } else if (type == ParameterType::PARAMETER_BOOL) {
+      if (name == _name + ".downsample_costmap") {
+        reinit_downsampler = true;
+        _downsample_costmap = parameter.as_bool();
+      } else if (name == _name + ".allow_unknown") {
+        reinit_a_star = true;
+        _allow_unknown = parameter.as_bool();
+      } else if (name == _name + ".use_final_approach_orientation") {
+        _use_final_approach_orientation = parameter.as_bool();
+      }
+    } else if (type == ParameterType::PARAMETER_INTEGER) {
+      if (name == _name + ".downsampling_factor") {
+        reinit_downsampler = true;
+        _downsampling_factor = parameter.as_int();
+      } else if (name == _name + ".max_iterations") {
+        reinit_a_star = true;
+        _max_iterations = parameter.as_int();
+        if (_max_iterations <= 0) {
+          RCLCPP_INFO(
+            _logger, "maximum iteration selected as <= 0, "
+            "disabling maximum iterations.");
+          _max_iterations = std::numeric_limits<int>::max();
+        }
+      } else if (name == _name + ".max_on_approach_iterations") {
+        reinit_a_star = true;
+        _max_on_approach_iterations = parameter.as_int();
+        if (_max_on_approach_iterations <= 0) {
+          RCLCPP_INFO(
+            _logger, "On approach iteration selected as <= 0, "
+            "disabling tolerance and on approach iterations.");
+          _max_on_approach_iterations = std::numeric_limits<int>::max();
+        }
+      }
+    }
+  }
+
+  // Re-init if needed with mutex lock (to avoid re-init while creating a plan)
+  if (reinit_a_star || reinit_downsampler) {
+    // Re-Initialize A* template
+    if (reinit_a_star) {
+      _a_star = std::make_unique<AStarAlgorithm<Node2D>>(_motion_model, _search_info);
+      _a_star->initialize(
+        _allow_unknown,
+        _max_iterations,
+        _max_on_approach_iterations,
+        _max_planning_time,
+        0.0 /*unused for 2D*/,
+        1.0 /*unused for 2D*/);
+    }
+
+    // Re-Initialize costmap downsampler
+    if (reinit_downsampler) {
+      if (_downsample_costmap && _downsampling_factor > 1) {
+        auto node = _node.lock();
+        std::string topic_name = "downsampled_costmap";
+        _costmap_downsampler = std::make_unique<CostmapDownsampler>();
+        _costmap_downsampler->on_configure(
+          node, _global_frame, topic_name, _costmap, _downsampling_factor);
+      }
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_smac_planner
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlanner2D, nav2_core::GlobalPlanner)

navigation2/nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -0,0 +1,583 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <string>
+#include <memory>
+#include <vector>
+#include <algorithm>
+#include <limits>
+
+#include "Eigen/Core"
+#include "nav2_smac_planner/smac_planner_hybrid.hpp"
+
+// #define BENCHMARK_TESTING
+
+namespace nav2_smac_planner
+{
+
+using namespace std::chrono;  // NOLINT
+using rcl_interfaces::msg::ParameterType;
+using std::placeholders::_1;
+
+SmacPlannerHybrid::SmacPlannerHybrid()
+: _a_star(nullptr),
+  _collision_checker(nullptr, 1, nullptr),
+  _smoother(nullptr),
+  _costmap(nullptr),
+  _costmap_downsampler(nullptr)
+{
+}
+
+SmacPlannerHybrid::~SmacPlannerHybrid()
+{
+  RCLCPP_INFO(
+    _logger, "Destroying plugin %s of type SmacPlannerHybrid",
+    _name.c_str());
+}
+
+void SmacPlannerHybrid::configure(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  std::string name, std::shared_ptr<tf2_ros::Buffer>/*tf*/,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  _node = parent;
+  auto node = parent.lock();
+  _logger = node->get_logger();
+  _clock = node->get_clock();
+  _costmap = costmap_ros->getCostmap();
+  _costmap_ros = costmap_ros;
+  _name = name;
+  _global_frame = costmap_ros->getGlobalFrameID();
+
+  RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerHybrid", name.c_str());
+
+  int angle_quantizations;
+  double analytic_expansion_max_length_m;
+  bool smooth_path;
+
+  // General planner params
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".downsample_costmap", rclcpp::ParameterValue(false));
+  node->get_parameter(name + ".downsample_costmap", _downsample_costmap);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".downsampling_factor", rclcpp::ParameterValue(1));
+  node->get_parameter(name + ".downsampling_factor", _downsampling_factor);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".angle_quantization_bins", rclcpp::ParameterValue(72));
+  node->get_parameter(name + ".angle_quantization_bins", angle_quantizations);
+  _angle_bin_size = 2.0 * M_PI / angle_quantizations;
+  _angle_quantizations = static_cast<unsigned int>(angle_quantizations);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".tolerance", rclcpp::ParameterValue(0.25));
+  _tolerance = static_cast<float>(node->get_parameter(name + ".tolerance").as_double());
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".allow_unknown", rclcpp::ParameterValue(true));
+  node->get_parameter(name + ".allow_unknown", _allow_unknown);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".max_iterations", rclcpp::ParameterValue(1000000));
+  node->get_parameter(name + ".max_iterations", _max_iterations);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000));
+  node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".smooth_path", rclcpp::ParameterValue(true));
+  node->get_parameter(name + ".smooth_path", smooth_path);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4));
+  node->get_parameter(name + ".minimum_turning_radius", _minimum_turning_radius_global_coords);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".cache_obstacle_heuristic", rclcpp::ParameterValue(false));
+  node->get_parameter(name + ".cache_obstacle_heuristic", _search_info.cache_obstacle_heuristic);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0));
+  node->get_parameter(name + ".reverse_penalty", _search_info.reverse_penalty);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".change_penalty", rclcpp::ParameterValue(0.0));
+  node->get_parameter(name + ".change_penalty", _search_info.change_penalty);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.2));
+  node->get_parameter(name + ".non_straight_penalty", _search_info.non_straight_penalty);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".cost_penalty", rclcpp::ParameterValue(2.0));
+  node->get_parameter(name + ".cost_penalty", _search_info.cost_penalty);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".retrospective_penalty", rclcpp::ParameterValue(0.015));
+  node->get_parameter(name + ".retrospective_penalty", _search_info.retrospective_penalty);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(3.5));
+  node->get_parameter(name + ".analytic_expansion_ratio", _search_info.analytic_expansion_ratio);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".analytic_expansion_max_length", rclcpp::ParameterValue(3.0));
+  node->get_parameter(name + ".analytic_expansion_max_length", analytic_expansion_max_length_m);
+  _search_info.analytic_expansion_max_length =
+    analytic_expansion_max_length_m / _costmap->getResolution();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".max_planning_time", rclcpp::ParameterValue(5.0));
+  node->get_parameter(name + ".max_planning_time", _max_planning_time);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".lookup_table_size", rclcpp::ParameterValue(20.0));
+  node->get_parameter(name + ".lookup_table_size", _lookup_table_size);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN")));
+  node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search);
+  _motion_model = fromString(_motion_model_for_search);
+  if (_motion_model == MotionModel::UNKNOWN) {
+    RCLCPP_WARN(
+      _logger,
+      "Unable to get MotionModel search type. Given '%s', "
+      "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP, STATE_LATTICE.",
+      _motion_model_for_search.c_str());
+  }
+
+  if (_max_on_approach_iterations <= 0) {
+    RCLCPP_INFO(
+      _logger, "On approach iteration selected as <= 0, "
+      "disabling tolerance and on approach iterations.");
+    _max_on_approach_iterations = std::numeric_limits<int>::max();
+  }
+
+  if (_max_iterations <= 0) {
+    RCLCPP_INFO(
+      _logger, "maximum iteration selected as <= 0, "
+      "disabling maximum iterations.");
+    _max_iterations = std::numeric_limits<int>::max();
+  }
+
+  // convert to grid coordinates
+  if (!_downsample_costmap) {
+    _downsampling_factor = 1;
+  }
+  _search_info.minimum_turning_radius =
+    _minimum_turning_radius_global_coords / (_costmap->getResolution() * _downsampling_factor);
+  _lookup_table_dim =
+    static_cast<float>(_lookup_table_size) /
+    static_cast<float>(_costmap->getResolution() * _downsampling_factor);
+
+  // Make sure its a whole number
+  _lookup_table_dim = static_cast<float>(static_cast<int>(_lookup_table_dim));
+
+  // Make sure its an odd number
+  if (static_cast<int>(_lookup_table_dim) % 2 == 0) {
+    RCLCPP_INFO(
+      _logger,
+      "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd",
+      _lookup_table_dim);
+    _lookup_table_dim += 1.0;
+  }
+
+  // Initialize collision checker
+  _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node);
+  _collision_checker.setFootprint(
+    _costmap_ros->getRobotFootprint(),
+    _costmap_ros->getUseRadius(),
+    findCircumscribedCost(_costmap_ros));
+
+  // Initialize A* template
+  _a_star = std::make_unique<AStarAlgorithm<NodeHybrid>>(_motion_model, _search_info);
+  _a_star->initialize(
+    _allow_unknown,
+    _max_iterations,
+    _max_on_approach_iterations,
+    _max_planning_time,
+    _lookup_table_dim,
+    _angle_quantizations);
+
+  // Initialize path smoother
+  if (smooth_path) {
+    SmootherParams params;
+    params.get(node, name);
+    _smoother = std::make_unique<Smoother>(params);
+    _smoother->initialize(_minimum_turning_radius_global_coords);
+  }
+
+  // Initialize costmap downsampler
+  if (_downsample_costmap && _downsampling_factor > 1) {
+    _costmap_downsampler = std::make_unique<CostmapDownsampler>();
+    std::string topic_name = "downsampled_costmap";
+    _costmap_downsampler->on_configure(
+      node, _global_frame, topic_name, _costmap, _downsampling_factor);
+  }
+
+  _raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
+
+  RCLCPP_INFO(
+    _logger, "Configured plugin %s of type SmacPlannerHybrid with "
+    "maximum iterations %i, max on approach iterations %i, and %s. Tolerance %.2f."
+    "Using motion model: %s.",
+    _name.c_str(), _max_iterations, _max_on_approach_iterations,
+    _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal",
+    _tolerance, toString(_motion_model).c_str());
+}
+
+void SmacPlannerHybrid::activate()
+{
+  RCLCPP_INFO(
+    _logger, "Activating plugin %s of type SmacPlannerHybrid",
+    _name.c_str());
+  _raw_plan_publisher->on_activate();
+  if (_costmap_downsampler) {
+    _costmap_downsampler->on_activate();
+  }
+  auto node = _node.lock();
+  // Add callback for dynamic parameters
+  _dyn_params_handler = node->add_on_set_parameters_callback(
+    std::bind(&SmacPlannerHybrid::dynamicParametersCallback, this, _1));
+}
+
+void SmacPlannerHybrid::deactivate()
+{
+  RCLCPP_INFO(
+    _logger, "Deactivating plugin %s of type SmacPlannerHybrid",
+    _name.c_str());
+  _raw_plan_publisher->on_deactivate();
+  if (_costmap_downsampler) {
+    _costmap_downsampler->on_deactivate();
+  }
+  _dyn_params_handler.reset();
+}
+
+void SmacPlannerHybrid::cleanup()
+{
+  RCLCPP_INFO(
+    _logger, "Cleaning up plugin %s of type SmacPlannerHybrid",
+    _name.c_str());
+  _a_star.reset();
+  _smoother.reset();
+  if (_costmap_downsampler) {
+    _costmap_downsampler->on_cleanup();
+    _costmap_downsampler.reset();
+  }
+  _raw_plan_publisher.reset();
+}
+
+nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
+  const geometry_msgs::msg::PoseStamped & start,
+  const geometry_msgs::msg::PoseStamped & goal)
+{
+  std::lock_guard<std::mutex> lock_reinit(_mutex);
+  steady_clock::time_point a = steady_clock::now();
+
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(_costmap->getMutex()));
+
+  // Downsample costmap, if required
+  nav2_costmap_2d::Costmap2D * costmap = _costmap;
+  if (_costmap_downsampler) {
+    costmap = _costmap_downsampler->downsample(_downsampling_factor);
+    _collision_checker.setCostmap(costmap);
+  }
+
+  // Set collision checker and costmap information
+  _collision_checker.setFootprint(
+    _costmap_ros->getRobotFootprint(),
+    _costmap_ros->getUseRadius(),
+    findCircumscribedCost(_costmap_ros));
+  _a_star->setCollisionChecker(&_collision_checker);
+
+  // Set starting point, in A* bin search coordinates
+  unsigned int mx, my;
+  if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) {
+    throw std::runtime_error("Start pose is out of costmap!");
+  }
+
+  double orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size);
+  while (orientation_bin < 0.0) {
+    orientation_bin += static_cast<float>(_angle_quantizations);
+  }
+  // This is needed to handle precision issues
+  if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
+    orientation_bin -= static_cast<float>(_angle_quantizations);
+  }
+  _a_star->setStart(mx, my, static_cast<unsigned int>(orientation_bin));
+
+  // Set goal point, in A* bin search coordinates
+  if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) {
+    throw std::runtime_error("Goal pose is out of costmap!");
+  }
+  orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size);
+  while (orientation_bin < 0.0) {
+    orientation_bin += static_cast<float>(_angle_quantizations);
+  }
+  // This is needed to handle precision issues
+  if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
+    orientation_bin -= static_cast<float>(_angle_quantizations);
+  }
+  _a_star->setGoal(mx, my, static_cast<unsigned int>(orientation_bin));
+
+  // Setup message
+  nav_msgs::msg::Path plan;
+  plan.header.stamp = _clock->now();
+  plan.header.frame_id = _global_frame;
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header = plan.header;
+  pose.pose.position.z = 0.0;
+  pose.pose.orientation.x = 0.0;
+  pose.pose.orientation.y = 0.0;
+  pose.pose.orientation.z = 0.0;
+  pose.pose.orientation.w = 1.0;
+
+  // Compute plan
+  NodeHybrid::CoordinateVector path;
+  int num_iterations = 0;
+  std::string error;
+  try {
+    if (!_a_star->createPath(
+        path, num_iterations, _tolerance / static_cast<float>(costmap->getResolution())))
+    {
+      if (num_iterations < _a_star->getMaxIterations()) {
+        error = std::string("no valid path found");
+      } else {
+        error = std::string("exceeded maximum iterations");
+      }
+    }
+  } catch (const std::runtime_error & e) {
+    error = "invalid use: ";
+    error += e.what();
+  }
+
+  if (!error.empty()) {
+    RCLCPP_WARN(
+      _logger,
+      "%s: failed to create plan, %s.",
+      _name.c_str(), error.c_str());
+    return plan;
+  }
+
+  // Convert to world coordinates
+  plan.poses.reserve(path.size());
+  for (int i = path.size() - 1; i >= 0; --i) {
+    pose.pose = getWorldCoords(path[i].x, path[i].y, costmap);
+    pose.pose.orientation = getWorldOrientation(path[i].theta);
+    plan.poses.push_back(pose);
+  }
+
+  // Publish raw path for debug
+  if (_raw_plan_publisher->get_subscription_count() > 0) {
+    _raw_plan_publisher->publish(plan);
+  }
+
+  // Find how much time we have left to do smoothing
+  steady_clock::time_point b = steady_clock::now();
+  duration<double> time_span = duration_cast<duration<double>>(b - a);
+  double time_remaining = _max_planning_time - static_cast<double>(time_span.count());
+
+#ifdef BENCHMARK_TESTING
+  std::cout << "It took " << time_span.count() * 1000 <<
+    " milliseconds with " << num_iterations << " iterations." << std::endl;
+#endif
+
+  // Smooth plan
+  if (_smoother && num_iterations > 1) {
+    _smoother->smooth(plan, costmap, time_remaining);
+  }
+
+#ifdef BENCHMARK_TESTING
+  steady_clock::time_point c = steady_clock::now();
+  duration<double> time_span2 = duration_cast<duration<double>>(c - b);
+  std::cout << "It took " << time_span2.count() * 1000 <<
+    " milliseconds to smooth path." << std::endl;
+#endif
+
+  return plan;
+}
+
+rcl_interfaces::msg::SetParametersResult
+SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  std::lock_guard<std::mutex> lock_reinit(_mutex);
+
+  bool reinit_collision_checker = false;
+  bool reinit_a_star = false;
+  bool reinit_downsampler = false;
+  bool reinit_smoother = false;
+
+  for (auto parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == _name + ".max_planning_time") {
+        reinit_a_star = true;
+        _max_planning_time = parameter.as_double();
+      } else if (name == _name + ".tolerance") {
+        _tolerance = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".lookup_table_size") {
+        reinit_a_star = true;
+        _lookup_table_size = parameter.as_double();
+      } else if (name == _name + ".minimum_turning_radius") {
+        reinit_a_star = true;
+        if (_smoother) {
+          reinit_smoother = true;
+        }
+        _minimum_turning_radius_global_coords = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".reverse_penalty") {
+        reinit_a_star = true;
+        _search_info.reverse_penalty = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".change_penalty") {
+        reinit_a_star = true;
+        _search_info.change_penalty = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".non_straight_penalty") {
+        reinit_a_star = true;
+        _search_info.non_straight_penalty = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".cost_penalty") {
+        reinit_a_star = true;
+        _search_info.cost_penalty = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".analytic_expansion_ratio") {
+        reinit_a_star = true;
+        _search_info.analytic_expansion_ratio = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".analytic_expansion_max_length") {
+        reinit_a_star = true;
+        _search_info.analytic_expansion_max_length =
+          static_cast<float>(parameter.as_double()) / _costmap->getResolution();
+      }
+    } else if (type == ParameterType::PARAMETER_BOOL) {
+      if (name == _name + ".downsample_costmap") {
+        reinit_downsampler = true;
+        _downsample_costmap = parameter.as_bool();
+      } else if (name == _name + ".allow_unknown") {
+        reinit_a_star = true;
+        _allow_unknown = parameter.as_bool();
+      } else if (name == _name + ".cache_obstacle_heuristic") {
+        reinit_a_star = true;
+        _search_info.cache_obstacle_heuristic = parameter.as_bool();
+      } else if (name == _name + ".smooth_path") {
+        if (parameter.as_bool()) {
+          reinit_smoother = true;
+        } else {
+          _smoother.reset();
+        }
+      }
+    } else if (type == ParameterType::PARAMETER_INTEGER) {
+      if (name == _name + ".downsampling_factor") {
+        reinit_a_star = true;
+        reinit_downsampler = true;
+        _downsampling_factor = parameter.as_int();
+      } else if (name == _name + ".max_iterations") {
+        reinit_a_star = true;
+        _max_iterations = parameter.as_int();
+        if (_max_iterations <= 0) {
+          RCLCPP_INFO(
+            _logger, "maximum iteration selected as <= 0, "
+            "disabling maximum iterations.");
+          _max_iterations = std::numeric_limits<int>::max();
+        }
+      } else if (name == _name + ".max_on_approach_iterations") {
+        reinit_a_star = true;
+        _max_on_approach_iterations = parameter.as_int();
+        if (_max_on_approach_iterations <= 0) {
+          RCLCPP_INFO(
+            _logger, "On approach iteration selected as <= 0, "
+            "disabling tolerance and on approach iterations.");
+          _max_on_approach_iterations = std::numeric_limits<int>::max();
+        }
+      } else if (name == _name + ".angle_quantization_bins") {
+        reinit_collision_checker = true;
+        reinit_a_star = true;
+        int angle_quantizations = parameter.as_int();
+        _angle_bin_size = 2.0 * M_PI / angle_quantizations;
+        _angle_quantizations = static_cast<unsigned int>(angle_quantizations);
+      }
+    } else if (type == ParameterType::PARAMETER_STRING) {
+      if (name == _name + ".motion_model_for_search") {
+        reinit_a_star = true;
+        _motion_model = fromString(parameter.as_string());
+        if (_motion_model == MotionModel::UNKNOWN) {
+          RCLCPP_WARN(
+            _logger,
+            "Unable to get MotionModel search type. Given '%s', "
+            "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.",
+            _motion_model_for_search.c_str());
+        }
+      }
+    }
+  }
+
+  // Re-init if needed with mutex lock (to avoid re-init while creating a plan)
+  if (reinit_a_star || reinit_downsampler || reinit_collision_checker || reinit_smoother) {
+    // convert to grid coordinates
+    if (!_downsample_costmap) {
+      _downsampling_factor = 1;
+    }
+    _search_info.minimum_turning_radius =
+      _minimum_turning_radius_global_coords / (_costmap->getResolution() * _downsampling_factor);
+    _lookup_table_dim =
+      static_cast<float>(_lookup_table_size) /
+      static_cast<float>(_costmap->getResolution() * _downsampling_factor);
+
+    // Make sure its a whole number
+    _lookup_table_dim = static_cast<float>(static_cast<int>(_lookup_table_dim));
+
+    // Make sure its an odd number
+    if (static_cast<int>(_lookup_table_dim) % 2 == 0) {
+      RCLCPP_INFO(
+        _logger,
+        "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd",
+        _lookup_table_dim);
+      _lookup_table_dim += 1.0;
+    }
+
+    auto node = _node.lock();
+
+    // Re-Initialize A* template
+    if (reinit_a_star) {
+      _a_star = std::make_unique<AStarAlgorithm<NodeHybrid>>(_motion_model, _search_info);
+      _a_star->initialize(
+        _allow_unknown,
+        _max_iterations,
+        _max_on_approach_iterations,
+        _max_planning_time,
+        _lookup_table_dim,
+        _angle_quantizations);
+    }
+
+    // Re-Initialize costmap downsampler
+    if (reinit_downsampler) {
+      if (_downsample_costmap && _downsampling_factor > 1) {
+        std::string topic_name = "downsampled_costmap";
+        _costmap_downsampler = std::make_unique<CostmapDownsampler>();
+        _costmap_downsampler->on_configure(
+          node, _global_frame, topic_name, _costmap, _downsampling_factor);
+      }
+    }
+
+    // Re-Initialize collision checker
+    if (reinit_collision_checker) {
+      _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node);
+      _collision_checker.setFootprint(
+        _costmap_ros->getRobotFootprint(),
+        _costmap_ros->getUseRadius(),
+        findCircumscribedCost(_costmap_ros));
+    }
+
+    // Re-Initialize smoother
+    if (reinit_smoother) {
+      SmootherParams params;
+      params.get(node, _name);
+      _smoother = std::make_unique<Smoother>(params);
+      _smoother->initialize(_minimum_turning_radius_global_coords);
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_smac_planner
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlannerHybrid, nav2_core::GlobalPlanner)

navigation2/nav2_smac_planner/src/smac_planner_lattice.cpp

@@ -0,0 +1,500 @@
+// Copyright (c) 2021, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <string>
+#include <memory>
+#include <vector>
+#include <algorithm>
+#include <limits>
+
+#include "Eigen/Core"
+#include "nav2_smac_planner/smac_planner_lattice.hpp"
+
+// #define BENCHMARK_TESTING
+
+namespace nav2_smac_planner
+{
+
+using namespace std::chrono;  // NOLINT
+using rcl_interfaces::msg::ParameterType;
+
+SmacPlannerLattice::SmacPlannerLattice()
+: _a_star(nullptr),
+  _collision_checker(nullptr, 1, nullptr),
+  _smoother(nullptr),
+  _costmap(nullptr)
+{
+}
+
+SmacPlannerLattice::~SmacPlannerLattice()
+{
+  RCLCPP_INFO(
+    _logger, "Destroying plugin %s of type SmacPlannerLattice",
+    _name.c_str());
+}
+
+void SmacPlannerLattice::configure(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  std::string name, std::shared_ptr<tf2_ros::Buffer>/*tf*/,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  _node = parent;
+  auto node = parent.lock();
+  _logger = node->get_logger();
+  _clock = node->get_clock();
+  _costmap = costmap_ros->getCostmap();
+  _costmap_ros = costmap_ros;
+  _name = name;
+  _global_frame = costmap_ros->getGlobalFrameID();
+  _raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
+
+  RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerLattice", name.c_str());
+
+  // General planner params
+  double analytic_expansion_max_length_m;
+  bool smooth_path;
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".tolerance", rclcpp::ParameterValue(0.25));
+  _tolerance = static_cast<float>(node->get_parameter(name + ".tolerance").as_double());
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".allow_unknown", rclcpp::ParameterValue(true));
+  node->get_parameter(name + ".allow_unknown", _allow_unknown);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".max_iterations", rclcpp::ParameterValue(1000000));
+  node->get_parameter(name + ".max_iterations", _max_iterations);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000));
+  node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".smooth_path", rclcpp::ParameterValue(true));
+  node->get_parameter(name + ".smooth_path", smooth_path);
+
+  // Default to a well rounded model: 16 bin, 0.4m turning radius, ackermann model
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".lattice_filepath", rclcpp::ParameterValue(
+      ament_index_cpp::get_package_share_directory("nav2_smac_planner") +
+      "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json"));
+  node->get_parameter(name + ".lattice_filepath", _search_info.lattice_filepath);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".cache_obstacle_heuristic", rclcpp::ParameterValue(false));
+  node->get_parameter(name + ".cache_obstacle_heuristic", _search_info.cache_obstacle_heuristic);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0));
+  node->get_parameter(name + ".reverse_penalty", _search_info.reverse_penalty);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".change_penalty", rclcpp::ParameterValue(0.05));
+  node->get_parameter(name + ".change_penalty", _search_info.change_penalty);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.05));
+  node->get_parameter(name + ".non_straight_penalty", _search_info.non_straight_penalty);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".cost_penalty", rclcpp::ParameterValue(2.0));
+  node->get_parameter(name + ".cost_penalty", _search_info.cost_penalty);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".retrospective_penalty", rclcpp::ParameterValue(0.015));
+  node->get_parameter(name + ".retrospective_penalty", _search_info.retrospective_penalty);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".rotation_penalty", rclcpp::ParameterValue(5.0));
+  node->get_parameter(name + ".rotation_penalty", _search_info.rotation_penalty);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(3.5));
+  node->get_parameter(name + ".analytic_expansion_ratio", _search_info.analytic_expansion_ratio);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".analytic_expansion_max_length", rclcpp::ParameterValue(3.0));
+  node->get_parameter(name + ".analytic_expansion_max_length", analytic_expansion_max_length_m);
+  _search_info.analytic_expansion_max_length =
+    analytic_expansion_max_length_m / _costmap->getResolution();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".max_planning_time", rclcpp::ParameterValue(5.0));
+  node->get_parameter(name + ".max_planning_time", _max_planning_time);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".lookup_table_size", rclcpp::ParameterValue(20.0));
+  node->get_parameter(name + ".lookup_table_size", _lookup_table_size);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".allow_reverse_expansion", rclcpp::ParameterValue(false));
+  node->get_parameter(name + ".allow_reverse_expansion", _search_info.allow_reverse_expansion);
+
+  _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath);
+  _search_info.minimum_turning_radius =
+    _metadata.min_turning_radius / (_costmap->getResolution());
+  _motion_model = MotionModel::STATE_LATTICE;
+
+  if (_max_on_approach_iterations <= 0) {
+    RCLCPP_INFO(
+      _logger, "On approach iteration selected as <= 0, "
+      "disabling tolerance and on approach iterations.");
+    _max_on_approach_iterations = std::numeric_limits<int>::max();
+  }
+
+  if (_max_iterations <= 0) {
+    RCLCPP_INFO(
+      _logger, "maximum iteration selected as <= 0, "
+      "disabling maximum iterations.");
+    _max_iterations = std::numeric_limits<int>::max();
+  }
+
+  float lookup_table_dim =
+    static_cast<float>(_lookup_table_size) /
+    static_cast<float>(_costmap->getResolution());
+
+  // Make sure its a whole number
+  lookup_table_dim = static_cast<float>(static_cast<int>(lookup_table_dim));
+
+  // Make sure its an odd number
+  if (static_cast<int>(lookup_table_dim) % 2 == 0) {
+    RCLCPP_INFO(
+      _logger,
+      "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd",
+      lookup_table_dim);
+    lookup_table_dim += 1.0;
+  }
+
+  // Initialize collision checker using 72 evenly sized bins instead of the lattice
+  // heading angles. This is done so that we have precomputed angles every 5 degrees.
+  // If we used the sparse lattice headings (usually 16), then when we attempt to collision
+  // check for intermediary points of the primitives, we're forced to round to one of the 16
+  // increments causing "wobbly" checks that could cause larger robots to virtually show collisions
+  // in valid configurations. This approximation helps to bound orientation error for all checks
+  // in exchange for slight inaccuracies in the collision headings in terminal search states.
+  _collision_checker = GridCollisionChecker(_costmap, 72u, node);
+  _collision_checker.setFootprint(
+    costmap_ros->getRobotFootprint(),
+    costmap_ros->getUseRadius(),
+    findCircumscribedCost(costmap_ros));
+
+  // Initialize A* template
+  _a_star = std::make_unique<AStarAlgorithm<NodeLattice>>(_motion_model, _search_info);
+  _a_star->initialize(
+    _allow_unknown,
+    _max_iterations,
+    _max_on_approach_iterations,
+    _max_planning_time,
+    lookup_table_dim,
+    _metadata.number_of_headings);
+
+  // Initialize path smoother
+  if (smooth_path) {
+    SmootherParams params;
+    params.get(node, name);
+    _smoother = std::make_unique<Smoother>(params);
+    _smoother->initialize(_metadata.min_turning_radius);
+  }
+
+  RCLCPP_INFO(
+    _logger, "Configured plugin %s of type SmacPlannerLattice with "
+    "maximum iterations %i, max on approach iterations %i, "
+    "and %s. Tolerance %.2f. Using motion model: %s. State lattice file: %s.",
+    _name.c_str(), _max_iterations, _max_on_approach_iterations,
+    _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal",
+    _tolerance, toString(_motion_model).c_str(), _search_info.lattice_filepath.c_str());
+}
+
+void SmacPlannerLattice::activate()
+{
+  RCLCPP_INFO(
+    _logger, "Activating plugin %s of type SmacPlannerLattice",
+    _name.c_str());
+  _raw_plan_publisher->on_activate();
+  auto node = _node.lock();
+  // Add callback for dynamic parameters
+  _dyn_params_handler = node->add_on_set_parameters_callback(
+    std::bind(&SmacPlannerLattice::dynamicParametersCallback, this, std::placeholders::_1));
+}
+
+void SmacPlannerLattice::deactivate()
+{
+  RCLCPP_INFO(
+    _logger, "Deactivating plugin %s of type SmacPlannerLattice",
+    _name.c_str());
+  _raw_plan_publisher->on_deactivate();
+  _dyn_params_handler.reset();
+}
+
+void SmacPlannerLattice::cleanup()
+{
+  RCLCPP_INFO(
+    _logger, "Cleaning up plugin %s of type SmacPlannerLattice",
+    _name.c_str());
+  _a_star.reset();
+  _smoother.reset();
+  _raw_plan_publisher.reset();
+}
+
+nav_msgs::msg::Path SmacPlannerLattice::createPlan(
+  const geometry_msgs::msg::PoseStamped & start,
+  const geometry_msgs::msg::PoseStamped & goal)
+{
+  std::lock_guard<std::mutex> lock_reinit(_mutex);
+  steady_clock::time_point a = steady_clock::now();
+
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(_costmap->getMutex()));
+
+  // Set collision checker and costmap information
+  _collision_checker.setFootprint(
+    _costmap_ros->getRobotFootprint(),
+    _costmap_ros->getUseRadius(),
+    findCircumscribedCost(_costmap_ros));
+  _a_star->setCollisionChecker(&_collision_checker);
+
+  // Set starting point, in A* bin search coordinates
+  unsigned int mx, my;
+  _costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my);
+  _a_star->setStart(
+    mx, my,
+    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation)));
+
+  // Set goal point, in A* bin search coordinates
+  _costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my);
+  _a_star->setGoal(
+    mx, my,
+    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)));
+
+  // Setup message
+  nav_msgs::msg::Path plan;
+  plan.header.stamp = _clock->now();
+  plan.header.frame_id = _global_frame;
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header = plan.header;
+  pose.pose.position.z = 0.0;
+  pose.pose.orientation.x = 0.0;
+  pose.pose.orientation.y = 0.0;
+  pose.pose.orientation.z = 0.0;
+  pose.pose.orientation.w = 1.0;
+
+  // Compute plan
+  NodeLattice::CoordinateVector path;
+  int num_iterations = 0;
+  std::string error;
+  try {
+    if (!_a_star->createPath(
+        path, num_iterations, _tolerance / static_cast<float>(_costmap->getResolution())))
+    {
+      if (num_iterations < _a_star->getMaxIterations()) {
+        error = std::string("no valid path found");
+      } else {
+        error = std::string("exceeded maximum iterations");
+      }
+    }
+  } catch (const std::runtime_error & e) {
+    error = "invalid use: ";
+    error += e.what();
+  }
+
+  if (!error.empty()) {
+    RCLCPP_WARN(
+      _logger,
+      "%s: failed to create plan, %s.",
+      _name.c_str(), error.c_str());
+    return plan;
+  }
+
+  // Convert to world coordinates
+  plan.poses.reserve(path.size());
+  geometry_msgs::msg::PoseStamped last_pose = pose;
+  for (int i = path.size() - 1; i >= 0; --i) {
+    pose.pose = getWorldCoords(path[i].x, path[i].y, _costmap);
+    pose.pose.orientation = getWorldOrientation(path[i].theta);
+    if (fabs(pose.pose.position.x - last_pose.pose.position.x) < 1e-4 &&
+      fabs(pose.pose.position.y - last_pose.pose.position.y) < 1e-4 &&
+      fabs(tf2::getYaw(pose.pose.orientation) - tf2::getYaw(last_pose.pose.orientation)) < 1e-4)
+    {
+      RCLCPP_DEBUG(
+        _logger,
+        "Removed a path from the path due to replication. "
+        "Make sure your minimum control set does not contain duplicate values!");
+      continue;
+    }
+    last_pose = pose;
+    plan.poses.push_back(pose);
+  }
+
+  // Publish raw path for debug
+  if (_raw_plan_publisher->get_subscription_count() > 0) {
+    _raw_plan_publisher->publish(plan);
+  }
+
+  // Find how much time we have left to do smoothing
+  steady_clock::time_point b = steady_clock::now();
+  duration<double> time_span = duration_cast<duration<double>>(b - a);
+  double time_remaining = _max_planning_time - static_cast<double>(time_span.count());
+
+#ifdef BENCHMARK_TESTING
+  std::cout << "It took " << time_span.count() * 1000 <<
+    " milliseconds with " << num_iterations << " iterations." << std::endl;
+#endif
+
+  // Smooth plan
+  if (_smoother && num_iterations > 1) {
+    _smoother->smooth(plan, _costmap, time_remaining);
+  }
+
+#ifdef BENCHMARK_TESTING
+  steady_clock::time_point c = steady_clock::now();
+  duration<double> time_span2 = duration_cast<duration<double>>(c - b);
+  std::cout << "It took " << time_span2.count() * 1000 <<
+    " milliseconds to smooth path." << std::endl;
+#endif
+
+  return plan;
+}
+
+rcl_interfaces::msg::SetParametersResult
+SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  std::lock_guard<std::mutex> lock_reinit(_mutex);
+
+  bool reinit_a_star = false;
+  bool reinit_smoother = false;
+
+  for (auto parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == _name + ".max_planning_time") {
+        reinit_a_star = true;
+        _max_planning_time = parameter.as_double();
+      } else if (name == _name + ".tolerance") {
+        _tolerance = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".lookup_table_size") {
+        reinit_a_star = true;
+        _lookup_table_size = parameter.as_double();
+      } else if (name == _name + ".reverse_penalty") {
+        reinit_a_star = true;
+        _search_info.reverse_penalty = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".change_penalty") {
+        reinit_a_star = true;
+        _search_info.change_penalty = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".non_straight_penalty") {
+        reinit_a_star = true;
+        _search_info.non_straight_penalty = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".cost_penalty") {
+        reinit_a_star = true;
+        _search_info.cost_penalty = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".rotation_penalty") {
+        reinit_a_star = true;
+        _search_info.rotation_penalty = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".analytic_expansion_ratio") {
+        reinit_a_star = true;
+        _search_info.analytic_expansion_ratio = static_cast<float>(parameter.as_double());
+      } else if (name == _name + ".analytic_expansion_max_length") {
+        reinit_a_star = true;
+        _search_info.analytic_expansion_max_length =
+          static_cast<float>(parameter.as_double()) / _costmap->getResolution();
+      }
+    } else if (type == ParameterType::PARAMETER_BOOL) {
+      if (name == _name + ".allow_unknown") {
+        reinit_a_star = true;
+        _allow_unknown = parameter.as_bool();
+      } else if (name == _name + ".cache_obstacle_heuristic") {
+        reinit_a_star = true;
+        _search_info.cache_obstacle_heuristic = parameter.as_bool();
+      } else if (name == _name + ".allow_reverse_expansion") {
+        reinit_a_star = true;
+        _search_info.allow_reverse_expansion = parameter.as_bool();
+      } else if (name == _name + ".smooth_path") {
+        if (parameter.as_bool()) {
+          reinit_smoother = true;
+        } else {
+          _smoother.reset();
+        }
+      }
+    } else if (type == ParameterType::PARAMETER_INTEGER) {
+      if (name == _name + ".max_iterations") {
+        reinit_a_star = true;
+        _max_iterations = parameter.as_int();
+        if (_max_iterations <= 0) {
+          RCLCPP_INFO(
+            _logger, "maximum iteration selected as <= 0, "
+            "disabling maximum iterations.");
+          _max_iterations = std::numeric_limits<int>::max();
+        }
+      }
+    } else if (name == _name + ".max_on_approach_iterations") {
+      reinit_a_star = true;
+      _max_on_approach_iterations = parameter.as_int();
+      if (_max_on_approach_iterations <= 0) {
+        RCLCPP_INFO(
+          _logger, "On approach iteration selected as <= 0, "
+          "disabling tolerance and on approach iterations.");
+        _max_on_approach_iterations = std::numeric_limits<int>::max();
+      }
+    } else if (type == ParameterType::PARAMETER_STRING) {
+      if (name == _name + ".lattice_filepath") {
+        reinit_a_star = true;
+        if (_smoother) {
+          reinit_smoother = true;
+        }
+        _search_info.lattice_filepath = parameter.as_string();
+        _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath);
+        _search_info.minimum_turning_radius =
+          _metadata.min_turning_radius / (_costmap->getResolution());
+      }
+    }
+  }
+
+  // Re-init if needed with mutex lock (to avoid re-init while creating a plan)
+  if (reinit_a_star || reinit_smoother) {
+    // convert to grid coordinates
+    _search_info.minimum_turning_radius =
+      _metadata.min_turning_radius / (_costmap->getResolution());
+    float lookup_table_dim =
+      static_cast<float>(_lookup_table_size) /
+      static_cast<float>(_costmap->getResolution());
+
+    // Make sure its a whole number
+    lookup_table_dim = static_cast<float>(static_cast<int>(lookup_table_dim));
+
+    // Make sure its an odd number
+    if (static_cast<int>(lookup_table_dim) % 2 == 0) {
+      RCLCPP_INFO(
+        _logger,
+        "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd",
+        lookup_table_dim);
+      lookup_table_dim += 1.0;
+    }
+
+    // Re-Initialize smoother
+    if (reinit_smoother) {
+      auto node = _node.lock();
+      SmootherParams params;
+      params.get(node, _name);
+      _smoother = std::make_unique<Smoother>(params);
+      _smoother->initialize(_metadata.min_turning_radius);
+    }
+
+    // Re-Initialize A* template
+    if (reinit_a_star) {
+      _a_star = std::make_unique<AStarAlgorithm<NodeLattice>>(_motion_model, _search_info);
+      _a_star->initialize(
+        _allow_unknown,
+        _max_iterations,
+        _max_on_approach_iterations,
+        _max_planning_time,
+        lookup_table_dim,
+        _metadata.number_of_headings);
+    }
+  }
+
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_smac_planner
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlannerLattice, nav2_core::GlobalPlanner)

navigation2/nav2_smac_planner/src/smoother.cpp

@@ -0,0 +1,512 @@
+// Copyright (c) 2021, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <ompl/base/ScopedState.h>
+#include <ompl/base/spaces/DubinsStateSpace.h>
+#include <vector>
+#include <memory>
+#include "nav2_smac_planner/smoother.hpp"
+
+namespace nav2_smac_planner
+{
+using namespace nav2_util::geometry_utils;  // NOLINT
+using namespace std::chrono;  // NOLINT
+
+Smoother::Smoother(const SmootherParams & params)
+{
+  tolerance_ = params.tolerance_;
+  max_its_ = params.max_its_;
+  data_w_ = params.w_data_;
+  smooth_w_ = params.w_smooth_;
+  is_holonomic_ = params.holonomic_;
+  do_refinement_ = params.do_refinement_;
+}
+
+void Smoother::initialize(const double & min_turning_radius)
+{
+  min_turning_rad_ = min_turning_radius;
+  state_space_ = std::make_unique<ompl::base::DubinsStateSpace>(min_turning_rad_);
+}
+
+bool Smoother::smooth(
+  nav_msgs::msg::Path & path,
+  const nav2_costmap_2d::Costmap2D * costmap,
+  const double & max_time)
+{
+  // by-pass path orientations approximation when skipping smac smoother
+  if (max_its_ == 0) {
+    return false;
+  }
+
+  refinement_ctr_ = 0;
+  steady_clock::time_point start = steady_clock::now();
+  double time_remaining = max_time;
+  bool success = true, reversing_segment;
+  nav_msgs::msg::Path curr_path_segment;
+  curr_path_segment.header = path.header;
+  std::vector<PathSegment> path_segments = findDirectionalPathSegments(path);
+
+  for (unsigned int i = 0; i != path_segments.size(); i++) {
+    if (path_segments[i].end - path_segments[i].start > 10) {
+      // Populate path segment
+      curr_path_segment.poses.clear();
+      std::copy(
+        path.poses.begin() + path_segments[i].start,
+        path.poses.begin() + path_segments[i].end + 1,
+        std::back_inserter(curr_path_segment.poses));
+
+      // Make sure we're still able to smooth with time remaining
+      steady_clock::time_point now = steady_clock::now();
+      time_remaining = max_time - duration_cast<duration<double>>(now - start).count();
+
+      // Smooth path segment naively
+      const geometry_msgs::msg::Pose start_pose = curr_path_segment.poses.front().pose;
+      const geometry_msgs::msg::Pose goal_pose = curr_path_segment.poses.back().pose;
+      bool local_success =
+        smoothImpl(curr_path_segment, reversing_segment, costmap, time_remaining);
+      success = success && local_success;
+
+      // Enforce boundary conditions
+      if (!is_holonomic_ && local_success) {
+        enforceStartBoundaryConditions(start_pose, curr_path_segment, costmap, reversing_segment);
+        enforceEndBoundaryConditions(goal_pose, curr_path_segment, costmap, reversing_segment);
+      }
+
+      // Assemble the path changes to the main path
+      std::copy(
+        curr_path_segment.poses.begin(),
+        curr_path_segment.poses.end(),
+        path.poses.begin() + path_segments[i].start);
+    }
+  }
+
+  return success;
+}
+
+bool Smoother::smoothImpl(
+  nav_msgs::msg::Path & path,
+  bool & reversing_segment,
+  const nav2_costmap_2d::Costmap2D * costmap,
+  const double & max_time)
+{
+  steady_clock::time_point a = steady_clock::now();
+  rclcpp::Duration max_dur = rclcpp::Duration::from_seconds(max_time);
+
+  int its = 0;
+  double change = tolerance_;
+  const unsigned int & path_size = path.poses.size();
+  double x_i, y_i, y_m1, y_ip1, y_i_org;
+  unsigned int mx, my;
+
+  nav_msgs::msg::Path new_path = path;
+  nav_msgs::msg::Path last_path = path;
+
+  while (change >= tolerance_) {
+    its += 1;
+    change = 0.0;
+
+    // Make sure the smoothing function will converge
+    if (its >= max_its_) {
+      RCLCPP_DEBUG(
+        rclcpp::get_logger("SmacPlannerSmoother"),
+        "Number of iterations has exceeded limit of %i.", max_its_);
+      path = last_path;
+      updateApproximatePathOrientations(path, reversing_segment);
+      return false;
+    }
+
+    // Make sure still have time left to process
+    steady_clock::time_point b = steady_clock::now();
+    rclcpp::Duration timespan(duration_cast<duration<double>>(b - a));
+    if (timespan > max_dur) {
+      RCLCPP_DEBUG(
+        rclcpp::get_logger("SmacPlannerSmoother"),
+        "Smoothing time exceeded allowed duration of %0.2f.", max_time);
+      path = last_path;
+      updateApproximatePathOrientations(path, reversing_segment);
+      return false;
+    }
+
+    for (unsigned int i = 1; i != path_size - 1; i++) {
+      for (unsigned int j = 0; j != 2; j++) {
+        x_i = getFieldByDim(path.poses[i], j);
+        y_i = getFieldByDim(new_path.poses[i], j);
+        y_m1 = getFieldByDim(new_path.poses[i - 1], j);
+        y_ip1 = getFieldByDim(new_path.poses[i + 1], j);
+        y_i_org = y_i;
+
+        // Smooth based on local 3 point neighborhood and original data locations
+        y_i += data_w_ * (x_i - y_i) + smooth_w_ * (y_ip1 + y_m1 - (2.0 * y_i));
+        setFieldByDim(new_path.poses[i], j, y_i);
+        change += abs(y_i - y_i_org);
+      }
+
+      // validate update is admissible, only checks cost if a valid costmap pointer is provided
+      float cost = 0.0;
+      if (costmap) {
+        costmap->worldToMap(
+          getFieldByDim(new_path.poses[i], 0),
+          getFieldByDim(new_path.poses[i], 1),
+          mx, my);
+        cost = static_cast<float>(costmap->getCost(mx, my));
+      }
+
+      if (cost > MAX_NON_OBSTACLE && cost != UNKNOWN) {
+        RCLCPP_DEBUG(
+          rclcpp::get_logger("SmacPlannerSmoother"),
+          "Smoothing process resulted in an infeasible collision. "
+          "Returning the last path before the infeasibility was introduced.");
+        path = last_path;
+        updateApproximatePathOrientations(path, reversing_segment);
+        return false;
+      }
+    }
+
+    last_path = new_path;
+  }
+
+  // Lets do additional refinement, it shouldn't take more than a couple milliseconds
+  // but really puts the path quality over the top.
+  if (do_refinement_ && refinement_ctr_ < 4) {
+    refinement_ctr_++;
+    smoothImpl(new_path, reversing_segment, costmap, max_time);
+  }
+
+  updateApproximatePathOrientations(new_path, reversing_segment);
+  path = new_path;
+  return true;
+}
+
+double Smoother::getFieldByDim(
+  const geometry_msgs::msg::PoseStamped & msg, const unsigned int & dim)
+{
+  if (dim == 0) {
+    return msg.pose.position.x;
+  } else if (dim == 1) {
+    return msg.pose.position.y;
+  } else {
+    return msg.pose.position.z;
+  }
+}
+
+void Smoother::setFieldByDim(
+  geometry_msgs::msg::PoseStamped & msg, const unsigned int dim,
+  const double & value)
+{
+  if (dim == 0) {
+    msg.pose.position.x = value;
+  } else if (dim == 1) {
+    msg.pose.position.y = value;
+  } else {
+    msg.pose.position.z = value;
+  }
+}
+
+std::vector<PathSegment> Smoother::findDirectionalPathSegments(const nav_msgs::msg::Path & path)
+{
+  std::vector<PathSegment> segments;
+  PathSegment curr_segment;
+  curr_segment.start = 0;
+
+  // If holonomic, no directional changes and
+  // may have abrupt angular changes from naive grid search
+  if (is_holonomic_) {
+    curr_segment.end = path.poses.size() - 1;
+    segments.push_back(curr_segment);
+    return segments;
+  }
+
+  // Iterating through the path to determine the position of the cusp
+  for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
+    // We have two vectors for the dot product OA and AB. Determining the vectors.
+    double oa_x = path.poses[idx].pose.position.x -
+      path.poses[idx - 1].pose.position.x;
+    double oa_y = path.poses[idx].pose.position.y -
+      path.poses[idx - 1].pose.position.y;
+    double ab_x = path.poses[idx + 1].pose.position.x -
+      path.poses[idx].pose.position.x;
+    double ab_y = path.poses[idx + 1].pose.position.y -
+      path.poses[idx].pose.position.y;
+
+    // Checking for the existance of cusp, in the path, using the dot product.
+    double dot_product = (oa_x * ab_x) + (oa_y * ab_y);
+    if (dot_product < 0.0) {
+      curr_segment.end = idx;
+      segments.push_back(curr_segment);
+      curr_segment.start = idx;
+    }
+
+    // Checking for the existance of a differential rotation in place.
+    double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation);
+    double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation);
+    double dtheta = angles::shortest_angular_distance(cur_theta, next_theta);
+    if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) {
+      curr_segment.end = idx;
+      segments.push_back(curr_segment);
+      curr_segment.start = idx;
+    }
+  }
+
+  curr_segment.end = path.poses.size() - 1;
+  segments.push_back(curr_segment);
+  return segments;
+}
+
+void Smoother::updateApproximatePathOrientations(
+  nav_msgs::msg::Path & path,
+  bool & reversing_segment)
+{
+  double dx, dy, theta, pt_yaw;
+  reversing_segment = false;
+
+  // Find if this path segment is in reverse
+  dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x;
+  dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y;
+  theta = atan2(dy, dx);
+  pt_yaw = tf2::getYaw(path.poses[1].pose.orientation);
+  if (!is_holonomic_ && fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) {
+    reversing_segment = true;
+  }
+
+  // Find the angle relative the path position vectors
+  for (unsigned int i = 0; i != path.poses.size() - 1; i++) {
+    dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x;
+    dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y;
+    theta = atan2(dy, dx);
+
+    // If points are overlapping, pass
+    if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) {
+      continue;
+    }
+
+    // Flip the angle if this path segment is in reverse
+    if (reversing_segment) {
+      theta += M_PI;  // orientationAroundZAxis will normalize
+    }
+
+    path.poses[i].pose.orientation = orientationAroundZAxis(theta);
+  }
+}
+
+unsigned int Smoother::findShortestBoundaryExpansionIdx(
+  const BoundaryExpansions & boundary_expansions)
+{
+  // Check which is valid with the minimum integrated length such that
+  // shorter end-points away that are infeasible to achieve without
+  // a loop-de-loop are punished
+  double min_length = 1e9;
+  int shortest_boundary_expansion_idx = 1e9;
+  for (unsigned int idx = 0; idx != boundary_expansions.size(); idx++) {
+    if (boundary_expansions[idx].expansion_path_length<min_length &&
+      !boundary_expansions[idx].in_collision &&
+      boundary_expansions[idx].path_end_idx>0.0 &&
+      boundary_expansions[idx].expansion_path_length > 0.0)
+    {
+      min_length = boundary_expansions[idx].expansion_path_length;
+      shortest_boundary_expansion_idx = idx;
+    }
+  }
+
+  return shortest_boundary_expansion_idx;
+}
+
+void Smoother::findBoundaryExpansion(
+  const geometry_msgs::msg::Pose & start,
+  const geometry_msgs::msg::Pose & end,
+  BoundaryExpansion & expansion,
+  const nav2_costmap_2d::Costmap2D * costmap)
+{
+  static ompl::base::ScopedState<> from(state_space_), to(state_space_), s(state_space_);
+
+  from[0] = start.position.x;
+  from[1] = start.position.y;
+  from[2] = tf2::getYaw(start.orientation);
+  to[0] = end.position.x;
+  to[1] = end.position.y;
+  to[2] = tf2::getYaw(end.orientation);
+
+  double d = state_space_->distance(from(), to());
+  // If this path is too long compared to the original, then this is probably
+  // a loop-de-loop, treat as invalid as to not deviate too far from the original path.
+  // 2.0 selected from prinicipled choice of boundary test points
+  // r, 2 * r, r * PI, and 2 * PI * r. If there is a loop, it will be
+  // approximately 2 * PI * r, which is 2 * PI > r, PI > 2 * r, and 2 > r * PI.
+  // For all but the last backup test point, a loop would be approximately
+  // 2x greater than any of the selections.
+  if (d > 2.0 * expansion.original_path_length) {
+    return;
+  }
+
+  std::vector<double> reals;
+  double theta(0.0), x(0.0), y(0.0);
+  double x_m = start.position.x;
+  double y_m = start.position.y;
+
+  // Get intermediary poses
+  for (double i = 0; i <= expansion.path_end_idx; i++) {
+    state_space_->interpolate(from(), to(), i / expansion.path_end_idx, s());
+    reals = s.reals();
+    // Make sure in range [0, 2PI)
+    theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2];
+    theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta;
+    x = reals[0];
+    y = reals[1];
+
+    // Check for collision
+    unsigned int mx, my;
+    costmap->worldToMap(x, y, mx, my);
+    if (static_cast<float>(costmap->getCost(mx, my)) >= INSCRIBED) {
+      expansion.in_collision = true;
+    }
+
+    // Integrate path length
+    expansion.expansion_path_length += hypot(x - x_m, y - y_m);
+    x_m = x;
+    y_m = y;
+
+    // Store point
+    expansion.pts.emplace_back(x, y, theta);
+  }
+}
+
+template<typename IteratorT>
+BoundaryExpansions Smoother::generateBoundaryExpansionPoints(IteratorT start, IteratorT end)
+{
+  std::vector<double> distances = {
+    min_turning_rad_,  // Radius
+    2.0 * min_turning_rad_,  // Diameter
+    M_PI * min_turning_rad_,  // 50% Circumference
+    2.0 * M_PI * min_turning_rad_  // Circumference
+  };
+
+  BoundaryExpansions boundary_expansions;
+  boundary_expansions.resize(distances.size());
+  double curr_dist = 0.0;
+  double x_last = start->pose.position.x;
+  double y_last = start->pose.position.y;
+  geometry_msgs::msg::Point pt;
+  unsigned int curr_dist_idx = 0;
+
+  for (IteratorT iter = start; iter != end; iter++) {
+    pt = iter->pose.position;
+    curr_dist += hypot(pt.x - x_last, pt.y - y_last);
+    x_last = pt.x;
+    y_last = pt.y;
+
+    if (curr_dist >= distances[curr_dist_idx]) {
+      boundary_expansions[curr_dist_idx].path_end_idx = iter - start;
+      boundary_expansions[curr_dist_idx].original_path_length = curr_dist;
+      curr_dist_idx++;
+    }
+
+    if (curr_dist_idx == boundary_expansions.size()) {
+      break;
+    }
+  }
+
+  return boundary_expansions;
+}
+
+void Smoother::enforceStartBoundaryConditions(
+  const geometry_msgs::msg::Pose & start_pose,
+  nav_msgs::msg::Path & path,
+  const nav2_costmap_2d::Costmap2D * costmap,
+  const bool & reversing_segment)
+{
+  // Find range of points for testing
+  BoundaryExpansions boundary_expansions =
+    generateBoundaryExpansionPoints<PathIterator>(path.poses.begin(), path.poses.end());
+
+  // Generate the motion model and metadata from start -> test points
+  for (unsigned int i = 0; i != boundary_expansions.size(); i++) {
+    BoundaryExpansion & expansion = boundary_expansions[i];
+    if (expansion.path_end_idx == 0.0) {
+      continue;
+    }
+
+    if (!reversing_segment) {
+      findBoundaryExpansion(
+        start_pose, path.poses[expansion.path_end_idx].pose, expansion,
+        costmap);
+    } else {
+      findBoundaryExpansion(
+        path.poses[expansion.path_end_idx].pose, start_pose, expansion,
+        costmap);
+    }
+  }
+
+  // Find the shortest kinematically feasible boundary expansion
+  unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions);
+  if (best_expansion_idx > boundary_expansions.size()) {
+    return;
+  }
+
+  // Override values to match curve
+  BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx];
+  if (reversing_segment) {
+    std::reverse(best_expansion.pts.begin(), best_expansion.pts.end());
+  }
+  for (unsigned int i = 0; i != best_expansion.pts.size(); i++) {
+    path.poses[i].pose.position.x = best_expansion.pts[i].x;
+    path.poses[i].pose.position.y = best_expansion.pts[i].y;
+    path.poses[i].pose.orientation = orientationAroundZAxis(best_expansion.pts[i].theta);
+  }
+}
+
+void Smoother::enforceEndBoundaryConditions(
+  const geometry_msgs::msg::Pose & end_pose,
+  nav_msgs::msg::Path & path,
+  const nav2_costmap_2d::Costmap2D * costmap,
+  const bool & reversing_segment)
+{
+  // Find range of points for testing
+  BoundaryExpansions boundary_expansions =
+    generateBoundaryExpansionPoints<ReversePathIterator>(path.poses.rbegin(), path.poses.rend());
+
+  // Generate the motion model and metadata from start -> test points
+  unsigned int expansion_starting_idx;
+  for (unsigned int i = 0; i != boundary_expansions.size(); i++) {
+    BoundaryExpansion & expansion = boundary_expansions[i];
+    if (expansion.path_end_idx == 0.0) {
+      continue;
+    }
+    expansion_starting_idx = path.poses.size() - expansion.path_end_idx - 1;
+    if (!reversing_segment) {
+      findBoundaryExpansion(path.poses[expansion_starting_idx].pose, end_pose, expansion, costmap);
+    } else {
+      findBoundaryExpansion(end_pose, path.poses[expansion_starting_idx].pose, expansion, costmap);
+    }
+  }
+
+  // Find the shortest kinematically feasible boundary expansion
+  unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions);
+  if (best_expansion_idx > boundary_expansions.size()) {
+    return;
+  }
+
+  // Override values to match curve
+  BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx];
+  if (reversing_segment) {
+    std::reverse(best_expansion.pts.begin(), best_expansion.pts.end());
+  }
+  expansion_starting_idx = path.poses.size() - best_expansion.path_end_idx - 1;
+  for (unsigned int i = 0; i != best_expansion.pts.size(); i++) {
+    path.poses[expansion_starting_idx + i].pose.position.x = best_expansion.pts[i].x;
+    path.poses[expansion_starting_idx + i].pose.position.y = best_expansion.pts[i].y;
+    path.poses[expansion_starting_idx + i].pose.orientation = orientationAroundZAxis(
+      best_expansion.pts[i].theta);
+  }
+}
+
+}  // namespace nav2_smac_planner

navigation2/nav2_smac_planner/test/CMakeLists.txt

@@ -0,0 +1,118 @@
+# Test costmap downsampler
+ament_add_gtest(test_costmap_downsampler
+  test_costmap_downsampler.cpp
+)
+ament_target_dependencies(test_costmap_downsampler
+  ${dependencies}
+)
+target_link_libraries(test_costmap_downsampler
+  ${library_name}
+)
+
+# Test Node2D
+ament_add_gtest(test_node2d
+  test_node2d.cpp
+)
+ament_target_dependencies(test_node2d
+  ${dependencies}
+)
+target_link_libraries(test_node2d
+  ${library_name}
+)
+
+# Test NodeHybrid
+ament_add_gtest(test_nodehybrid
+  test_nodehybrid.cpp
+)
+ament_target_dependencies(test_nodehybrid
+  ${dependencies}
+)
+target_link_libraries(test_nodehybrid
+  ${library_name}
+)
+
+# Test NodeBasic
+ament_add_gtest(test_nodebasic
+  test_nodebasic.cpp
+)
+ament_target_dependencies(test_nodebasic
+  ${dependencies}
+)
+target_link_libraries(test_nodebasic
+  ${library_name}
+)
+
+# Test collision checker
+ament_add_gtest(test_collision_checker
+  test_collision_checker.cpp
+)
+ament_target_dependencies(test_collision_checker
+  ${dependencies}
+)
+target_link_libraries(test_collision_checker
+  ${library_name}
+)
+
+# Test A*
+ament_add_gtest(test_a_star
+  test_a_star.cpp
+)
+ament_target_dependencies(test_a_star
+  ${dependencies}
+)
+target_link_libraries(test_a_star
+  ${library_name}
+)
+
+# Test SMAC Hybrid
+ament_add_gtest(test_smac_hybrid
+  test_smac_hybrid.cpp
+)
+ament_target_dependencies(test_smac_hybrid
+  ${dependencies}
+)
+target_link_libraries(test_smac_hybrid
+  ${library_name}
+)
+
+# Test SMAC 2D
+ament_add_gtest(test_smac_2d
+  test_smac_2d.cpp
+)
+ament_target_dependencies(test_smac_2d
+  ${dependencies}
+)
+target_link_libraries(test_smac_2d
+  ${library_name}_2d
+)
+
+# Test SMAC lattice
+ament_add_gtest(test_smac_lattice
+  test_smac_lattice.cpp
+)
+ament_target_dependencies(test_smac_lattice
+  ${dependencies}
+)
+target_link_libraries(test_smac_lattice
+  ${library_name}_lattice
+)
+
+# Test SMAC Smoother
+ament_add_gtest(test_smoother
+  test_smoother.cpp
+)
+ament_target_dependencies(test_smoother
+  ${dependencies}
+)
+target_link_libraries(test_smoother
+  ${library_name}_lattice
+  ${library_name}
+  ${library_name}_2d
+)
+
+#Test Lattice node
+ament_add_gtest(test_lattice_node test_nodelattice.cpp)
+
+ament_target_dependencies(test_lattice_node ${dependencies})
+
+target_link_libraries(test_lattice_node ${library_name})

navigation2/nav2_smac_planner/test/deprecated/options.hpp

@@ -0,0 +1,207 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef DEPRECATED__OPTIONS_HPP_
+#define DEPRECATED__OPTIONS_HPP_
+
+#include <string>
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_smac_planner
+{
+
+/**
+ * @struct nav2_smac_planner::SmootherParams
+ * @brief Parameters for the smoother cost function
+ */
+struct SmootherParams
+{
+  /**
+   * @brief A constructor for nav2_smac_planner::SmootherParams
+   */
+  SmootherParams()
+  {
+  }
+
+  /**
+   * @brief Get params from ROS parameter
+   * @param node_ Ptr to node
+   * @param name Name of plugin
+   */
+  void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name)
+  {
+    std::string local_name = name + std::string(".smoother.smoother.");
+
+    // Smoother params
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "w_curve", rclcpp::ParameterValue(1.5));
+    node->get_parameter(local_name + "w_curve", curvature_weight);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "w_cost", rclcpp::ParameterValue(0.0));
+    node->get_parameter(local_name + "w_cost", costmap_weight);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "w_dist", rclcpp::ParameterValue(0.0));
+    node->get_parameter(local_name + "w_dist", distance_weight);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "w_smooth", rclcpp::ParameterValue(15000.0));
+    node->get_parameter(local_name + "w_smooth", smooth_weight);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "cost_scaling_factor", rclcpp::ParameterValue(10.0));
+    node->get_parameter(local_name + "cost_scaling_factor", costmap_factor);
+  }
+
+  double smooth_weight{0.0};
+  double costmap_weight{0.0};
+  double distance_weight{0.0};
+  double curvature_weight{0.0};
+  double max_curvature{0.0};
+  double costmap_factor{0.0};
+  double max_time;
+};
+
+/**
+ * @struct nav2_smac_planner::OptimizerParams
+ * @brief Parameters for the ceres optimizer
+ */
+struct OptimizerParams
+{
+  OptimizerParams()
+  : debug(false),
+    max_iterations(50),
+    max_time(1e4),
+    param_tol(1e-8),
+    fn_tol(1e-6),
+    gradient_tol(1e-10)
+  {
+  }
+
+  /**
+   * @struct AdvancedParams
+   * @brief Advanced parameters for the ceres optimizer
+   */
+  struct AdvancedParams
+  {
+    AdvancedParams()
+    : min_line_search_step_size(1e-9),
+      max_num_line_search_step_size_iterations(20),
+      line_search_sufficient_function_decrease(1e-4),
+      max_num_line_search_direction_restarts(20),
+      max_line_search_step_contraction(1e-3),
+      min_line_search_step_contraction(0.6),
+      line_search_sufficient_curvature_decrease(0.9),
+      max_line_search_step_expansion(10)
+    {
+    }
+
+    /**
+     * @brief Get advanced params from ROS parameter
+     * @param node_ Ptr to node
+     * @param name Name of plugin
+     */
+    void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name)
+    {
+      std::string local_name = name + std::string(".smoother.optimizer.advanced.");
+
+      // Optimizer advanced params
+      nav2_util::declare_parameter_if_not_declared(
+        node, local_name + "min_line_search_step_size",
+        rclcpp::ParameterValue(1e-20));
+      node->get_parameter(
+        local_name + "min_line_search_step_size",
+        min_line_search_step_size);
+      nav2_util::declare_parameter_if_not_declared(
+        node, local_name + "max_num_line_search_step_size_iterations",
+        rclcpp::ParameterValue(50));
+      node->get_parameter(
+        local_name + "max_num_line_search_step_size_iterations",
+        max_num_line_search_step_size_iterations);
+      nav2_util::declare_parameter_if_not_declared(
+        node, local_name + "line_search_sufficient_function_decrease",
+        rclcpp::ParameterValue(1e-20));
+      node->get_parameter(
+        local_name + "line_search_sufficient_function_decrease",
+        line_search_sufficient_function_decrease);
+      nav2_util::declare_parameter_if_not_declared(
+        node, local_name + "max_num_line_search_direction_restarts",
+        rclcpp::ParameterValue(10));
+      node->get_parameter(
+        local_name + "max_num_line_search_direction_restarts",
+        max_num_line_search_direction_restarts);
+      nav2_util::declare_parameter_if_not_declared(
+        node, local_name + "max_line_search_step_expansion",
+        rclcpp::ParameterValue(50));
+      node->get_parameter(
+        local_name + "max_line_search_step_expansion",
+        max_line_search_step_expansion);
+    }
+
+
+    double min_line_search_step_size;  // Ceres default: 1e-9
+    int max_num_line_search_step_size_iterations;  // Ceres default: 20
+    double line_search_sufficient_function_decrease;  // Ceres default: 1e-4
+    int max_num_line_search_direction_restarts;  // Ceres default: 5
+
+    double max_line_search_step_contraction;  // Ceres default: 1e-3
+    double min_line_search_step_contraction;  // Ceres default: 0.6
+    double line_search_sufficient_curvature_decrease;  // Ceres default: 0.9
+    int max_line_search_step_expansion;  // Ceres default: 10
+  };
+
+  /**
+   * @brief Get params from ROS parameter
+   * @param node_ Ptr to node
+   * @param name Name of plugin
+   */
+  void get(rclcpp_lifecycle::LifecycleNode * node, const std::string & name)
+  {
+    std::string local_name = name + std::string(".smoother.optimizer.");
+
+    // Optimizer params
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "param_tol", rclcpp::ParameterValue(1e-15));
+    node->get_parameter(local_name + "param_tol", param_tol);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "fn_tol", rclcpp::ParameterValue(1e-7));
+    node->get_parameter(local_name + "fn_tol", fn_tol);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "gradient_tol", rclcpp::ParameterValue(1e-10));
+    node->get_parameter(local_name + "gradient_tol", gradient_tol);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "max_iterations", rclcpp::ParameterValue(500));
+    node->get_parameter(local_name + "max_iterations", max_iterations);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "max_time", rclcpp::ParameterValue(0.100));
+    node->get_parameter(local_name + "max_time", max_time);
+    nav2_util::declare_parameter_if_not_declared(
+      node, local_name + "debug_optimizer", rclcpp::ParameterValue(false));
+    node->get_parameter(local_name + "debug_optimizer", debug);
+
+    advanced.get(node, name);
+  }
+
+  bool debug;
+  int max_iterations;  // Ceres default: 50
+  double max_time;  // Ceres default: 1e4
+
+  double param_tol;  // Ceres default: 1e-8
+  double fn_tol;  // Ceres default: 1e-6
+  double gradient_tol;  // Ceres default: 1e-10
+
+  AdvancedParams advanced;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // DEPRECATED__OPTIONS_HPP_

navigation2/nav2_smac_planner/test/deprecated/smoother.hpp

@@ -0,0 +1,146 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef DEPRECATED__SMOOTHER_HPP_
+#define DEPRECATED__SMOOTHER_HPP_
+
+#include <cmath>
+#include <vector>
+#include <iostream>
+#include <memory>
+#include <queue>
+#include <utility>
+
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/smoother_cost_function.hpp"
+
+#include "ceres/ceres.h"
+#include "Eigen/Core"
+
+namespace nav2_smac_planner
+{
+
+/**
+ * @class nav2_smac_planner::Smoother
+ * @brief A Conjugate Gradient 2D path smoother implementation
+ */
+class Smoother
+{
+public:
+  /**
+   * @brief A constructor for nav2_smac_planner::Smoother
+   */
+  Smoother() {}
+
+  /**
+   * @brief A destructor for nav2_smac_planner::Smoother
+   */
+  ~Smoother() {}
+
+  /**
+   * @brief Initialization of the smoother
+   * @param params OptimizerParam struct
+   */
+  void initialize(const OptimizerParams params)
+  {
+    _debug = params.debug;
+
+    // General Params
+
+    // 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT
+    _options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT;
+    _options.line_search_type = ceres::WOLFE;
+    _options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE;
+    _options.line_search_interpolation_type = ceres::CUBIC;
+
+    _options.max_num_iterations = params.max_iterations;
+    _options.max_solver_time_in_seconds = params.max_time;
+
+    _options.function_tolerance = params.fn_tol;
+    _options.gradient_tolerance = params.gradient_tol;
+    _options.parameter_tolerance = params.param_tol;
+
+    _options.min_line_search_step_size = params.advanced.min_line_search_step_size;
+    _options.max_num_line_search_step_size_iterations =
+      params.advanced.max_num_line_search_step_size_iterations;
+    _options.line_search_sufficient_function_decrease =
+      params.advanced.line_search_sufficient_function_decrease;
+    _options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction;
+    _options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction;
+    _options.max_num_line_search_direction_restarts =
+      params.advanced.max_num_line_search_direction_restarts;
+    _options.line_search_sufficient_curvature_decrease =
+      params.advanced.line_search_sufficient_curvature_decrease;
+    _options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion;
+
+    if (_debug) {
+      _options.minimizer_progress_to_stdout = true;
+    } else {
+      _options.logging_type = ceres::SILENT;
+    }
+  }
+
+  /**
+   * @brief Smoother method
+   * @param path Reference to path
+   * @param costmap Pointer to minimal costmap
+   * @param smoother parameters weights
+   * @return If smoothing was successful
+   */
+  bool smooth(
+    std::vector<Eigen::Vector2d> & path,
+    nav2_costmap_2d::Costmap2D * costmap,
+    const SmootherParams & params)
+  {
+    _options.max_solver_time_in_seconds = params.max_time;
+
+#ifdef _MSC_VER
+    std::vector<double> parameters_vec(path.size() * 2);
+    double * parameters = parameters_vec.data();
+#else
+    double parameters[path.size() * 2];    // NOLINT
+#endif
+    for (unsigned int i = 0; i != path.size(); i++) {
+      parameters[2 * i] = path[i][0];
+      parameters[2 * i + 1] = path[i][1];
+    }
+
+    ceres::GradientProblemSolver::Summary summary;
+    ceres::GradientProblem problem(new UnconstrainedSmootherCostFunction(&path, costmap, params));
+    ceres::Solve(_options, problem, parameters, &summary);
+
+    if (_debug) {
+      std::cout << summary.FullReport() << '\n';
+    }
+
+    if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) {
+      return false;
+    }
+
+    for (unsigned int i = 0; i != path.size(); i++) {
+      path[i][0] = parameters[2 * i];
+      path[i][1] = parameters[2 * i + 1];
+    }
+
+    return true;
+  }
+
+private:
+  bool _debug;
+  ceres::GradientProblemSolver::Options _options;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // DEPRECATED__SMOOTHER_HPP_

navigation2/nav2_smac_planner/test/deprecated/smoother_cost_function.hpp

@@ -0,0 +1,542 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_
+#define DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_
+
+#include <cmath>
+#include <vector>
+#include <iostream>
+#include <unordered_map>
+#include <memory>
+#include <queue>
+#include <utility>
+
+#include "ceres/ceres.h"
+#include "Eigen/Core"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_smac_planner/options.hpp"
+
+#define EPSILON 0.0001
+
+namespace nav2_smac_planner
+{
+
+/**
+ * @struct nav2_smac_planner::UnconstrainedSmootherCostFunction
+ * @brief Cost function for path smoothing with multiple terms
+ * including curvature, smoothness, collision, and avoid obstacles.
+ */
+class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction
+{
+public:
+  /**
+   * @brief A constructor for nav2_smac_planner::UnconstrainedSmootherCostFunction
+   * @param original_path Original unsmoothed path to smooth
+   * @param costmap A costmap to get values for collision and obstacle avoidance
+   */
+  UnconstrainedSmootherCostFunction(
+    std::vector<Eigen::Vector2d> * original_path,
+    nav2_costmap_2d::Costmap2D * costmap,
+    const SmootherParams & params)
+  : _original_path(original_path),
+    _num_params(2 * original_path->size()),
+    _costmap(costmap),
+    _params(params)
+  {
+    // int height = costmap->getSizeInCellsX();
+    // int width = costmap->getSizeInCellsY();
+    // bool** binMap;
+    // binMap = new bool*[width];
+
+    // for (int x = 0; x < width; x++) { binMap[x] = new bool[height]; }
+
+    // for (int x = 0; x < width; ++x) {
+    //   for (int y = 0; y < height; ++y) {
+    //     binMap[x][y] = costmap->getCost(x,y) >= 253 ? true : false;
+    //   }
+    // }
+
+    // voronoiDiagram.initializeMap(width, height, binMap);
+    // voronoiDiagram.update();
+    // voronoiDiagram.visualize();
+  }
+
+  /**
+   * @struct CurvatureComputations
+   * @brief Cache common computations between the curvature terms to minimize recomputations
+   */
+  struct CurvatureComputations
+  {
+    /**
+     * @brief A constructor for nav2_smac_planner::CurvatureComputations
+     */
+    CurvatureComputations()
+    {
+      valid = true;
+    }
+
+    bool valid;
+    /**
+     * @brief Check if result is valid for penalty
+     * @return is valid (non-nan, non-inf, and turning angle > max)
+     */
+    bool isValid()
+    {
+      return valid;
+    }
+
+    Eigen::Vector2d delta_xi{0.0, 0.0};
+    Eigen::Vector2d delta_xi_p{0.0, 0.0};
+    double delta_xi_norm{0};
+    double delta_xi_p_norm{0};
+    double delta_phi_i{0};
+    double turning_rad{0};
+    double ki_minus_kmax{0};
+  };
+
+  /**
+   * @brief Smoother cost function evaluation
+   * @param parameters X,Y pairs of points
+   * @param cost total cost of path
+   * @param gradient of path at each X,Y pair from cost function derived analytically
+   * @return if successful in computing values
+   */
+  virtual bool Evaluate(
+    const double * parameters,
+    double * cost,
+    double * gradient) const
+  {
+    Eigen::Vector2d xi;
+    Eigen::Vector2d xi_p1;
+    Eigen::Vector2d xi_m1;
+    unsigned int x_index, y_index;
+    cost[0] = 0.0;
+    double cost_raw = 0.0;
+    double grad_x_raw = 0.0;
+    double grad_y_raw = 0.0;
+    unsigned int mx, my;
+    bool valid_coords = true;
+    double costmap_cost = 0.0;
+
+    // cache some computations between the residual and jacobian
+    CurvatureComputations curvature_params;
+
+    for (int i = 0; i != NumParameters() / 2; i++) {
+      x_index = 2 * i;
+      y_index = 2 * i + 1;
+      gradient[x_index] = 0.0;
+      gradient[y_index] = 0.0;
+      if (i < 1 || i >= NumParameters() / 2 - 1) {
+        continue;
+      }
+
+      xi = Eigen::Vector2d(parameters[x_index], parameters[y_index]);
+      xi_p1 = Eigen::Vector2d(parameters[x_index + 2], parameters[y_index + 2]);
+      xi_m1 = Eigen::Vector2d(parameters[x_index - 2], parameters[y_index - 2]);
+
+      // compute cost
+      addSmoothingResidual(_params.smooth_weight, xi, xi_p1, xi_m1, cost_raw);
+      addCurvatureResidual(_params.curvature_weight, xi, xi_p1, xi_m1, curvature_params, cost_raw);
+      addDistanceResidual(_params.distance_weight, xi, _original_path->at(i), cost_raw);
+
+      if (valid_coords = _costmap->worldToMap(xi[0], xi[1], mx, my)) {
+        costmap_cost = _costmap->getCost(mx, my);
+        addCostResidual(_params.costmap_weight, costmap_cost, cost_raw, xi);
+      }
+
+      if (gradient != NULL) {
+        // compute gradient
+        gradient[x_index] = 0.0;
+        gradient[y_index] = 0.0;
+        addSmoothingJacobian(_params.smooth_weight, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw);
+        addCurvatureJacobian(
+          _params.curvature_weight, xi, xi_p1, xi_m1, curvature_params,
+          grad_x_raw, grad_y_raw);
+        addDistanceJacobian(
+          _params.distance_weight, xi, _original_path->at(
+            i), grad_x_raw, grad_y_raw);
+
+        if (valid_coords) {
+          addCostJacobian(_params.costmap_weight, mx, my, costmap_cost, grad_x_raw, grad_y_raw);
+        }
+
+        gradient[x_index] = grad_x_raw;
+        gradient[y_index] = grad_y_raw;
+      }
+    }
+
+    cost[0] = cost_raw;
+
+    return true;
+  }
+
+  /**
+   * @brief Get number of parameter blocks
+   * @return Number of parameters in cost function
+   */
+  virtual int NumParameters() const {return _num_params;}
+
+protected:
+  /**
+   * @brief Cost function term for smooth paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param r Residual (cost) of term
+   */
+  inline void addSmoothingResidual(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    double & r) const
+  {
+    r += weight * (
+      pt_p.dot(pt_p) -
+      4 * pt_p.dot(pt) +
+      2 * pt_p.dot(pt_m) +
+      4 * pt.dot(pt) -
+      4 * pt.dot(pt_m) +
+      pt_m.dot(pt_m));    // objective function value
+  }
+
+  /**
+   * @brief Cost function derivative term for smooth paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param j0 Gradient of X term
+   * @param j1 Gradient of Y term
+   */
+  inline void addSmoothingJacobian(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    double & j0,
+    double & j1) const
+  {
+    j0 += weight *
+      (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]);   // xi x component of partial-derivative
+    j1 += weight *
+      (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]);   // xi y component of partial-derivative
+  }
+
+  /**
+   * @brief Cost function term for maximum curved paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param curvature_params A struct to cache computations for the jacobian to use
+   * @param r Residual (cost) of term
+   */
+  inline void addCurvatureResidual(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    CurvatureComputations & curvature_params,
+    double & r) const
+  {
+    curvature_params.valid = true;
+    curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]);
+    curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]);
+    curvature_params.delta_xi_norm = curvature_params.delta_xi.norm();
+    curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm();
+
+    if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
+      std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) ||
+      std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm))
+    {
+      // ensure we have non-nan values returned
+      curvature_params.valid = false;
+      return;
+    }
+
+    const double & delta_xi_by_xi_p =
+      curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
+    double projection =
+      curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p;
+    if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) {
+      projection = 1.0;
+    }
+
+    curvature_params.delta_phi_i = std::acos(projection);
+    curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
+
+    curvature_params.ki_minus_kmax = curvature_params.turning_rad - _params.max_curvature;
+
+    if (curvature_params.ki_minus_kmax <= EPSILON) {
+      // Quadratic penalty need not apply
+      curvature_params.valid = false;
+      return;
+    }
+
+    r += weight *
+      curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax;  // objective function value
+  }
+
+  /**
+   * @brief Cost function derivative term for maximum curvature paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param curvature_params A struct with cached values to speed up Jacobian computation
+   * @param j0 Gradient of X term
+   * @param j1 Gradient of Y term
+   */
+  inline void addCurvatureJacobian(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & /*pt_m*/,
+    CurvatureComputations & curvature_params,
+    double & j0,
+    double & j1) const
+  {
+    if (!curvature_params.isValid()) {
+      return;
+    }
+
+    const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
+      -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2));
+    // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0);
+    auto neg_pt_plus = -1 * pt_p;
+    Eigen::Vector2d p1 = normalizedOrthogonalComplement(
+      pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm);
+    Eigen::Vector2d p2 = normalizedOrthogonalComplement(
+      neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm);
+
+    const double & u = 2 * curvature_params.ki_minus_kmax;
+    const double & common_prefix =
+      (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i;
+    const double & common_suffix = curvature_params.delta_phi_i /
+      (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm);
+
+    const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
+      curvature_params.delta_xi_norm;
+
+    const Eigen::Vector2d jacobian = u *
+      (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi));
+    const Eigen::Vector2d jacobian_im1 = u *
+      (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi));
+    const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1);
+
+    // Old formulation we may require again.
+    // j0 += weight *
+    //   (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]);
+    // j1 += weight *
+    //   (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]);
+
+    j0 += weight * jacobian[0];  // xi x component of partial-derivative
+    j1 += weight * jacobian[1];  // xi x component of partial-derivative
+  }
+
+  /**
+   * @brief Cost function derivative term for steering away changes in pose
+   * @param weight Weight to apply to function
+   * @param xi Point Xi for evaluation
+   * @param xi_original original point Xi for evaluation
+   * @param r Residual (cost) of term
+   */
+  inline void addDistanceResidual(
+    const double & weight,
+    const Eigen::Vector2d & xi,
+    const Eigen::Vector2d & xi_original,
+    double & r) const
+  {
+    r += weight * (xi - xi_original).dot(xi - xi_original);  // objective function value
+  }
+
+  /**
+   * @brief Cost function derivative term for steering away changes in pose
+   * @param weight Weight to apply to function
+   * @param xi Point Xi for evaluation
+   * @param xi_original original point Xi for evaluation
+   * @param j0 Gradient of X term
+   * @param j1 Gradient of Y term
+   */
+  inline void addDistanceJacobian(
+    const double & weight,
+    const Eigen::Vector2d & xi,
+    const Eigen::Vector2d & xi_original,
+    double & j0,
+    double & j1) const
+  {
+    j0 += weight * 2 * (xi[0] - xi_original[0]);  // xi y component of partial-derivative
+    j1 += weight * 2 * (xi[1] - xi_original[1]);  // xi y component of partial-derivative
+  }
+
+
+  /**
+   * @brief Cost function term for steering away from costs
+   * @param weight Weight to apply to function
+   * @param value Point Xi's cost'
+   * @param params computed values to reduce overhead
+   * @param r Residual (cost) of term
+   */
+  inline void addCostResidual(
+    const double & weight,
+    const double & value,
+    double & r,
+    Eigen::Vector2d & xi) const
+  {
+    if (value == FREE) {
+      return;
+    }
+
+    r += weight * value * value;  // objective function value
+
+
+    // float obsDst = voronoiDiagram.getDistance((int)xi[0], (int)xi[1]);
+
+    // if (abs(obsDst) > 0.3) {
+    //   return;
+    // }
+
+    // r += weight * (abs(obsDst) - 0.3) * (abs(obsDst) - 0.3);
+  }
+
+  /**
+   * @brief Cost function derivative term for steering away from costs
+   * @param weight Weight to apply to function
+   * @param mx Point Xi's x coordinate in map frame
+   * @param mx Point Xi's y coordinate in map frame
+   * @param value Point Xi's cost'
+   * @param params computed values to reduce overhead
+   * @param j0 Gradient of X term
+   * @param j1 Gradient of Y term
+   */
+  inline void addCostJacobian(
+    const double & weight,
+    const unsigned int & mx,
+    const unsigned int & my,
+    const double & value,
+    double & j0,
+    double & j1) const
+  {
+    if (value == FREE) {
+      return;
+    }
+
+    const Eigen::Vector2d grad = getCostmapGradient(mx, my);
+    const double common_prefix = -2.0 * _params.costmap_factor * weight * value * value;
+
+    j0 += common_prefix * grad[0];  // xi x component of partial-derivative
+    j1 += common_prefix * grad[1];  // xi y component of partial-derivative
+  }
+
+  /**
+   * @brief Computing the gradient of the costmap using
+   * the 2 point numerical differentiation method
+   * @param mx Point Xi's x coordinate in map frame
+   * @param mx Point Xi's y coordinate in map frame
+   * @param params Params reference to store gradients
+   */
+  inline Eigen::Vector2d getCostmapGradient(
+    const unsigned int mx,
+    const unsigned int my) const
+  {
+    // find unit vector that describes that direction
+    // via 7 point taylor series approximation for gradient at Xi
+    Eigen::Vector2d gradient;
+
+    double l_1 = 0.0;
+    double l_2 = 0.0;
+    double l_3 = 0.0;
+    double r_1 = 0.0;
+    double r_2 = 0.0;
+    double r_3 = 0.0;
+
+    if (mx < _costmap->getSizeInCellsX()) {
+      r_1 = static_cast<double>(_costmap->getCost(mx + 1, my));
+    }
+    if (mx + 1 < _costmap->getSizeInCellsX()) {
+      r_2 = static_cast<double>(_costmap->getCost(mx + 2, my));
+    }
+    if (mx + 2 < _costmap->getSizeInCellsX()) {
+      r_3 = static_cast<double>(_costmap->getCost(mx + 3, my));
+    }
+
+    if (mx > 0) {
+      l_1 = static_cast<double>(_costmap->getCost(mx - 1, my));
+    }
+    if (mx - 1 > 0) {
+      l_2 = static_cast<double>(_costmap->getCost(mx - 2, my));
+    }
+    if (mx - 2 > 0) {
+      l_3 = static_cast<double>(_costmap->getCost(mx - 3, my));
+    }
+
+    gradient[1] = (45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3) / 60;
+
+    if (my < _costmap->getSizeInCellsY()) {
+      r_1 = static_cast<double>(_costmap->getCost(mx, my + 1));
+    }
+    if (my + 1 < _costmap->getSizeInCellsY()) {
+      r_2 = static_cast<double>(_costmap->getCost(mx, my + 2));
+    }
+    if (my + 2 < _costmap->getSizeInCellsY()) {
+      r_3 = static_cast<double>(_costmap->getCost(mx, my + 3));
+    }
+
+    if (my > 0) {
+      l_1 = static_cast<double>(_costmap->getCost(mx, my - 1));
+    }
+    if (my - 1 > 0) {
+      l_2 = static_cast<double>(_costmap->getCost(mx, my - 2));
+    }
+    if (my - 2 > 0) {
+      l_3 = static_cast<double>(_costmap->getCost(mx, my - 3));
+    }
+
+    gradient[0] = (45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3) / 60;
+
+    gradient.normalize();
+    return gradient;
+  }
+
+  /**
+   * @brief Computing the normalized orthogonal component of 2 vectors
+   * @param a Vector
+   * @param b Vector
+   * @param norm a Vector's norm
+   * @param norm b Vector's norm
+   * @return Normalized vector of orthogonal components
+   */
+  inline Eigen::Vector2d normalizedOrthogonalComplement(
+    const Eigen::Vector2d & a,
+    const Eigen::Vector2d & b,
+    const double & a_norm,
+    const double & b_norm) const
+  {
+    return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm);
+  }
+
+  std::vector<Eigen::Vector2d> * _original_path{nullptr};
+  int _num_params;
+  nav2_costmap_2d::Costmap2D * _costmap{nullptr};
+  SmootherParams _params;
+  // DynamicVoronoi voronoiDiagram;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_

navigation2/nav2_smac_planner/test/deprecated/upsampler.hpp

@@ -0,0 +1,213 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef DEPRECATED__UPSAMPLER_HPP_
+#define DEPRECATED__UPSAMPLER_HPP_
+
+#include <cmath>
+#include <vector>
+#include <iostream>
+#include <memory>
+#include <queue>
+#include <algorithm>
+#include <utility>
+
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/upsampler_cost_function.hpp"
+#include "nav2_smac_planner/upsampler_cost_function_nlls.hpp"
+
+#include "ceres/ceres.h"
+#include "Eigen/Core"
+
+namespace nav2_smac_planner
+{
+
+/**
+ * @class nav2_smac_planner::Upsampler
+ * @brief A Conjugate Gradient 2D path upsampler implementation
+ */
+class Upsampler
+{
+public:
+  /**
+   * @brief A constructor for nav2_smac_planner::Upsampler
+   */
+  Upsampler() {}
+
+  /**
+   * @brief A destructor for nav2_smac_planner::Upsampler
+   */
+  ~Upsampler() {}
+
+  /**
+   * @brief Initialization of the Upsampler
+   */
+  void initialize(const OptimizerParams params)
+  {
+    _debug = params.debug;
+
+    // General Params
+
+    // 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT
+    _options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT;
+    _options.line_search_type = ceres::WOLFE;
+    _options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE;
+    _options.line_search_interpolation_type = ceres::CUBIC;
+
+    _options.max_num_iterations = params.max_iterations;  // 5000
+    _options.max_solver_time_in_seconds = params.max_time;  // 5.0; // TODO
+
+    _options.function_tolerance = params.fn_tol;
+    _options.gradient_tolerance = params.gradient_tol;
+    _options.parameter_tolerance = params.param_tol;  // 1e-20;
+
+    _options.min_line_search_step_size = params.advanced.min_line_search_step_size;  // 1e-30;
+    _options.max_num_line_search_step_size_iterations =
+      params.advanced.max_num_line_search_step_size_iterations;
+    _options.line_search_sufficient_function_decrease =
+      params.advanced.line_search_sufficient_function_decrease;  // 1e-30;
+    _options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction;
+    _options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction;
+    _options.max_num_line_search_direction_restarts =
+      params.advanced.max_num_line_search_direction_restarts;
+    _options.line_search_sufficient_curvature_decrease =
+      params.advanced.line_search_sufficient_curvature_decrease;
+    _options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion;
+
+    if (_debug) {
+      _options.minimizer_progress_to_stdout = true;
+    } else {
+      _options.logging_type = ceres::SILENT;
+    }
+  }
+
+  /**
+   * @brief Upsampling method
+   * @param path Reference to path
+   * @param upsample parameters weights
+   * @param upsample_ratio upsample ratio
+   * @return If Upsampler was successful
+   */
+  bool upsample(
+    std::vector<Eigen::Vector2d> & path,
+    const SmootherParams & params,
+    const int & upsample_ratio)
+  {
+    _options.max_solver_time_in_seconds = params.max_time;
+
+    if (upsample_ratio != 2 && upsample_ratio != 4) {
+      // invalid inputs
+      return false;
+    }
+
+    const int param_ratio = upsample_ratio * 2.0;
+    const int total_size = 2 * (path.size() * upsample_ratio - upsample_ratio + 1);
+    double parameters[total_size];  // NOLINT
+
+    // 20-4hz regularly, but dosnt work in faster cases
+    // Linearly distribute initial poses for optimization
+    // TODO(stevemacenski) generalize for 2x and 4x
+    unsigned int next_pt;
+    Eigen::Vector2d interpolated;
+    std::vector<Eigen::Vector2d> temp_path;
+    for (unsigned int pt = 0; pt != path.size() - 1; pt++) {
+      next_pt = pt + 1;
+      interpolated = (path[next_pt] + path[pt]) / 2.0;
+
+      parameters[param_ratio * pt] = path[pt][0];
+      parameters[param_ratio * pt + 1] = path[pt][1];
+      temp_path.push_back(path[pt]);
+
+      parameters[param_ratio * pt + 2] = interpolated[0];
+      parameters[param_ratio * pt + 3] = interpolated[1];
+      temp_path.push_back(interpolated);
+    }
+
+    parameters[total_size - 2] = path.back()[0];
+    parameters[total_size - 1] = path.back()[1];
+    temp_path.push_back(path.back());
+
+    // Solve the upsampling problem
+    ceres::GradientProblemSolver::Summary summary;
+    ceres::GradientProblem problem(new UpsamplerCostFunction(temp_path, params, upsample_ratio));
+    ceres::Solve(_options, problem, parameters, &summary);
+
+
+    path.resize(total_size / 2);
+    for (int i = 0; i != total_size / 2; i++) {
+      path[i][0] = parameters[2 * i];
+      path[i][1] = parameters[2 * i + 1];
+    }
+
+    // 10-15 hz, regularly
+    // std::vector<Eigen::Vector2d> path_double_sampled;
+    // for (int i = 0; i != path.size() - 1; i++) {  // last term should not be upsampled
+    //   path_double_sampled.push_back(path[i]);
+    //   path_double_sampled.push_back((path[i+1] + path[i]) / 2);
+    // }
+
+    // std::unique_ptr<ceres::Problem> problem = std::make_unique<ceres::Problem>();
+    // for (uint i = 1; i != path_double_sampled.size() - 1; i++) {
+    //   ceres::CostFunction * cost_fn =
+    //     new UpsamplerConstrainedCostFunction(path_double_sampled, params, 2, i);
+    //   problem->AddResidualBlock(
+    //     cost_fn, nullptr, &path_double_sampled[i][0], &path_double_sampled[i][1]);
+    //   // locking initial coordinates unnecessary since there's no update between terms in NLLS
+    // }
+
+    // ceres::Solver::Summary summary;
+    // _options.minimizer_type = ceres::LINE_SEARCH;
+    // ceres::Solve(_options, problem.get(), &summary);
+
+    // if (upsample_ratio == 4) {
+    //   std::vector<Eigen::Vector2d> path_quad_sampled;
+    //   for (int i = 0; i != path_double_sampled.size() - 1; i++) {
+    //     path_quad_sampled.push_back(path_double_sampled[i]);
+    //     path_quad_sampled.push_back((path_double_sampled[i+1] + path_double_sampled[i]) / 2.0);
+    //   }
+
+    //   std::unique_ptr<ceres::Problem> problem2 = std::make_unique<ceres::Problem>();
+    //   for (uint i = 1; i != path_quad_sampled.size() - 1; i++) {
+    //     ceres::CostFunction * cost_fn =
+    //       new UpsamplerConstrainedCostFunction(path_quad_sampled, params, 4, i);
+    //     problem2->AddResidualBlock(
+    //       cost_fn, nullptr, &path_quad_sampled[i][0], &path_quad_sampled[i][1]);
+    //   }
+
+    //   ceres::Solve(_options, problem2.get(), &summary);
+
+    //   path = path_quad_sampled;
+    // } else {
+    //   path = path_double_sampled;
+    // }
+
+    if (_debug) {
+      std::cout << summary.FullReport() << '\n';
+    }
+
+    if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) {
+      return false;
+    }
+
+    return true;
+  }
+
+private:
+  bool _debug;
+  ceres::GradientProblemSolver::Options _options;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // DEPRECATED__UPSAMPLER_HPP_

navigation2/nav2_smac_planner/test/deprecated/upsampler_cost_function.hpp

@@ -0,0 +1,366 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_
+#define DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_
+
+#include <cmath>
+#include <vector>
+#include <iostream>
+#include <unordered_map>
+#include <memory>
+#include <queue>
+#include <utility>
+
+#include "ceres/ceres.h"
+#include "Eigen/Core"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/options.hpp"
+
+#define EPSILON 0.0001
+
+namespace nav2_smac_planner
+{
+/**
+ * @struct nav2_smac_planner::UpsamplerCostFunction
+ * @brief Cost function for path upsampling with multiple terms using unconstrained
+ * optimization including curvature, smoothness, collision, and avoid obstacles.
+ */
+class UpsamplerCostFunction : public ceres::FirstOrderFunction
+{
+public:
+  /**
+   * @brief A constructor for nav2_smac_planner::UpsamplerCostFunction
+   * @param num_points Number of path points to consider
+   */
+  UpsamplerCostFunction(
+    const std::vector<Eigen::Vector2d> & path,
+    const SmootherParams & params,
+    const int & upsample_ratio)
+  : _num_params(2 * path.size()),
+    _params(params),
+    _upsample_ratio(upsample_ratio),
+    _path(path)
+  {
+  }
+  // TODO(stevemacenski) removed upsample_ratio because temp upsampling on path size
+
+  /**
+   * @struct CurvatureComputations
+   * @brief Cache common computations between the curvature terms to minimize recomputations
+   */
+  struct CurvatureComputations
+  {
+    /**
+     * @brief A constructor for nav2_smac_planner::CurvatureComputations
+     */
+    CurvatureComputations()
+    {
+      valid = false;
+    }
+
+    bool valid;
+    /**
+     * @brief Check if result is valid for penalty
+     * @return is valid (non-nan, non-inf, and turning angle > max)
+     */
+    bool isValid()
+    {
+      return valid;
+    }
+
+    Eigen::Vector2d delta_xi{0, 0};
+    Eigen::Vector2d delta_xi_p{0, 0};
+    double delta_xi_norm{0};
+    double delta_xi_p_norm{0};
+    double delta_phi_i{0};
+    double turning_rad{0};
+    double ki_minus_kmax{0};
+  };
+
+  /**
+   * @brief Smoother cost function evaluation
+   * @param parameters X,Y pairs of points
+   * @param cost total cost of path
+   * @param gradient of path at each X,Y pair from cost function derived analytically
+   * @return if successful in computing values
+   */
+  virtual bool Evaluate(
+    const double * parameters,
+    double * cost,
+    double * gradient) const
+  {
+    Eigen::Vector2d xi;
+    Eigen::Vector2d xi_p1;
+    Eigen::Vector2d xi_m1;
+    uint x_index, y_index;
+    cost[0] = 0.0;
+    double cost_raw = 0.0;
+    double grad_x_raw = 0.0;
+    double grad_y_raw = 0.0;
+
+    // cache some computations between the residual and jacobian
+    CurvatureComputations curvature_params;
+
+    for (int i = 0; i != NumParameters() / 2; i++) {
+      x_index = 2 * i;
+      y_index = 2 * i + 1;
+      gradient[x_index] = 0.0;
+      gradient[y_index] = 0.0;
+      if (i < 1 || i >= NumParameters() / 2 - 1) {
+        continue;
+      }
+
+      // if original point's neighbors TODO
+      if (i % _upsample_ratio == 1) {
+        continue;
+      }
+
+      xi = Eigen::Vector2d(parameters[x_index], parameters[y_index]);
+
+      // TODO(stevemacenski): from deep copy to make sure no feedback _path
+      xi_p1 = _path.at(i + 1);
+      xi_m1 = _path.at(i - 1);
+      // xi_p1 = Eigen::Vector2d(parameters[x_index + 2], parameters[y_index + 2]);
+      // xi_m1 = Eigen::Vector2d(parameters[x_index - 2], parameters[y_index - 2]);
+
+      // compute cost
+      addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw);
+      addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw);
+
+      if (gradient != NULL) {
+        // compute gradient
+        addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw);
+        addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw);
+
+        gradient[x_index] = grad_x_raw;
+        gradient[y_index] = grad_y_raw;
+      }
+    }
+
+    cost[0] = cost_raw;
+    return true;
+  }
+
+  /**
+   * @brief Get number of parameter blocks
+   * @return Number of parameters in cost function
+   */
+  virtual int NumParameters() const {return _num_params;}
+
+protected:
+  /**
+   * @brief Cost function term for smooth paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param r Residual (cost) of term
+   */
+  inline void addSmoothingResidual(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    double & r) const
+  {
+    r += weight * (
+      pt_p.dot(pt_p) -
+      4 * pt_p.dot(pt) +
+      2 * pt_p.dot(pt_m) +
+      4 * pt.dot(pt) -
+      4 * pt.dot(pt_m) +
+      pt_m.dot(pt_m));    // objective function value
+  }
+
+  /**
+   * @brief Cost function derivative term for smooth paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param j0 Gradient of X term
+   * @param j1 Gradient of Y term
+   */
+  inline void addSmoothingJacobian(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    double & j0,
+    double & j1) const
+  {
+    j0 += weight *
+      (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]);   // xi x component of partial-derivative
+    j1 += weight *
+      (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]);   // xi y component of partial-derivative
+  }
+
+  /**
+   * @brief Get path curvature information
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param curvature_params A struct to cache computations for the jacobian to use
+   */
+  inline void getCurvatureParams(
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    CurvatureComputations & curvature_params) const
+  {
+    curvature_params.valid = true;
+    curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]);
+    curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]);
+    curvature_params.delta_xi_norm = curvature_params.delta_xi.norm();
+    curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm();
+
+    if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
+      std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) ||
+      std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm))
+    {
+      // ensure we have non-nan values returned
+      curvature_params.valid = false;
+      return;
+    }
+
+    const double & delta_xi_by_xi_p =
+      curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
+    double projection =
+      curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p;
+    if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) {
+      projection = 1.0;
+    }
+
+    curvature_params.delta_phi_i = std::acos(projection);
+    curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
+
+    curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio *
+      _params.max_curvature;
+    // TODO(stevemacenski) is use of upsample_ratio correct here? small number?
+    // TODO(stevemacenski) can remove the subtraction with a
+    // lower weight value, does have direction issue, maybe just tuning?
+
+    if (curvature_params.ki_minus_kmax <= EPSILON) {
+      // Quadratic penalty need not apply
+      curvature_params.valid = false;
+    }
+  }
+
+  /**
+   * @brief Cost function term for maximum curved paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param curvature_params A struct to cache computations for the jacobian to use
+   * @param r Residual (cost) of term
+   */
+  inline void addCurvatureResidual(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    CurvatureComputations & curvature_params,
+    double & r) const
+  {
+    getCurvatureParams(pt, pt_p, pt_m, curvature_params);
+
+    if (!curvature_params.isValid()) {
+      return;
+    }
+
+    r += weight *
+      curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax;  // objective function value
+  }
+
+  /**
+   * @brief Cost function derivative term for maximum curvature paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param curvature_params A struct with cached values to speed up Jacobian computation
+   * @param j0 Gradient of X term
+   * @param j1 Gradient of Y term
+   */
+  inline void addCurvatureJacobian(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & /*pt_m*/,
+    CurvatureComputations & curvature_params,
+    double & j0,
+    double & j1) const
+  {
+    if (!curvature_params.isValid()) {
+      return;
+    }
+
+    const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
+      -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2));
+    // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0);
+    auto neg_pt_plus = -1 * pt_p;
+    Eigen::Vector2d p1 = normalizedOrthogonalComplement(
+      pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm);
+    Eigen::Vector2d p2 = normalizedOrthogonalComplement(
+      neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm);
+
+    const double & u = 2 * curvature_params.ki_minus_kmax;
+    const double & common_prefix =
+      (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i;
+    const double & common_suffix = curvature_params.delta_phi_i /
+      (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm);
+    const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
+      curvature_params.delta_xi_norm;
+
+    const Eigen::Vector2d jacobian = u *
+      (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi));
+    const Eigen::Vector2d jacobian_im1 = u *
+      (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi));
+    const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1);
+    j0 += weight * jacobian[0];  // xi y component of partial-derivative
+    j1 += weight * jacobian[1];  // xi x component of partial-derivative
+    // j0 += weight *
+    //   (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]);
+    // j1 += weight *
+    //   (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]);
+  }
+
+  /**
+   * @brief Computing the normalized orthogonal component of 2 vectors
+   * @param a Vector
+   * @param b Vector
+   * @param norm a Vector's norm
+   * @param norm b Vector's norm
+   * @return Normalized vector of orthogonal components
+   */
+  inline Eigen::Vector2d normalizedOrthogonalComplement(
+    const Eigen::Vector2d & a,
+    const Eigen::Vector2d & b,
+    const double & a_norm,
+    const double & b_norm) const
+  {
+    return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm);
+  }
+
+  int _num_params;
+  SmootherParams _params;
+  int _upsample_ratio;
+  std::vector<Eigen::Vector2d> _path;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_

navigation2/nav2_smac_planner/test/deprecated/upsampler_cost_function_nlls.hpp

@@ -0,0 +1,334 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#ifndef DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_
+#define DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_
+
+#include <cmath>
+#include <vector>
+#include <iostream>
+#include <unordered_map>
+#include <memory>
+#include <queue>
+#include <utility>
+
+#include "ceres/ceres.h"
+#include "Eigen/Core"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/options.hpp"
+
+#define EPSILON 0.0001
+
+namespace nav2_smac_planner
+{
+/**
+ * @struct nav2_smac_planner::UpsamplerConstrainedCostFunction
+ * @brief Cost function for path upsampling with multiple terms using NLLS
+ * including curvature, smoothness, collision, and avoid obstacles.
+ */
+class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1>
+{
+public:
+  /**
+   * @brief A constructor for nav2_smac_planner::UpsamplerConstrainedCostFunction
+   * @param num_points Number of path points to consider
+   */
+  UpsamplerConstrainedCostFunction(
+    const std::vector<Eigen::Vector2d> & path,
+    const SmootherParams & params,
+    const int & upsample_ratio,
+    const int & i)
+  : _path(path),
+    _params(params),
+    _upsample_ratio(upsample_ratio),
+    index(i)
+  {
+  }
+
+  /**
+   * @struct CurvatureComputations
+   * @brief Cache common computations between the curvature terms to minimize recomputations
+   */
+  struct CurvatureComputations
+  {
+    /**
+     * @brief A constructor for nav2_smac_planner::CurvatureComputations
+     */
+    CurvatureComputations()
+    {
+      valid = true;
+    }
+
+    bool valid;
+    /**
+     * @brief Check if result is valid for penalty
+     * @return is valid (non-nan, non-inf, and turning angle > max)
+     */
+    bool isValid()
+    {
+      return valid;
+    }
+
+    Eigen::Vector2d delta_xi{0, 0};
+    Eigen::Vector2d delta_xi_p{0, 0};
+    double delta_xi_norm{0};
+    double delta_xi_p_norm{0};
+    double delta_phi_i{0};
+    double turning_rad{0};
+    double ki_minus_kmax{0};
+  };
+
+  /**
+   * @brief Smoother cost function evaluation
+   * @param parameters X,Y pairs of points
+   * @param cost total cost of path
+   * @param gradient of path at each X,Y pair from cost function derived analytically
+   * @return if successful in computing values
+   */
+
+  bool Evaluate(
+    double const * const * parameters,
+    double * residuals,
+    double ** jacobians) const override
+  {
+    Eigen::Vector2d xi = Eigen::Vector2d(parameters[0][0], parameters[1][0]);
+    Eigen::Vector2d xi_p1 = _path.at(index + 1);
+    Eigen::Vector2d xi_m1 = _path.at(index - 1);
+    CurvatureComputations curvature_params;
+    double grad_x_raw = 0, grad_y_raw = 0, cost_raw = 0;
+
+    // compute cost
+    addSmoothingResidual(15000, xi, xi_p1, xi_m1, cost_raw);
+    addCurvatureResidual(60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw);
+
+    residuals[0] = 0;
+    residuals[0] = cost_raw;  // objective function value x
+
+    if (jacobians != NULL && jacobians[0] != NULL) {
+      addSmoothingJacobian(15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw);
+      addCurvatureJacobian(60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw);
+
+      jacobians[0][0] = 0;
+      jacobians[1][0] = 0;
+      jacobians[0][0] = grad_x_raw;  // x derivative
+      jacobians[1][0] = grad_y_raw;  // y derivative
+      jacobians[0][1] = 0.0;
+      jacobians[1][1] = 0.0;
+    }
+
+    return true;
+  }
+
+protected:
+  /**
+   * @brief Cost function term for smooth paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param r Residual (cost) of term
+   */
+  inline void addSmoothingResidual(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    double & r) const
+  {
+    r += weight * (
+      pt_p.dot(pt_p) -
+      4 * pt_p.dot(pt) +
+      2 * pt_p.dot(pt_m) +
+      4 * pt.dot(pt) -
+      4 * pt.dot(pt_m) +
+      pt_m.dot(pt_m));    // objective function value
+  }
+
+  /**
+   * @brief Cost function derivative term for smooth paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param j0 Gradient of X term
+   * @param j1 Gradient of Y term
+   */
+  inline void addSmoothingJacobian(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    double & j0,
+    double & j1) const
+  {
+    j0 += weight *
+      (-4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0]);   // xi x component of partial-derivative
+    j1 += weight *
+      (-4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1]);   // xi y component of partial-derivative
+  }
+
+  /**
+   * @brief Get path curvature information
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param curvature_params A struct to cache computations for the jacobian to use
+   */
+  inline void getCurvatureParams(
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    CurvatureComputations & curvature_params) const
+  {
+    curvature_params.valid = true;
+    curvature_params.delta_xi = Eigen::Vector2d(pt[0] - pt_m[0], pt[1] - pt_m[1]);
+    curvature_params.delta_xi_p = Eigen::Vector2d(pt_p[0] - pt[0], pt_p[1] - pt[1]);
+    curvature_params.delta_xi_norm = curvature_params.delta_xi.norm();
+    curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm();
+
+    if (curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
+      std::isnan(curvature_params.delta_xi_p_norm) || std::isnan(curvature_params.delta_xi_norm) ||
+      std::isinf(curvature_params.delta_xi_p_norm) || std::isinf(curvature_params.delta_xi_norm))
+    {
+      // ensure we have non-nan values returned
+      curvature_params.valid = false;
+      return;
+    }
+
+    const double & delta_xi_by_xi_p =
+      curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
+    double projection =
+      curvature_params.delta_xi.dot(curvature_params.delta_xi_p) / delta_xi_by_xi_p;
+    if (fabs(1 - projection) < EPSILON || fabs(projection + 1) < EPSILON) {
+      projection = 1.0;
+    }
+
+    curvature_params.delta_phi_i = std::acos(projection);
+    curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
+
+    curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio *
+      _params.max_curvature;
+
+    if (curvature_params.ki_minus_kmax <= EPSILON) {
+      // Quadratic penalty need not apply
+      curvature_params.valid = false;
+    }
+  }
+
+  /**
+   * @brief Cost function term for maximum curved paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param curvature_params A struct to cache computations for the jacobian to use
+   * @param r Residual (cost) of term
+   */
+  inline void addCurvatureResidual(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & pt_m,
+    CurvatureComputations & curvature_params,
+    double & r) const
+  {
+    getCurvatureParams(pt, pt_p, pt_m, curvature_params);
+
+    if (!curvature_params.isValid()) {
+      return;
+    }
+
+    // objective function value
+    r += weight *
+      curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax;
+  }
+
+  /**
+   * @brief Cost function derivative term for maximum curvature paths
+   * @param weight Weight to apply to function
+   * @param pt Point Xi for evaluation
+   * @param pt Point Xi+1 for calculating Xi's cost
+   * @param pt Point Xi-1 for calculating Xi's cost
+   * @param curvature_params A struct with cached values to speed up Jacobian computation
+   * @param j0 Gradient of X term
+   * @param j1 Gradient of Y term
+   */
+  inline void addCurvatureJacobian(
+    const double & weight,
+    const Eigen::Vector2d & pt,
+    const Eigen::Vector2d & pt_p,
+    const Eigen::Vector2d & /*pt_m*/,
+    CurvatureComputations & curvature_params,
+    double & j0,
+    double & j1) const
+  {
+    if (!curvature_params.isValid()) {
+      return;
+    }
+
+    const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
+      -1 / std::sqrt(1 - std::pow(std::cos(curvature_params.delta_phi_i), 2));
+    // const Eigen::Vector2d ones = Eigen::Vector2d(1.0, 1.0);
+    auto neg_pt_plus = -1 * pt_p;
+    Eigen::Vector2d p1 = normalizedOrthogonalComplement(
+      pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm);
+    Eigen::Vector2d p2 = normalizedOrthogonalComplement(
+      neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm);
+
+    const double & u = 2 * curvature_params.ki_minus_kmax;
+    const double & common_prefix =
+      (1 / curvature_params.delta_xi_norm) * partial_delta_phi_i_wrt_cost_delta_phi_i;
+    const double & common_suffix = curvature_params.delta_phi_i /
+      (curvature_params.delta_xi_norm * curvature_params.delta_xi_norm);
+    const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
+      curvature_params.delta_xi_norm;
+
+    const Eigen::Vector2d jacobian = u *
+      (common_prefix * (-p1 - p2) - (common_suffix * d_delta_xi_d_xi));
+    const Eigen::Vector2d jacobian_im1 = u *
+      (common_prefix * p2 + (common_suffix * d_delta_xi_d_xi));
+    const Eigen::Vector2d jacobian_ip1 = u * (common_prefix * p1);
+    j0 += weight * jacobian[0];  // xi x component of partial-derivative
+    j1 += weight * jacobian[1];  // xi y component of partial-derivative
+    // j0 += weight *
+    //   (jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0]);
+    // j1 += weight *
+    //   (jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1]);
+  }
+  /**
+   * @brief Computing the normalized orthogonal component of 2 vectors
+   * @param a Vector
+   * @param b Vector
+   * @param norm a Vector's norm
+   * @param norm b Vector's norm
+   * @return Normalized vector of orthogonal components
+   */
+  inline Eigen::Vector2d normalizedOrthogonalComplement(
+    const Eigen::Vector2d & a,
+    const Eigen::Vector2d & b,
+    const double & a_norm,
+    const double & b_norm) const
+  {
+    return (a - (a.dot(b) * b / b.squaredNorm())) / (a_norm * b_norm);
+  }
+
+  std::vector<Eigen::Vector2d> _path;
+  SmootherParams _params;
+  int _upsample_ratio;
+  int index;
+};
+
+}  // namespace nav2_smac_planner
+
+#endif  // DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_

navigation2/nav2_smac_planner/test/test_a_star.cpp

@@ -0,0 +1,308 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <math.h>
+#include <memory>
+#include <string>
+#include <vector>
+#include <limits>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_smac_planner/node_hybrid.hpp"
+#include "nav2_smac_planner/node_lattice.hpp"
+#include "nav2_smac_planner/a_star.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+#include "ament_index_cpp/get_package_share_directory.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(AStarTest, test_a_star_2d)
+{
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  nav2_smac_planner::SearchInfo info;
+  nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star(
+    nav2_smac_planner::MotionModel::TWOD, info);
+  int max_iterations = 10000;
+  float tolerance = 0.0;
+  float some_tolerance = 20.0;
+  int it_on_approach = 10;
+  double max_planning_time = 120.0;
+  int num_it = 0;
+
+  a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 0.0, 1);
+
+  nav2_costmap_2d::Costmap2D * costmapA =
+    new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
+  // island in the middle of lethal cost to cross
+  for (unsigned int i = 40; i <= 60; ++i) {
+    for (unsigned int j = 40; j <= 60; ++j) {
+      costmapA->setCost(i, j, 254);
+    }
+  }
+
+  // functional case testing
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 1, lnode);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+  a_star.setCollisionChecker(checker.get());
+  a_star.setStart(20u, 20u, 0);
+  a_star.setGoal(80u, 80u, 0);
+  nav2_smac_planner::Node2D::CoordinateVector path;
+  EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
+  EXPECT_EQ(num_it, 2414);
+
+  // check path is the right size and collision free
+  EXPECT_EQ(path.size(), 82u);
+  for (unsigned int i = 0; i != path.size(); i++) {
+    EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0);
+  }
+
+  // setting non-zero dim 3 for 2D search
+  EXPECT_THROW(a_star.setGoal(0, 0, 10), std::runtime_error);
+  EXPECT_THROW(a_star.setStart(0, 0, 10), std::runtime_error);
+
+  path.clear();
+  // failure cases with invalid inputs
+  nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star_2(
+    nav2_smac_planner::MotionModel::TWOD, info);
+  a_star_2.initialize(false, max_iterations, it_on_approach, max_planning_time, 0, 1);
+  num_it = 0;
+  EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error);
+  a_star_2.setCollisionChecker(checker.get());
+  num_it = 0;
+  EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error);
+  a_star_2.setStart(50, 50, 0);  // invalid
+  a_star_2.setGoal(0, 0, 0);  // valid
+  num_it = 0;
+  EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error);
+  a_star_2.setStart(0, 0, 0);  // valid
+  a_star_2.setGoal(50, 50, 0);  // invalid
+  num_it = 0;
+  EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error);
+  num_it = 0;
+  // invalid goal but liberal tolerance
+  a_star_2.setStart(20, 20, 0);  // valid
+  a_star_2.setGoal(50, 50, 0);  // invalid
+  EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance));
+  EXPECT_EQ(path.size(), 21u);
+  for (unsigned int i = 0; i != path.size(); i++) {
+    EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0);
+  }
+
+  EXPECT_TRUE(a_star_2.getStart() != nullptr);
+  EXPECT_TRUE(a_star_2.getGoal() != nullptr);
+  EXPECT_EQ(a_star_2.getSizeX(), 100u);
+  EXPECT_EQ(a_star_2.getSizeY(), 100u);
+  EXPECT_EQ(a_star_2.getSizeDim3(), 1u);
+  EXPECT_EQ(a_star_2.getToleranceHeuristic(), 20.0);
+  EXPECT_EQ(a_star_2.getOnApproachMaxIterations(), 10);
+
+  delete costmapA;
+}
+
+TEST(AStarTest, test_a_star_se2)
+{
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 0.1;
+  info.non_straight_penalty = 1.1;
+  info.reverse_penalty = 2.0;
+  info.minimum_turning_radius = 8;  // in grid coordinates
+  info.retrospective_penalty = 0.015;
+  info.analytic_expansion_max_length = 20.0;  // in grid coordinates
+  info.analytic_expansion_ratio = 3.5;
+  unsigned int size_theta = 72;
+  info.cost_penalty = 1.7;
+  nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(
+    nav2_smac_planner::MotionModel::DUBIN, info);
+  int max_iterations = 10000;
+  float tolerance = 10.0;
+  int it_on_approach = 10;
+  double max_planning_time = 120.0;
+  int num_it = 0;
+
+  a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta);
+
+  nav2_costmap_2d::Costmap2D * costmapA =
+    new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
+  // island in the middle of lethal cost to cross
+  for (unsigned int i = 40; i <= 60; ++i) {
+    for (unsigned int j = 40; j <= 60; ++j) {
+      costmapA->setCost(i, j, 254);
+    }
+  }
+
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta, lnode);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+
+  // functional case testing
+  a_star.setCollisionChecker(checker.get());
+  a_star.setStart(10u, 10u, 0u);
+  a_star.setGoal(80u, 80u, 40u);
+  nav2_smac_planner::NodeHybrid::CoordinateVector path;
+  EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
+
+  // check path is the right size and collision free
+  EXPECT_EQ(num_it, 3222);
+  EXPECT_EQ(path.size(), 63u);
+  for (unsigned int i = 0; i != path.size(); i++) {
+    EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0);
+  }
+  // no skipped nodes
+  for (unsigned int i = 1; i != path.size(); i++) {
+    EXPECT_LT(hypotf(path[i].x - path[i - 1].x, path[i].y - path[i - 1].y), 2.1f);
+  }
+
+  delete costmapA;
+}
+
+TEST(AStarTest, test_a_star_lattice)
+{
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 0.05;
+  info.non_straight_penalty = 1.05;
+  info.reverse_penalty = 2.0;
+  info.retrospective_penalty = 0.1;
+  info.analytic_expansion_ratio = 3.5;
+  info.lattice_filepath =
+    ament_index_cpp::get_package_share_directory("nav2_smac_planner") +
+    "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
+    "/output.json";
+  info.minimum_turning_radius = 8;  // in grid coordinates 0.4/0.05
+  info.analytic_expansion_max_length = 20.0;  // in grid coordinates
+  unsigned int size_theta = 16;
+  info.cost_penalty = 2.0;
+  nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeLattice> a_star(
+    nav2_smac_planner::MotionModel::STATE_LATTICE, info);
+  int max_iterations = 10000;
+  float tolerance = 10.0;
+  int it_on_approach = 10;
+  double max_planning_time = 120.0;
+  int num_it = 0;
+
+  a_star.initialize(
+    false, max_iterations, std::numeric_limits<int>::max(), max_planning_time, 401, size_theta);
+
+  nav2_costmap_2d::Costmap2D * costmapA =
+    new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0.0, 0.0, 0);
+  // island in the middle of lethal cost to cross
+  for (unsigned int i = 20; i <= 30; ++i) {
+    for (unsigned int j = 20; j <= 30; ++j) {
+      costmapA->setCost(i, j, 254);
+    }
+  }
+
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta, lnode);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+
+  // functional case testing
+  a_star.setCollisionChecker(checker.get());
+  a_star.setStart(5u, 5u, 0u);
+  a_star.setGoal(40u, 40u, 1u);
+  nav2_smac_planner::NodeLattice::CoordinateVector path;
+  EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
+
+  // check path is the right size and collision free
+  EXPECT_EQ(num_it, 21);
+  EXPECT_GT(path.size(), 47u);
+  for (unsigned int i = 0; i != path.size(); i++) {
+    EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0);
+  }
+  // no skipped nodes
+  for (unsigned int i = 1; i != path.size(); i++) {
+    EXPECT_LT(hypotf(path[i].x - path[i - 1].x, path[i].y - path[i - 1].y), 2.1f);
+  }
+
+  delete costmapA;
+}
+
+TEST(AStarTest, test_se2_single_pose_path)
+{
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 0.1;
+  info.non_straight_penalty = 1.1;
+  info.reverse_penalty = 2.0;
+  info.retrospective_penalty = 0.0;
+  info.minimum_turning_radius = 8;  // in grid coordinates
+  info.analytic_expansion_max_length = 20.0;  // in grid coordinates
+  info.analytic_expansion_ratio = 3.5;
+  unsigned int size_theta = 72;
+  info.cost_penalty = 1.7;
+  nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(
+    nav2_smac_planner::MotionModel::DUBIN, info);
+  int max_iterations = 100;
+  float tolerance = 10.0;
+  int it_on_approach = 10;
+  double max_planning_time = 120.0;
+  int num_it = 0;
+
+  a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta);
+
+  nav2_costmap_2d::Costmap2D * costmapA =
+    new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
+
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta, lnode);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+
+  // functional case testing
+  a_star.setCollisionChecker(checker.get());
+  a_star.setStart(10u, 10u, 0u);
+  // Goal is one costmap cell away
+  a_star.setGoal(12u, 10u, 0u);
+  nav2_smac_planner::NodeHybrid::CoordinateVector path;
+  EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
+
+  // Check that the path is length one
+  // With the current implementation, this produces a longer path
+  // EXPECT_EQ(path.size(), 1u);
+  EXPECT_GE(path.size(), 1u);
+
+  delete costmapA;
+}
+
+TEST(AStarTest, test_constants)
+{
+  nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN;  // unknown
+  EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Unknown"));
+  mm = nav2_smac_planner::MotionModel::TWOD;  // 2d
+  EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("2D"));
+  mm = nav2_smac_planner::MotionModel::DUBIN;  // dubin
+  EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Dubin"));
+  mm = nav2_smac_planner::MotionModel::REEDS_SHEPP;  // reeds-shepp
+  EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Reeds-Shepp"));
+
+  EXPECT_EQ(
+    nav2_smac_planner::fromString(
+      "2D"), nav2_smac_planner::MotionModel::TWOD);
+  EXPECT_EQ(nav2_smac_planner::fromString("DUBIN"), nav2_smac_planner::MotionModel::DUBIN);
+  EXPECT_EQ(
+    nav2_smac_planner::fromString(
+      "REEDS_SHEPP"), nav2_smac_planner::MotionModel::REEDS_SHEPP);
+  EXPECT_EQ(nav2_smac_planner::fromString("NONE"), nav2_smac_planner::MotionModel::UNKNOWN);
+}

navigation2/nav2_smac_planner/test/test_collision_checker.cpp

@@ -0,0 +1,185 @@
+// Copyright (c) 2020 Shivang Patel
+// Copyright (c) 2020 Samsung Research
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "nav2_smac_planner/collision_checker.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+
+using namespace nav2_costmap_2d;  // NOLINT
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(collision_footprint, test_basic)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testA");
+  nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0);
+
+  geometry_msgs::msg::Point p1;
+  p1.x = -0.5;
+  p1.y = 0.0;
+  geometry_msgs::msg::Point p2;
+  p2.x = 0.0;
+  p2.y = 0.5;
+  geometry_msgs::msg::Point p3;
+  p3.x = 0.5;
+  p3.y = 0.0;
+  geometry_msgs::msg::Point p4;
+  p4.x = 0.0;
+  p4.y = -0.5;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
+  collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0);
+  collision_checker.inCollision(5.0, 5.0, 0.0, false);
+  float cost = collision_checker.getCost();
+  EXPECT_NEAR(cost, 0.0, 0.001);
+  delete costmap_;
+}
+
+TEST(collision_footprint, test_point_cost)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testB");
+  nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0);
+
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
+  nav2_costmap_2d::Footprint footprint;
+  collision_checker.setFootprint(footprint, true /*radius / pointcose*/, 0.0);
+
+  collision_checker.inCollision(5.0, 5.0, 0.0, false);
+  float cost = collision_checker.getCost();
+  EXPECT_NEAR(cost, 0.0, 0.001);
+  delete costmap_;
+}
+
+TEST(collision_footprint, test_world_to_map)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testC");
+  nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0);
+
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
+  nav2_costmap_2d::Footprint footprint;
+  collision_checker.setFootprint(footprint, true /*radius / point cost*/, 0.0);
+
+  unsigned int x, y;
+
+  collision_checker.worldToMap(1.0, 1.0, x, y);
+
+  collision_checker.inCollision(x, y, 0.0, false);
+  float cost = collision_checker.getCost();
+
+  EXPECT_NEAR(cost, 0.0, 0.001);
+
+  costmap_->setCost(50, 50, 200);
+  collision_checker.worldToMap(5.0, 5.0, x, y);
+
+  collision_checker.inCollision(x, y, 0.0, false);
+  EXPECT_NEAR(collision_checker.getCost(), 200.0, 0.001);
+  delete costmap_;
+}
+
+TEST(collision_footprint, test_footprint_at_pose_with_movement)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testD");
+  nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 254);
+
+  for (unsigned int i = 40; i <= 60; ++i) {
+    for (unsigned int j = 40; j <= 60; ++j) {
+      costmap_->setCost(i, j, 128);
+    }
+  }
+
+  geometry_msgs::msg::Point p1;
+  p1.x = -1.0;
+  p1.y = 1.0;
+  geometry_msgs::msg::Point p2;
+  p2.x = 1.0;
+  p2.y = 1.0;
+  geometry_msgs::msg::Point p3;
+  p3.x = 1.0;
+  p3.y = -1.0;
+  geometry_msgs::msg::Point p4;
+  p4.x = -1.0;
+  p4.y = -1.0;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
+  collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0);
+
+  collision_checker.inCollision(50, 50, 0.0, false);
+  float cost = collision_checker.getCost();
+  EXPECT_NEAR(cost, 128.0, 0.001);
+
+  collision_checker.inCollision(50, 49, 0.0, false);
+  float up_value = collision_checker.getCost();
+  EXPECT_NEAR(up_value, 254.0, 0.001);
+
+  collision_checker.inCollision(50, 52, 0.0, false);
+  float down_value = collision_checker.getCost();
+  EXPECT_NEAR(down_value, 254.0, 0.001);
+  delete costmap_;
+}
+
+TEST(collision_footprint, test_point_and_line_cost)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testE");
+  nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(
+    100, 100, 0.10000, 0, 0.0, 128.0);
+
+  costmap_->setCost(62, 50, 254);
+  costmap_->setCost(39, 60, 254);
+
+  geometry_msgs::msg::Point p1;
+  p1.x = -1.0;
+  p1.y = 1.0;
+  geometry_msgs::msg::Point p2;
+  p2.x = 1.0;
+  p2.y = 1.0;
+  geometry_msgs::msg::Point p3;
+  p3.x = 1.0;
+  p3.y = -1.0;
+  geometry_msgs::msg::Point p4;
+  p4.x = -1.0;
+  p4.y = -1.0;
+
+  nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
+
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
+  collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0);
+
+  collision_checker.inCollision(50, 50, 0.0, false);
+  float value = collision_checker.getCost();
+  EXPECT_NEAR(value, 128.0, 0.001);
+
+  collision_checker.inCollision(49, 50, 0.0, false);
+  float left_value = collision_checker.getCost();
+  EXPECT_NEAR(left_value, 254.0, 0.001);
+
+  collision_checker.inCollision(52, 50, 0.0, false);
+  float right_value = collision_checker.getCost();
+  EXPECT_NEAR(right_value, 254.0, 0.001);
+  delete costmap_;
+}

navigation2/nav2_smac_planner/test/test_costmap_downsampler.cpp

@@ -0,0 +1,67 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <memory>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_smac_planner/costmap_downsampler.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(CostmapDownsampler, costmap_downsample_test)
+{
+  nav2_util::LifecycleNode::SharedPtr node = std::make_shared<nav2_util::LifecycleNode>(
+    "CostmapDownsamplerTest");
+  nav2_smac_planner::CostmapDownsampler downsampler;
+
+  // create basic costmap
+  nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0);
+  costmapA.setCost(0, 0, 100);
+  costmapA.setCost(5, 5, 50);
+
+  // downsample it
+  downsampler.on_configure(node, "map", "unused_topic", &costmapA, 2);
+  nav2_costmap_2d::Costmap2D * downsampledCostmapA = downsampler.downsample(2);
+
+  // validate it
+  EXPECT_EQ(downsampledCostmapA->getCost(0, 0), 100);
+  EXPECT_EQ(downsampledCostmapA->getCost(2, 2), 50);
+  EXPECT_EQ(downsampledCostmapA->getSizeInCellsX(), 5u);
+  EXPECT_EQ(downsampledCostmapA->getSizeInCellsY(), 5u);
+
+  // give it another costmap of another size
+  nav2_costmap_2d::Costmap2D costmapB(4, 4, 0.10, 0.0, 0.0, 0);
+
+  // downsample it
+  downsampler.on_configure(node, "map", "unused_topic", &costmapB, 4);
+  downsampler.on_activate();
+  nav2_costmap_2d::Costmap2D * downsampledCostmapB = downsampler.downsample(4);
+  downsampler.on_deactivate();
+
+  // validate size
+  EXPECT_EQ(downsampledCostmapB->getSizeInCellsX(), 1u);
+  EXPECT_EQ(downsampledCostmapB->getSizeInCellsY(), 1u);
+
+  downsampler.resizeCostmap();
+}

navigation2/nav2_smac_planner/test/test_node2d.cpp

@@ -0,0 +1,143 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_smac_planner/node_2d.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(Node2DTest, test_node_2d)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0);
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(&costmapA, 72, node);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+
+  // test construction
+  unsigned char cost = static_cast<unsigned char>(1);
+  nav2_smac_planner::Node2D testA(1);
+  testA.setCost(cost);
+  nav2_smac_planner::Node2D testB(1);
+  testB.setCost(cost);
+  EXPECT_EQ(testA.getCost(), 1.0f);
+  nav2_smac_planner::SearchInfo info;
+  info.cost_penalty = 1.0;
+  unsigned int size = 10;
+  nav2_smac_planner::Node2D::initMotionModel(
+    nav2_smac_planner::MotionModel::TWOD, size, size, size, info);
+
+  // test reset
+  testA.reset();
+  EXPECT_TRUE(std::isnan(testA.getCost()));
+
+  // check collision checking
+  EXPECT_EQ(testA.isNodeValid(false, checker.get()), true);
+  testA.setCost(255);
+  EXPECT_EQ(testA.isNodeValid(true, checker.get()), true);
+  testA.setCost(10);
+
+  // check traversal cost computation
+  EXPECT_NEAR(testB.getTraversalCost(&testA), 1.03f, 0.1f);
+
+  // check heuristic cost computation
+  nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0);
+  nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0);
+  EXPECT_NEAR(testB.getHeuristicCost(A, B, nullptr), 11.18, 0.02);
+
+  // check operator== works on index
+  unsigned char costC = '2';
+  nav2_smac_planner::Node2D testC(1);
+  testC.setCost(costC);
+  EXPECT_TRUE(testA == testC);
+
+  // check accumulated costs are set
+  testC.setAccumulatedCost(100);
+  EXPECT_EQ(testC.getAccumulatedCost(), 100.0f);
+
+  // check visiting state
+  EXPECT_EQ(testC.wasVisited(), false);
+  testC.queued();
+  EXPECT_EQ(testC.isQueued(), true);
+  testC.visited();
+  EXPECT_EQ(testC.wasVisited(), true);
+  EXPECT_EQ(testC.isQueued(), false);
+
+  // check index
+  EXPECT_EQ(testC.getIndex(), 1u);
+
+  // check static index functions
+  EXPECT_EQ(nav2_smac_planner::Node2D::getIndex(1u, 1u, 10u), 11u);
+  EXPECT_EQ(nav2_smac_planner::Node2D::getIndex(6u, 43u, 10u), 436u);
+  EXPECT_EQ(nav2_smac_planner::Node2D::getCoords(436u, 10u, 1u).x, 6u);
+  EXPECT_EQ(nav2_smac_planner::Node2D::getCoords(436u, 10u, 1u).y, 43u);
+  EXPECT_THROW(nav2_smac_planner::Node2D::getCoords(436u, 10u, 10u), std::runtime_error);
+}
+
+TEST(Node2DTest, test_node_2d_neighbors)
+{
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  nav2_smac_planner::SearchInfo info;
+  unsigned int size_x = 10u;
+  unsigned int size_y = 10u;
+  unsigned int quant = 0u;
+  // test neighborhood computation
+  size_x = 100u;
+  nav2_smac_planner::Node2D::initMotionModel(
+    nav2_smac_planner::MotionModel::TWOD, size_x, size_y,
+    quant, info);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 8u);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[1], 1);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[2], -100);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[3], 100);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[4], -101);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[5], -99);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[6], 99);
+  EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[7], 101);
+
+  nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0);
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(&costmapA, 72, lnode);
+  unsigned char cost = static_cast<unsigned int>(1);
+  nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(1);
+  node->setCost(cost);
+  std::function<bool(const unsigned int &, nav2_smac_planner::Node2D * &)> neighborGetter =
+    [&, this](const unsigned int & index, nav2_smac_planner::Node2D * & neighbor_rtn) -> bool
+    {
+      return false;
+    };
+
+  nav2_smac_planner::Node2D::NodeVector neighbors;
+  node->getNeighbors(neighborGetter, checker.get(), false, neighbors);
+  delete node;
+
+  // should be empty since totally invalid
+  EXPECT_EQ(neighbors.size(), 0u);
+}

navigation2/nav2_smac_planner/test/test_nodebasic.cpp

@@ -0,0 +1,55 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <math.h>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_smac_planner/node_basic.hpp"
+#include "nav2_smac_planner/node_2d.hpp"
+#include "nav2_smac_planner/node_hybrid.hpp"
+#include "nav2_smac_planner/node_lattice.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(NodeBasicTest, test_node_basic)
+{
+  nav2_smac_planner::NodeBasic<nav2_smac_planner::NodeHybrid> node(50);
+
+  EXPECT_EQ(node.index, 50u);
+  EXPECT_EQ(node.graph_node_ptr, nullptr);
+
+  nav2_smac_planner::NodeBasic<nav2_smac_planner::Node2D> node2(100);
+
+  EXPECT_EQ(node2.index, 100u);
+  EXPECT_EQ(node2.graph_node_ptr, nullptr);
+
+  nav2_smac_planner::NodeBasic<nav2_smac_planner::NodeLattice> node3(200);
+
+  EXPECT_EQ(node3.index, 200u);
+  EXPECT_EQ(node3.graph_node_ptr, nullptr);
+}

navigation2/nav2_smac_planner/test/test_nodehybrid.cpp

@@ -0,0 +1,349 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <math.h>
+#include <cmath>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_smac_planner/node_hybrid.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(NodeHybridTest, test_node_hybrid)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 0.1;
+  info.non_straight_penalty = 1.1;
+  info.reverse_penalty = 2.0;
+  info.minimum_turning_radius = 8;  // 0.4m/5cm resolution costmap
+  info.cost_penalty = 1.7;
+  info.retrospective_penalty = 0.1;
+  unsigned int size_x = 10;
+  unsigned int size_y = 10;
+  unsigned int size_theta = 72;
+
+  // Check defaulted constants
+  nav2_smac_planner::NodeHybrid testA(49);
+  EXPECT_EQ(testA.travel_distance_cost, sqrt(2));
+
+  nav2_smac_planner::NodeHybrid::initMotionModel(
+    nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);
+
+  nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
+    10, 10, 0.05, 0.0, 0.0, 0);
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 72, node);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+
+  // test construction
+  nav2_smac_planner::NodeHybrid testB(49);
+  EXPECT_TRUE(std::isnan(testA.getCost()));
+
+  // test node valid and cost
+  testA.pose.x = 5;
+  testA.pose.y = 5;
+  testA.pose.theta = 0;
+  EXPECT_EQ(testA.isNodeValid(true, checker.get()), true);
+  EXPECT_EQ(testA.isNodeValid(false, checker.get()), true);
+  EXPECT_EQ(testA.getCost(), 0.0f);
+
+  // test reset
+  testA.reset();
+  EXPECT_TRUE(std::isnan(testA.getCost()));
+
+  // Check motion-specific constants
+  EXPECT_NEAR(testA.travel_distance_cost, 2.08842, 0.1);
+
+  // check collision checking
+  EXPECT_EQ(testA.isNodeValid(false, checker.get()), true);
+
+  // check traversal cost computation
+  // simulated first node, should return neutral cost
+  EXPECT_NEAR(testB.getTraversalCost(&testA), 2.088, 0.1);
+  // now with straight motion, cost is 0, so will be neutral as well
+  // but now reduced by retrospective penalty (10%)
+  testB.setMotionPrimitiveIndex(1);
+  testA.setMotionPrimitiveIndex(0);
+  EXPECT_NEAR(testB.getTraversalCost(&testA), 2.088 * 0.9, 0.1);
+  // same direction as parent, testB
+  testA.setMotionPrimitiveIndex(1);
+  EXPECT_NEAR(testB.getTraversalCost(&testA), 2.297f * 0.9, 0.01);
+  // opposite direction as parent, testB
+  testA.setMotionPrimitiveIndex(2);
+  EXPECT_NEAR(testB.getTraversalCost(&testA), 2.506f * 0.9, 0.01);
+
+  // will throw because never collision checked testB
+  EXPECT_THROW(testA.getTraversalCost(&testB), std::runtime_error);
+
+  // check motion primitives
+  EXPECT_EQ(testA.getMotionPrimitiveIndex(), 2u);
+
+  // check operator== works on index
+  nav2_smac_planner::NodeHybrid testC(49);
+  EXPECT_TRUE(testA == testC);
+
+  // check accumulated costs are set
+  testC.setAccumulatedCost(100);
+  EXPECT_EQ(testC.getAccumulatedCost(), 100.0f);
+
+  // check visiting state
+  EXPECT_EQ(testC.wasVisited(), false);
+  testC.visited();
+  EXPECT_EQ(testC.wasVisited(), true);
+
+  // check index
+  EXPECT_EQ(testC.getIndex(), 49u);
+
+  // check set pose and pose
+  testC.setPose(nav2_smac_planner::NodeHybrid::Coordinates(10.0, 5.0, 4));
+  EXPECT_EQ(testC.pose.x, 10.0);
+  EXPECT_EQ(testC.pose.y, 5.0);
+  EXPECT_EQ(testC.pose.theta, 4);
+
+  // check static index functions
+  EXPECT_EQ(nav2_smac_planner::NodeHybrid::getIndex(1u, 1u, 4u, 10u, 72u), 796u);
+  EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).x, 1u);
+  EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).y, 1u);
+  EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).theta, 4u);
+
+  delete costmapA;
+}
+
+TEST(NodeHybridTest, test_obstacle_heuristic)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 0.1;
+  info.non_straight_penalty = 1.1;
+  info.reverse_penalty = 2.0;
+  info.minimum_turning_radius = 8;  // 0.4m/5cm resolution costmap
+  info.cost_penalty = 1.7;
+  info.retrospective_penalty = 0.0;
+  unsigned int size_x = 100;
+  unsigned int size_y = 100;
+  unsigned int size_theta = 72;
+
+  nav2_smac_planner::NodeHybrid::initMotionModel(
+    nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);
+
+  nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
+    100, 100, 0.1, 0.0, 0.0, 0);
+  // island in the middle of lethal cost to cross
+  for (unsigned int i = 20; i <= 80; ++i) {
+    for (unsigned int j = 40; j <= 60; ++j) {
+      costmapA->setCost(i, j, 254);
+    }
+  }
+  // path on the right is narrow and thus with high cost
+  for (unsigned int i = 20; i <= 80; ++i) {
+    for (unsigned int j = 61; j <= 70; ++j) {
+      costmapA->setCost(i, j, 250);
+    }
+  }
+  for (unsigned int i = 20; i <= 80; ++i) {
+    for (unsigned int j = 71; j < 100; ++j) {
+      costmapA->setCost(i, j, 254);
+    }
+  }
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 72, node);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+
+  nav2_smac_planner::NodeHybrid testA(0);
+  testA.pose.x = 10;
+  testA.pose.y = 50;
+  testA.pose.theta = 0;
+
+  nav2_smac_planner::NodeHybrid testB(1);
+  testB.pose.x = 90;
+  testB.pose.y = 51;  // goal is a bit closer to the high-cost passage
+  testB.pose.theta = 0;
+
+  // first block the high-cost passage to make sure the cost spreads through the better path
+  for (unsigned int j = 61; j <= 70; ++j) {
+    costmapA->setCost(50, j, 254);
+  }
+  nav2_smac_planner::NodeHybrid::resetObstacleHeuristic(
+    costmapA, testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y);
+  float wide_passage_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic(
+    testA.pose,
+    testB.pose,
+    info.cost_penalty);
+
+  EXPECT_NEAR(wide_passage_cost, 91.1f, 0.1f);
+
+  // then unblock it to check if cost remains the same
+  // (it should, since the unblocked narrow path will have higher cost than the wide one
+  //  and thus lower bound of the path cost should be unchanged)
+  for (unsigned int j = 61; j <= 70; ++j) {
+    costmapA->setCost(50, j, 250);
+  }
+  nav2_smac_planner::NodeHybrid::resetObstacleHeuristic(
+    costmapA,
+    testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y);
+  float two_passages_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic(
+    testA.pose,
+    testB.pose,
+    info.cost_penalty);
+
+  EXPECT_EQ(wide_passage_cost, two_passages_cost);
+
+  delete costmapA;
+}
+
+TEST(NodeHybridTest, test_node_debin_neighbors)
+{
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 1.2;
+  info.non_straight_penalty = 1.4;
+  info.reverse_penalty = 2.1;
+  info.minimum_turning_radius = 4;  // 0.2 in grid coordinates
+  info.retrospective_penalty = 0.0;
+  unsigned int size_x = 100;
+  unsigned int size_y = 100;
+  unsigned int size_theta = 72;
+  nav2_smac_planner::NodeHybrid::initMotionModel(
+    nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info);
+
+  // test neighborhood computation
+  EXPECT_EQ(nav2_smac_planner::NodeHybrid::motion_table.projections.size(), 3u);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._x, 1.731517, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._y, 0, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._theta, 0, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._x, 1.69047, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._y, 0.3747, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._theta, 5, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._x, 1.69047, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._y, -0.3747, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._theta, -5, 0.01);
+}
+
+TEST(NodeHybridTest, test_node_reeds_neighbors)
+{
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 1.2;
+  info.non_straight_penalty = 1.4;
+  info.reverse_penalty = 2.1;
+  info.minimum_turning_radius = 8;  // 0.4 in grid coordinates
+  info.retrospective_penalty = 0.0;
+  unsigned int size_x = 100;
+  unsigned int size_y = 100;
+  unsigned int size_theta = 72;
+  nav2_smac_planner::NodeHybrid::initMotionModel(
+    nav2_smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info);
+
+  EXPECT_EQ(nav2_smac_planner::NodeHybrid::motion_table.projections.size(), 6u);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._x, 2.088, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._y, 0, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._theta, 0, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._x, 2.070, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._y, 0.272, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._theta, 3, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._x, 2.070, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._y, -0.272, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._theta, -3, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._x, -2.088, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._y, 0, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._theta, 0, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._x, -2.07, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._y, 0.272, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._theta, -3, 0.01);
+
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._x, -2.07, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._y, -0.272, 0.01);
+  EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._theta, 3, 0.01);
+
+  nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0);
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(&costmapA, 72, lnode);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+  nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid(49);
+  std::function<bool(const unsigned int &, nav2_smac_planner::NodeHybrid * &)> neighborGetter =
+    [&, this](const unsigned int & index, nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool
+    {
+      // because we don't return a real object
+      return false;
+    };
+
+  nav2_smac_planner::NodeHybrid::NodeVector neighbors;
+  node->getNeighbors(neighborGetter, checker.get(), false, neighbors);
+  delete node;
+
+  // should be empty since totally invalid
+  EXPECT_EQ(neighbors.size(), 0u);
+}
+
+TEST(NodeHybridTest, basic_get_closest_angular_bin_test)
+{
+  // Tests to check getClosestAngularBin behavior for different input types
+  nav2_smac_planner::HybridMotionTable motion_table;
+
+  {
+    motion_table.bin_size = 3.1415926;
+    motion_table.num_angle_quantization = 2;
+    double test_theta = 3.1415926;
+    unsigned int expected_angular_bin = 1;
+    unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
+    EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
+  }
+
+  {
+    motion_table.bin_size = M_PI;
+    motion_table.num_angle_quantization = 2;
+    double test_theta = M_PI / 2.0 - 0.000001;
+    unsigned int expected_angular_bin = 0;
+    unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
+    EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
+  }
+
+  {
+    motion_table.bin_size = M_PI;
+    motion_table.num_angle_quantization = 2;
+    float test_theta = M_PI;
+    unsigned int expected_angular_bin = 1;
+    unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
+    EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
+  }
+
+  {
+    motion_table.bin_size = 0.0872664675;
+    motion_table.num_angle_quantization = 72;
+    double test_theta = 6.28317530718;  // 0.0001 less than 2 pi
+    unsigned int expected_angular_bin = 0;  // should be closer to wrap around
+    unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
+    EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
+  }
+}

navigation2/nav2_smac_planner/test/test_nodelattice.cpp

@@ -0,0 +1,369 @@
+// Copyright (c) 2021 Joshua Wallace
+// Copyright (c) 2021 Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <fstream>
+#include <string>
+#include <vector>
+#include <memory>
+#include <unordered_map>
+#include <limits>
+#include "nav2_smac_planner/node_lattice.hpp"
+#include "gtest/gtest.h"
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+
+using json = nlohmann::json;
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+TEST(NodeLatticeTest, parser_test)
+{
+  std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner");
+  std::string filePath =
+    pkg_share_dir +
+    "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
+    "/output.json";
+  std::ifstream myJsonFile(filePath);
+
+  ASSERT_TRUE(myJsonFile.is_open());
+
+  json j;
+  myJsonFile >> j;
+
+  nav2_smac_planner::LatticeMetadata metaData;
+  nav2_smac_planner::MotionPrimitive myPrimitive;
+  nav2_smac_planner::MotionPose pose;
+
+  json jsonMetaData = j["lattice_metadata"];
+  json jsonPrimatives = j["primitives"];
+  json jsonPose = jsonPrimatives[0]["poses"][0];
+
+  nav2_smac_planner::fromJsonToMetaData(jsonMetaData, metaData);
+
+  // Checks for parsing meta data
+  EXPECT_NEAR(metaData.min_turning_radius, 0.5, 0.001);
+  EXPECT_NEAR(metaData.grid_resolution, 0.05, 0.001);
+  EXPECT_NEAR(metaData.number_of_headings, 16, 0.01);
+  EXPECT_NEAR(metaData.heading_angles[0], 0.0, 0.01);
+  EXPECT_EQ(metaData.number_of_trajectories, 80u);
+  EXPECT_EQ(metaData.motion_model, std::string("ackermann"));
+
+  std::vector<nav2_smac_planner::MotionPrimitive> myPrimitives;
+  for (unsigned int i = 0; i < jsonPrimatives.size(); ++i) {
+    nav2_smac_planner::MotionPrimitive newPrimative;
+    nav2_smac_planner::fromJsonToMotionPrimitive(jsonPrimatives[i], newPrimative);
+    myPrimitives.push_back(newPrimative);
+  }
+
+  // Checks for parsing primitives
+  EXPECT_EQ(myPrimitives.size(), 80u);
+  EXPECT_NEAR(myPrimitives[0].trajectory_id, 0, 0.01);
+  EXPECT_NEAR(myPrimitives[0].start_angle, 0.0, 0.01);
+  EXPECT_NEAR(myPrimitives[0].end_angle, 13, 0.01);
+  EXPECT_NEAR(myPrimitives[0].turning_radius, 0.5259, 0.01);
+  EXPECT_NEAR(myPrimitives[0].trajectory_length, 0.64856, 0.01);
+  EXPECT_NEAR(myPrimitives[0].arc_length, 0.58225, 0.01);
+  EXPECT_NEAR(myPrimitives[0].straight_length, 0.06631, 0.01);
+
+  EXPECT_NEAR(myPrimitives[0].poses[0]._x, 0.04981, 0.01);
+  EXPECT_NEAR(myPrimitives[0].poses[0]._y, -0.00236, 0.01);
+  EXPECT_NEAR(myPrimitives[0].poses[0]._theta, 6.1883, 0.01);
+
+  EXPECT_NEAR(myPrimitives[0].poses[1]._x, 0.09917, 0.01);
+  EXPECT_NEAR(myPrimitives[0].poses[1]._y, -0.00944, 0.01);
+  EXPECT_NEAR(myPrimitives[0].poses[1]._theta, 6.09345, 0.015);
+}
+
+TEST(NodeLatticeTest, test_node_lattice_neighbors_and_parsing)
+{
+  std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner");
+  std::string filePath =
+    pkg_share_dir +
+    "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
+    "/output.json";
+
+  nav2_smac_planner::SearchInfo info;
+  info.minimum_turning_radius = 1.1;
+  info.non_straight_penalty = 1;
+  info.change_penalty = 1;
+  info.reverse_penalty = 1;
+  info.cost_penalty = 1;
+  info.retrospective_penalty = 0.0;
+  info.analytic_expansion_ratio = 1;
+  info.lattice_filepath = filePath;
+  info.cache_obstacle_heuristic = true;
+  info.allow_reverse_expansion = true;
+
+  unsigned int x = 100;
+  unsigned int y = 100;
+  unsigned int angle_quantization = 16;
+
+  nav2_smac_planner::NodeLattice::initMotionModel(
+    nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info);
+
+  nav2_smac_planner::NodeLattice aNode(0);
+  aNode.setPose(nav2_smac_planner::NodeHybrid::Coordinates(0, 0, 0));
+  nav2_smac_planner::MotionPrimitivePtrs projections =
+    nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&aNode);
+
+  EXPECT_NEAR(projections[0]->poses.back()._x, 0.5, 0.01);
+  EXPECT_NEAR(projections[0]->poses.back()._y, -0.35, 0.01);
+  EXPECT_NEAR(projections[0]->poses.back()._theta, 5.176, 0.01);
+
+  EXPECT_NEAR(
+    nav2_smac_planner::NodeLattice::motion_table.getLatticeMetadata(
+      filePath).grid_resolution, 0.05, 0.005);
+}
+
+TEST(NodeLatticeTest, test_node_lattice_conversions)
+{
+  std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner");
+  std::string filePath =
+    pkg_share_dir +
+    "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
+    "/output.json";
+
+  nav2_smac_planner::SearchInfo info;
+  info.minimum_turning_radius = 1.1;
+  info.non_straight_penalty = 1;
+  info.change_penalty = 1;
+  info.reverse_penalty = 1;
+  info.cost_penalty = 1;
+  info.retrospective_penalty = 0.0;
+  info.analytic_expansion_ratio = 1;
+  info.lattice_filepath = filePath;
+  info.cache_obstacle_heuristic = true;
+
+  unsigned int x = 100;
+  unsigned int y = 100;
+  unsigned int angle_quantization = 16;
+
+  nav2_smac_planner::NodeLattice::initMotionModel(
+    nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info);
+
+  nav2_smac_planner::NodeLattice aNode(0);
+  aNode.setPose(nav2_smac_planner::NodeHybrid::Coordinates(0, 0, 0));
+
+  EXPECT_NEAR(aNode.motion_table.getAngleFromBin(0u), 0.0, 0.005);
+  EXPECT_NEAR(aNode.motion_table.getAngleFromBin(1u), 0.46364, 0.005);
+  EXPECT_NEAR(aNode.motion_table.getAngleFromBin(2u), 0.78539, 0.005);
+
+  EXPECT_EQ(aNode.motion_table.getClosestAngularBin(0.0), 0u);
+  EXPECT_EQ(aNode.motion_table.getClosestAngularBin(0.5), 1u);
+  EXPECT_EQ(aNode.motion_table.getClosestAngularBin(1.5), 4u);
+}
+
+TEST(NodeLatticeTest, test_node_lattice)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner");
+  std::string filePath =
+    pkg_share_dir +
+    "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
+    "/output.json";
+
+  nav2_smac_planner::SearchInfo info;
+  info.minimum_turning_radius = 1.1;
+  info.non_straight_penalty = 1;
+  info.change_penalty = 1;
+  info.reverse_penalty = 1;
+  info.cost_penalty = 1;
+  info.retrospective_penalty = 0.1;
+  info.analytic_expansion_ratio = 1;
+  info.lattice_filepath = filePath;
+  info.cache_obstacle_heuristic = true;
+  info.allow_reverse_expansion = true;
+
+  unsigned int x = 100;
+  unsigned int y = 100;
+  unsigned int angle_quantization = 16;
+
+  nav2_smac_planner::NodeLattice::initMotionModel(
+    nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info);
+
+  // Check defaults
+  nav2_smac_planner::NodeLattice aNode(0);
+  nav2_smac_planner::NodeLattice testA(49);
+  EXPECT_EQ(testA.getIndex(), 49u);
+  EXPECT_EQ(testA.getAccumulatedCost(), std::numeric_limits<float>::max());
+  EXPECT_TRUE(std::isnan(testA.getCost()));
+  EXPECT_EQ(testA.getMotionPrimitive(), nullptr);
+
+  // Test visited state / reset
+  EXPECT_EQ(testA.wasVisited(), false);
+  testA.visited();
+  EXPECT_EQ(testA.wasVisited(), true);
+  testA.reset();
+  EXPECT_EQ(testA.wasVisited(), false);
+
+  nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
+    10, 10, 0.05, 0.0, 0.0, 0);
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 72, node);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+
+  // test node valid and cost
+  testA.pose.x = 5;
+  testA.pose.y = 5;
+  testA.pose.theta = 0;
+  EXPECT_EQ(testA.isNodeValid(true, checker.get()), true);
+  EXPECT_EQ(testA.isNodeValid(false, checker.get()), true);
+  EXPECT_EQ(testA.getCost(), 0.0f);
+
+  // check collision checking
+  EXPECT_EQ(testA.isNodeValid(false, checker.get()), true);
+
+  // check operator== works on index
+  nav2_smac_planner::NodeLattice testC(49);
+  EXPECT_TRUE(testA == testC);
+
+  // check accumulated costs are set
+  testC.setAccumulatedCost(100);
+  EXPECT_EQ(testC.getAccumulatedCost(), 100.0f);
+
+  // check set pose and pose
+  testC.setPose(nav2_smac_planner::NodeLattice::Coordinates(10.0, 5.0, 4));
+  EXPECT_EQ(testC.pose.x, 10.0);
+  EXPECT_EQ(testC.pose.y, 5.0);
+  EXPECT_EQ(testC.pose.theta, 4);
+
+  delete costmapA;
+}
+
+
+TEST(NodeLatticeTest, test_get_neighbors)
+{
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner");
+  std::string filePath =
+    pkg_share_dir +
+    "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
+    "/output.json";
+
+  nav2_smac_planner::SearchInfo info;
+  info.minimum_turning_radius = 1.1;
+  info.non_straight_penalty = 1;
+  info.change_penalty = 1;
+  info.reverse_penalty = 1;
+  info.cost_penalty = 1;
+  info.analytic_expansion_ratio = 1;
+  info.retrospective_penalty = 0.0;
+  info.lattice_filepath = filePath;
+  info.cache_obstacle_heuristic = true;
+  info.allow_reverse_expansion = true;
+
+  unsigned int x = 100;
+  unsigned int y = 100;
+  unsigned int angle_quantization = 16;
+
+  nav2_smac_planner::NodeLattice::initMotionModel(
+    nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info);
+
+  nav2_smac_planner::NodeLattice node(49);
+
+  nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
+    10, 10, 0.05, 0.0, 0.0, 0);
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 72, lnode);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+
+  std::function<bool(const unsigned int &, nav2_smac_planner::NodeLattice * &)> neighborGetter =
+    [&, this](const unsigned int & index, nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool
+    {
+      // because we don't return a real object
+      return false;
+    };
+
+  nav2_smac_planner::NodeLattice::NodeVector neighbors;
+  node.getNeighbors(neighborGetter, checker.get(), false, neighbors);
+  // should be empty since totally invalid
+  EXPECT_EQ(neighbors.size(), 0u);
+
+  delete costmapA;
+}
+
+TEST(NodeLatticeTest, test_node_lattice_custom_footprint)
+{
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner");
+  std::string filePath =
+    pkg_share_dir +
+    "/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
+    "/output.json";
+
+  nav2_smac_planner::SearchInfo info;
+  info.minimum_turning_radius = 0.5;
+  info.non_straight_penalty = 1;
+  info.change_penalty = 1;
+  info.reverse_penalty = 1;
+  info.cost_penalty = 1;
+  info.retrospective_penalty = 0.1;
+  info.analytic_expansion_ratio = 1;
+  info.lattice_filepath = filePath;
+  info.cache_obstacle_heuristic = true;
+  info.allow_reverse_expansion = true;
+
+  unsigned int x = 100;
+  unsigned int y = 100;
+  unsigned int angle_quantization = 16;
+
+  nav2_smac_planner::NodeLattice::initMotionModel(
+    nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info);
+
+  nav2_smac_planner::NodeLattice node(49);
+
+  nav2_costmap_2d::Costmap2D * costmap = new nav2_costmap_2d::Costmap2D(
+    40, 40, 0.05, 0.0, 0.0, 0);
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap, 72, lnode);
+
+  // Make some custom asymmetrical footprint
+  nav2_costmap_2d::Footprint footprint;
+  geometry_msgs::msg::Point p;
+  p.x = -0.1;
+  p.y = -0.15;
+  footprint.push_back(p);
+  p.x = 0.35;
+  p.y = -0.15;
+  footprint.push_back(p);
+  p.x = 0.35;
+  p.y = 0.22;
+  footprint.push_back(p);
+  p.x = -0.1;
+  p.y = 0.22;
+  footprint.push_back(p);
+  checker->setFootprint(footprint, false, 0.0);
+
+  // Setting initial robot pose to (1.0, 1.0, 0.0)
+  node.pose.x = 20;
+  node.pose.y = 20;
+  node.pose.theta = 0;
+  // Test that the node is valid though all motion primitives poses for custom footprint
+  nav2_smac_planner::MotionPrimitivePtrs motion_primitives =
+    nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&node);
+  EXPECT_GT(motion_primitives.size(), 0u);
+  for (unsigned int i = 0; i < motion_primitives.size(); i++) {
+    EXPECT_EQ(node.isNodeValid(true, checker.get(), motion_primitives[i], false), true);
+    EXPECT_EQ(node.isNodeValid(true, checker.get(), motion_primitives[i], true), true);
+  }
+
+  delete costmap;
+}

navigation2/nav2_smac_planner/test/test_smac_2d.cpp

@@ -0,0 +1,137 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <math.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "gtest/gtest.h"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "nav2_smac_planner/a_star.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/node_hybrid.hpp"
+#include "nav2_smac_planner/smac_planner_2d.hpp"
+#include "nav2_smac_planner/smac_planner_hybrid.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+// SMAC smoke tests for plugin-level issues rather than algorithms
+// (covered by more extensively testing in other files)
+// System tests in nav2_system_tests will actually plan with this work
+
+TEST(SmacTest, test_smac_2d) {
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node2D =
+    std::make_shared<rclcpp_lifecycle::LifecycleNode>("Smac2DTest");
+
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
+    std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+
+  node2D->declare_parameter("test.smooth_path", true);
+  node2D->set_parameter(rclcpp::Parameter("test.smooth_path", true));
+  node2D->declare_parameter("test.downsample_costmap", true);
+  node2D->set_parameter(rclcpp::Parameter("test.downsample_costmap", true));
+  node2D->declare_parameter("test.downsampling_factor", 2);
+  node2D->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2));
+
+  geometry_msgs::msg::PoseStamped start, goal;
+  start.pose.position.x = 0.0;
+  start.pose.position.y = 0.0;
+  start.pose.orientation.w = 1.0;
+  // goal = start;
+  goal.pose.position.x = 7.0;
+  goal.pose.position.y = 0.0;
+  goal.pose.orientation.w = 1.0;
+  auto planner_2d = std::make_unique<nav2_smac_planner::SmacPlanner2D>();
+  planner_2d->configure(node2D, "test", nullptr, costmap_ros);
+  planner_2d->activate();
+  try {
+    planner_2d->createPlan(start, goal);
+  } catch (...) {
+  }
+
+  planner_2d->deactivate();
+  planner_2d->cleanup();
+
+  planner_2d.reset();
+  costmap_ros->on_cleanup(rclcpp_lifecycle::State());
+  node2D.reset();
+  costmap_ros.reset();
+}
+
+TEST(SmacTest, test_smac_2d_reconfigure) {
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node2D =
+    std::make_shared<rclcpp_lifecycle::LifecycleNode>("Smac2DTest");
+
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
+    std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+
+  auto planner_2d = std::make_unique<nav2_smac_planner::SmacPlanner2D>();
+  planner_2d->configure(node2D, "test", nullptr, costmap_ros);
+  planner_2d->activate();
+
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    node2D->get_node_base_interface(), node2D->get_node_topics_interface(),
+    node2D->get_node_graph_interface(),
+    node2D->get_node_services_interface());
+
+  auto results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("test.tolerance", 1.0),
+      rclcpp::Parameter("test.cost_travel_multiplier", 1.0),
+      rclcpp::Parameter("test.max_planning_time", 2.0),
+      rclcpp::Parameter("test.downsample_costmap", false),
+      rclcpp::Parameter("test.allow_unknown", false),
+      rclcpp::Parameter("test.downsampling_factor", 2),
+      rclcpp::Parameter("test.max_iterations", -1),
+      rclcpp::Parameter("test.max_on_approach_iterations", -1),
+      rclcpp::Parameter("test.use_final_approach_orientation", false)});
+
+  rclcpp::spin_until_future_complete(
+    node2D->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(node2D->get_parameter("test.tolerance").as_double(), 1.0);
+  EXPECT_EQ(
+    node2D->get_parameter("test.cost_travel_multiplier").as_double(),
+    1.0);
+  EXPECT_EQ(node2D->get_parameter("test.max_planning_time").as_double(), 2.0);
+  EXPECT_EQ(node2D->get_parameter("test.downsample_costmap").as_bool(), false);
+  EXPECT_EQ(node2D->get_parameter("test.allow_unknown").as_bool(), false);
+  EXPECT_EQ(node2D->get_parameter("test.downsampling_factor").as_int(), 2);
+  EXPECT_EQ(node2D->get_parameter("test.max_iterations").as_int(), -1);
+  EXPECT_EQ(node2D->get_parameter("test.use_final_approach_orientation").as_bool(), false);
+  EXPECT_EQ(
+    node2D->get_parameter("test.max_on_approach_iterations").as_int(),
+    -1);
+
+  results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("test.downsample_costmap", true)});
+
+  rclcpp::spin_until_future_complete(
+    node2D->get_node_base_interface(),
+    results);
+}

navigation2/nav2_smac_planner/test/test_smac_hybrid.cpp

@@ -0,0 +1,149 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <math.h>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_smac_planner/node_hybrid.hpp"
+#include "nav2_smac_planner/a_star.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/smac_planner_hybrid.hpp"
+#include "nav2_smac_planner/smac_planner_2d.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+// SMAC smoke tests for plugin-level issues rather than algorithms
+// (covered by more extensively testing in other files)
+// System tests in nav2_system_tests will actually plan with this work
+
+TEST(SmacTest, test_smac_se2)
+{
+  rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 =
+    std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSE2Test");
+
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
+    std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+
+  nodeSE2->declare_parameter("test.downsample_costmap", true);
+  nodeSE2->set_parameter(rclcpp::Parameter("test.downsample_costmap", true));
+  nodeSE2->declare_parameter("test.downsampling_factor", 2);
+  nodeSE2->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2));
+
+  geometry_msgs::msg::PoseStamped start, goal;
+  start.pose.position.x = 0.0;
+  start.pose.position.y = 0.0;
+  start.pose.orientation.w = 1.0;
+  goal.pose.position.x = 1.0;
+  goal.pose.position.y = 1.0;
+  goal.pose.orientation.w = 1.0;
+  auto planner = std::make_unique<nav2_smac_planner::SmacPlannerHybrid>();
+  planner->configure(nodeSE2, "test", nullptr, costmap_ros);
+  planner->activate();
+
+  try {
+    planner->createPlan(start, goal);
+  } catch (...) {
+  }
+
+  planner->deactivate();
+  planner->cleanup();
+
+  planner.reset();
+  costmap_ros->on_cleanup(rclcpp_lifecycle::State());
+  costmap_ros.reset();
+  nodeSE2.reset();
+}
+
+TEST(SmacTest, test_smac_se2_reconfigure)
+{
+  rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 =
+    std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSE2Test");
+
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
+    std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+
+  auto planner = std::make_unique<nav2_smac_planner::SmacPlannerHybrid>();
+  planner->configure(nodeSE2, "test", nullptr, costmap_ros);
+  planner->activate();
+
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    nodeSE2->get_node_base_interface(), nodeSE2->get_node_topics_interface(),
+    nodeSE2->get_node_graph_interface(),
+    nodeSE2->get_node_services_interface());
+
+  auto results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("test.downsample_costmap", true),
+      rclcpp::Parameter("test.downsampling_factor", 2),
+      rclcpp::Parameter("test.angle_quantization_bins", 100),
+      rclcpp::Parameter("test.allow_unknown", false),
+      rclcpp::Parameter("test.max_iterations", -1),
+      rclcpp::Parameter("test.minimum_turning_radius", 1.0),
+      rclcpp::Parameter("test.cache_obstacle_heuristic", true),
+      rclcpp::Parameter("test.reverse_penalty", 5.0),
+      rclcpp::Parameter("test.change_penalty", 1.0),
+      rclcpp::Parameter("test.non_straight_penalty", 2.0),
+      rclcpp::Parameter("test.cost_penalty", 2.0),
+      rclcpp::Parameter("test.tolerance", 0.2),
+      rclcpp::Parameter("test.retrospective_penalty", 0.2),
+      rclcpp::Parameter("test.analytic_expansion_ratio", 4.0),
+      rclcpp::Parameter("test.max_planning_time", 10.0),
+      rclcpp::Parameter("test.lookup_table_size", 30.0),
+      rclcpp::Parameter("test.smooth_path", false),
+      rclcpp::Parameter("test.analytic_expansion_max_length", 42.0),
+      rclcpp::Parameter("test.max_on_approach_iterations", 42),
+      rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))});
+
+  rclcpp::spin_until_future_complete(
+    nodeSE2->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(nodeSE2->get_parameter("test.downsample_costmap").as_bool(), true);
+  EXPECT_EQ(nodeSE2->get_parameter("test.downsampling_factor").as_int(), 2);
+  EXPECT_EQ(nodeSE2->get_parameter("test.angle_quantization_bins").as_int(), 100);
+  EXPECT_EQ(nodeSE2->get_parameter("test.allow_unknown").as_bool(), false);
+  EXPECT_EQ(nodeSE2->get_parameter("test.max_iterations").as_int(), -1);
+  EXPECT_EQ(nodeSE2->get_parameter("test.minimum_turning_radius").as_double(), 1.0);
+  EXPECT_EQ(nodeSE2->get_parameter("test.cache_obstacle_heuristic").as_bool(), true);
+  EXPECT_EQ(nodeSE2->get_parameter("test.reverse_penalty").as_double(), 5.0);
+  EXPECT_EQ(nodeSE2->get_parameter("test.change_penalty").as_double(), 1.0);
+  EXPECT_EQ(nodeSE2->get_parameter("test.non_straight_penalty").as_double(), 2.0);
+  EXPECT_EQ(nodeSE2->get_parameter("test.cost_penalty").as_double(), 2.0);
+  EXPECT_EQ(nodeSE2->get_parameter("test.retrospective_penalty").as_double(), 0.2);
+  EXPECT_EQ(nodeSE2->get_parameter("test.tolerance").as_double(), 0.2);
+  EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_ratio").as_double(), 4.0);
+  EXPECT_EQ(nodeSE2->get_parameter("test.smooth_path").as_bool(), false);
+  EXPECT_EQ(nodeSE2->get_parameter("test.max_planning_time").as_double(), 10.0);
+  EXPECT_EQ(nodeSE2->get_parameter("test.lookup_table_size").as_double(), 30.0);
+  EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_max_length").as_double(), 42.0);
+  EXPECT_EQ(nodeSE2->get_parameter("test.max_on_approach_iterations").as_int(), 42);
+  EXPECT_EQ(
+    nodeSE2->get_parameter("test.motion_model_for_search").as_string(),
+    std::string("REEDS_SHEPP"));
+}

navigation2/nav2_smac_planner/test/test_smac_lattice.cpp

@@ -0,0 +1,146 @@
+// Copyright (c) 2021 Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <math.h>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_smac_planner/node_hybrid.hpp"
+#include "nav2_smac_planner/a_star.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/smac_planner_lattice.hpp"
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+// Simple wrapper to be able to call a private member
+class LatticeWrap : public nav2_smac_planner::SmacPlannerLattice
+{
+public:
+  void callDynamicParams(std::vector<rclcpp::Parameter> parameters)
+  {
+    dynamicParametersCallback(parameters);
+  }
+};
+
+// SMAC smoke tests for plugin-level issues rather than algorithms
+// (covered by more extensively testing in other files)
+// System tests in nav2_system_tests will actually plan with this work
+
+TEST(SmacTest, test_smac_lattice)
+{
+  rclcpp_lifecycle::LifecycleNode::SharedPtr nodeLattice =
+    std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacLatticeTest");
+
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
+    std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+
+  geometry_msgs::msg::PoseStamped start, goal;
+  start.pose.position.x = 0.0;
+  start.pose.position.y = 0.0;
+  start.pose.orientation.w = 1.0;
+  goal.pose.position.x = 1.0;
+  goal.pose.position.y = 1.0;
+  goal.pose.orientation.w = 1.0;
+  auto planner = std::make_unique<nav2_smac_planner::SmacPlannerLattice>();
+  try {
+    // Expect to throw due to invalid prims file in param
+    planner->configure(nodeLattice, "test", nullptr, costmap_ros);
+  } catch (...) {
+  }
+  planner->activate();
+
+  try {
+    planner->createPlan(start, goal);
+  } catch (...) {
+  }
+
+  planner->deactivate();
+  planner->cleanup();
+
+  planner.reset();
+  costmap_ros->on_cleanup(rclcpp_lifecycle::State());
+  costmap_ros.reset();
+  nodeLattice.reset();
+}
+
+TEST(SmacTest, test_smac_lattice_reconfigure)
+{
+  rclcpp_lifecycle::LifecycleNode::SharedPtr nodeLattice =
+    std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacLatticeTest");
+
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
+    std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+
+  auto planner = std::make_unique<LatticeWrap>();
+  try {
+    // Expect to throw due to invalid prims file in param
+    planner->configure(nodeLattice, "test", nullptr, costmap_ros);
+  } catch (...) {
+  }
+  planner->activate();
+
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    nodeLattice->get_node_base_interface(), nodeLattice->get_node_topics_interface(),
+    nodeLattice->get_node_graph_interface(),
+    nodeLattice->get_node_services_interface());
+
+  auto results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("test.allow_unknown", false),
+      rclcpp::Parameter("test.max_iterations", -1),
+      rclcpp::Parameter("test.cache_obstacle_heuristic", true),
+      rclcpp::Parameter("test.reverse_penalty", 5.0),
+      rclcpp::Parameter("test.change_penalty", 1.0),
+      rclcpp::Parameter("test.non_straight_penalty", 2.0),
+      rclcpp::Parameter("test.cost_penalty", 2.0),
+      rclcpp::Parameter("test.retrospective_penalty", 0.2),
+      rclcpp::Parameter("test.analytic_expansion_ratio", 4.0),
+      rclcpp::Parameter("test.max_planning_time", 10.0),
+      rclcpp::Parameter("test.lookup_table_size", 30.0),
+      rclcpp::Parameter("test.smooth_path", false),
+      rclcpp::Parameter("test.analytic_expansion_max_length", 42.0),
+      rclcpp::Parameter("test.tolerance", 42.0),
+      rclcpp::Parameter("test.rotation_penalty", 42.0),
+      rclcpp::Parameter("test.max_on_approach_iterations", 42),
+      rclcpp::Parameter("test.allow_reverse_expansion", true)});
+
+  try {
+    // All of these params will re-init A* which will involve loading the control set file
+    // which will cause an exception because the file does not exist. This will cause an
+    // expected failure preventing parameter updates from being successfully processed
+    rclcpp::spin_until_future_complete(
+      nodeLattice->get_node_base_interface(),
+      results);
+  } catch (...) {
+  }
+
+  // So instead, lets call manually on a change
+  std::vector<rclcpp::Parameter> parameters;
+  parameters.push_back(rclcpp::Parameter("test.lattice_filepath", std::string("HI")));
+  EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error);
+}

navigation2/nav2_smac_planner/test/test_smoother.cpp

@@ -0,0 +1,178 @@
+// Copyright (c) 2020, Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <math.h>
+#include <memory>
+#include <string>
+#include <vector>
+#include <limits>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_smac_planner/node_hybrid.hpp"
+#include "nav2_smac_planner/a_star.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/smoother.hpp"
+#include "ament_index_cpp/get_package_share_directory.hpp"
+
+using namespace nav2_smac_planner;  // NOLINT
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class SmootherWrapper : public nav2_smac_planner::Smoother
+{
+public:
+  explicit SmootherWrapper(const SmootherParams & params)
+  : nav2_smac_planner::Smoother(params)
+  {}
+
+  std::vector<PathSegment> findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path)
+  {
+    return findDirectionalPathSegments(path);
+  }
+};
+
+TEST(SmootherTest, test_full_smoother)
+{
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node =
+    std::make_shared<rclcpp_lifecycle::LifecycleNode>("SmacSmootherTest");
+  nav2_smac_planner::SmootherParams params;
+  params.get(node, "test");
+  double maxtime = 1.0;
+
+  // Make smoother and costmap to smooth in
+  auto smoother = std::make_unique<SmootherWrapper>(params);
+  smoother->initialize(0.4 /*turning radius*/);
+
+  nav2_costmap_2d::Costmap2D * costmap =
+    new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0.0, 0.0, 0);
+  // island in the middle of lethal cost to cross
+  for (unsigned int i = 20; i <= 30; ++i) {
+    for (unsigned int j = 20; j <= 30; ++j) {
+      costmap->setCost(i, j, 254);
+    }
+  }
+
+  // Setup A* search to get path to smooth
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 0.05;
+  info.non_straight_penalty = 1.05;
+  info.reverse_penalty = 2.0;
+  info.cost_penalty = 2.0;
+  info.retrospective_penalty = 0.0;
+  info.analytic_expansion_ratio = 3.5;
+  info.minimum_turning_radius = 8;  // in grid coordinates 0.4/0.05
+  info.analytic_expansion_max_length = 20.0;  // in grid coordinates
+  unsigned int size_theta = 72;
+  nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(
+    nav2_smac_planner::MotionModel::REEDS_SHEPP, info);
+  int max_iterations = 10000;
+  float tolerance = 10.0;
+  int it_on_approach = 10;
+  double max_planning_time = 120.0;
+  int num_it = 0;
+
+  a_star.initialize(
+    false, max_iterations, std::numeric_limits<int>::max(), max_planning_time, 401, size_theta);
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap, size_theta, node);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+
+  // Create A* search to smooth
+  a_star.setCollisionChecker(checker.get());
+  a_star.setStart(5u, 5u, 0u);
+  a_star.setGoal(45u, 45u, 36u);
+  nav2_smac_planner::NodeHybrid::CoordinateVector path;
+  EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
+
+  // Convert to world coordinates and get length to compare to smoothed length
+  nav_msgs::msg::Path plan;
+  plan.header.stamp = node->now();
+  plan.header.frame_id = "map";
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header = plan.header;
+  pose.pose.position.z = 0.0;
+  pose.pose.orientation.x = 0.0;
+  pose.pose.orientation.y = 0.0;
+  pose.pose.orientation.z = 0.0;
+  pose.pose.orientation.w = 1.0;
+  double initial_length = 0.0;
+  double x_m = path[path.size() - 1].x, y_m = path[path.size() - 1].y;
+  plan.poses.reserve(path.size());
+  for (int i = path.size() - 1; i >= 0; --i) {
+    pose.pose = nav2_smac_planner::getWorldCoords(path[i].x, path[i].y, costmap);
+    pose.pose.orientation = nav2_smac_planner::getWorldOrientation(path[i].theta);
+    plan.poses.push_back(pose);
+    initial_length += hypot(path[i].x - x_m, path[i].y - y_m);
+    x_m = path[i].x;
+    y_m = path[i].y;
+  }
+
+  // Check that we accurately detect that this path has a reversing segment
+  EXPECT_EQ(smoother->findDirectionalPathSegmentsWrapper(plan).size(), 2u);
+
+  // Test smoother, should succeed with same number of points
+  // and shorter overall length, while still being collision free.
+  auto path_size_in = plan.poses.size();
+  EXPECT_TRUE(smoother->smooth(plan, costmap, maxtime));
+  EXPECT_EQ(plan.poses.size(), path_size_in);  // Should have same number of poses
+  double length = 0.0;
+  x_m = plan.poses[0].pose.position.x;
+  y_m = plan.poses[0].pose.position.y;
+  for (unsigned int i = 0; i != plan.poses.size(); i++) {
+    // Should be collision free
+    EXPECT_EQ(costmap->getCost(plan.poses[i].pose.position.x, plan.poses[i].pose.position.y), 0);
+    length += hypot(plan.poses[i].pose.position.x - x_m, plan.poses[i].pose.position.y - y_m);
+    x_m = plan.poses[i].pose.position.x;
+    y_m = plan.poses[i].pose.position.y;
+  }
+  EXPECT_LT(length, initial_length);  // Should be shorter
+
+  // Try again but with failure modes
+
+  // Failure mode: not enough iterations to complete
+  params.max_its_ = 0;
+  auto smoother_bypass = std::make_unique<SmootherWrapper>(params);
+  EXPECT_FALSE(smoother_bypass->smooth(plan, costmap, maxtime));
+  params.max_its_ = 1;
+  auto smoother_failure = std::make_unique<SmootherWrapper>(params);
+  EXPECT_FALSE(smoother_failure->smooth(plan, costmap, maxtime));
+
+  // Failure mode: Not enough time
+  double max_no_time = 0.0;
+  EXPECT_FALSE(smoother->smooth(plan, costmap, max_no_time));
+
+  // Failure mode: Path is in collision, do 2x to exercise overlapping point
+  // attempts to update orientation should also fail
+  pose.pose.position.x = 1.25;
+  pose.pose.position.y = 1.25;
+  plan.poses.push_back(pose);
+  plan.poses.push_back(pose);
+  EXPECT_FALSE(smoother->smooth(plan, costmap, maxtime));
+  EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.z, 1.0, 1e-3);
+  EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.x, 0.0, 1e-3);
+  EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.y, 0.0, 1e-3);
+  EXPECT_NEAR(plan.poses.end()[-2].pose.orientation.w, 0.0, 1e-3);
+
+  delete costmap;
+}