Added nav2_ prefix to smac_planner

smac_planner/CMakeLists.txt → nav2_smac_planner/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(smac_planner)
+project(nav2_smac_planner)
 
 set(CMAKE_BUILD_TYPE Release) #Debug, Release
 
@@ -42,7 +42,7 @@ include_directories(
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
 set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
 
-set(library_name smac_planner)
+set(library_name nav2_smac_planner)
 
 set(dependencies
   rclcpp

smac_planner/README.md → nav2_smac_planner/README.md

@@ -16,7 +16,7 @@ We have users reporting using this on:
 
 ## Introduction
 
-The `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 differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support cars, car-like, and ackermann vehicles using the `SmacPlanner` plugin which implements a Hybrid-A\* planner. This plugin is also useful for curvature constrained 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 `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 differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support cars, car-like, and ackermann vehicles using the `SmacPlanner` plugin which implements a Hybrid-A\* planner. This plugin is also useful for curvature constrained 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 `SmacPlanner` fully-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), including hybrid searching, CG smoothing, analytic expansions and hueristic functions.
 
@@ -88,7 +88,7 @@ planner_server:
     use_sim_time: True
 
     GridBased:
-      plugin: "smac_planner/SmacPlanner"
+      plugin: "nav2_smac_planner/SmacPlanner"
       tolerance: 0.5                    # tolerance for planning if unable to reach exact pose, in meters, for 2D node
       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)

smac_planner/include/smac_planner/a_star.hpp → nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -13,8 +13,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__A_STAR_HPP_
-#define SMAC_PLANNER__A_STAR_HPP_
+#ifndef NAV2_SMAC_PLANNER__A_STAR_HPP_
+#define NAV2_SMAC_PLANNER__A_STAR_HPP_
 
 #include <vector>
 #include <iostream>
@@ -26,13 +26,13 @@
 
 #include "nav2_costmap_2d/costmap_2d.hpp"
 
-#include "smac_planner/node_2d.hpp"
-#include "smac_planner/node_se2.hpp"
-#include "smac_planner/node_basic.hpp"
-#include "smac_planner/types.hpp"
-#include "smac_planner/constants.hpp"
+#include "nav2_smac_planner/node_2d.hpp"
+#include "nav2_smac_planner/node_se2.hpp"
+#include "nav2_smac_planner/node_basic.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/constants.hpp"
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 inline double squaredDistance(
@@ -45,7 +45,7 @@ inline double squaredDistance(
 }
 
 /**
- * @class smac_planner::AStarAlgorithm
+ * @class nav2_smac_planner::AStarAlgorithm
  * @brief An A* implementation for planning in a costmap. Templated based on the Node type.
  */
 template<typename NodeT>
@@ -62,7 +62,7 @@ class AStarAlgorithm
   typedef std::function<bool (const unsigned int &, NodeT * &)> NodeGetter;
 
   /**
-   * @struct smac_planner::NodeComparator
+   * @struct nav2_smac_planner::NodeComparator
    * @brief Node comparison for priority queue sorting
    */
   struct NodeComparator
@@ -76,13 +76,13 @@ class AStarAlgorithm
   typedef std::priority_queue<NodeElement, std::vector<NodeElement>, NodeComparator> NodeQueue;
 
   /**
-   * @brief A constructor for smac_planner::PlannerServer
+   * @brief A constructor for nav2_smac_planner::PlannerServer
    * @param neighborhood The type of neighborhood to use for search (4 or 8 connected)
    */
   explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info);
 
   /**
-   * @brief A destructor for smac_planner::AStarAlgorithm
+   * @brief A destructor for nav2_smac_planner::AStarAlgorithm
    */
   ~AStarAlgorithm();
 
@@ -316,6 +316,6 @@ class AStarAlgorithm
   nav2_costmap_2d::Costmap2D * _costmap;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__A_STAR_HPP_
+#endif  // NAV2_SMAC_PLANNER__A_STAR_HPP_

smac_planner/include/smac_planner/collision_checker.hpp → nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp

@@ -13,24 +13,24 @@
 // limitations under the License. Reserved.
 
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
-#include "smac_planner/constants.hpp"
+#include "nav2_smac_planner/constants.hpp"
 
