code

nav2_smac_planner/CMakeLists.txt

@@ -1,6 +1,9 @@
 cmake_minimum_required(VERSION 3.8)
 project(nav2_smac_planner)
 
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
 find_package(ament_cmake REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(angles REQUIRED)
@@ -43,6 +46,7 @@ add_library(${library_name}_common SHARED
   src/node_hybrid.cpp
   src/node_lattice.cpp
   src/smoother.cpp
+  src/types.cpp
 )
 # Add GenerateExportHeader support for symbol visibility, as we are using
 # static members we need to explicitly export them on Windows, as

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -125,6 +125,15 @@ class AStarAlgorithm
    */
   void setCollisionChecker(GridCollisionChecker * collision_checker);
 
+
+  /**
+   * @brief Used to limit the planner to explore ahead of the specified pose
+   * @param search_bounds the pose beyoind which we want to limit the planner
+   * @param start_point the start point
+   * @param allow_goal_overshoot enable/disable this feature
+   */
+  void setSearchBounds(const geometry_msgs::msg::Pose& search_bounds, const geometry_msgs::msg::Point& start_point, bool allow_goal_overshoot);
+
   /**
    * @brief Set the goal for planning, as a node index
    * @param mx The node X index of the goal
@@ -270,6 +279,13 @@ class AStarAlgorithm
   inline void populateExpansionsLog(
     const NodePtr & node, std::vector<std::tuple<float, float, float>> * expansions_log);
 
+  /**
+   * @brief check the position of a node, with respect to a pose. If it is behind the pose then return true
+   * @param node the node which we want to check
+   * @param pose the pose relative to which we want to check the position of the node
+   */
+  bool isBehindPose(const NodePtr& node, const geometry_msgs::msg::Pose& pose);
+
   bool _traverse_unknown;
   bool _is_initialized;
   int _max_iterations;

nav2_smac_planner/include/nav2_smac_planner/types.hpp

@@ -51,6 +51,16 @@ struct SearchInfo
   bool allow_primitive_interpolation{false};
   bool downsample_obstacle_heuristic{true};
   bool use_quadratic_cost_penalty{false};
+  bool allow_goal_overshoot{false};
+  void setStart(const geometry_msgs::msg::Point& start);
+  geometry_msgs::msg::Pose getSearchBound();
+  void setSearchBound(const geometry_msgs::msg::Pose& search_bound);
+  bool isStartBehindSearchBounds();
+
+  private:
+    geometry_msgs::msg::Point _start_pose;
+    geometry_msgs::msg::Pose _search_bound;
+    std::optional<bool> is_start_behind_goal;
 };
 
 /**

nav2_smac_planner/src/a_star.cpp

@@ -102,6 +102,26 @@ void AStarAlgorithm<Node2D>::initialize(
     _motion_model, _search_info, _traverse_unknown, _dim3_size);
 }
 
+template<>
+bool AStarAlgorithm<Node2D>::isBehindPose(
+  const NodePtr & node,
+  const geometry_msgs::msg::Pose & pose)
+{
+  const Node2D::Coordinates node_coords = node->getCoords(node->getIndex());
+  const geometry_msgs::msg::Pose node_in_world_frame =  Utils::getWorldCoords(node_coords.x, node_coords.y, _costmap);
+  return Utils::isBehindPose(node_in_world_frame.position, pose);
+}
+
+template<typename NodeT>
+bool AStarAlgorithm<NodeT>::isBehindPose(
+  const NodePtr & node,
+  const geometry_msgs::msg::Pose & pose)
+{
+  typename NodeT::Coordinates node_coords = node->pose;
+  const geometry_msgs::msg::Pose node_in_world_frame =  Utils::getWorldCoords(node_coords.x, node_coords.y, _costmap);
+  return Utils::isBehindPose(node_in_world_frame.position, pose);
+}
+
 template<typename NodeT>
 void AStarAlgorithm<NodeT>::setCollisionChecker(GridCollisionChecker * collision_checker)
 {
@@ -120,6 +140,15 @@ void AStarAlgorithm<NodeT>::setCollisionChecker(GridCollisionChecker * collision
   _expander->setCollisionChecker(_collision_checker);
 }
 
+template <typename NodeT>
+void AStarAlgorithm<NodeT>::setSearchBounds(const geometry_msgs::msg::Pose& search_bounds, const geometry_msgs::msg::Point& start_point, bool allow_goal_overshoot)
+{
+  _search_info.setSearchBound(search_bounds);
+  _search_info.setStart(start_point);
+  _search_info.allow_goal_overshoot = allow_goal_overshoot;
+  _expander->setSearchBounds(search_bounds, start_point, allow_goal_overshoot);
+}
+
 template<typename NodeT>
 typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::addToGraph(
   const uint64_t & index)
@@ -367,6 +396,7 @@ bool AStarAlgorithm<NodeT>::createPath(
   NeighborIterator neighbor_iterator;
   int analytic_iterations = 0;
   int closest_distance = std::numeric_limits<int>::max();
+  const bool is_start_behind_goal = _search_info.isStartBehindSearchBounds();
 
   // Given an index, return a node ptr reference if its collision-free and valid
   const uint64_t max_index = static_cast<uint64_t>(getSizeX()) *
@@ -379,6 +409,14 @@ bool AStarAlgorithm<NodeT>::createPath(
         return false;
       }
 
+      if (!_search_info.allow_goal_overshoot){
+        auto iter = _graph.find(index);
+        if (iter != _graph.end()) {
+               if (isBehindPose(&(iter->second), _search_info.getSearchBound()) != is_start_behind_goal){
+                return false;
+               }
+            }
+        }
       neighbor_rtn = addToGraph(index);
       return true;
     };
@@ -446,6 +484,12 @@ bool AStarAlgorithm<NodeT>::createPath(
     {
       neighbor = *neighbor_iterator;
 
+      if (!_search_info.allow_goal_overshoot) {
+        if ((isBehindPose(neighbor, _search_info.getSearchBound())) != is_start_behind_goal) {
+          continue;
+        }
+      }
+
       // 4.1) Compute the cost to go to this node
       g_cost = current_node->getAccumulatedCost() + current_node->getTraversalCost(neighbor);
 

nav2_smac_planner/src/analytic_expansion.cpp

@@ -42,6 +42,15 @@ void AnalyticExpansion<NodeT>::setCollisionChecker(
   _collision_checker = collision_checker;
 }
 
+template<typename NodeT>
+void AnalyticExpansion<NodeT>::setSearchBounds(
+  const geometry_msgs::msg::Pose &search_bounds, const geometry_msgs::msg::Point& start_point, bool allow_goal_overshoot)
+{
+  _search_info.setSearchBound(search_bounds);
+  _search_info.setStart(start_point);
+  _search_info.allow_goal_overshoot = allow_goal_overshoot;
+}
+
 template<typename NodeT>
 typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalyticExpansion(
   const NodePtr & current_node,
@@ -204,6 +213,8 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   // When "from" and "to" are zero or one cell away,
   // num_intervals == 0
   possible_nodes.nodes.reserve(num_intervals);  // We won't store this node or the goal
+  const bool is_start_behind_goal = _search_info.isStartBehindSearchBounds();
+
   std::vector<double> reals;
   double theta;
 
@@ -221,6 +232,16 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
   for (float i = 1; i <= num_intervals; i++) {
     state_space->interpolate(from(), to(), i / num_intervals, s());
     reals = s.reals();
+
+    if (!_search_info.allow_goal_overshoot){
+      geometry_msgs::msg::Pose node_in_world_frame = Utils::getWorldCoords(reals[0], reals[1], _collision_checker->getCostmap());
+      const bool is_node_behind_goal = Utils::isBehindPose(node_in_world_frame.position, _search_info.getSearchBound());
+      if (is_node_behind_goal != is_start_behind_goal){ // not equal means not on the same side
+          failure = true;
+          break;
+      }
+    }
+
     // Make sure in range [0, 2PI)
     theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2];
     theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta;

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -13,6 +13,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
+#include <alloca.h>
 #include <string>
 #include <memory>
 #include <vector>
@@ -75,6 +76,10 @@ void SmacPlannerHybrid::configure(
     node, name + ".downsampling_factor", rclcpp::ParameterValue(1));
   node->get_parameter(name + ".downsampling_factor", _downsampling_factor);
 
+  nav2::declare_parameter_if_not_declared(
+    node, name + ".allow_goal_overshoot", rclcpp::ParameterValue(false));
+  nav2::get_parameter(name + ".allow_goal_overshoot", _search_info.allow_goal_overshoot)
+
   nav2::declare_parameter_if_not_declared(
     node, name + ".angle_quantization_bins", rclcpp::ParameterValue(72));
   node->get_parameter(name + ".angle_quantization_bins", angle_quantizations);
@@ -400,6 +405,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
     _costmap_ros->getUseRadius(),
     findCircumscribedCost(_costmap_ros));
   _a_star->setCollisionChecker(&_collision_checker);
+  _a_star->setSearchBounds(goal.pose, start.pose.position, _search_info.allow_goal_overshoot);
 
   // Set starting point, in A* bin search coordinates
   float mx_start, my_start, mx_goal, my_goal;

nav2_smac_planner/src/types.cpp

@@ -0,0 +1,33 @@
+#include <smac_planner/types.hpp>
+#include "geometry_msgs/msg/PoseStamped.hpp"
+#include <smac_planner/utils.hpp>
+
+namespace smac_planner
+{
+
+void SearchInfo::setStart(const geometry_msgs::msg::Point& start){
+  _start_pose = start;
+  is_start_behind_goal.reset();
+}
+
+geometry_msgs::msg::Pose SearchInfo::getSearchBound(){
+  return _search_bound;
+}
+
+void SearchInfo::setSearchBound(const geometry_msgs::msg::Pose& search_bound){
+  _search_bound = search_bound;
+  is_start_behind_goal.reset();
+}
+
+bool SearchInfo::isStartBehindSearchBounds(){
+
+  if (is_start_behind_goal){
+    return *is_start_behind_goal;
+  }
+  else{
+    is_start_behind_goal = smac_planner::Utils::isBehindPose(_start_pose, _search_bound);
+    return *is_start_behind_goal;
+  }
+}
+
+}  // namespace smac_planner

nav2_util/include/nav2_util/geometry_utils.hpp

@@ -44,7 +44,7 @@ inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle)
 }
 
 /**
- * @brief Get the euclidean distance between 2 geometry_msgs::Points
+ * @brief Get the euclidean distance between 2 geometry_msgs::msg::Points
  * @param pos1 First point
  * @param pos1 Second point
  * @param is_3d True if a true L2 distance is desired (default false)
@@ -67,7 +67,7 @@ inline double euclidean_distance(
 }
 
 /**
- * @brief Get the L2 distance between 2 geometry_msgs::Poses
+ * @brief Get the L2 distance between 2 geometry_msgs::msg::Poses
  * @param pos1 First pose
  * @param pos1 Second pose
  * @param is_3d True if a true L2 distance is desired (default false)
@@ -90,7 +90,7 @@ inline double euclidean_distance(
 }
 
 /**
- * @brief Get the L2 distance between 2 geometry_msgs::PoseStamped
+ * @brief Get the L2 distance between 2 geometry_msgs::msg::PoseStamped
  * @param pos1 First pose
  * @param pos1 Second pose
  * @param is_3d True if a true L2 distance is desired (default false)