refactor(behavior_path_planner): move utils function to `behavior_pat…

…h_planner_common` package (autowarefoundation#5877)

* refactor(bpp): remove unused header file

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(bpp, bpp-common): move occ grid based collision detector

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner_common/CMakeLists.txt

@@ -23,6 +23,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/utils/drivable_area_expansion/footprints.cpp
   src/utils/parking_departure/geometric_parallel_parking.cpp
   src/utils/parking_departure/utils.cpp
+  src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp
   src/marker_utils/utils.cpp
 )
 

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp

@@ -0,0 +1,152 @@
+// Copyright 2022 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
+#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
+
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#include <tf2/utils.h>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <vector>
+
+namespace behavior_path_planner
+{
+int discretizeAngle(const double theta, const int theta_size);
+
+struct IndexXYT
+{
+  int x;
+  int y;
+  int theta;
+};
+
+struct IndexXY
+{
+  int x;
+  int y;
+};
+
+IndexXYT pose2index(
+  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local,
+  const int theta_size);
+
+geometry_msgs::msg::Pose index2pose(
+  const nav_msgs::msg::OccupancyGrid & costmap, const IndexXYT & index, const int theta_size);
+
+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]
+  double width;      // Y [m]
+  double base2back;  // base_link to rear [m]
+};
+
+struct OccupancyGridMapParam
+{
+  // robot configs
+  VehicleShape vehicle_shape;
+
+  // costmap configs
+  int theta_size;          // discretized angle table size [-]
+  int obstacle_threshold;  // obstacle threshold on grid [-]
+};
+
+struct PlannerWaypoint
+{
+  geometry_msgs::msg::PoseStamped pose;
+  bool is_back = false;
+};
+
+struct PlannerWaypoints
+{
+  std_msgs::msg::Header header;
+  std::vector<PlannerWaypoint> waypoints;
+};
+
+class OccupancyGridBasedCollisionDetector
+{
+public:
+  OccupancyGridBasedCollisionDetector() {}
+  void setParam(const OccupancyGridMapParam & param) { param_ = param; };
+  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(
+    const autoware_auto_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;
+  virtual ~OccupancyGridBasedCollisionDetector() {}
+
+protected:
+  void computeCollisionIndexes(int theta_index, std::vector<IndexXY> & indexes);
+  inline bool isOutOfRange(const IndexXYT & index) const
+  {
+    if (index.x < 0 || static_cast<int>(costmap_.info.width) <= index.x) {
+      return true;
+    }
+    if (index.y < 0 || static_cast<int>(costmap_.info.height) <= index.y) {
+      return true;
+    }
+    return false;
+  }
+  inline bool isObs(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.
+    // So, basically .at() is not necessary.
+    return is_obstacle_table_[index.y][index.x];
+  }
+
+  OccupancyGridMapParam param_;
+
+  // costmap as occupancy grid
+  nav_msgs::msg::OccupancyGrid costmap_;
+
+  // collision indexes cache
+  std::vector<std::vector<IndexXY>> coll_indexes_table_;
+
+  // 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 behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_

planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp

@@ -0,0 +1,211 @@
+// Copyright 2022 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
+
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/math/normalization.hpp>
+
+#include <vector>
+
+namespace behavior_path_planner
+{
+using tier4_autoware_utils::createQuaternionFromYaw;
+using tier4_autoware_utils::normalizeRadian;
+using tier4_autoware_utils::transformPose;
+
+int discretizeAngle(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;
+}
+
+IndexXYT pose2index(
+  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local,
+  const int theta_size)
+{
+  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);
+  return {index_x, index_y, index_theta};
+}
+
+geometry_msgs::msg::Pose index2pose(
+  const nav_msgs::msg::OccupancyGrid & costmap, const IndexXYT & index, const int theta_size)
+{
+  geometry_msgs::msg::Pose pose_local;
+
+  pose_local.position.x = index.x * costmap.info.resolution;
+  pose_local.position.y = index.y * costmap.info.resolution;
+
+  const double one_angle_range = 2.0 * M_PI / theta_size;
+  const double yaw = index.theta * one_angle_range;
+  pose_local.orientation = createQuaternionFromYaw(yaw);
+
+  return pose_local;
+}
+
+geometry_msgs::msg::Pose global2local(
+  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_global)
+{
+  tf2::Transform tf_origin;
+  tf2::convert(costmap.info.origin, tf_origin);
+
+  geometry_msgs::msg::TransformStamped transform;
+  transform.transform = tf2::toMsg(tf_origin.inverse());
+
+  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;
+  const auto height = costmap_.info.height;
+  const auto width = costmap_.info.width;
+
+  // Initialize status
+  std::vector<std::vector<bool>> is_obstacle_table;
+  is_obstacle_table.resize(height);
+  for (uint32_t i = 0; i < height; i++) {
+    is_obstacle_table.at(i).resize(width);
+    for (uint32_t j = 0; j < width; j++) {
+      const int cost = costmap_.data[i * width + j];
+
+      if (cost < 0 || param_.obstacle_threshold <= cost) {
+        is_obstacle_table[i][j] = true;
+      }
+    }
+  }
+  is_obstacle_table_ = is_obstacle_table;
+
+  // construct collision indexes table
+  coll_indexes_table_.clear();
+  for (int i = 0; i < param_.theta_size; i++) {
+    std::vector<IndexXY> indexes_2d;
+    computeCollisionIndexes(i, indexes_2d);
+    coll_indexes_table_.push_back(indexes_2d);
+  }
+}
+
+void OccupancyGridBasedCollisionDetector::computeCollisionIndexes(
+  int theta_index, std::vector<IndexXY> & indexes_2d)
+{
+  IndexXYT base_index{0, 0, theta_index};
+  const VehicleShape & vehicle_shape = param_.vehicle_shape;
+
+  // Define the robot as rectangle
+  const double back = -1.0 * vehicle_shape.base2back;
+  const double front = vehicle_shape.length - vehicle_shape.base2back;
+  const double right = -1.0 * vehicle_shape.width / 2.0;
+  const double left = vehicle_shape.width / 2.0;
+
+  const auto base_pose = index2pose(costmap_, base_index, param_.theta_size);
+  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) {
+    // 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;
+
+    geometry_msgs::msg::Pose pose_local;
+    pose_local.position.x = base_pose.position.x + offset_x;
+    pose_local.position.y = base_pose.position.y + offset_y;
+
+    const auto index = pose2index(costmap_, pose_local, param_.theta_size);
+    const auto index_2d = IndexXY{index.x, index.y};
+    indexes_2d.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, left);
+  }
+  for (double y = right; y <= left; y += costmap_.info.resolution / 2) {
+    addIndex2d(front, y);
+  }
+  addIndex2d(front, left);
+}
+
+bool OccupancyGridBasedCollisionDetector::detectCollision(
+  const IndexXYT & base_index, const bool check_out_of_range) const
+{
+  if (coll_indexes_table_.empty()) {
+    std::cerr << "[occupancy_grid_based_collision_detector] setMap has not yet been done."
+              << std::endl;
+    return false;
+  }
+  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.
+    IndexXYT coll_index{coll_index_2d.x, coll_index_2d.y, idx_theta};
+    // must slide to current base position
+    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;
+    } 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;
+    }
+  }
+
+  return false;
+}
+
+bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const bool check_out_of_range) const
+{
+  for (const auto & p : path.points) {
+    const auto pose_local = global2local(costmap_, p.point.pose);
+    const auto index = pose2index(costmap_, pose_local, param_.theta_size);
+
+    if (detectCollision(index, check_out_of_range)) {
+      return true;
+    }
+  }
+
+  return false;
+}
+
+}  // namespace behavior_path_planner