refactor(occlusion_spot): remove unused functions and refactor utils (#…

…1628)

* refactor(occlusion_spot): remove unused functions and refactor utils

Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp>

* style: fix pre-commit

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp

@@ -117,7 +117,6 @@ void denoiseOccupancyGridCV(
   const Polygons2d & stuck_vehicle_foot_prints, const Polygons2d & moving_vehicle_foot_prints,
   grid_map::GridMap & grid_map, const GridParam & param, const bool is_show_debug_window,
   const int num_iter, const bool use_object_footprints, const bool use_object_ray_casts);
-void addObjectsToGridMap(const std::vector<PredictedObject> & objs, grid_map::GridMap & grid);
 }  // namespace grid_utils
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp

@@ -226,8 +226,6 @@ bool generatePossibleCollisionsFromObjects(
   std::vector<PossibleCollisionInfo> & possible_collisions, const PathWithLaneId & path,
   const PlannerParam & param, const double offset_from_start_to_ego,
   const std::vector<PredictedObject> & dyn_objects);
-ROAD_TYPE getCurrentRoadType(
-  const lanelet::ConstLanelet & current_lanelet, const LaneletMapPtr & lanelet_map_ptr);
 //!< @brief calculate intersection and collision point from occlusion spot
 void calculateCollisionPathPointFromOcclusionSpot(
   PossibleCollisionInfo & pc, const lanelet::BasicPoint2d & obstacle_point,

planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp

@@ -30,10 +30,6 @@ void applySafeVelocityConsideringPossibleCollision(
   PathWithLaneId * inout_path, std::vector<PossibleCollisionInfo> & possible_collisions,
   const PlannerParam & param);
 
-int insertSafeVelocityToPath(
-  const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param,
-  PathWithLaneId * inout_path);
-
 /**
  * @param: v: ego velocity config
  * @param: ttv: time to vehicle

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp

@@ -56,30 +56,6 @@ std::vector<std::pair<grid_map::Position, grid_map::Position>> pointsToRays(
   return lines;
 }
 
-void addObjectsToGridMap(const std::vector<PredictedObject> & objs, grid_map::GridMap & grid)
-{
-  auto & grid_data = grid["layer"];
-  for (const auto & obj : objs) {
-    Polygon2d foot_print_polygon = planning_utils::toFootprintPolygon(obj);
-    grid_map::Polygon grid_polygon;
-    const auto & pos = obj.kinematics.initial_pose_with_covariance.pose.position;
-    if (grid.isInside(grid_map::Position(pos.x, pos.y))) continue;
-    try {
-      for (const auto & point : foot_print_polygon.outer()) {
-        grid_polygon.addVertex({point.x(), point.y()});
-      }
-      for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd();
-           ++iterator) {
-        const grid_map::Index & index = *iterator;
-        if (!grid.isValid(index)) continue;
-        grid_data(index.x(), index.y()) = grid_utils::occlusion_cost_value::OCCUPIED;
-      }
-    } catch (const std::invalid_argument & e) {
-      std::cerr << e.what() << std::endl;
-    }
-  }
-}
-
 void findOcclusionSpots(
   std::vector<grid_map::Position> & occlusion_spot_positions, const grid_map::GridMap & grid,
   const Polygon2d & polygon, [[maybe_unused]] double min_size)

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

@@ -79,39 +79,6 @@ bool buildDetectionAreaPolygon(
     slices, path, pose, da_range, p.pedestrian_vel);
 }
 
-ROAD_TYPE getCurrentRoadType(
-  const lanelet::ConstLanelet & current_lanelet,
-  [[maybe_unused]] const lanelet::LaneletMapPtr & lanelet_map_ptr)
-{
-  const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot")};
-  rclcpp::Clock clock{RCL_ROS_TIME};
-  occlusion_spot_utils::ROAD_TYPE road_type;
-  std::string location;
-  if (
-    current_lanelet.hasAttribute(lanelet::AttributeNamesString::Subtype) &&
-    current_lanelet.attribute(lanelet::AttributeNamesString::Subtype) ==
-      lanelet::AttributeValueString::Highway) {
-    location = "highway";
-  } else {
-    location = current_lanelet.attributeOr("location", "else");
-  }
-  RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "location: " << location);
-  if (location == "urban" || location == "public") {
-    road_type = occlusion_spot_utils::ROAD_TYPE::PUBLIC;
-    RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "public road: " << location);
-  } else if (location == "private") {
-    road_type = occlusion_spot_utils::ROAD_TYPE::PRIVATE;
-    RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "private road");
-  } else if (location == "highway") {
-    road_type = occlusion_spot_utils::ROAD_TYPE::HIGHWAY;
-    RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "highway road");
-  } else {
-    road_type = occlusion_spot_utils::ROAD_TYPE::UNKNOWN;
-    RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "unknown road");
-  }
-  return road_type;
-}
-
 void calcSlowDownPointsForPossibleCollision(
   const int closest_idx, const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const double offset, std::vector<PossibleCollisionInfo> & possible_collisions)

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp

@@ -64,34 +64,6 @@ void applySafeVelocityConsideringPossibleCollision(
   }
 }
 
-int insertSafeVelocityToPath(
-  const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param,
-  PathWithLaneId * inout_path)
-{
-  const auto closest_idx_opt =
-    motion_utils::findNearestIndex(inout_path->points, in_pose, param.dist_thr, param.angle_thr);
-  if (!closest_idx_opt) {
-    return -1;
-  }
-  const size_t closest_idx = closest_idx_opt.get();
-
-  PathPointWithLaneId inserted_point;
-  inserted_point = inout_path->points.at(closest_idx);
-  size_t insert_idx = closest_idx;
-  if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) {
-    ++insert_idx;
-    if (insert_idx == static_cast<size_t>(inout_path->points.size())) return -1;
-  }
-  // return if index is after the last path point
-  inserted_point.point.pose = in_pose;
-  // insert velocity to path if distance is not too close else insert new collision point
-  // if original path has narrow points it's better to set higher distance threshold
-  // TODO(tanaka): use Spherical Linear Interpolation for inserting point
-  const double eps = 0.05;
-  planning_utils::insertVelocity(*inout_path, inserted_point, safe_vel, insert_idx, eps);
-  return 0;
-}
-
 SafeMotion calculateSafeMotion(const Velocity & v, const double ttv)
 {
   SafeMotion sm;

planning/behavior_velocity_planner/src/utilization/util.cpp

@@ -227,28 +227,6 @@ void insertVelocity(
   setVelocityFromIndex(insert_index, v, &path);
 }
 
-boost::optional<geometry_msgs::msg::Pose> insertDecelPoint(
-  const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output,
-  const float target_velocity)
-{
-  // TODO(tanaka): consider proper overlap threshold for inserting decel point
-  const double overlap_threshold = 5e-2;
-  const size_t base_idx = motion_utils::findNearestSegmentIndex(output.points, stop_point);
-  const auto insert_idx =
-    motion_utils::insertTargetPoint(base_idx, stop_point, output.points, overlap_threshold);
-
-  if (!insert_idx) {
-    return {};
-  }
-
-  for (size_t i = insert_idx.get(); i < output.points.size(); ++i) {
-    const auto & original_velocity = output.points.at(i).point.longitudinal_velocity_mps;
-    output.points.at(i).point.longitudinal_velocity_mps =
-      std::min(original_velocity, target_velocity);
-  }
-  return tier4_autoware_utils::getPose(output.points.at(insert_idx.get()));
-}
-
 Polygon2d toFootprintPolygon(const PredictedObject & object)
 {
   Polygon2d obj_footprint;
@@ -646,6 +624,28 @@ bool isOverLine(
          0.0;
 }
 
+boost::optional<geometry_msgs::msg::Pose> insertDecelPoint(
+  const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output,
+  const float target_velocity)
+{
+  // TODO(tanaka): consider proper overlap threshold for inserting decel point
+  const double overlap_threshold = 5e-2;
+  const size_t base_idx = motion_utils::findNearestSegmentIndex(output.points, stop_point);
+  const auto insert_idx =
+    motion_utils::insertTargetPoint(base_idx, stop_point, output.points, overlap_threshold);
+
+  if (!insert_idx) {
+    return {};
+  }
+
+  for (size_t i = insert_idx.get(); i < output.points.size(); ++i) {
+    const auto & original_velocity = output.points.at(i).point.longitudinal_velocity_mps;
+    output.points.at(i).point.longitudinal_velocity_mps =
+      std::min(original_velocity, target_velocity);
+  }
+  return tier4_autoware_utils::getPose(output.points.at(insert_idx.get()));
+}
+
 boost::optional<geometry_msgs::msg::Pose> insertStopPoint(
   const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output)
 {

planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp

@@ -64,58 +64,3 @@ TEST(safeMotion, delay_jerk_acceleration)
     EXPECT_NEAR(std::round(sm.stop_dist * 100.0) / 100.0, 13.92, eps);
   }
 }
-
-TEST(insertSafeVelocityToPath, replace_original_at_too_close_case)
-{
-  /* straight path
-    0 1 2 3 4
-  0 x x z x x
-    --->
-  straight path
-     0 1 2 3 4
-  0  x x r xox
-  */
-  using behavior_velocity_planner::occlusion_spot_utils::insertSafeVelocityToPath;
-  const int num_path_point = 5;
-  const double inf = std::numeric_limits<double>::max();
-  behavior_velocity_planner::occlusion_spot_utils::PlannerParam param;
-  param.angle_thr = 1.0;
-  param.dist_thr = 10.0;
-  autoware_auto_planning_msgs::msg::PathWithLaneId path =
-    test::generatePath(0.0, 0.0, static_cast<double>(num_path_point - 1), 0.0, num_path_point);
-  geometry_msgs::msg::Pose pose{};
-  pose.position.x = 2;
-  double safe_vel = inf;
-  // insert x=2 new point -> replace original
-  insertSafeVelocityToPath(pose, safe_vel, param, &path);
-  ASSERT_EQ(path.points.size(), static_cast<size_t>(num_path_point));
-  pose.position.x = 3.4;
-  insertSafeVelocityToPath(pose, safe_vel, param, &path);
-  ASSERT_EQ(path.points.size(), static_cast<size_t>(num_path_point + 1));
-}
-
-TEST(insertSafeVelocityToPath, dont_insert_last_point)
-{
-  /* straight path
-      0 1 2 3 4
-    0 x x z x xo
-    --->
-    straight path
-      0 1 2 3 4
-    0 x x x x x
-  */
-  using behavior_velocity_planner::occlusion_spot_utils::insertSafeVelocityToPath;
-  const int num_path = 5;
-  geometry_msgs::msg::Pose pose{};
-  pose.position.x = 5;
-  pose.position.y = 0;
-  pose.position.z = 0;
-  double safe_vel = 10;
-  behavior_velocity_planner::occlusion_spot_utils::PlannerParam param;
-  param.angle_thr = 1.0;
-  param.dist_thr = 10.0;
-  autoware_auto_planning_msgs::msg::PathWithLaneId path =
-    test::generatePath(0.0, 0.0, static_cast<double>(num_path - 1), 0.0, num_path);
-  ASSERT_EQ(insertSafeVelocityToPath(pose, safe_vel, param, &path), -1);
-  ASSERT_EQ(path.points.size(), static_cast<size_t>(num_path));
-}