fix: path from waypt-goal is reversed

src/a_star.cpp

@@ -318,12 +318,14 @@ uint32_t AStarAlgorithm<NodeT>::getStraightPath(
 
   std::vector<Coordinates> path_coordinates;
 
-  // Add goal
+  // Add goal first, start searching from goal and move towards start
+  // because the path is then reversed in smac_planner_hybrid.cpp,
+  // smac_planner_lattice.cpp, smac_planner_2d.cpp
   path_coordinates.push_back(goal_coords);
 
-  // Simple interpolation between start and goal
-  const float dx = goal_coords.x - start_coords.x;
-  const float dy = goal_coords.y - start_coords.y;
+  // Simple interpolation between goal and start
+  const float dx = start_coords.x - goal_coords.x;
+  const float dy = start_coords.y - goal_coords.y;
   const float steps = std::max(std::abs(dx), std::abs(dy));
 
   // Add intermediate points along the straight line
@@ -333,12 +335,12 @@ uint32_t AStarAlgorithm<NodeT>::getStraightPath(
       }
     const float ratio = static_cast<float>(i) / steps;
     typename NodeT::Coordinates intermediate;
-    intermediate.x = start_coords.x + dx * ratio;
-    intermediate.y = start_coords.y + dy * ratio;
+    intermediate.x = goal_coords.x + dx * ratio;
+    intermediate.y = goal_coords.y + dy * ratio;
 
     if constexpr (!std::is_same<NodeT, Node2D>::value) {
       // Only non-Node2D has theta
-      intermediate.theta = start_coords.theta;
+      intermediate.theta = goal_coords.theta;
     }
 
     unsigned int index;
@@ -375,7 +377,7 @@ uint32_t AStarAlgorithm<NodeT>::getStraightPath(
     path_coordinates.push_back(intermediate);
   }
 
-  // Add start
+  // Add start at the end
   path_coordinates.push_back(start_coords);
 
   if constexpr (!std::is_same<NodeT, Node2D>::value) {

src/smac_planner_2d.cpp

@@ -275,6 +275,7 @@ nav_msgs::Path SmacPlanner2D::createPlan(
 
   // Convert to world coordinates
   plan.poses.reserve(path.size());
+  // reverse the path because in a_star.cpp backtracepath gives reversed path
   for (int i = path.size() - 1; i >= 0; --i) {
     pose.pose = getWorldCoords(path[i].x, path[i].y, costmap);
     plan.poses.push_back(pose);

src/smac_planner_hybrid.cpp

@@ -599,6 +599,7 @@ void SmacPlannerHybrid::getPath(
 
   // Convert to world coordinates
   output_path.poses.reserve(path.size());
+  // reverse the path because in a_star.cpp backtracepath gives reversed path
   for (int i = path.size() - 1; i >= 0; --i) {
     pose.pose = Utils::getWorldCoords(path[i].x, path[i].y, costmap);
     pose.pose.orientation = Utils::getWorldOrientation(path[i].theta);

src/smac_planner_lattice.cpp

@@ -343,6 +343,7 @@ nav_msgs::Path SmacPlannerLattice::createPlan(
   // Convert to world coordinates
   plan.poses.reserve(path.size());
   geometry_msgs::PoseStamped last_pose = pose;
+  // reverse the path because in a_star.cpp backtracepath gives reversed path
   for (int i = path.size() - 1; i >= 0; --i) {
     pose.pose = getWorldCoords(path[i].x, path[i].y, _costmap);
     pose.pose.orientation = Utils::getWorldOrientation(path[i].theta);