refactor(behavior_velocity): move inline functions to cpp and remove …

…unused (#462)

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

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

@@ -179,60 +179,14 @@ struct PossibleCollisionInfo
 
 lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path);
 // Note : consider offset_from_start_to_ego and safety margin for collision here
-inline void handleCollisionOffset(
-  std::vector<PossibleCollisionInfo> & possible_collisions, double offset, double margin)
-{
-  for (auto & pc : possible_collisions) {
-    pc.arc_lane_dist_at_collision.length -= offset;
-    pc.arc_lane_dist_at_collision.length -= margin;
-  }
-}
-
-inline double offsetFromStartToEgo(
-  const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx)
-{
-  double offset_from_ego_to_closest = 0;
-  for (int i = 0; i < closest_idx; i++) {
-    const auto & curr_p = path.points.at(i).point.pose.position;
-    const auto & next_p = path.points.at(i + 1).point.pose.position;
-    offset_from_ego_to_closest += tier4_autoware_utils::calcDistance2d(curr_p, next_p);
-  }
-  const double offset_from_closest_to_target =
-    -planning_utils::transformRelCoordinate2D(ego_pose, path.points[closest_idx].point.pose)
-       .position.x;
-  return offset_from_ego_to_closest + offset_from_closest_to_target;
-}
-
-inline void clipPathByLength(
-  const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length = 100.0)
-{
-  double length_sum = 0;
-  for (int i = 0; i < static_cast<int>(path.points.size()) - 1; i++) {
-    length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
-    if (length_sum > max_length) return;
-    clipped.points.emplace_back(path.points.at(i));
-  }
-}
-
-inline bool isStuckVehicle(PredictedObject obj, const double min_vel)
-{
-  if (
-    obj.classification.at(0).label == ObjectClassification::CAR ||
-    obj.classification.at(0).label == ObjectClassification::TRUCK ||
-    obj.classification.at(0).label == ObjectClassification::BUS) {
-    if (std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x) < min_vel) {
-      return true;
-    }
-  }
-  return false;
-}
-void filterCollisionByRoadType(
-  std::vector<PossibleCollisionInfo> & possible_collisions, const DetectionAreaIdx road_type);
+void handleCollisionOffset(std::vector<PossibleCollisionInfo> & possible_collisions, double offset);
+void clipPathByLength(
+  const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length = 100.0);
+bool isStuckVehicle(PredictedObject obj, const double min_vel);
+double offsetFromStartToEgo(
+  const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx);
 std::vector<PredictedObject> filterDynamicObjectByDetectionArea(
   std::vector<PredictedObject> & objs, const std::vector<Slice> polys);
-bool splineInterpolate(
-  const PathWithLaneId & input, const double interval, PathWithLaneId * output,
-  const rclcpp::Logger logger);
 std::vector<PredictedObject> getParkedVehicles(
   const PredictedObjects & dyn_objects, const PlannerParam & param,
   std::vector<Point> & debug_point);

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

@@ -34,87 +34,25 @@ int insertSafeVelocityToPath(
   const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param,
   PathWithLaneId * inout_path);
 
-// @brief calculates the maximum velocity allowing to decelerate within the given distance
-inline double calculateMinSlowDownVelocity(
-  const double v0, const double len, const double a_max, const double safe_vel)
-{
-  // if target velocity is inserted backward return current velocity as limit
-  if (len < 0) return safe_vel;
-  return std::sqrt(std::max(std::pow(v0, 2.0) - 2.0 * std::abs(a_max) * len, 0.0));
-}
-
 /**
  *
  * @param: longitudinal_distance: longitudinal distance to collision
  * @param: param: planner param
  * @return lateral distance
  **/
-inline double calculateLateralDistanceFromTTC(
-  const double longitudinal_distance, const PlannerParam & param)
-{
-  const auto & v = param.v;
-  const auto & p = param;
-  double v_min = 1.0;
-  const double lateral_buffer = 0.5;
-  const double min_distance = p.half_vehicle_width + lateral_buffer;
-  const double max_distance = p.detection_area.max_lateral_distance;
-  if (longitudinal_distance <= 0) return min_distance;
-  if (v_min < param.v.min_allowed_velocity) v_min = param.v.min_allowed_velocity;
-  // use min velocity if ego velocity is below min allowed
-  const double v0 = (v.v_ego > v_min) ? v.v_ego : v_min;
-  // here is a part where ego t(ttc) can be replaced by calculation of velocity smoother or ?
-  double t = longitudinal_distance / v0;
-  double lateral_distance = t * param.pedestrian_vel + p.half_vehicle_width;
-  return std::min(max_distance, std::max(min_distance, lateral_distance));
-}
+double calculateLateralDistanceFromTTC(
+  const double longitudinal_distance, const PlannerParam & param);
 
 /**
  * @param: v: ego velocity config
  * @param: ttc: time to collision
  * @return safe motion
  **/
