perf(freespace_planning_algorithms): improve freespace planning perfo…

…rmance (autowarefoundation#2040)

* perf(freespace_planning_algorithms): eliminate unnecessary calculation steps in collision detection function

Signed-off-by: NorahXiong <[email protected]>

* perf(freespace_planning_algorithms): improve node data storage

Signed-off-by: NorahXiong <[email protected]>

* ci(pre-commit): autofix

* ci(pre-commit): autofix

* fix(freespace_planning_algorithms): reset data explicitly as class object construction strategy changed

Signed-off-by: NorahXiong <[email protected]>

Signed-off-by: NorahXiong <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp

@@ -114,7 +114,9 @@ class AbstractPlanningAlgorithm
   virtual ~AbstractPlanningAlgorithm() {}
 
 protected:
-  void computeCollisionIndexes(int theta_index, std::vector<IndexXY> & indexes) const;
+  void computeCollisionIndexes(
+    int theta_index, std::vector<IndexXY> & indexes,
+    std::vector<IndexXY> & vertex_indexes_2d) const;
   bool detectCollision(const IndexXYT & base_index) const;
   inline bool isOutOfRange(const IndexXYT & index) const
   {
@@ -143,6 +145,9 @@ class AbstractPlanningAlgorithm
   // collision indexes cache
   std::vector<std::vector<IndexXY>> coll_indexes_table_;
 
+  // vehicle vertex indexes cache
+  std::vector<std::vector<IndexXY>> vertex_indexes_table_;
+
   // is_obstacle's table
   std::vector<std::vector<bool>> is_obstacle_table_;
 

planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp

@@ -29,6 +29,7 @@
 #include <queue>
 #include <string>
 #include <tuple>
+#include <unordered_map>
 #include <vector>
 
 namespace freespace_planning_algorithms
@@ -130,6 +131,11 @@ class AstarSearch : public AbstractPlanningAlgorithm
 
   const PlannerWaypoints & getWaypoints() const { return waypoints_; }
 
+  inline int getKey(const IndexXYT & index)
+  {
+    return (index.theta + (index.y * x_scale_ + index.x) * y_scale_);
+  }
+
 private:
   bool search();
   void clearNodes();
@@ -139,21 +145,28 @@ class AstarSearch : public AbstractPlanningAlgorithm
   double estimateCost(const geometry_msgs::msg::Pose & pose) const;
   bool isGoal(const AstarNode & node) const;
 
-  AstarNode * getNodeRef(const IndexXYT & index) { return &nodes_[index.y][index.x][index.theta]; }
+  AstarNode * getNodeRef(const IndexXYT & index)
+  {
+    return &(graph_.emplace(getKey(index), AstarNode()).first->second);
+  }
 
   // Algorithm specific param
   AstarParam astar_param_;
 
   // hybrid astar variables
   TransitionTable transition_table_;
-  std::vector<std::vector<std::vector<AstarNode>>> nodes_;
+  std::unordered_map<uint, AstarNode> graph_;
+
   std::priority_queue<AstarNode *, std::vector<AstarNode *>, NodeComparison> openlist_;
 
   // goal node, which may helpful in testing and debugging
   AstarNode * goal_node_;
 
   // distance metric option (removed when the reeds_shepp gets stable)
   bool use_reeds_shepp_;
+
+  int x_scale_;
+  int y_scale_;
 };
 }  // namespace freespace_planning_algorithms
 

planning/freespace_planning_algorithms/src/abstract_algorithm.cpp

