fix(autoware_behavior_path_planner_common): fix unusedFunction (#8654)

* fix:unusedFunction 0-2

Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>

* fix:unusedFunction 3-5

Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>

* fix:unusedFunction

Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>

---------

Signed-off-by: kobayu858 <yutaro.kobayashi@tier4.jp>

Author: kobayu858

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_utils.hpp

@@ -67,11 +67,6 @@ void clipPathLength(
 void clipPathLength(
   PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params);
 
-std::pair<TurnIndicatorsCommand, double> getPathTurnSignal(
-  const lanelet::ConstLanelets & current_lanes, const ShiftedPath & path,
-  const ShiftLine & shift_line, const Pose & pose, const double & velocity,
-  const BehaviorPathPlannerParameters & common_parameter);
-
 PathWithLaneId convertWayPointsToPathWithLaneId(
   const autoware::freespace_planning_algorithms::PlannerWaypoints & waypoints,
   const double velocity, const lanelet::ConstLanelets & lanelets);
@@ -83,8 +78,6 @@ std::vector<PathWithLaneId> dividePath(
 
 void correctDividedPathVelocity(std::vector<PathWithLaneId> & divided_paths);
 
-bool isCloseToPath(const PathWithLaneId & path, const Pose & pose, const double distance_threshold);
-
 // only two points is supported
 std::vector<double> splineTwoPoints(
   const std::vector<double> & base_s, const std::vector<double> & base_x, const double begin_diff,
@@ -103,8 +96,6 @@ PathWithLaneId calcCenterLinePath(
 
 PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
 
-std::optional<Pose> getFirstStopPoseFromPath(const PathWithLaneId & path);
-
 BehaviorModuleOutput getReferencePath(
   const lanelet::ConstLanelet & current_lane,
   const std::shared_ptr<const PlannerData> & planner_data);

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp

@@ -104,8 +104,6 @@ FrenetPoint convertToFrenetPoint(
   return frenet_point;
 }
 
-std::vector<lanelet::Id> getIds(const lanelet::ConstLanelets & lanelets);
-
 // distance (arclength) calculation
 
 double l2Norm(const Vector3 vector);
@@ -126,14 +124,6 @@ double getArcLengthToTargetLanelet(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelet & target_lane,
   const Pose & pose);
 
-double getDistanceBetweenPredictedPaths(
-  const PredictedPath & path1, const PredictedPath & path2, const double start_time,
-  const double end_time, const double resolution);
-
-double getDistanceBetweenPredictedPathAndObject(
-  const PredictedObject & object, const PredictedPath & path, const double start_time,
-  const double end_time, const double resolution);
-
 /**
  * @brief Check collision between ego path footprints with extra longitudinal stopping margin and
  * objects.
@@ -223,8 +213,6 @@ PathWithLaneId refinePathForGoal(
   const double search_radius_range, const double search_rad_range, const PathWithLaneId & input,
   const Pose & goal, const int64_t goal_lane_id);
 
-bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id);
-
 bool isAllowedGoalModification(const std::shared_ptr<RouteHandler> & route_handler);
 bool checkOriginalGoalIsInShoulder(const std::shared_ptr<RouteHandler> & route_handler);
 
@@ -269,10 +257,6 @@ Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet);
 
 Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon);
 
-std::vector<Polygon2d> getTargetLaneletPolygons(
-  const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length,
-  const std::string & target_type);
-
 PathWithLaneId getCenterLinePathFromLanelet(
   const lanelet::ConstLanelet & current_route_lanelet,
   const std::shared_ptr<const PlannerData> & planner_data);
@@ -283,11 +267,6 @@ PathWithLaneId getCenterLinePath(
   const Pose & pose, const double backward_path_length, const double forward_path_length,
   const BehaviorPathPlannerParameters & parameter);
 
-PathWithLaneId setDecelerationVelocity(
-  const RouteHandler & route_handler, const PathWithLaneId & input,
-  const lanelet::ConstLanelets & lanelet_sequence, const double lane_change_prepare_duration,
-  const double lane_change_buffer);
-
 // object label
 std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & classification);
 

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp

@@ -341,6 +341,7 @@ MarkerArray createObjectsMarkerArray(
   return msg;
 }
 
+// cppcheck-suppress unusedFunction
 MarkerArray createDrivableLanesMarkerArray(
   const std::vector<DrivableLanes> & drivable_lanes, std::string && ns)
 {

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

@@ -622,20 +622,6 @@ std::vector<Point> updateBoundary(
   return updated_bound;
 }
 
-[[maybe_unused]] geometry_msgs::msg::Point calcCenterOfGeometry(const Polygon2d & obj_poly)
-{
-  geometry_msgs::msg::Point center_pos;
-  for (const auto & point : obj_poly.outer()) {
-    center_pos.x += point.x();
-    center_pos.y += point.y();
-  }
-
-  center_pos.x = center_pos.x / obj_poly.outer().size();
-  center_pos.y = center_pos.y / obj_poly.outer().size();
-  center_pos.z = center_pos.z / obj_poly.outer().size();
-
-  return center_pos;
-}
 }  // namespace autoware::behavior_path_planner::utils::drivable_area_processing
 
 namespace autoware::behavior_path_planner::utils

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp

@@ -104,16 +104,6 @@ void initializeCollisionCheckDebugMap(CollisionCheckDebugMap & collision_check_d
   collision_check_debug_map.clear();
 }
 
-void updateSafetyCheckTargetObjectsData(
-  StartGoalPlannerData & data, const PredictedObjects & filtered_objects,
-  const TargetObjectsOnLane & target_objects_on_lane,
-  const std::vector<PoseWithVelocityStamped> & ego_predicted_path)
-{
-  data.filtered_objects = filtered_objects;
-  data.target_objects_on_lane = target_objects_on_lane;
-  data.ego_predicted_path = ego_predicted_path;
-}
-
 std::pair<double, double> getPairsTerminalVelocityAndAccel(
   const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel,
   const size_t current_path_idx)

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp

@@ -209,127 +209,6 @@ void clipPathLength(
   clipPathLength(path, target_idx, params.forward_path_length, params.backward_path_length);
 }
 
-std::pair<TurnIndicatorsCommand, double> getPathTurnSignal(
-  const lanelet::ConstLanelets & current_lanes, const ShiftedPath & path,
-  const ShiftLine & shift_line, const Pose & pose, const double & velocity,
-  const BehaviorPathPlannerParameters & common_parameter)
-{
-  TurnIndicatorsCommand turn_signal;
-  turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
-  const double max_time = std::numeric_limits<double>::max();
-  const double max_distance = std::numeric_limits<double>::max();
-  if (path.shift_length.size() < shift_line.end_idx + 1) {
-    return std::make_pair(turn_signal, max_distance);
-  }
-  const auto base_link2front = common_parameter.base_link2front;
-  const auto vehicle_width = common_parameter.vehicle_width;
-  const auto shift_to_outside = vehicle_width / 2;
-  const auto turn_signal_shift_length_threshold =
-    common_parameter.turn_signal_shift_length_threshold;
-  const auto turn_signal_minimum_search_distance =
-    common_parameter.turn_signal_minimum_search_distance;
-  const auto turn_signal_search_time = common_parameter.turn_signal_search_time;
-  constexpr double epsilon = 1e-6;
-  const auto arc_position_current_pose = lanelet::utils::getArcCoordinates(current_lanes, pose);
-
-  // Start turn signal when 1 or 2 is satisfied
-  //  1. time to shift start point is less than prev_sec
-  //  2. distance to shift point is shorter than tl_on_threshold_long
-
-  // Turn signal on when conditions below are satisfied
-  //  1. lateral offset is larger than tl_on_threshold_lat for left signal
-  //                      smaller than tl_on_threshold_lat for right signal
-  //  2. side point at shift start/end point cross the line
-  const double distance_to_shift_start =
-    std::invoke([&current_lanes, &shift_line, &arc_position_current_pose]() {
-      const auto arc_position_shift_start =
-        lanelet::utils::getArcCoordinates(current_lanes, shift_line.start);
-      return arc_position_shift_start.length - arc_position_current_pose.length;
-    });
-
-  const auto time_to_shift_start =
-    (std::abs(velocity) < epsilon) ? max_time : distance_to_shift_start / velocity;
-
-  const double diff =
-    path.shift_length.at(shift_line.end_idx) - path.shift_length.at(shift_line.start_idx);
-
-  Pose shift_start_point = path.path.points.at(shift_line.start_idx).point.pose;
-  Pose shift_end_point = path.path.points.at(shift_line.end_idx).point.pose;
-  Pose left_start_point = shift_start_point;
-  Pose right_start_point = shift_start_point;
-  Pose left_end_point = shift_end_point;
-  Pose right_end_point = shift_end_point;
-  {
-    const double start_yaw = tf2::getYaw(shift_line.start.orientation);
-    const double end_yaw = tf2::getYaw(shift_line.end.orientation);
-    left_start_point.position.x -= std::sin(start_yaw) * (shift_to_outside);
-    left_start_point.position.y += std::cos(start_yaw) * (shift_to_outside);
-    right_start_point.position.x -= std::sin(start_yaw) * (-shift_to_outside);
-    right_start_point.position.y += std::cos(start_yaw) * (-shift_to_outside);
-    left_end_point.position.x -= std::sin(end_yaw) * (shift_to_outside);
-    left_end_point.position.y += std::cos(end_yaw) * (shift_to_outside);
-    right_end_point.position.x -= std::sin(end_yaw) * (-shift_to_outside);
-    right_end_point.position.y += std::cos(end_yaw) * (-shift_to_outside);
-  }
-
-  bool left_start_point_is_in_lane = false;
-  bool right_start_point_is_in_lane = false;
-  bool left_end_point_is_in_lane = false;
-  bool right_end_point_is_in_lane = false;
-  {
-    for (const auto & llt : current_lanes) {
-      if (lanelet::utils::isInLanelet(left_start_point, llt, 0.1)) {
-        left_start_point_is_in_lane = true;
-      }
-      if (lanelet::utils::isInLanelet(right_start_point, llt, 0.1)) {
-        right_start_point_is_in_lane = true;
-      }
-      if (lanelet::utils::isInLanelet(left_end_point, llt, 0.1)) {
-        left_end_point_is_in_lane = true;
-      }
-      if (lanelet::utils::isInLanelet(right_end_point, llt, 0.1)) {
-        right_end_point_is_in_lane = true;
-      }
-    }
-  }
-
-  const bool cross_line = std::invoke([&]() {
-    constexpr bool temporary_set_cross_line_true =
-      true;  // due to a bug. See link:
-             // https://github.com/autowarefoundation/autoware.universe/pull/748
-    if (temporary_set_cross_line_true) {
-      return true;
-    }
-    return (
-      left_start_point_is_in_lane != left_end_point_is_in_lane ||
-      right_start_point_is_in_lane != right_end_point_is_in_lane);
-  });
-
-  if (
-    time_to_shift_start < turn_signal_search_time ||
-    distance_to_shift_start < turn_signal_minimum_search_distance) {
-    if (diff > turn_signal_shift_length_threshold && cross_line) {
-      turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
-    } else if (diff < -turn_signal_shift_length_threshold && cross_line) {
-      turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
-    }
-  }
-
-  // calc distance from ego vehicle front to shift end point.
-  const double distance_from_vehicle_front =
-    std::invoke([&current_lanes, &shift_line, &arc_position_current_pose, &base_link2front]() {
-      const auto arc_position_shift_end =
-        lanelet::utils::getArcCoordinates(current_lanes, shift_line.end);
-      return arc_position_shift_end.length - arc_position_current_pose.length - base_link2front;
-    });
-
-  if (distance_from_vehicle_front >= 0.0) {
-    return std::make_pair(turn_signal, distance_from_vehicle_front);
-  }
-
-  return std::make_pair(turn_signal, max_distance);
-}
-
 PathWithLaneId convertWayPointsToPathWithLaneId(
   const autoware::freespace_planning_algorithms::PlannerWaypoints & waypoints,
   const double velocity, const lanelet::ConstLanelets & lanelets)
@@ -431,18 +310,6 @@ void correctDividedPathVelocity(std::vector<PathWithLaneId> & divided_paths)
   }
 }
 
-bool isCloseToPath(const PathWithLaneId & path, const Pose & pose, const double distance_threshold)
-{
-  for (const auto & point : path.points) {
-    const auto & p = point.point.pose.position;
-    const double dist = std::hypot(pose.position.x - p.x, pose.position.y - p.y);
-    if (dist < distance_threshold) {
-      return true;
-    }
-  }
-  return false;
-}
-
 // only two points is supported
 std::vector<double> splineTwoPoints(
   const std::vector<double> & base_s, const std::vector<double> & base_x, const double begin_diff,
@@ -579,16 +446,6 @@ PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId &
   return filtered_path;
 }
 
-std::optional<Pose> getFirstStopPoseFromPath(const PathWithLaneId & path)
-{
-  for (const auto & p : path.points) {
-    if (std::abs(p.point.longitudinal_velocity_mps) < 0.01) {
-      return p.point.pose;
-    }
-  }
-  return std::nullopt;
-}
-
 BehaviorModuleOutput getReferencePath(
   const lanelet::ConstLanelet & current_lane,
   const std::shared_ptr<const PlannerData> & planner_data)