-inline SafeMotion calculateSafeMotion(const Velocity & v, const double ttc)
-{
-  SafeMotion sm;
-  const double j_max = v.safety_ratio * v.max_stop_jerk;
-  const double a_max = v.safety_ratio * v.max_stop_accel;
-  const double t1 = v.delay_time;
-  double t2 = a_max / j_max;
-  double & v_safe = sm.safe_velocity;
-  double & stop_dist = sm.stop_dist;
-  if (ttc <= t1) {
-    // delay
-    v_safe = 0;
-    stop_dist = 0;
-  } else if (ttc <= t2 + t1) {
-    // delay + const jerk
-    t2 = ttc - t1;
-    v_safe = -0.5 * j_max * t2 * t2;
-    stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6;
-  } else {
-    const double t3 = ttc - t2 - t1;
-    // delay + const jerk + const accel
-    const double v2 = -0.5 * j_max * t2 * t2;
-    v_safe = v2 - a_max * t3;
-    stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6 + v2 * t3 - 0.5 * a_max * t3 * t3;
-  }
-  stop_dist += v.safe_margin;
-  return sm;
-}
+SafeMotion calculateSafeMotion(const Velocity & v, const double ttc);
 
-inline double calculateInsertVelocity(
+double calculateInsertVelocity(
   const double min_allowed_vel, const double safe_vel, const double min_vel,
-  const double original_vel)
-{
-  const double max_vel_noise = 0.05;
-  // ensure safe velocity doesn't exceed maximum allowed pbs deceleration
-  double cmp_safe_vel = std::max(min_allowed_vel + max_vel_noise, safe_vel);
-  // ensure safe path velocity is also above ego min velocity
-  cmp_safe_vel = std::max(cmp_safe_vel, min_vel);
-  // ensure we only lower the original velocity (and do not increase it)
-  cmp_safe_vel = std::min(cmp_safe_vel, original_vel);
-  return cmp_safe_vel;
-}
+  const double original_vel);
 
 }  // namespace occlusion_spot_utils
 }  // namespace behavior_velocity_planner

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

@@ -146,6 +146,52 @@ void calcSlowDownPointsForPossibleCollision(
   }
 }
 
+void handleCollisionOffset(std::vector<PossibleCollisionInfo> & possible_collisions, double offset)
+{
+  for (auto & pc : possible_collisions) {
+    pc.arc_lane_dist_at_collision.length -= offset;
+  }
+}
+
+void clipPathByLength(
+  const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length)
+{
+  double length_sum = 0;
+  for (int i = 0; i < static_cast<int>(path.points.size()) - 1; i++) {
+    length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
+    if (length_sum > max_length) return;
+    clipped.points.emplace_back(path.points.at(i));
+  }
+}
+
+bool isStuckVehicle(PredictedObject obj, const double min_vel)
+{
+  if (
+    obj.classification.at(0).label == ObjectClassification::CAR ||
+    obj.classification.at(0).label == ObjectClassification::TRUCK ||
+    obj.classification.at(0).label == ObjectClassification::BUS) {
+    if (std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x) < min_vel) {
+      return true;
+    }
+  }
+  return false;
+}
+
+double offsetFromStartToEgo(
+  const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx)
+{
+  double offset_from_ego_to_closest = 0;
+  for (int i = 0; i < closest_idx; i++) {
+    const auto & curr_p = path.points.at(i).point.pose.position;
+    const auto & next_p = path.points.at(i + 1).point.pose.position;
+    offset_from_ego_to_closest += tier4_autoware_utils::calcDistance2d(curr_p, next_p);
+  }
+  const double offset_from_closest_to_target =
+    -planning_utils::transformRelCoordinate2D(ego_pose, path.points[closest_idx].point.pose)
+       .position.x;
+  return offset_from_ego_to_closest + offset_from_closest_to_target;
+}
+
 std::vector<PredictedObject> getParkedVehicles(
   const PredictedObjects & dyn_objects, const PlannerParam & param,
   std::vector<Point> & debug_point)
@@ -337,21 +383,6 @@ std::vector<PredictedObject> filterDynamicObjectByDetectionArea(
   return filtered_obj;
 }
 