@@ -121,14 +121,16 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost
 
   // construct collision indexes table
   for (int i = 0; i < planner_common_param_.theta_size; i++) {
-    std::vector<IndexXY> indexes_2d;
-    computeCollisionIndexes(i, indexes_2d);
+    std::vector<IndexXY> indexes_2d, vertex_indexes_2d;
+    computeCollisionIndexes(i, indexes_2d, vertex_indexes_2d);
     coll_indexes_table_.push_back(indexes_2d);
+    vertex_indexes_table_.push_back(vertex_indexes_2d);
   }
 }
 
 void AbstractPlanningAlgorithm::computeCollisionIndexes(
-  int theta_index, std::vector<IndexXY> & indexes_2d) const
+  int theta_index, std::vector<IndexXY> & indexes_2d,
+  std::vector<IndexXY> & vertex_indexes_2d) const
 {
   IndexXYT base_index{0, 0, theta_index};
   const VehicleShape & vehicle_shape = collision_vehicle_shape_;
@@ -143,7 +145,8 @@ void AbstractPlanningAlgorithm::computeCollisionIndexes(
   const auto base_theta = tf2::getYaw(base_pose.orientation);
 
   // Convert each point to index and check if the node is Obstacle
-  const auto addIndex2d = [&](const double x, const double y) {
+  const auto addIndex2d = [&](
+                            const double x, const double y, std::vector<IndexXY> & indexes_cache) {
     // Calculate offset in rotated frame
     const double offset_x = std::cos(base_theta) * x - std::sin(base_theta) * y;
     const double offset_y = std::sin(base_theta) * x + std::cos(base_theta) * y;
@@ -154,19 +157,41 @@ void AbstractPlanningAlgorithm::computeCollisionIndexes(
 
     const auto index = pose2index(costmap_, pose_local, planner_common_param_.theta_size);
     const auto index_2d = IndexXY{index.x, index.y};
-    indexes_2d.push_back(index_2d);
+    indexes_cache.push_back(index_2d);
   };
 
   for (double x = back; x <= front; x += costmap_.info.resolution / 2) {
     for (double y = right; y <= left; y += costmap_.info.resolution / 2) {
-      addIndex2d(x, y);
+      addIndex2d(x, y, indexes_2d);
     }
-    addIndex2d(x, left);
+    addIndex2d(x, left, indexes_2d);
   }
   for (double y = right; y <= left; y += costmap_.info.resolution / 2) {
-    addIndex2d(front, y);
+    addIndex2d(front, y, indexes_2d);
   }
-  addIndex2d(front, left);
+  addIndex2d(front, left, indexes_2d);
+
+  const auto compareIndex2d = [](const IndexXY & left, const IndexXY & right) {
+    if (left.x != right.x) {
+      return (left.x < right.x);
+    } else {
+      return (left.y < right.y);
+    }
+  };
+
+  const auto equalIndex2d = [](const IndexXY & left, const IndexXY & right) {
+    return ((left.x == right.x) && (left.y == right.y));
+  };
+
+  // remove duplicate indexes
+  std::sort(indexes_2d.begin(), indexes_2d.end(), compareIndex2d);
+  indexes_2d.erase(
+    std::unique(indexes_2d.begin(), indexes_2d.end(), equalIndex2d), indexes_2d.end());
+
+  addIndex2d(front, left, vertex_indexes_2d);
+  addIndex2d(front, right, vertex_indexes_2d);
+  addIndex2d(back, right, vertex_indexes_2d);
+  addIndex2d(back, left, vertex_indexes_2d);
 }
 
 bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) const
@@ -175,6 +200,18 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con
     std::cerr << "[abstract_algorithm] setMap has not yet been done." << std::endl;
     return false;
   }
+
+  const auto & vertex_indexes_2d = vertex_indexes_table_[base_index.theta];
+  for (const auto & vertex_index_2d : vertex_indexes_2d) {
+    IndexXYT vertex_index{vertex_index_2d.x, vertex_index_2d.y, 0};
+    // must slide to current base position
+    vertex_index.x += base_index.x;
+    vertex_index.y += base_index.y;
+    if (isOutOfRange(vertex_index)) {
+      return true;
+    }
+  }
+
   const auto & coll_indexes_2d = coll_indexes_table_[base_index.theta];
   for (const auto & coll_index_2d : coll_indexes_2d) {
     int idx_theta = 0;  // whatever. Yaw is nothing to do with collision detection between grids.
@@ -183,10 +220,11 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con
     coll_index.x += base_index.x;
     coll_index.y += base_index.y;
 
-    if (isOutOfRange(coll_index) || isObs(coll_index)) {
+    if (isObs(coll_index)) {
       return true;
     }
   }
+
   return false;
 }
 

planning/freespace_planning_algorithms/src/astar_search.cpp

@@ -133,6 +133,8 @@ AstarSearch::AstarSearch(
     planner_common_param_.minimum_turning_radius, planner_common_param_.maximum_turning_radius,
     planner_common_param_.turning_radius_size, planner_common_param_.theta_size,
     astar_param_.use_back);
+
+  y_scale_ = planner_common_param.theta_size;
 }
 
 void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap)
@@ -141,17 +143,8 @@ void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap)
 
   clearNodes();
 
-  const auto height = costmap_.info.height;
-  const auto width = costmap_.info.width;
-
-  // Initialize nodes
-  nodes_.resize(height);
-  for (size_t i = 0; i < height; i++) {
-    nodes_[i].resize(width);
-    for (size_t j = 0; j < width; j++) {
-      nodes_[i][j].resize(planner_common_param_.theta_size);
-    }
-  }
+  x_scale_ = costmap_.info.height;
+  graph_.reserve(100000);
 }
 
 bool AstarSearch::makePlan(
@@ -175,8 +168,9 @@ void AstarSearch::clearNodes()
 {
   // clearing openlist is necessary because otherwise remaining elements of openlist
   // point to deleted node.
-  nodes_.clear();
   openlist_ = std::priority_queue<AstarNode *, std::vector<AstarNode *>, NodeComparison>();
+
+  graph_ = std::unordered_map<uint, AstarNode>();
 }
 
 bool AstarSearch::setStartNode()