diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index 9c28b1ab3a..6344c86fb8 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -94,11 +94,11 @@ inline GoalHeadingMode fromStringToGH(const std::string & n) } } -const float UNKNOWN = 255.0; -const float OCCUPIED = 254.0; -const float INSCRIBED = 253.0; -const float MAX_NON_OBSTACLE = 252.0; -const float FREE = 0; +const float UNKNOWN_COST = 255.0; +const float OCCUPIED_COST = 254.0; +const float INSCRIBED_COST = 253.0; +const float MAX_NON_OBSTACLE_COST = 252.0; +const float FREE_COST = 0; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index f57aaf47ca..e15308c97d 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -195,6 +195,7 @@ void AStarAlgorithm::setGoal( const unsigned int & dim_3, const GoalHeadingMode & goal_heading_mode) { + (void) goal_heading_mode; if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } @@ -550,13 +551,6 @@ typename AStarAlgorithm::CoordinateVector & AStarAlgorithm::getGoa return _goals_coordinates; } -template -void AStarAlgorithm::clearStart() -{ - auto coords = NodeT::getCoords(_start->getIndex(), _costmap->getSizeInCellsX(), getSizeDim3()); - _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); -} - // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index a5698f7d5a..0020794a9b 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -375,9 +375,9 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyt template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodeSet & goals_node, const CoordinateVector & goals_coords, - const NodeGetter & getter, int & analytic_iterations, - int & closest_distance) + const NodePtr & , const NodeSet & , const CoordinateVector & , + const NodeGetter & , int & , + int & ) { return NodePtr(nullptr); } diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index e2c3c1e1c8..117a556931 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -403,7 +403,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (orientation_bin >= static_cast(_angle_quantizations)) { orientation_bin -= static_cast(_angle_quantizations); } - orientation_bin_id = static_cast(floor(orientation_bin)); + int orientation_bin_id = static_cast(floor(orientation_bin)); _a_star->setGoal(mx, my, orientation_bin_id, _goal_heading_mode); // Setup message