-#ifndef SMAC_PLANNER__COLLISION_CHECKER_HPP_
-#define SMAC_PLANNER__COLLISION_CHECKER_HPP_
+#ifndef NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_
+#define NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 /**
- * @class smac_planner::GridCollisionChecker
+ * @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 smac_planner::GridCollisionChecker
+   * @brief A constructor for nav2_smac_planner::GridCollisionChecker
    * @param costmap The costmap to collision check against
    */
   GridCollisionChecker(
@@ -108,6 +108,6 @@ class GridCollisionChecker
   bool footprint_is_radius_;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__COLLISION_CHECKER_HPP_
+#endif  // NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_

smac_planner/include/smac_planner/constants.hpp → nav2_smac_planner/include/nav2_smac_planner/constants.hpp

@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__CONSTANTS_HPP_
-#define SMAC_PLANNER__CONSTANTS_HPP_
+#ifndef NAV2_SMAC_PLANNER__CONSTANTS_HPP_
+#define NAV2_SMAC_PLANNER__CONSTANTS_HPP_
 
 #include <string>
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 enum class MotionModel
 {
@@ -65,6 +65,6 @@ const float INSCRIBED = 253;
 const float MAX_NON_OBSTACLE = 252;
 const float FREE = 0;
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__CONSTANTS_HPP_
+#endif  // NAV2_SMAC_PLANNER__CONSTANTS_HPP_

smac_planner/include/smac_planner/costmap_downsampler.hpp → nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp

@@ -12,21 +12,21 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
-#define SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
+#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 "smac_planner/constants.hpp"
+#include "nav2_smac_planner/constants.hpp"
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 /**
- * @class smac_planner::CostmapDownsampler
+ * @class nav2_smac_planner::CostmapDownsampler
  * @brief A costmap downsampler for more efficient path planning
  */
 class CostmapDownsampler
@@ -110,6 +110,6 @@ class CostmapDownsampler
   std::unique_ptr<nav2_costmap_2d::Costmap2DPublisher> _downsampled_costmap_pub;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
+#endif  // NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_

smac_planner/include/smac_planner/node_2d.hpp → nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__NODE_2D_HPP_
-#define SMAC_PLANNER__NODE_2D_HPP_
+#ifndef NAV2_SMAC_PLANNER__NODE_2D_HPP_
+#define NAV2_SMAC_PLANNER__NODE_2D_HPP_
 
 #include <math.h>
 #include <vector>
@@ -24,14 +24,14 @@
 #include <utility>
 #include <functional>
 
-#include "smac_planner/constants.hpp"
-#include "smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/constants.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 /**
- * @class smac_planner::Node2D
+ * @class nav2_smac_planner::Node2D
  * @brief Node2D implementation for graph
  */
 class Node2D
@@ -42,7 +42,7 @@ class Node2D
   typedef std::vector<NodePtr> NodeVector;
 
   /**
-   * @class smac_planner::Node2D::Coordinates
+   * @class nav2_smac_planner::Node2D::Coordinates
    * @brief Node2D implementation of coordinate structure
    */
   struct Coordinates
@@ -57,14 +57,14 @@ class Node2D
   typedef std::vector<Coordinates> CoordinateVector;
 
   /**
-   * @brief A constructor for smac_planner::Node2D
+   * @brief A constructor for nav2_smac_planner::Node2D
    * @param cost_in The costmap cost at this node
    * @param index The index of this node for self-reference
    */
   explicit Node2D(unsigned char & cost_in, const unsigned int index);
 
   /**
-   * @brief A destructor for smac_planner::Node2D
+   * @brief A destructor for nav2_smac_planner::Node2D
    */
   ~Node2D();
 
@@ -226,7 +226,7 @@ class Node2D
    */
   static void getNeighbors(
     NodePtr & node,
-    std::function<bool(const unsigned int &, smac_planner::Node2D * &)> & validity_checker,
+    std::function<bool(const unsigned int &, nav2_smac_planner::Node2D * &)> & validity_checker,
     GridCollisionChecker collision_checker,
     const bool & traverse_unknown,
     NodeVector & neighbors);
@@ -243,6 +243,6 @@ class Node2D
   bool _is_queued;
 };
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__NODE_2D_HPP_
+#endif  // NAV2_SMAC_PLANNER__NODE_2D_HPP_

smac_planner/include/smac_planner/node_basic.hpp → nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef SMAC_PLANNER__NODE_BASIC_HPP_
-#define SMAC_PLANNER__NODE_BASIC_HPP_
+#ifndef NAV2_SMAC_PLANNER__NODE_BASIC_HPP_
+#define NAV2_SMAC_PLANNER__NODE_BASIC_HPP_
 
 #include <math.h>
 #include <vector>
@@ -27,25 +27,25 @@
 
 #include "ompl/base/StateSpace.h"
 
-#include "smac_planner/constants.hpp"
-#include "smac_planner/node_se2.hpp"
-#include "smac_planner/node_2d.hpp"
-#include "smac_planner/types.hpp"
-#include "smac_planner/collision_checker.hpp"
+#include "nav2_smac_planner/constants.hpp"
+#include "nav2_smac_planner/node_se2.hpp"
+#include "nav2_smac_planner/node_2d.hpp"
+#include "nav2_smac_planner/types.hpp"
+#include "nav2_smac_planner/collision_checker.hpp"
 
-namespace smac_planner
+namespace nav2_smac_planner
 {
 
 /**
- * @class smac_planner::NodeBasic
+ * @class nav2_smac_planner::NodeBasic
  * @brief NodeBasic implementation for priority queue insertion
  */
 template<typename NodeT>
 class NodeBasic
 {
 public:
   /**
-   * @brief A constructor for smac_planner::NodeBasic
+   * @brief A constructor for nav2_smac_planner::NodeBasic
    * @param cost_in The costmap cost at this node
    * @param index The index of this node for self-reference
    */
@@ -63,6 +63,6 @@ class NodeBasic
 template class NodeBasic<Node2D>;
 template class NodeBasic<NodeSE2>;
 
-}  // namespace smac_planner
+}  // namespace nav2_smac_planner
 
-#endif  // SMAC_PLANNER__NODE_BASIC_HPP_
+#endif  // NAV2_SMAC_PLANNER__NODE_BASIC_HPP_