From 1605253f9dd69e2ff81bd706d23e977a6647152a Mon Sep 17 00:00:00 2001 From: Nisarg236 Date: Mon, 29 Sep 2025 20:31:25 +0900 Subject: [PATCH 1/3] fix reversed path --- src/a_star.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/a_star.cpp b/src/a_star.cpp index 047fe82..fd02a93 100644 --- a/src/a_star.cpp +++ b/src/a_star.cpp @@ -321,9 +321,9 @@ uint32_t AStarAlgorithm::getStraightPath( // Add goal 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 +333,12 @@ uint32_t AStarAlgorithm::getStraightPath( } const float ratio = static_cast(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::value) { // Only non-Node2D has theta - intermediate.theta = start_coords.theta; + intermediate.theta = goal_coords.theta; } unsigned int index; From 2af3a5e84865eb196b41c4535f1483f6e2d80230 Mon Sep 17 00:00:00 2001 From: Nisarg236 Date: Tue, 30 Sep 2025 13:06:38 +0900 Subject: [PATCH 2/3] add comments --- src/a_star.cpp | 4 +++- src/smac_planner_2d.cpp | 1 + src/smac_planner_hybrid.cpp | 1 + src/smac_planner_lattice.cpp | 1 + 4 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/a_star.cpp b/src/a_star.cpp index fd02a93..e5be05f 100644 --- a/src/a_star.cpp +++ b/src/a_star.cpp @@ -318,7 +318,9 @@ uint32_t AStarAlgorithm::getStraightPath( std::vector 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 goal and start diff --git a/src/smac_planner_2d.cpp b/src/smac_planner_2d.cpp index 0808259..3acd7d3 100644 --- a/src/smac_planner_2d.cpp +++ b/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); diff --git a/src/smac_planner_hybrid.cpp b/src/smac_planner_hybrid.cpp index 80132a9..df19fc7 100644 --- a/src/smac_planner_hybrid.cpp +++ b/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); diff --git a/src/smac_planner_lattice.cpp b/src/smac_planner_lattice.cpp index 26de76b..1a80850 100644 --- a/src/smac_planner_lattice.cpp +++ b/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); From 0af688049c805b9375f596be3aa1944da0ca1edf Mon Sep 17 00:00:00 2001 From: Nisarg Panchal <71684502+Nisarg236@users.noreply.github.com> Date: Tue, 30 Sep 2025 13:15:15 +0900 Subject: [PATCH 3/3] comment --- src/a_star.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/a_star.cpp b/src/a_star.cpp index e5be05f..4002c61 100644 --- a/src/a_star.cpp +++ b/src/a_star.cpp @@ -377,7 +377,7 @@ uint32_t AStarAlgorithm::getStraightPath( path_coordinates.push_back(intermediate); } - // Add start + // Add start at the end path_coordinates.push_back(start_coords); if constexpr (!std::is_same::value) {