test(bpp_common): add test for occupancy grid based collision detector (

#9066) * add test for occupancy grid based collision detector Signed-off-by: Go Sakayori <[email protected]> * remove unnnecessary header Signed-off-by: Go Sakayori <[email protected]> * fix Signed-off-by: Go Sakayori <[email protected]> * change map resolution and corresponding index Signed-off-by: Go Sakayori <[email protected]> --------- Signed-off-by: Go Sakayori <[email protected]>
autowarefoundation · Oct 10, 2024 · b93ddd9 · b93ddd9
1 parent 1746798
commit b93ddd9
Show file tree

Hide file tree

Showing 4 changed files with 226 additions and 68 deletions.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt
@@ -50,6 +50,14 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}
   )
 
+  ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_occupancy_grid_based_collision_detector
+    test/test_occupancy_grid_based_collision_detector.cpp
+  )
+
+  target_link_libraries(test_${PROJECT_NAME}_occupancy_grid_based_collision_detector
+    ${PROJECT_NAME}
+  )
+
   ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_safety_check
     test/test_safety_check.cpp
   )

diff --git a/...utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/...utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp
@@ -25,7 +25,13 @@
 
 namespace autoware::behavior_path_planner
 {
-int discretizeAngle(const double theta, const int theta_size);
+/**
+ * @brief Discretizes a given angle into an index within a specified range.
+ * @param theta The angle in radians to discretize.
+ * @param theta_size The number of discrete bins or angular intervals.
+ * @return The discretized angle as an integer index.
+ */
+int discretize_angle(const double theta, const int theta_size);
 
 struct IndexXYT
 {
@@ -40,19 +46,36 @@ struct IndexXY
   int y;
 };
 
+/**
+ * @brief Converts a given local pose into a 3D grid index (x, y, theta).
+ * @param costmap The occupancy grid used for indexing.
+ * @param pose_local The local pose.
+ * @param theta_size The number of discrete angular intervals for yaw.
+ * @return IndexXYT The grid index representing the position and discretized orientation.
+ */
 IndexXYT pose2index(
   const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local,
   const int theta_size);
 
+/**
+ * @brief Converts a grid index (x, y, theta) back into a pose in the local frame.
+ * @param costmap The occupancy grid used for indexing.
+ * @param index The grid index containing x, y, and theta.
+ * @param theta_size The number of discrete angular intervals for yaw.
+ * @return geometry_msgs::msg::Pose The corresponding local pose.
+ */
 geometry_msgs::msg::Pose index2pose(
   const nav_msgs::msg::OccupancyGrid & costmap, const IndexXYT & index, const int theta_size);
 
+/**
+ * @brief Transforms a global pose into a local pose relative to the costmap's origin.
+ * @param costmap The occupancy grid that defines the local frame.
+ * @param pose_global The global pose to transform.
+ * @return geometry_msgs::msg::Pose The transformed pose in the local frame.
+ */
 geometry_msgs::msg::Pose global2local(
   const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_global);
 
-geometry_msgs::msg::Pose local2global(
-  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local);
-
 struct VehicleShape
 {
   double length;     // X [m]
@@ -76,12 +99,6 @@ struct PlannerWaypoint
   bool is_back = false;
 };
 
-struct PlannerWaypoints
-{
-  std_msgs::msg::Header header;
-  std::vector<PlannerWaypoint> waypoints;
-};
-
 class OccupancyGridBasedCollisionDetector
 {
 public:
@@ -92,21 +109,40 @@ class OccupancyGridBasedCollisionDetector
     default;
   OccupancyGridBasedCollisionDetector & operator=(OccupancyGridBasedCollisionDetector &&) = delete;
   void setParam(const OccupancyGridMapParam & param) { param_ = param; };
-  OccupancyGridMapParam getParam() const { return param_; };
+  [[nodiscard]] OccupancyGridMapParam getParam() const { return param_; };
   void setMap(const nav_msgs::msg::OccupancyGrid & costmap);
-  nav_msgs::msg::OccupancyGrid getMap() const { return costmap_; };
-  void setVehicleShape(const VehicleShape & vehicle_shape) { param_.vehicle_shape = vehicle_shape; }
-  bool hasObstacleOnPath(
-    const geometry_msgs::msg::PoseArray & path, const bool check_out_of_range) const;
-  bool hasObstacleOnPath(
+  [[nodiscard]] nav_msgs::msg::OccupancyGrid getMap() const { return costmap_; };
+
+  /**
+   * @brief Detects if a collision occurs with given path.
+   * @param path The path to check collision defined in a global frame
+   * @param check_out_of_range A boolean flag indicating whether out-of-range conditions is
+   * collision
+   * @return true if a collision is detected, false if no collision is detected.
+   */
+  [[nodiscard]] bool hasObstacleOnPath(
     const tier4_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const;
-  const PlannerWaypoints & getWaypoints() const { return waypoints_; }
-  bool detectCollision(const IndexXYT & base_index, const bool check_out_of_range) const;
+
+  /**
+   * @brief Detects if a collision occurs at the specified base index in the occupancy grid map.
+   * @param base_index The 3D index (x, y, theta) of the base position in the occupancy grid.
+   * @param check_out_of_range A boolean flag indicating whether out-of-range conditions is
+   * collision
+   * @return true if a collision is detected, false if no collision is detected.
+   */
+  [[nodiscard]] bool detectCollision(
+    const IndexXYT & base_index, const bool check_out_of_range) const;
   virtual ~OccupancyGridBasedCollisionDetector() = default;
 
 protected:
-  void computeCollisionIndexes(int theta_index, std::vector<IndexXY> & indexes);
-  inline bool isOutOfRange(const IndexXYT & index) const
+  /**
+   * @brief Computes the 2D grid indexes where collision might occur for a given theta index.
+   * @param theta_index The discretized orientation index for yaw.
+   * @param indexes_2d The output vector of 2D grid indexes where collisions could happen.
+   */
+  void compute_collision_indexes(int theta_index, std::vector<IndexXY> & indexes);
+
+  [[nodiscard]] inline bool is_out_of_range(const IndexXYT & index) const
   {
     if (index.x < 0 || static_cast<int>(costmap_.info.width) <= index.x) {
       return true;
@@ -116,7 +152,7 @@ class OccupancyGridBasedCollisionDetector
     }
     return false;
   }
-  inline bool isObs(const IndexXYT & index) const
+  [[nodiscard]] inline bool is_obs(const IndexXYT & index) const
   {
     // NOTE: Accessing by .at() instead makes 1.2 times slower here.
     // Also, boundary check is already done in isOutOfRange before calling this function.
@@ -134,15 +170,7 @@ class OccupancyGridBasedCollisionDetector
 
   // is_obstacle's table
   std::vector<std::vector<bool>> is_obstacle_table_;
-
-  // pose in costmap frame
-  geometry_msgs::msg::Pose start_pose_;
-  geometry_msgs::msg::Pose goal_pose_;
-
-  // result path
-  PlannerWaypoints waypoints_;
 };
-
 }  // namespace autoware::behavior_path_planner
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
diff --git a/...utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/...utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp
@@ -25,7 +25,7 @@ using autoware::universe_utils::createQuaternionFromYaw;
 using autoware::universe_utils::normalizeRadian;
 using autoware::universe_utils::transformPose;
 
-int discretizeAngle(const double theta, const int theta_size)
+int discretize_angle(const double theta, const int theta_size)
 {
   const double one_angle_range = 2.0 * M_PI / theta_size;
   return static_cast<int>(std::rint(normalizeRadian(theta, 0.0) / one_angle_range)) % theta_size;
@@ -37,7 +37,7 @@ IndexXYT pose2index(
 {
   const int index_x = pose_local.position.x / costmap.info.resolution;
   const int index_y = pose_local.position.y / costmap.info.resolution;
-  const int index_theta = discretizeAngle(tf2::getYaw(pose_local.orientation), theta_size);
+  const int index_theta = discretize_angle(tf2::getYaw(pose_local.orientation), theta_size);
   return {index_x, index_y, index_theta};
 }
 
@@ -68,18 +68,6 @@ geometry_msgs::msg::Pose global2local(
   return transformPose(pose_global, transform);
 }
 
-geometry_msgs::msg::Pose local2global(
-  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local)
-{
-  tf2::Transform tf_origin;
-  tf2::convert(costmap.info.origin, tf_origin);
-
-  geometry_msgs::msg::TransformStamped transform;
-  transform.transform = tf2::toMsg(tf_origin);
-
-  return transformPose(pose_local, transform);
-}
-
 void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyGrid & costmap)
 {
   costmap_ = costmap;
@@ -105,7 +93,7 @@ void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyG
   coll_indexes_table_.clear();
   for (int i = 0; i < param_.theta_size; i++) {
     std::vector<IndexXY> indexes_2d;
-    computeCollisionIndexes(i, indexes_2d);
+    compute_collision_indexes(i, indexes_2d);
     coll_indexes_table_.push_back(indexes_2d);
   }
 }
@@ -118,7 +106,7 @@ static IndexXY position2index(
   return {index_x, index_y};
 }
 
-void OccupancyGridBasedCollisionDetector::computeCollisionIndexes(
+void OccupancyGridBasedCollisionDetector::compute_collision_indexes(
   int theta_index, std::vector<IndexXY> & indexes_2d)
 {
   IndexXYT base_index{0, 0, theta_index};
@@ -134,7 +122,7 @@ void OccupancyGridBasedCollisionDetector::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 add_index2d = [&](const double x, const double y) {
     // 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;
@@ -149,14 +137,14 @@ void OccupancyGridBasedCollisionDetector::computeCollisionIndexes(
 
   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);
+      add_index2d(x, y);
     }
-    addIndex2d(x, left);
+    add_index2d(x, left);
   }
   for (double y = right; y <= left; y += costmap_.info.resolution / 2) {
-    addIndex2d(front, y);
+    add_index2d(front, y);
   }
-  addIndex2d(front, left);
+  add_index2d(front, left);
 }
 
 bool OccupancyGridBasedCollisionDetector::detectCollision(
@@ -175,27 +163,12 @@ bool OccupancyGridBasedCollisionDetector::detectCollision(
     coll_index.x += base_index.x;
     coll_index.y += base_index.y;
     if (check_out_of_range) {
-      if (isOutOfRange(coll_index) || isObs(coll_index)) return true;
+      if (is_out_of_range(coll_index) || is_obs(coll_index)) return true;
     } else {
-      if (isOutOfRange(coll_index)) return false;
-      if (isObs(coll_index)) return true;
-    }
-  }
-  return false;
-}
-
-bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath(
-  const geometry_msgs::msg::PoseArray & path, const bool check_out_of_range) const
-{
-  for (const auto & pose : path.poses) {
-    const auto pose_local = global2local(costmap_, pose);
-    const auto index = pose2index(costmap_, pose_local, param_.theta_size);
-
-    if (detectCollision(index, check_out_of_range)) {
-      return true;
+      if (is_out_of_range(coll_index)) return false;
+      if (is_obs(coll_index)) return true;
     }
   }
-
   return false;
 }