-void filterCollisionByRoadType(
-  std::vector<PossibleCollisionInfo> & possible_collisions, const DetectionAreaIdx area)
-{
-  std::pair<int, int> focus_length = area.get();
-  for (auto it = possible_collisions.begin(); it != possible_collisions.end();) {
-    const auto & pc_len = it->arc_lane_dist_at_collision.length;
-    if (focus_length.first < pc_len && pc_len < focus_length.second) {
-      it++;
-    } else {
-      // -----erase-----|start------target-------end|----erase---
-      it = possible_collisions.erase(it);
-    }
-  }
-}
-
 void createPossibleCollisionsInDetectionArea(
   const std::vector<Slice> & detection_area_polygons,
   std::vector<PossibleCollisionInfo> & possible_collisions, const grid_map::GridMap & grid,

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

@@ -105,5 +105,66 @@ int insertSafeVelocityToPath(
   return 0;
 }
 
+double calculateLateralDistanceFromTTC(
+  const double longitudinal_distance, const PlannerParam & param)
+{
+  const auto & v = param.v;
+  const auto & p = param;
+  double v_min = 1.0;
+  const double lateral_buffer = 0.5;
+  const double min_distance = p.half_vehicle_width + lateral_buffer;
+  const double max_distance = p.detection_area.max_lateral_distance;
+  if (longitudinal_distance <= 0) return min_distance;
+  if (v_min < param.v.min_allowed_velocity) v_min = param.v.min_allowed_velocity;
+  // use min velocity if ego velocity is below min allowed
+  const double v0 = (v.v_ego > v_min) ? v.v_ego : v_min;
+  // here is a part where ego t(ttc) can be replaced by calculation of velocity smoother or ?
+  double t = longitudinal_distance / v0;
+  double lateral_distance = t * param.pedestrian_vel + p.half_vehicle_width;
+  return std::min(max_distance, std::max(min_distance, lateral_distance));
+}
+
+SafeMotion calculateSafeMotion(const Velocity & v, const double ttc)
+{
+  SafeMotion sm;
+  const double j_max = v.safety_ratio * v.max_stop_jerk;
+  const double a_max = v.safety_ratio * v.max_stop_accel;
+  const double t1 = v.delay_time;
+  double t2 = a_max / j_max;
+  double & v_safe = sm.safe_velocity;
+  double & stop_dist = sm.stop_dist;
+  if (ttc <= t1) {
+    // delay
+    v_safe = 0;
+    stop_dist = 0;
+  } else if (ttc <= t2 + t1) {
+    // delay + const jerk
+    t2 = ttc - t1;
+    v_safe = -0.5 * j_max * t2 * t2;
+    stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6;
+  } else {
+    const double t3 = ttc - t2 - t1;
+    // delay + const jerk + const accel
+    const double v2 = -0.5 * j_max * t2 * t2;
+    v_safe = v2 - a_max * t3;
+    stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6 + v2 * t3 - 0.5 * a_max * t3 * t3;
+  }
+  stop_dist += v.safe_margin;
+  return sm;
+}
+
+double calculateInsertVelocity(
+  const double min_allowed_vel, const double safe_vel, const double min_vel,
+  const double original_vel)
+{
+  const double max_vel_noise = 0.05;
+  // ensure safe velocity doesn't exceed maximum allowed pbs deceleration
+  double cmp_safe_vel = std::max(min_allowed_vel + max_vel_noise, safe_vel);
+  // ensure safe path velocity is also above ego min velocity
+  cmp_safe_vel = std::max(cmp_safe_vel, min_vel);
+  // ensure we only lower the original velocity (and do not increase it)
+  cmp_safe_vel = std::min(cmp_safe_vel, original_vel);
+  return cmp_safe_vel;
+}
 }  // namespace occlusion_spot_utils
 }  // namespace behavior_velocity_planner

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

@@ -99,7 +99,7 @@ bool OcclusionSpotModule::modifyPathVelocity(
     logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size());
   utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions);
   // Note: Consider offset from path start to ego here
-  utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego, 0.0);
+  utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego);
   // apply safe velocity using ebs and pbs deceleration
   utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_);
   // these debug topics needs computation resource

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

@@ -93,7 +93,7 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity(
       interp_path, param_, offset_from_start_to_ego, filtered_obj);
   utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions);
   // Note: Consider offset from path start to ego here
-  utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego, 0.0);
+  utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego);
   // apply safe velocity using ebs and pbs deceleration
   utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_);
   if (param_.debug) {