diff --git a/smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt similarity index 98% rename from smac_planner/CMakeLists.txt rename to nav2_smac_planner/CMakeLists.txt index d161ba516e8..e15c474d5f4 100644 --- a/smac_planner/CMakeLists.txt +++ b/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 diff --git a/smac_planner/README.md b/nav2_smac_planner/README.md similarity index 93% rename from smac_planner/README.md rename to nav2_smac_planner/README.md index 5bf0cd4217b..42beb8955e2 100644 --- a/smac_planner/README.md +++ b/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) diff --git a/smac_planner/include/smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp similarity index 93% rename from smac_planner/include/smac_planner/a_star.hpp rename to nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index a53b872a913..6158422a7d7 100644 --- a/smac_planner/include/smac_planner/a_star.hpp +++ b/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 #include @@ -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 @@ -62,7 +62,7 @@ class AStarAlgorithm typedef std::function 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, 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_ diff --git a/smac_planner/include/smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp similarity index 89% rename from smac_planner/include/smac_planner/collision_checker.hpp rename to nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index 588514901ff..6b77d13a55a 100644 --- a/smac_planner/include/smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -13,16 +13,16 @@ // 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 @@ -30,7 +30,7 @@ class GridCollisionChecker { 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_ diff --git a/smac_planner/include/smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp similarity index 89% rename from smac_planner/include/smac_planner/constants.hpp rename to nav2_smac_planner/include/nav2_smac_planner/constants.hpp index bc2ccfd8c63..1a67c746e24 100644 --- a/smac_planner/include/smac_planner/constants.hpp +++ b/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 -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_ diff --git a/smac_planner/include/smac_planner/costmap_downsampler.hpp b/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp similarity index 91% rename from smac_planner/include/smac_planner/costmap_downsampler.hpp rename to nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp index 9521b15fac0..95d12a7f02e 100644 --- a/smac_planner/include/smac_planner/costmap_downsampler.hpp +++ b/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 #include #include #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 _downsampled_costmap_pub; }; -} // namespace smac_planner +} // namespace nav2_smac_planner -#endif // SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ +#endif // NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ diff --git a/smac_planner/include/smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp similarity index 90% rename from smac_planner/include/smac_planner/node_2d.hpp rename to nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index b80a316cbb1..688e6ea5b97 100644 --- a/smac_planner/include/smac_planner/node_2d.hpp +++ b/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 #include @@ -24,14 +24,14 @@ #include #include -#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 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 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 & validity_checker, + std::function & 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_ diff --git a/smac_planner/include/smac_planner/node_basic.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp similarity index 72% rename from smac_planner/include/smac_planner/node_basic.hpp rename to nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp index 3f3941717d1..290ca131624 100644 --- a/smac_planner/include/smac_planner/node_basic.hpp +++ b/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 #include @@ -27,17 +27,17 @@ #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 @@ -45,7 +45,7 @@ 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; template class NodeBasic; -} // namespace smac_planner +} // namespace nav2_smac_planner -#endif // SMAC_PLANNER__NODE_BASIC_HPP_ +#endif // NAV2_SMAC_PLANNER__NODE_BASIC_HPP_ diff --git a/smac_planner/include/smac_planner/node_se2.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_se2.hpp similarity index 90% rename from smac_planner/include/smac_planner/node_se2.hpp rename to nav2_smac_planner/include/nav2_smac_planner/node_se2.hpp index 4fb1ae579dc..4ea431c1875 100644 --- a/smac_planner/include/smac_planner/node_se2.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_se2.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef SMAC_PLANNER__NODE_SE2_HPP_ -#define SMAC_PLANNER__NODE_SE2_HPP_ +#ifndef NAV2_SMAC_PLANNER__NODE_SE2_HPP_ +#define NAV2_SMAC_PLANNER__NODE_SE2_HPP_ #include #include @@ -27,28 +27,28 @@ #include "ompl/base/StateSpace.h" -#include "smac_planner/constants.hpp" -#include "smac_planner/types.hpp" -#include "smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/constants.hpp" +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/collision_checker.hpp" -namespace smac_planner +namespace nav2_smac_planner { // Need seperate pose struct for motion table operations /** - * @struct smac_planner::MotionPose + * @struct nav2_smac_planner::MotionPose * @brief A struct for poses in motion primitives */ struct MotionPose { /** - * @brief A constructor for smac_planner::MotionPose + * @brief A constructor for nav2_smac_planner::MotionPose */ MotionPose() {} /** - * @brief A constructor for smac_planner::MotionPose + * @brief A constructor for nav2_smac_planner::MotionPose * @param x X pose * @param y Y pose * @param theta Angle of pose @@ -68,13 +68,13 @@ typedef std::vector MotionPoses; class NodeSE2; /** - * @struct smac_planner::MotionTable + * @struct nav2_smac_planner::MotionTable * @brief A table of motion primitives and related functions */ struct MotionTable { /** - * @brief A constructor for smac_planner::MotionTable + * @brief A constructor for nav2_smac_planner::MotionTable */ MotionTable() {} @@ -131,7 +131,7 @@ struct MotionTable }; /** - * @class smac_planner::NodeSE2 + * @class nav2_smac_planner::NodeSE2 * @brief NodeSE2 implementation for graph */ class NodeSE2 @@ -141,18 +141,18 @@ class NodeSE2 typedef std::unique_ptr> Graph; typedef std::vector NodeVector; /** - * @class smac_planner::NodeSE2::Coordinates + * @class nav2_smac_planner::NodeSE2::Coordinates * @brief NodeSE2 implementation of coordinate structure */ struct Coordinates { /** - * @brief A constructor for smac_planner::NodeSE2::Coordinates + * @brief A constructor for nav2_smac_planner::NodeSE2::Coordinates */ Coordinates() {} /** - * @brief A constructor for smac_planner::NodeSE2::Coordinates + * @brief A constructor for nav2_smac_planner::NodeSE2::Coordinates * @param x_in X coordinate * @param y_in Y coordinate * @param theta_in Theta coordinate @@ -167,13 +167,13 @@ class NodeSE2 typedef std::vector CoordinateVector; /** - * @brief A constructor for smac_planner::NodeSE2 + * @brief A constructor for nav2_smac_planner::NodeSE2 * @param index The index of this node for self-reference */ explicit NodeSE2(const unsigned int index); /** - * @brief A destructor for smac_planner::NodeSE2 + * @brief A destructor for nav2_smac_planner::NodeSE2 */ ~NodeSE2(); @@ -398,7 +398,7 @@ class NodeSE2 */ static void getNeighbors( const NodePtr & node, - std::function & validity_checker, + std::function & validity_checker, GridCollisionChecker collision_checker, const bool & traverse_unknown, NodeVector & neighbors); @@ -418,6 +418,6 @@ class NodeSE2 static std::vector _wavefront_heuristic; }; -} // namespace smac_planner +} // namespace nav2_smac_planner -#endif // SMAC_PLANNER__NODE_SE2_HPP_ +#endif // NAV2_SMAC_PLANNER__NODE_SE2_HPP_ diff --git a/smac_planner/include/smac_planner/options.hpp b/nav2_smac_planner/include/nav2_smac_planner/options.hpp similarity index 95% rename from smac_planner/include/smac_planner/options.hpp rename to nav2_smac_planner/include/nav2_smac_planner/options.hpp index 01951d8950a..159ab8b53aa 100644 --- a/smac_planner/include/smac_planner/options.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/options.hpp @@ -12,24 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef SMAC_PLANNER__OPTIONS_HPP_ -#define SMAC_PLANNER__OPTIONS_HPP_ +#ifndef NAV2_SMAC_PLANNER__OPTIONS_HPP_ +#define NAV2_SMAC_PLANNER__OPTIONS_HPP_ #include #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" -namespace smac_planner +namespace nav2_smac_planner { /** - * @struct smac_planner::SmootherParams + * @struct nav2_smac_planner::SmootherParams * @brief Parameters for the smoother cost function */ struct SmootherParams { /** - * @brief A constructor for smac_planner::SmootherParams + * @brief A constructor for nav2_smac_planner::SmootherParams */ SmootherParams() { @@ -72,7 +72,7 @@ struct SmootherParams }; /** - * @struct smac_planner::OptimizerParams + * @struct nav2_smac_planner::OptimizerParams * @brief Parameters for the ceres optimizer */ struct OptimizerParams @@ -202,6 +202,6 @@ struct OptimizerParams AdvancedParams advanced; }; -} // namespace smac_planner +} // namespace nav2_smac_planner -#endif // SMAC_PLANNER__OPTIONS_HPP_ +#endif // NAV2_SMAC_PLANNER__OPTIONS_HPP_ diff --git a/smac_planner/include/smac_planner/smac_planner.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner.hpp similarity index 91% rename from smac_planner/include/smac_planner/smac_planner.hpp rename to nav2_smac_planner/include/nav2_smac_planner/smac_planner.hpp index 7617c481fb1..0f49dc7a4c7 100644 --- a/smac_planner/include/smac_planner/smac_planner.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef SMAC_PLANNER__SMAC_PLANNER_HPP_ -#define SMAC_PLANNER__SMAC_PLANNER_HPP_ +#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_HPP_ +#define NAV2_SMAC_PLANNER__SMAC_PLANNER_HPP_ #include #include #include -#include "smac_planner/a_star.hpp" -#include "smac_planner/smoother.hpp" -#include "smac_planner/costmap_downsampler.hpp" +#include "nav2_smac_planner/a_star.hpp" +#include "nav2_smac_planner/smoother.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" @@ -32,7 +32,7 @@ #include "nav2_util/node_utils.hpp" #include "tf2/utils.h" -namespace smac_planner +namespace nav2_smac_planner { class SmacPlanner : public nav2_core::GlobalPlanner @@ -127,6 +127,6 @@ class SmacPlanner : public nav2_core::GlobalPlanner double _max_planning_time; }; -} // namespace smac_planner +} // namespace nav2_smac_planner -#endif // SMAC_PLANNER__SMAC_PLANNER_HPP_ +#endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_HPP_ diff --git a/smac_planner/include/smac_planner/smac_planner_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp similarity index 90% rename from smac_planner/include/smac_planner/smac_planner_2d.hpp rename to nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp index 3b6331fdb4c..31f10496092 100644 --- a/smac_planner/include/smac_planner/smac_planner_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ -#define SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ +#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ +#define NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ #include #include #include -#include "smac_planner/a_star.hpp" -#include "smac_planner/smoother.hpp" -#include "smac_planner/costmap_downsampler.hpp" +#include "nav2_smac_planner/a_star.hpp" +#include "nav2_smac_planner/smoother.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" @@ -32,7 +32,7 @@ #include "nav2_util/node_utils.hpp" #include "tf2/utils.h" -namespace smac_planner +namespace nav2_smac_planner { class SmacPlanner2D : public nav2_core::GlobalPlanner @@ -118,6 +118,6 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner double _max_planning_time; }; -} // namespace smac_planner +} // namespace nav2_smac_planner -#endif // SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ +#endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_ diff --git a/smac_planner/include/smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp similarity index 90% rename from smac_planner/include/smac_planner/smoother.hpp rename to nav2_smac_planner/include/nav2_smac_planner/smoother.hpp index 756e6b57939..b3b0fe82375 100644 --- a/smac_planner/include/smac_planner/smoother.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef SMAC_PLANNER__SMOOTHER_HPP_ -#define SMAC_PLANNER__SMOOTHER_HPP_ +#ifndef NAV2_SMAC_PLANNER__SMOOTHER_HPP_ +#define NAV2_SMAC_PLANNER__SMOOTHER_HPP_ #include #include @@ -22,29 +22,29 @@ #include #include -#include "smac_planner/types.hpp" -#include "smac_planner/smoother_cost_function.hpp" +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/smoother_cost_function.hpp" #include "ceres/ceres.h" #include "Eigen/Core" -namespace smac_planner +namespace nav2_smac_planner { /** - * @class smac_planner::Smoother + * @class nav2_smac_planner::Smoother * @brief A Conjugate Gradient 2D path smoother implementation */ class Smoother { public: /** - * @brief A constructor for smac_planner::Smoother + * @brief A constructor for nav2_smac_planner::Smoother */ Smoother() {} /** - * @brief A destructor for smac_planner::Smoother + * @brief A destructor for nav2_smac_planner::Smoother */ ~Smoother() {} @@ -136,6 +136,6 @@ class Smoother ceres::GradientProblemSolver::Options _options; }; -} // namespace smac_planner +} // namespace nav2_smac_planner -#endif // SMAC_PLANNER__SMOOTHER_HPP_ +#endif // NAV2_SMAC_PLANNER__SMOOTHER_HPP_ diff --git a/smac_planner/include/smac_planner/smoother_cost_function.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother_cost_function.hpp similarity index 96% rename from smac_planner/include/smac_planner/smoother_cost_function.hpp rename to nav2_smac_planner/include/nav2_smac_planner/smoother_cost_function.hpp index 82d1c88a6dc..eec8c61b4e3 100644 --- a/smac_planner/include/smac_planner/smoother_cost_function.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smoother_cost_function.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ -#define SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ +#ifndef NAV2_SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ +#define NAV2_SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ #include #include @@ -25,17 +25,17 @@ #include "ceres/ceres.h" #include "Eigen/Core" -#include "smac_planner/types.hpp" +#include "nav2_smac_planner/types.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" -#include "smac_planner/options.hpp" +#include "nav2_smac_planner/options.hpp" #define EPSILON 0.0001 -namespace smac_planner +namespace nav2_smac_planner { /** - * @struct smac_planner::UnconstrainedSmootherCostFunction + * @struct nav2_smac_planner::UnconstrainedSmootherCostFunction * @brief Cost function for path smoothing with multiple terms * including curvature, smoothness, collision, and avoid obstacles. */ @@ -43,7 +43,7 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction { public: /** - * @brief A constructor for smac_planner::UnconstrainedSmootherCostFunction + * @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 */ @@ -65,7 +65,7 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction struct CurvatureComputations { /** - * @brief A constructor for smac_planner::CurvatureComputations + * @brief A constructor for nav2_smac_planner::CurvatureComputations */ CurvatureComputations() { @@ -510,6 +510,6 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction SmootherParams _params; }; -} // namespace smac_planner +} // namespace nav2_smac_planner -#endif // SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ +#endif // NAV2_SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ diff --git a/smac_planner/include/smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp similarity index 81% rename from smac_planner/include/smac_planner/types.hpp rename to nav2_smac_planner/include/nav2_smac_planner/types.hpp index e39564d333b..eeeb9218737 100644 --- a/smac_planner/include/smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef SMAC_PLANNER__TYPES_HPP_ -#define SMAC_PLANNER__TYPES_HPP_ +#ifndef NAV2_SMAC_PLANNER__TYPES_HPP_ +#define NAV2_SMAC_PLANNER__TYPES_HPP_ #include #include -namespace smac_planner +namespace nav2_smac_planner { typedef std::pair NodeHeuristicPair; /** - * @struct smac_planner::SearchInfo + * @struct nav2_smac_planner::SearchInfo * @brief Search properties and penalties */ struct SearchInfo @@ -37,6 +37,6 @@ struct SearchInfo float analytic_expansion_ratio; }; -} // namespace smac_planner +} // namespace nav2_smac_planner -#endif // SMAC_PLANNER__TYPES_HPP_ +#endif // NAV2_SMAC_PLANNER__TYPES_HPP_ diff --git a/smac_planner/package.xml b/nav2_smac_planner/package.xml similarity index 97% rename from smac_planner/package.xml rename to nav2_smac_planner/package.xml index 8de16907864..a266e7d50a6 100644 --- a/smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -1,7 +1,7 @@ - smac_planner + nav2_smac_planner 0.3.0 Smac global planning plugin Steve Macenski diff --git a/nav2_smac_planner/smac_plugin.xml b/nav2_smac_planner/smac_plugin.xml new file mode 100644 index 00000000000..c28cb32c7f9 --- /dev/null +++ b/nav2_smac_planner/smac_plugin.xml @@ -0,0 +1,5 @@ + + + SE2 version of the SMAC planner + + diff --git a/nav2_smac_planner/smac_plugin_2d.xml b/nav2_smac_planner/smac_plugin_2d.xml new file mode 100644 index 00000000000..aab22602e58 --- /dev/null +++ b/nav2_smac_planner/smac_plugin_2d.xml @@ -0,0 +1,5 @@ + + + 2D version of the SMAC Planner + + diff --git a/smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp similarity index 99% rename from smac_planner/src/a_star.cpp rename to nav2_smac_planner/src/a_star.cpp index fae4694f758..788f71cba09 100644 --- a/smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -30,10 +30,10 @@ #include #include -#include "smac_planner/a_star.hpp" +#include "nav2_smac_planner/a_star.hpp" using namespace std::chrono; // NOLINT -namespace smac_planner +namespace nav2_smac_planner { template @@ -648,4 +648,4 @@ typename AStarAlgorithm::NodePtr AStarAlgorithm::tryAnalyticExpans template class AStarAlgorithm; template class AStarAlgorithm; -} // namespace smac_planner +} // namespace nav2_smac_planner diff --git a/smac_planner/src/costmap_downsampler.cpp b/nav2_smac_planner/src/costmap_downsampler.cpp similarity index 97% rename from smac_planner/src/costmap_downsampler.cpp rename to nav2_smac_planner/src/costmap_downsampler.cpp index 4e4824f4567..c7225234232 100644 --- a/smac_planner/src/costmap_downsampler.cpp +++ b/nav2_smac_planner/src/costmap_downsampler.cpp @@ -13,13 +13,13 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include "smac_planner/costmap_downsampler.hpp" +#include "nav2_smac_planner/costmap_downsampler.hpp" #include #include #include -namespace smac_planner +namespace nav2_smac_planner { CostmapDownsampler::CostmapDownsampler() @@ -139,4 +139,4 @@ void CostmapDownsampler::setCostOfCell( _downsampled_costmap->setCost(new_mx, new_my, cost); } -} // namespace smac_planner +} // namespace nav2_smac_planner diff --git a/smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp similarity index 96% rename from smac_planner/src/node_2d.cpp rename to nav2_smac_planner/src/node_2d.cpp index b6afd5b944b..cd8a2238791 100644 --- a/smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -13,12 +13,12 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include "smac_planner/node_2d.hpp" +#include "nav2_smac_planner/node_2d.hpp" #include #include -namespace smac_planner +namespace nav2_smac_planner { // defining static member for all instance to share @@ -118,7 +118,7 @@ void Node2D::initNeighborhood( void Node2D::getNeighbors( NodePtr & node, - std::function & NeighborGetter, + std::function & NeighborGetter, GridCollisionChecker collision_checker, const bool & traverse_unknown, NodeVector & neighbors) @@ -148,4 +148,4 @@ void Node2D::getNeighbors( } } -} // namespace smac_planner +} // namespace nav2_smac_planner diff --git a/smac_planner/src/node_se2.cpp b/nav2_smac_planner/src/node_se2.cpp similarity index 98% rename from smac_planner/src/node_se2.cpp rename to nav2_smac_planner/src/node_se2.cpp index 9764df0dbd2..9bc2516d12b 100644 --- a/smac_planner/src/node_se2.cpp +++ b/nav2_smac_planner/src/node_se2.cpp @@ -25,11 +25,11 @@ #include "ompl/base/spaces/DubinsStateSpace.h" #include "ompl/base/spaces/ReedsSheppStateSpace.h" -#include "smac_planner/node_se2.hpp" +#include "nav2_smac_planner/node_se2.hpp" using namespace std::chrono; // NOLINT -namespace smac_planner +namespace nav2_smac_planner { // defining static member for all instance to share @@ -393,7 +393,7 @@ void NodeSE2::computeWavefrontHeuristic( void NodeSE2::getNeighbors( const NodePtr & node, - std::function & NeighborGetter, + std::function & NeighborGetter, GridCollisionChecker collision_checker, const bool & traverse_unknown, NodeVector & neighbors) @@ -429,4 +429,4 @@ void NodeSE2::getNeighbors( } } -} // namespace smac_planner +} // namespace nav2_smac_planner diff --git a/smac_planner/src/smac_planner.cpp b/nav2_smac_planner/src/smac_planner.cpp similarity index 98% rename from smac_planner/src/smac_planner.cpp rename to nav2_smac_planner/src/smac_planner.cpp index 8d56f49a9d1..b22b61a2009 100644 --- a/smac_planner/src/smac_planner.cpp +++ b/nav2_smac_planner/src/smac_planner.cpp @@ -19,11 +19,11 @@ #include #include "Eigen/Core" -#include "smac_planner/smac_planner.hpp" +#include "nav2_smac_planner/smac_planner.hpp" #define BENCHMARK_TESTING -namespace smac_planner +namespace nav2_smac_planner { using namespace std::chrono; // NOLINT @@ -382,7 +382,7 @@ geometry_msgs::msg::Quaternion SmacPlanner::getWorldOrientation(const float & th return tf2::toMsg(q); } -} // namespace smac_planner +} // namespace nav2_smac_planner #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(smac_planner::SmacPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlanner, nav2_core::GlobalPlanner) diff --git a/smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp similarity index 98% rename from smac_planner/src/smac_planner_2d.cpp rename to nav2_smac_planner/src/smac_planner_2d.cpp index 820218f5f1a..f8c82f08a1f 100644 --- a/smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -18,11 +18,11 @@ #include #include -#include "smac_planner/smac_planner_2d.hpp" +#include "nav2_smac_planner/smac_planner_2d.hpp" // #define BENCHMARK_TESTING -namespace smac_planner +namespace nav2_smac_planner { using namespace std::chrono; // NOLINT @@ -336,7 +336,7 @@ Eigen::Vector2d SmacPlanner2D::getWorldCoords( return Eigen::Vector2d(world_x, world_y); } -} // namespace smac_planner +} // namespace nav2_smac_planner #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(smac_planner::SmacPlanner2D, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlanner2D, nav2_core::GlobalPlanner) diff --git a/smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt similarity index 100% rename from smac_planner/test/CMakeLists.txt rename to nav2_smac_planner/test/CMakeLists.txt diff --git a/smac_planner/test/deprecated_upsampler/upsampler.hpp b/nav2_smac_planner/test/deprecated_upsampler/upsampler.hpp similarity index 94% rename from smac_planner/test/deprecated_upsampler/upsampler.hpp rename to nav2_smac_planner/test/deprecated_upsampler/upsampler.hpp index 8bfc36c00e5..84ba625fa11 100644 --- a/smac_planner/test/deprecated_upsampler/upsampler.hpp +++ b/nav2_smac_planner/test/deprecated_upsampler/upsampler.hpp @@ -23,30 +23,30 @@ #include #include -#include "smac_planner/types.hpp" -#include "smac_planner/upsampler_cost_function.hpp" -#include "smac_planner/upsampler_cost_function_nlls.hpp" +#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 smac_planner +namespace nav2_smac_planner { /** - * @class smac_planner::Upsampler + * @class nav2_smac_planner::Upsampler * @brief A Conjugate Gradient 2D path upsampler implementation */ class Upsampler { public: /** - * @brief A constructor for smac_planner::Upsampler + * @brief A constructor for nav2_smac_planner::Upsampler */ Upsampler() {} /** - * @brief A destructor for smac_planner::Upsampler + * @brief A destructor for nav2_smac_planner::Upsampler */ ~Upsampler() {} @@ -208,6 +208,6 @@ class Upsampler ceres::GradientProblemSolver::Options _options; }; -} // namespace smac_planner +} // namespace nav2_smac_planner #endif // DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ diff --git a/smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp b/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp similarity index 97% rename from smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp rename to nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp index cf84acf6234..44df1c3f228 100644 --- a/smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp +++ b/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp @@ -25,15 +25,15 @@ #include "ceres/ceres.h" #include "Eigen/Core" -#include "smac_planner/types.hpp" -#include "smac_planner/options.hpp" +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/options.hpp" #define EPSILON 0.0001 -namespace smac_planner +namespace nav2_smac_planner { /** - * @struct smac_planner::UpsamplerCostFunction + * @struct nav2_smac_planner::UpsamplerCostFunction * @brief Cost function for path upsampling with multiple terms using unconstrained * optimization including curvature, smoothness, collision, and avoid obstacles. */ @@ -41,7 +41,7 @@ class UpsamplerCostFunction : public ceres::FirstOrderFunction { public: /** - * @brief A constructor for smac_planner::UpsamplerCostFunction + * @brief A constructor for nav2_smac_planner::UpsamplerCostFunction * @param num_points Number of path points to consider */ UpsamplerCostFunction( @@ -63,7 +63,7 @@ class UpsamplerCostFunction : public ceres::FirstOrderFunction struct CurvatureComputations { /** - * @brief A constructor for smac_planner::CurvatureComputations + * @brief A constructor for nav2_smac_planner::CurvatureComputations */ CurvatureComputations() { @@ -361,6 +361,6 @@ class UpsamplerCostFunction : public ceres::FirstOrderFunction std::vector _path; }; -} // namespace smac_planner +} // namespace nav2_smac_planner #endif // DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ diff --git a/smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp b/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp similarity index 96% rename from smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp rename to nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp index a6fd595f969..17204694b1b 100644 --- a/smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp +++ b/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp @@ -25,15 +25,15 @@ #include "ceres/ceres.h" #include "Eigen/Core" -#include "smac_planner/types.hpp" -#include "smac_planner/options.hpp" +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/options.hpp" #define EPSILON 0.0001 -namespace smac_planner +namespace nav2_smac_planner { /** - * @struct smac_planner::UpsamplerConstrainedCostFunction + * @struct nav2_smac_planner::UpsamplerConstrainedCostFunction * @brief Cost function for path upsampling with multiple terms using NLLS * including curvature, smoothness, collision, and avoid obstacles. */ @@ -41,7 +41,7 @@ class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1 { public: /** - * @brief A constructor for smac_planner::UpsamplerConstrainedCostFunction + * @brief A constructor for nav2_smac_planner::UpsamplerConstrainedCostFunction * @param num_points Number of path points to consider */ UpsamplerConstrainedCostFunction( @@ -63,7 +63,7 @@ class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1 struct CurvatureComputations { /** - * @brief A constructor for smac_planner::CurvatureComputations + * @brief A constructor for nav2_smac_planner::CurvatureComputations */ CurvatureComputations() { @@ -329,6 +329,6 @@ class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1 int index; }; -} // namespace smac_planner +} // namespace nav2_smac_planner #endif // DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ diff --git a/smac_planner/test/path.png b/nav2_smac_planner/test/path.png similarity index 100% rename from smac_planner/test/path.png rename to nav2_smac_planner/test/path.png diff --git a/smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp similarity index 73% rename from smac_planner/test/test_a_star.cpp rename to nav2_smac_planner/test/test_a_star.cpp index 091ee0cf00a..721eee47100 100644 --- a/smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -22,9 +22,9 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" -#include "smac_planner/node_se2.hpp" -#include "smac_planner/a_star.hpp" -#include "smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/a_star.hpp" +#include "nav2_smac_planner/collision_checker.hpp" class RclCppFixture { @@ -36,8 +36,9 @@ RclCppFixture g_rclcppfixture; TEST(AStarTest, test_a_star_2d) { - smac_planner::SearchInfo info; - smac_planner::AStarAlgorithm a_star(smac_planner::MotionModel::MOORE, info); + nav2_smac_planner::SearchInfo info; + nav2_smac_planner::AStarAlgorithm a_star( + nav2_smac_planner::MotionModel::MOORE, info); int max_iterations = 10000; float tolerance = 0.0; float some_tolerance = 20.0; @@ -60,7 +61,7 @@ TEST(AStarTest, test_a_star_2d) a_star.createGraph(costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 1, costmapA); a_star.setStart(20u, 20u, 0); a_star.setGoal(80u, 80u, 0); - smac_planner::Node2D::CoordinateVector path; + nav2_smac_planner::Node2D::CoordinateVector path; EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); EXPECT_EQ(num_it, 556); @@ -79,8 +80,8 @@ TEST(AStarTest, test_a_star_2d) path.clear(); // failure cases with invalid inputs - smac_planner::AStarAlgorithm a_star_2( - smac_planner::MotionModel::VON_NEUMANN, info); + nav2_smac_planner::AStarAlgorithm a_star_2( + nav2_smac_planner::MotionModel::VON_NEUMANN, info); a_star_2.initialize(false, max_iterations, it_on_approach); a_star_2.setFootprint(nav2_costmap_2d::Footprint(), true); num_it = 0; @@ -119,14 +120,14 @@ TEST(AStarTest, test_a_star_2d) TEST(AStarTest, test_a_star_se2) { - smac_planner::SearchInfo info; + 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 = 2.0; // in grid coordinates unsigned int size_theta = 72; - smac_planner::AStarAlgorithm a_star( - smac_planner::MotionModel::DUBIN, info); + nav2_smac_planner::AStarAlgorithm a_star( + nav2_smac_planner::MotionModel::DUBIN, info); int max_iterations = 10000; float tolerance = 10.0; int it_on_approach = 10; @@ -149,7 +150,7 @@ TEST(AStarTest, test_a_star_se2) costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), size_theta, costmapA); a_star.setStart(10u, 10u, 0u); a_star.setGoal(80u, 80u, 40u); - smac_planner::NodeSE2::CoordinateVector path; + nav2_smac_planner::NodeSE2::CoordinateVector path; EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); // check path is the right size and collision free @@ -164,20 +165,24 @@ TEST(AStarTest, test_a_star_se2) TEST(AStarTest, test_constants) { - smac_planner::MotionModel mm = smac_planner::MotionModel::UNKNOWN; // unknown - EXPECT_EQ(smac_planner::toString(mm), std::string("Unknown")); - mm = smac_planner::MotionModel::VON_NEUMANN; // vonneumann - EXPECT_EQ(smac_planner::toString(mm), std::string("Von Neumann")); - mm = smac_planner::MotionModel::MOORE; // moore - EXPECT_EQ(smac_planner::toString(mm), std::string("Moore")); - mm = smac_planner::MotionModel::DUBIN; // dubin - EXPECT_EQ(smac_planner::toString(mm), std::string("Dubin")); - mm = smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp - EXPECT_EQ(smac_planner::toString(mm), std::string("Reeds-Shepp")); - - EXPECT_EQ(smac_planner::fromString("VON_NEUMANN"), smac_planner::MotionModel::VON_NEUMANN); - EXPECT_EQ(smac_planner::fromString("MOORE"), smac_planner::MotionModel::MOORE); - EXPECT_EQ(smac_planner::fromString("DUBIN"), smac_planner::MotionModel::DUBIN); - EXPECT_EQ(smac_planner::fromString("REEDS_SHEPP"), smac_planner::MotionModel::REEDS_SHEPP); - EXPECT_EQ(smac_planner::fromString("NONE"), smac_planner::MotionModel::UNKNOWN); + 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::VON_NEUMANN; // vonneumann + EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Von Neumann")); + mm = nav2_smac_planner::MotionModel::MOORE; // moore + EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Moore")); + 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( + "VON_NEUMANN"), nav2_smac_planner::MotionModel::VON_NEUMANN); + EXPECT_EQ(nav2_smac_planner::fromString("MOORE"), nav2_smac_planner::MotionModel::MOORE); + 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); } diff --git a/smac_planner/test/test_collision_checker.cpp b/nav2_smac_planner/test/test_collision_checker.cpp similarity index 92% rename from smac_planner/test/test_collision_checker.cpp rename to nav2_smac_planner/test/test_collision_checker.cpp index e6590d610de..687b4b5cc35 100644 --- a/smac_planner/test/test_collision_checker.cpp +++ b/nav2_smac_planner/test/test_collision_checker.cpp @@ -18,7 +18,7 @@ #include #include "gtest/gtest.h" -#include "smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/collision_checker.hpp" using namespace nav2_costmap_2d; // NOLINT @@ -41,7 +41,7 @@ TEST(collision_footprint, test_basic) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); collision_checker.setFootprint(footprint, false /*use footprint*/); collision_checker.inCollision(5.0, 5.0, 0.0, false); float cost = collision_checker.getCost(); @@ -53,7 +53,7 @@ TEST(collision_footprint, test_point_cost) { nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); - smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); nav2_costmap_2d::Footprint footprint; collision_checker.setFootprint(footprint, true /*radius / pointcose*/); @@ -67,7 +67,7 @@ TEST(collision_footprint, test_world_to_map) { nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); - smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); nav2_costmap_2d::Footprint footprint; collision_checker.setFootprint(footprint, true /*radius / point cost*/); @@ -113,7 +113,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); collision_checker.setFootprint(footprint, false /*use footprint*/); collision_checker.inCollision(50, 50, 0.0, false); @@ -154,7 +154,7 @@ TEST(collision_footprint, test_point_and_line_cost) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); collision_checker.setFootprint(footprint, false /*use footprint*/); collision_checker.inCollision(50, 50, 0.0, false); diff --git a/smac_planner/test/test_costmap_downsampler.cpp b/nav2_smac_planner/test/test_costmap_downsampler.cpp similarity index 95% rename from smac_planner/test/test_costmap_downsampler.cpp rename to nav2_smac_planner/test/test_costmap_downsampler.cpp index 5353dcb3871..1317c343094 100644 --- a/smac_planner/test/test_costmap_downsampler.cpp +++ b/nav2_smac_planner/test/test_costmap_downsampler.cpp @@ -19,7 +19,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_util/lifecycle_node.hpp" -#include "smac_planner/costmap_downsampler.hpp" +#include "nav2_smac_planner/costmap_downsampler.hpp" class RclCppFixture { @@ -33,7 +33,7 @@ TEST(CostmapDownsampler, costmap_downsample_test) { nav2_util::LifecycleNode::SharedPtr node = std::make_shared( "CostmapDownsamplerTest"); - smac_planner::CostmapDownsampler downsampler; + nav2_smac_planner::CostmapDownsampler downsampler; // create basic costmap nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); diff --git a/smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp similarity index 52% rename from smac_planner/test/test_node2d.cpp rename to nav2_smac_planner/test/test_node2d.cpp index bc42988aca4..33377254a76 100644 --- a/smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -21,8 +21,8 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" -#include "smac_planner/node_2d.hpp" -#include "smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/node_2d.hpp" +#include "nav2_smac_planner/collision_checker.hpp" class RclCppFixture { @@ -35,12 +35,12 @@ RclCppFixture g_rclcppfixture; TEST(Node2DTest, test_node_2d) { nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); - smac_planner::GridCollisionChecker checker(&costmapA); + nav2_smac_planner::GridCollisionChecker checker(&costmapA); // test construction unsigned char cost = static_cast(1); - smac_planner::Node2D testA(cost, 1); - smac_planner::Node2D testB(cost, 1); + nav2_smac_planner::Node2D testA(cost, 1); + nav2_smac_planner::Node2D testB(cost, 1); EXPECT_EQ(testA.getCost(), 1.0f); // test reset @@ -63,13 +63,13 @@ TEST(Node2DTest, test_node_2d) EXPECT_EQ(testB.getTraversalCost(&testA), 58.0f); // check heuristic cost computation - smac_planner::Node2D::Coordinates A(0.0, 0.0); - smac_planner::Node2D::Coordinates B(10.0, 5.0); + 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), 559.016, 0.01); // check operator== works on index unsigned char costC = '2'; - smac_planner::Node2D testC(costC, 1); + nav2_smac_planner::Node2D testC(costC, 1); EXPECT_TRUE(testA == testC); // check accumulated costs are set @@ -88,46 +88,46 @@ TEST(Node2DTest, test_node_2d) EXPECT_EQ(testC.getIndex(), 1u); // check static index functions - EXPECT_EQ(smac_planner::Node2D::getIndex(1u, 1u, 10u), 11u); - EXPECT_EQ(smac_planner::Node2D::getIndex(6u, 43u, 10u), 436u); - EXPECT_EQ(smac_planner::Node2D::getCoords(436u, 10u, 1u).x, 6u); - EXPECT_EQ(smac_planner::Node2D::getCoords(436u, 10u, 1u).y, 43u); - EXPECT_THROW(smac_planner::Node2D::getCoords(436u, 10u, 10u), std::runtime_error); + 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) { // test neighborhood computation - smac_planner::Node2D::initNeighborhood(10u, smac_planner::MotionModel::VON_NEUMANN); - EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets.size(), 4u); - EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[0], -1); - EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[1], 1); - EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[2], -10); - EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[3], 10); - - smac_planner::Node2D::initNeighborhood(100u, smac_planner::MotionModel::MOORE); - EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets.size(), 8u); - EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[0], -1); - EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[1], 1); - EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[2], -100); - EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[3], 100); - EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[4], -101); - EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[5], -99); - EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[6], 99); - EXPECT_EQ(smac_planner::Node2D::_neighbors_grid_offsets[7], 101); + nav2_smac_planner::Node2D::initNeighborhood(10u, nav2_smac_planner::MotionModel::VON_NEUMANN); + EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 4u); + 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], -10); + EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[3], 10); + + nav2_smac_planner::Node2D::initNeighborhood(100u, nav2_smac_planner::MotionModel::MOORE); + 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); - smac_planner::GridCollisionChecker checker(&costmapA); + nav2_smac_planner::GridCollisionChecker checker(&costmapA); unsigned char cost = static_cast(1); - smac_planner::Node2D * node = new smac_planner::Node2D(cost, 1); - std::function neighborGetter = - [&, this](const unsigned int & index, smac_planner::Node2D * & neighbor_rtn) -> bool + nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(cost, 1); + std::function neighborGetter = + [&, this](const unsigned int & index, nav2_smac_planner::Node2D * & neighbor_rtn) -> bool { return true; }; - smac_planner::Node2D::NodeVector neighbors; - smac_planner::Node2D::getNeighbors(node, neighborGetter, checker, false, neighbors); + nav2_smac_planner::Node2D::NodeVector neighbors; + nav2_smac_planner::Node2D::getNeighbors(node, neighborGetter, checker, false, neighbors); delete node; // should be empty since totally invalid diff --git a/smac_planner/test/test_nodebasic.cpp b/nav2_smac_planner/test/test_nodebasic.cpp similarity index 79% rename from smac_planner/test/test_nodebasic.cpp rename to nav2_smac_planner/test/test_nodebasic.cpp index 0ec90a112f3..23d445ce547 100644 --- a/smac_planner/test/test_nodebasic.cpp +++ b/nav2_smac_planner/test/test_nodebasic.cpp @@ -22,10 +22,10 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" -#include "smac_planner/node_basic.hpp" -#include "smac_planner/node_2d.hpp" -#include "smac_planner/node_se2.hpp" -#include "smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/node_basic.hpp" +#include "nav2_smac_planner/node_2d.hpp" +#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/collision_checker.hpp" class RclCppFixture { @@ -37,12 +37,12 @@ RclCppFixture g_rclcppfixture; TEST(NodeBasicTest, test_node_basic) { - smac_planner::NodeBasic node(50); + nav2_smac_planner::NodeBasic node(50); EXPECT_EQ(node.index, 50u); EXPECT_EQ(node.graph_node_ptr, nullptr); - smac_planner::NodeBasic node2(100); + nav2_smac_planner::NodeBasic node2(100); EXPECT_EQ(node2.index, 100u); EXPECT_EQ(node2.graph_node_ptr, nullptr); diff --git a/nav2_smac_planner/test/test_nodese2.cpp b/nav2_smac_planner/test/test_nodese2.cpp new file mode 100644 index 00000000000..39b9aa2dfa9 --- /dev/null +++ b/nav2_smac_planner/test/test_nodese2.cpp @@ -0,0 +1,214 @@ +// 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 +#include +#include +#include + +#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_se2.hpp" +#include "nav2_smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(NodeSE2Test, test_node_se2) +{ + 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 = 0.20; + unsigned int size_x = 10; + unsigned int size_y = 10; + unsigned int size_theta = 72; + + nav2_smac_planner::NodeSE2::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); + nav2_smac_planner::GridCollisionChecker checker(costmapA); + checker.setFootprint(nav2_costmap_2d::Footprint(), true); + + // test construction + nav2_smac_planner::NodeSE2 testA(49); + nav2_smac_planner::NodeSE2 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), true); + EXPECT_EQ(testA.isNodeValid(false, checker), true); + EXPECT_EQ(testA.getCost(), 0.0f); + + // test reset + testA.reset(); + EXPECT_TRUE(std::isnan(testA.getCost())); + + // Check constants + EXPECT_EQ(testA.neutral_cost, sqrt(2)); + + // check collision checking + EXPECT_EQ(testA.isNodeValid(false, checker), true); + + // check traversal cost computation + // simulated first node, should return neutral cost + EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01); + // now with straight motion, cost is 0, so will be sqrt(2) as well + testB.setMotionPrimitiveIndex(1); + testA.setMotionPrimitiveIndex(0); + EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01); + // same direction as parent, testB + testA.setMotionPrimitiveIndex(1); + EXPECT_NEAR(testB.getTraversalCost(&testA), 1.9799f, 0.01); + // opposite direction as parent, testB + testA.setMotionPrimitiveIndex(2); + EXPECT_NEAR(testB.getTraversalCost(&testA), 3.67696f, 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 heuristic cost computation + nav2_smac_planner::NodeSE2::computeWavefrontHeuristic( + costmapA, + static_cast(10.0), + static_cast(5.0), + 0.0, 0.0); + nav2_smac_planner::NodeSE2::Coordinates A(0.0, 0.0, 4.2); + nav2_smac_planner::NodeSE2::Coordinates B(10.0, 5.0, 54.1); + EXPECT_NEAR(testB.getHeuristicCost(B, A), 16.723, 0.01); + + // check operator== works on index + nav2_smac_planner::NodeSE2 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.queued(); + EXPECT_EQ(testC.isQueued(), true); + testC.visited(); + EXPECT_EQ(testC.wasVisited(), true); + EXPECT_EQ(testC.isQueued(), false); + + // check index + EXPECT_EQ(testC.getIndex(), 49u); + + // check set pose and pose + testC.setPose(nav2_smac_planner::NodeSE2::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::NodeSE2::getIndex(1u, 1u, 4u, 10u, 72u), 796u); + EXPECT_EQ(nav2_smac_planner::NodeSE2::getCoords(796u, 10u, 72u).x, 1u); + EXPECT_EQ(nav2_smac_planner::NodeSE2::getCoords(796u, 10u, 72u).y, 1u); + EXPECT_EQ(nav2_smac_planner::NodeSE2::getCoords(796u, 10u, 72u).theta, 4u); + + delete costmapA; +} + +TEST(NodeSE2Test, test_node_2d_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 + unsigned int size_x = 100; + unsigned int size_y = 100; + unsigned int size_theta = 72; + nav2_smac_planner::NodeSE2::initMotionModel( + nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); + + + // test neighborhood computation + EXPECT_EQ(nav2_smac_planner::NodeSE2::motion_table.projections.size(), 3u); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01); + + nav2_smac_planner::NodeSE2::initMotionModel( + nav2_smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info); + + EXPECT_EQ(nav2_smac_planner::NodeSE2::motion_table.projections.size(), 6u); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[3]._x, -1.731517, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[3]._y, 0, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[3]._theta, 0, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[4]._x, -1.69047, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[4]._y, 0.3747, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[4]._theta, -5, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[5]._x, -1.69047, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[5]._y, -0.3747, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[5]._theta, 5, 0.01); + + nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0); + nav2_smac_planner::GridCollisionChecker checker(&costmapA); + nav2_smac_planner::NodeSE2 * node = new nav2_smac_planner::NodeSE2(49); + std::function neighborGetter = + [&, this](const unsigned int & index, nav2_smac_planner::NodeSE2 * & neighbor_rtn) -> bool + { + // because we don't return a real object + return false; + }; + + nav2_smac_planner::NodeSE2::NodeVector neighbors; + nav2_smac_planner::NodeSE2::getNeighbors(node, neighborGetter, checker, false, neighbors); + delete node; + + // should be empty since totally invalid + EXPECT_EQ(neighbors.size(), 0u); +} diff --git a/smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp similarity index 89% rename from smac_planner/test/test_smac_2d.cpp rename to nav2_smac_planner/test/test_smac_2d.cpp index 63b4ba99876..59a31eb17ba 100644 --- a/smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -23,11 +23,11 @@ #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "smac_planner/node_se2.hpp" -#include "smac_planner/a_star.hpp" -#include "smac_planner/collision_checker.hpp" -#include "smac_planner/smac_planner.hpp" -#include "smac_planner/smac_planner_2d.hpp" +#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/a_star.hpp" +#include "nav2_smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/smac_planner.hpp" +#include "nav2_smac_planner/smac_planner_2d.hpp" class RclCppFixture { @@ -62,7 +62,7 @@ TEST(SmacTest, test_smac_2d) start.pose.position.y = 0.0; start.pose.orientation.w = 1.0; goal = start; - auto planner_2d = std::make_unique(); + auto planner_2d = std::make_unique(); planner_2d->configure(node2D, "test", nullptr, costmap_ros); planner_2d->activate(); try { diff --git a/smac_planner/test/test_smac_se2.cpp b/nav2_smac_planner/test/test_smac_se2.cpp similarity index 87% rename from smac_planner/test/test_smac_se2.cpp rename to nav2_smac_planner/test/test_smac_se2.cpp index caf7d3381e9..c0a4cf8e8e2 100644 --- a/smac_planner/test/test_smac_se2.cpp +++ b/nav2_smac_planner/test/test_smac_se2.cpp @@ -23,11 +23,11 @@ #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "smac_planner/node_se2.hpp" -#include "smac_planner/a_star.hpp" -#include "smac_planner/collision_checker.hpp" -#include "smac_planner/smac_planner.hpp" -#include "smac_planner/smac_planner_2d.hpp" +#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/a_star.hpp" +#include "nav2_smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/smac_planner.hpp" +#include "nav2_smac_planner/smac_planner_2d.hpp" class RclCppFixture { @@ -62,7 +62,7 @@ TEST(SmacTest, test_smac_se2) start.pose.position.y = 0.0; start.pose.orientation.w = 1.0; goal = start; - auto planner = std::make_unique(); + auto planner = std::make_unique(); planner->configure(nodeSE2, "test", nullptr, costmap_ros); planner->activate(); @@ -88,5 +88,5 @@ TEST(SmacTestSE2, test_dist) Eigen::Vector2d p2; p2[0] = 0.0; p2[1] = 1.0; - EXPECT_NEAR(smac_planner::squaredDistance(p1, p2), 1.0, 0.001); + EXPECT_NEAR(nav2_smac_planner::squaredDistance(p1, p2), 1.0, 0.001); } diff --git a/smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp similarity index 87% rename from smac_planner/test/test_smoother.cpp rename to nav2_smac_planner/test/test_smoother.cpp index ab05a93959f..41c8280950e 100644 --- a/smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -21,8 +21,8 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_util/lifecycle_node.hpp" -#include "smac_planner/a_star.hpp" -#include "smac_planner/smoother.hpp" +#include "nav2_smac_planner/a_star.hpp" +#include "nav2_smac_planner/smoother.hpp" class RclCppFixture { @@ -47,14 +47,14 @@ TEST(SmootherTest, test_smoother) } // compute path to use - smac_planner::SearchInfo info; + 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; // in grid coordinates unsigned int size_theta = 72; - smac_planner::AStarAlgorithm a_star( - smac_planner::MotionModel::DUBIN, info); + nav2_smac_planner::AStarAlgorithm a_star( + nav2_smac_planner::MotionModel::DUBIN, info); int max_iterations = 1000000; float tolerance = 0.0; int it_on_approach = 1000000000; @@ -64,7 +64,7 @@ TEST(SmootherTest, test_smoother) a_star.createGraph(costmap->getSizeInCellsX(), costmap->getSizeInCellsY(), size_theta, costmap); a_star.setStart(10u, 10u, 0u); a_star.setGoal(80u, 80u, 40u); - smac_planner::NodeSE2::CoordinateVector plan; + nav2_smac_planner::NodeSE2::CoordinateVector plan; EXPECT_TRUE(a_star.createPath(plan, num_it, tolerance)); // populate our smoothing paths @@ -75,11 +75,11 @@ TEST(SmootherTest, test_smoother) initial_path.push_back(Eigen::Vector2d(plan[i].x, plan[i].y)); } - smac_planner::OptimizerParams params; + nav2_smac_planner::OptimizerParams params; params.debug = true; params.get(node2D.get(), "test"); - smac_planner::SmootherParams smoother_params; + nav2_smac_planner::SmootherParams smoother_params; smoother_params.get(node2D.get(), "test"); smoother_params.max_curvature = 5.0; smoother_params.curvature_weight = 30.0; @@ -87,7 +87,7 @@ TEST(SmootherTest, test_smoother) smoother_params.smooth_weight = 00.0; smoother_params.costmap_weight = 0.025; - smac_planner::Smoother smoother; + nav2_smac_planner::Smoother smoother; smoother.initialize(params); smoother.smooth(path, costmap, smoother_params); diff --git a/navigation2/package.xml b/navigation2/package.xml index bd72a39fc5e..989ce6aa50b 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -30,7 +30,7 @@ nav2_voxel_grid nav2_controller nav2_waypoint_follower - smac_planner + nav2_smac_planner ament_cmake diff --git a/smac_planner/smac_plugin.xml b/smac_planner/smac_plugin.xml deleted file mode 100644 index 09f17b666fc..00000000000 --- a/smac_planner/smac_plugin.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - SE2 version of the SMAC planner - - diff --git a/smac_planner/smac_plugin_2d.xml b/smac_planner/smac_plugin_2d.xml deleted file mode 100644 index 3845a0ffe2a..00000000000 --- a/smac_planner/smac_plugin_2d.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - 2D version of the SMAC Planner - - diff --git a/smac_planner/test/test_nodese2.cpp b/smac_planner/test/test_nodese2.cpp deleted file mode 100644 index 2f23c12aaf9..00000000000 --- a/smac_planner/test/test_nodese2.cpp +++ /dev/null @@ -1,214 +0,0 @@ -// 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 -#include -#include -#include - -#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 "smac_planner/node_se2.hpp" -#include "smac_planner/collision_checker.hpp" - -class RclCppFixture -{ -public: - RclCppFixture() {rclcpp::init(0, nullptr);} - ~RclCppFixture() {rclcpp::shutdown();} -}; -RclCppFixture g_rclcppfixture; - -TEST(NodeSE2Test, test_node_se2) -{ - smac_planner::SearchInfo info; - info.change_penalty = 1.2; - info.non_straight_penalty = 1.4; - info.reverse_penalty = 2.1; - info.minimum_turning_radius = 0.20; - unsigned int size_x = 10; - unsigned int size_y = 10; - unsigned int size_theta = 72; - - smac_planner::NodeSE2::initMotionModel( - 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); - smac_planner::GridCollisionChecker checker(costmapA); - checker.setFootprint(nav2_costmap_2d::Footprint(), true); - - // test construction - smac_planner::NodeSE2 testA(49); - smac_planner::NodeSE2 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), true); - EXPECT_EQ(testA.isNodeValid(false, checker), true); - EXPECT_EQ(testA.getCost(), 0.0f); - - // test reset - testA.reset(); - EXPECT_TRUE(std::isnan(testA.getCost())); - - // Check constants - EXPECT_EQ(testA.neutral_cost, sqrt(2)); - - // check collision checking - EXPECT_EQ(testA.isNodeValid(false, checker), true); - - // check traversal cost computation - // simulated first node, should return neutral cost - EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01); - // now with straight motion, cost is 0, so will be sqrt(2) as well - testB.setMotionPrimitiveIndex(1); - testA.setMotionPrimitiveIndex(0); - EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01); - // same direction as parent, testB - testA.setMotionPrimitiveIndex(1); - EXPECT_NEAR(testB.getTraversalCost(&testA), 1.9799f, 0.01); - // opposite direction as parent, testB - testA.setMotionPrimitiveIndex(2); - EXPECT_NEAR(testB.getTraversalCost(&testA), 3.67696f, 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 heuristic cost computation - smac_planner::NodeSE2::computeWavefrontHeuristic( - costmapA, - static_cast(10.0), - static_cast(5.0), - 0.0, 0.0); - smac_planner::NodeSE2::Coordinates A(0.0, 0.0, 4.2); - smac_planner::NodeSE2::Coordinates B(10.0, 5.0, 54.1); - EXPECT_NEAR(testB.getHeuristicCost(B, A), 16.723, 0.01); - - // check operator== works on index - smac_planner::NodeSE2 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.queued(); - EXPECT_EQ(testC.isQueued(), true); - testC.visited(); - EXPECT_EQ(testC.wasVisited(), true); - EXPECT_EQ(testC.isQueued(), false); - - // check index - EXPECT_EQ(testC.getIndex(), 49u); - - // check set pose and pose - testC.setPose(smac_planner::NodeSE2::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(smac_planner::NodeSE2::getIndex(1u, 1u, 4u, 10u, 72u), 796u); - EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).x, 1u); - EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).y, 1u); - EXPECT_EQ(smac_planner::NodeSE2::getCoords(796u, 10u, 72u).theta, 4u); - - delete costmapA; -} - -TEST(NodeSE2Test, test_node_2d_neighbors) -{ - 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 - unsigned int size_x = 100; - unsigned int size_y = 100; - unsigned int size_theta = 72; - smac_planner::NodeSE2::initMotionModel( - smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); - - - // test neighborhood computation - EXPECT_EQ(smac_planner::NodeSE2::motion_table.projections.size(), 3u); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); - - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01); - - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01); - - smac_planner::NodeSE2::initMotionModel( - smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info); - - EXPECT_EQ(smac_planner::NodeSE2::motion_table.projections.size(), 6u); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); - - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01); - - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01); - - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._x, -1.731517, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._y, 0, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._theta, 0, 0.01); - - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._x, -1.69047, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._y, 0.3747, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._theta, -5, 0.01); - - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._x, -1.69047, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._y, -0.3747, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._theta, 5, 0.01); - - nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0); - smac_planner::GridCollisionChecker checker(&costmapA); - smac_planner::NodeSE2 * node = new smac_planner::NodeSE2(49); - std::function neighborGetter = - [&, this](const unsigned int & index, smac_planner::NodeSE2 * & neighbor_rtn) -> bool - { - // because we don't return a real object - return false; - }; - - smac_planner::NodeSE2::NodeVector neighbors; - smac_planner::NodeSE2::getNeighbors(node, neighborGetter, checker, false, neighbors); - delete node; - - // should be empty since totally invalid - EXPECT_EQ(neighbors.size(), 0u); -}