From a8aaccfca52948959327d140f003d44102188dfa Mon Sep 17 00:00:00 2001
From: stevedan <stevedan.o.omodolor@gmail.com>
Date: Tue, 8 Oct 2024 23:46:20 +0200
Subject: [PATCH] wip

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
---
 .../include/nav2_smac_planner/constants.hpp            | 10 +++++-----
 nav2_smac_planner/src/a_star.cpp                       |  8 +-------
 nav2_smac_planner/src/analytic_expansion.cpp           |  6 +++---
 nav2_smac_planner/src/smac_planner_hybrid.cpp          |  2 +-
 4 files changed, 10 insertions(+), 16 deletions(-)

diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp
index 9c28b1ab3a0..6344c86fb89 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 f57aaf47caa..e15308c97d1 100644
--- a/nav2_smac_planner/src/a_star.cpp
+++ b/nav2_smac_planner/src/a_star.cpp
@@ -195,6 +195,7 @@ void AStarAlgorithm<Node2D>::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<NodeT>::CoordinateVector & AStarAlgorithm<NodeT>::getGoa
   return _goals_coordinates;
 }
 
-template<typename NodeT>
-void AStarAlgorithm<NodeT>::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<Node2D>;
 template class AStarAlgorithm<NodeHybrid>;
diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp
index a5698f7d5ac..0020794a9b8 100644
--- a/nav2_smac_planner/src/analytic_expansion.cpp
+++ b/nav2_smac_planner/src/analytic_expansion.cpp
@@ -375,9 +375,9 @@ typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::setAnalyt
 
 template<>
 typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::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 e2c3c1e1c82..117a5569319 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<float>(_angle_quantizations)) {
     orientation_bin -= static_cast<float>(_angle_quantizations);
   }
-  orientation_bin_id = static_cast<unsigned int>(floor(orientation_bin));
+  int orientation_bin_id = static_cast<unsigned int>(floor(orientation_bin));
   _a_star->setGoal(mx, my, orientation_bin_id, _goal_heading_mode);
 
   // Setup message