feat: search straight path from waypoint to goal (#29)

Author: Nisarg236

include/smac_planner/a_star.hpp

@@ -125,6 +125,16 @@ class AStarAlgorithm
    */
   void setSearchBounds(const geometry_msgs::Pose& search_bounds, const geometry_msgs::Point& start_point, bool allow_goal_overshoot);
 
+  void setSearchStraightPathFlag(const bool search_straight_path);
+
+
+    /**
+   * @brief get straight path by interpolating points between start and goal, fails if any interpolated point is in collision.
+   * @param path output path
+   * @param cancel_checker Function to check if the task has been canceled
+   */
+  uint32_t getStraightPath(CoordinateVector & path, std::function<bool()> cancel_checker);
+
   /**
    * @brief Set the goal for planning, as a node index
    * @param mx The node X index of the goal

include/smac_planner/types.hpp

@@ -51,6 +51,7 @@ struct SearchInfo
   bool allow_goal_overshoot{true};
   bool downsample_obstacle_heuristic{true};
   bool use_quadratic_cost_penalty{false};
+  bool search_straight_path{false};
   void setStart(const geometry_msgs::Point& start);
   geometry_msgs::Pose getSearchBound();
   void setSearchBound(const geometry_msgs::Pose& search_bound);

src/a_star.cpp

@@ -130,6 +130,11 @@ void AStarAlgorithm<NodeT>::setSearchBounds(const geometry_msgs::Pose& search_bo
   _expander->setSearchBounds(search_bounds, start_point, allow_goal_overshoot);
 }
 
+template <typename NodeT>
+void AStarAlgorithm<NodeT>::setSearchStraightPathFlag(const bool search_straight_path) {
+  _search_info.search_straight_path = search_straight_path;
+}
+
 
 template<typename NodeT>
 typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::addToGraph(
@@ -292,6 +297,98 @@ bool AStarAlgorithm<NodeT>::areInputsValid()
   return true;
 }
 
+template<typename NodeT>
+uint32_t AStarAlgorithm<NodeT>::getStraightPath(
+  CoordinateVector & path,
+  std::function<bool()> cancel_checker)
+{
+  if (!areInputsValid()) {
+    return mbf_msgs::GetPathResult::INTERNAL_ERROR;
+  }
+
+  // Start coordinates differ for Node2D
+  typename NodeT::Coordinates start_coords;
+  if constexpr (std::is_same<NodeT, Node2D>::value) {
+    start_coords = _start->getCoords(_start->getIndex());
+  } else {
+    start_coords = _start->pose;
+  }
+
+  typename NodeT::Coordinates goal_coords = _goal_coordinates;
+
+  std::vector<Coordinates> path_coordinates;
+
+  // 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;
+  const float steps = std::max(std::abs(dx), std::abs(dy));
+
+  // Add intermediate points along the straight line
+  for (int i = 1; i < steps; ++i) {
+      if (cancel_checker()) {
+        return mbf_msgs::GetPathResult::CANCELED;
+      }
+    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;
+
+    if constexpr (!std::is_same<NodeT, Node2D>::value) {
+      // Only non-Node2D has theta
+      intermediate.theta = start_coords.theta;
+    }
+
+    unsigned int index;
+    if constexpr (std::is_same<NodeT, Node2D>::value) {
+      index = Node2D::getIndex(
+        static_cast<unsigned int>(intermediate.x),
+        static_cast<unsigned int>(intermediate.y),
+        _costmap->getSizeInCellsX());
+    } else {
+      index = NodeT::getIndex(
+        static_cast<unsigned int>(intermediate.x),
+        static_cast<unsigned int>(intermediate.y),
+        static_cast<unsigned int>(intermediate.theta));
+    }
+
+    NodePtr node = addToGraph(index);
+
+    if constexpr (!std::is_same<NodeT, Node2D>::value) {
+      node->setPose(intermediate);
+    }
+
+    // Check if this node is valid
+    if (!node->isNodeValid(_traverse_unknown, _collision_checker)) {
+      double map_x, map_y;
+      _costmap->mapToWorld(
+        static_cast<unsigned int>(intermediate.x),
+        static_cast<unsigned int>(intermediate.y),
+        map_x, map_y);
+      ROS_ERROR_NAMED("smac_planner",
+        "Fail to plan straight path because point at (%.2f, %.2f) is in collision", map_x, map_y);
+      return mbf_msgs::GetPathResult::NO_PATH_FOUND;
+    }
+
+    path_coordinates.push_back(intermediate);
+  }
+
+  // Add start
+  path_coordinates.push_back(start_coords);
+
+  if constexpr (!std::is_same<NodeT, Node2D>::value) {
+    for (auto & coord : path_coordinates) {
+      coord.theta = NodeT::motion_table.getAngleFromBin(coord.theta);
+    }
+  }
+
+  path = std::move(path_coordinates);
+  return mbf_msgs::GetPathResult::SUCCESS;
+}
+
+
 template<typename NodeT>
 uint32_t AStarAlgorithm<NodeT>::createPath(
   CoordinateVector & path, int & iterations,
@@ -308,6 +405,12 @@ uint32_t AStarAlgorithm<NodeT>::createPath(
     return mbf_msgs::GetPathResult::INTERNAL_ERROR;
   }
 
+  // Check if we should use straight path
+  if (_search_info.search_straight_path) {
+    return getStraightPath(path, cancel_checker);
+  }
+
+
   // 0) Add starting point to the open set
   addNode(0.0, getStart());
   getStart()->setAccumulatedCost(0.0);

src/smac_planner_hybrid.cpp

@@ -185,9 +185,12 @@ PlanResult SmacPlannerHybrid::planWithWaypoint(
   _a_star->setSearchBounds(goal_pose.pose, waypoint.pose.position,  _search_info.allow_goal_overshoot);
   }
 
-  // waypoint to goal pose
+  // waypoint to goal pose (search_straight_path = true for this segment)
   PlanResult segment2;
+  _a_star->setSearchStraightPathFlag(true);
   getPath(waypoint, goal_pose, tolerance, segment2);
+  _a_star->setSearchStraightPathFlag(false);
+
   if (!segment2.isValid())
   {
     ROS_ERROR_NAMED("smac_planner_hybrid", "Could not find path from waypoint to goal, with error: %s",