add calculation cpp

Signed-off-by: Zulfaqar Azmi <[email protected]>

planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -14,6 +14,7 @@
 
 #include "scene.hpp"
 
+#include "autoware_behavior_path_lane_change_module/utils/calculation.hpp"
 #include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
 #include "autoware_behavior_path_planner_common/utils/path_utils.hpp"
@@ -283,8 +284,8 @@ double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
     return std::numeric_limits<double>::infinity();
   }
 
-  return utils::lane_change::calcMinimumLaneChangeLength(
-    getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_);
+  return utils::lane_change::calculation::calc_min_lane_change_length(
+    common_data_ptr_, current_lanes, direction_);
 }
 
 double AvoidanceByLaneChange::calcLateralOffset() const

planning/autoware_behavior_path_lane_change_module/CMakeLists.txt

@@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/interface.cpp
   src/manager.cpp
   src/scene.cpp
+  src/utils/calculation.cpp
   src/utils/markers.cpp
   src/utils/utils.cpp
 )

planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/base_class.hpp

@@ -50,7 +50,10 @@ class LaneChangeBase
   LaneChangeBase(
     std::shared_ptr<LaneChangeParameters> parameters, LaneChangeModuleType type,
     Direction direction)
-  : lane_change_parameters_{std::move(parameters)}, direction_{direction}, type_{type}
+  : lane_change_parameters_{std::move(parameters)},
+    common_data_ptr_{std::make_shared<lane_change::CommonData>()},
+    direction_{direction},
+    type_{type}
   {
   }
 
@@ -151,7 +154,18 @@ class LaneChangeBase
 
   bool isValidPath() const { return status_.is_valid_path; }
 
-  void setData(const std::shared_ptr<const PlannerData> & data) { planner_data_ = data; }
+  void setData(const std::shared_ptr<const PlannerData> & data)
+  {
+    planner_data_ = data;
+    if (!common_data_ptr_->bpp_params) {
+      common_data_ptr_->bpp_params =
+        std::make_shared<BehaviorPathPlannerParameters>(data->parameters);
+    }
+    common_data_ptr_->self_odometry = data->self_odometry;
+    common_data_ptr_->route_handler = data->route_handler;
+    common_data_ptr_->lc_params = lane_change_parameters_;
+    common_data_ptr_->direction = direction_;
+  }
 
   void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; }
 
@@ -219,6 +233,7 @@ class LaneChangeBase
   std::shared_ptr<LaneChangeParameters> lane_change_parameters_{};
   std::shared_ptr<LaneChangePath> abort_path_{};
   std::shared_ptr<const PlannerData> planner_data_{};
+  lane_change::CommonDataPtr common_data_ptr_{};
   BehaviorModuleOutput prev_module_output_{};
   std::optional<Pose> lane_change_stop_pose_{std::nullopt};
 

planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/calculation.hpp

@@ -0,0 +1,37 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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 AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_
+#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_
+
+#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp"
+
+namespace autoware::behavior_path_planner::utils::lane_change::calculation
+{
+using behavior_path_planner::lane_change::CommonDataPtr;
+using behavior_path_planner::lane_change::LCParamPtr;
+using behavior_path_planner::lane_change::RouteHandlerPtr;
+using route_handler::Direction;
+
+double calc_min_lane_change_length(
+  const LCParamPtr & lc_params, const std::vector<double> & shift_intervals);
+
+double calc_min_lane_change_length(
+  const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes,
+  Direction direction = Direction::NONE);
+
+double calc_ego_remaining_distance_in_current_lanes(const CommonDataPtr & common_data_ptr);
+}  // namespace autoware::behavior_path_planner::utils::lane_change::calculation
+
+#endif  // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_

planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/data_structs.hpp

@@ -17,11 +17,16 @@
 #include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp"
 
+#include <autoware_behavior_path_planner_common/parameters.hpp>
+#include <autoware_route_handler/route_handler.hpp>
 #include <interpolation/linear_interpolation.hpp>
 
+#include <nav_msgs/msg/odometry.hpp>
+
 #include <lanelet2_core/primitives/Lanelet.h>
 #include <lanelet2_core/primitives/Polygon.h>
 
+#include <memory>
 #include <utility>
 #include <vector>
 
@@ -205,6 +210,12 @@ enum class LaneChangeModuleType {
 
 namespace autoware::behavior_path_planner::lane_change
 {
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::Twist;
+using nav_msgs::msg::Odometry;
+using route_handler::Direction;
+using route_handler::RouteHandler;
+
 struct PathSafetyStatus
 {
   bool is_safe{true};
@@ -217,6 +228,44 @@ struct LanesPolygon
   std::optional<lanelet::BasicPolygon2d> target;
   std::vector<lanelet::BasicPolygon2d> target_backward;
 };
+
+struct Lanes
+{
+  lanelet::ConstLanelets current;
+  lanelet::ConstLanelets target;
+  std::vector<lanelet::ConstLanelets> preceding_target;
+};
+
+struct CommonData
+{
+  std::shared_ptr<RouteHandler> route_handler;
+  Odometry::ConstSharedPtr self_odometry;
+  std::shared_ptr<BehaviorPathPlannerParameters> bpp_params;
+  std::shared_ptr<LaneChangeParameters> lc_params;
+  Lanes lanes;
+  Direction direction;
+
+  [[nodiscard]] Pose get_ego_pose() const { return self_odometry->pose.pose; }
+
+  [[nodiscard]] Twist get_ego_twist() const { return self_odometry->twist.twist; }
+
+  [[nodiscard]] double get_ego_speed(bool use_norm = false) const
+  {
+    if (!use_norm) {
+      return get_ego_twist().linear.x;
+    }
+
+    const auto x = get_ego_twist().linear.x;
+    const auto y = get_ego_twist().linear.y;
+    return std::hypot(x, y);
+  }
+};
+
+using RouteHandlerPtr = std::shared_ptr<RouteHandler>;
+using BppParamPtr = std::shared_ptr<BehaviorPathPlannerParameters>;
+using LCParamPtr = std::shared_ptr<LaneChangeParameters>;
+using CommonDataPtr = std::shared_ptr<CommonData>;
+using LanesPtr = std::shared_ptr<Lanes>;
 }  // namespace autoware::behavior_path_planner::lane_change
 
 #endif  // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_

planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/utils.hpp

@@ -47,6 +47,7 @@ using autoware::route_handler::Direction;
 using autoware_perception_msgs::msg::PredictedObject;
 using autoware_perception_msgs::msg::PredictedObjects;
 using autoware_perception_msgs::msg::PredictedPath;
+using behavior_path_planner::lane_change::CommonDataPtr;
 using behavior_path_planner::lane_change::LanesPolygon;
 using behavior_path_planner::lane_change::PathSafetyStatus;
 using geometry_msgs::msg::Point;
@@ -59,13 +60,6 @@ using tier4_planning_msgs::msg::PathWithLaneId;
 double calcLaneChangeResampleInterval(
   const double lane_changing_length, const double lane_changing_velocity);
 
-double calcMinimumLaneChangeLength(
-  const LaneChangeParameters & lane_change_parameters, const std::vector<double> & shift_intervals);
-
-double calcMinimumLaneChangeLength(
-  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lane,
-  const LaneChangeParameters & lane_change_parameters, Direction direction);
-
 double calcMaximumLaneChangeLength(
   const double current_velocity, const LaneChangeParameters & lane_change_parameters,
   const std::vector<double> & shift_intervals, const double max_acc);
@@ -144,14 +138,10 @@ std::vector<DrivableLanes> generateDrivableLanes(
 double getLateralShift(const LaneChangePath & path);
 
 bool hasEnoughLengthToLaneChangeAfterAbort(
-  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelets & current_lanes,
-  const Pose & curent_pose, const double abort_return_dist,
-  const LaneChangeParameters & lane_change_parameters, const Direction direction);
+  const CommonDataPtr & common_data_ptr, const double abort_return_dist, const Direction direction);
 
 double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0);
 
-double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer);
-
 std::string getStrDirection(const std::string & name, const Direction direction);
 
 CandidateOutput assignToCandidate(

planning/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -14,6 +14,7 @@
 
 #include "autoware_behavior_path_lane_change_module/scene.hpp"
 
+#include "autoware_behavior_path_lane_change_module/utils/calculation.hpp"
 #include "autoware_behavior_path_lane_change_module/utils/utils.hpp"
 #include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
@@ -38,8 +39,8 @@
 namespace autoware::behavior_path_planner
 {
 using motion_utils::calcSignedArcLength;
-using utils::lane_change::calcMinimumLaneChangeLength;
 using utils::lane_change::createLanesPolygon;
+using utils::lane_change::calculation::calc_min_lane_change_length;
 using utils::path_safety_checker::isPolygonOverlapLanelet;
 using utils::traffic_light::getDistanceToNextTrafficLight;
 
@@ -172,10 +173,8 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info()
     lane_change_param.min_length_for_turn_signal_activation;
   const auto & route_handler = getRouteHandler();
   const auto & common_parameter = getCommonParam();
-  const auto shift_intervals =
-    route_handler->getLateralIntervalsToPreferredLane(current_lanes.back());
   const double next_lane_change_buffer =
-    utils::lane_change::calcMinimumLaneChangeLength(lane_change_param, shift_intervals);
+    calc_min_lane_change_length(common_data_ptr_, current_lanes);
   const double nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold;
   const double nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold;
   const double base_to_front = common_parameter.base_link2front;
@@ -338,7 +337,7 @@ void NormalLaneChange::insertStopPoint(
 
   const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back());
   const auto lane_change_buffer =
-    calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals);
+    calc_min_lane_change_length(lane_change_parameters_, shift_intervals);
 
   const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) {
     return utils::getSignedDistance(path.points.front().point.pose, target, lanelets);
@@ -418,7 +417,7 @@ void NormalLaneChange::insertStopPoint(
       0.0, lane_change_parameters_->minimum_lane_changing_velocity,
       lane_change_parameters_->rss_params);
     const double lane_change_buffer_for_blocking_object =
-      utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals);
+      calc_min_lane_change_length(lane_change_parameters_, shift_intervals);
 
     const auto stopping_distance_for_obj =
       distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object -
@@ -625,8 +624,7 @@ bool NormalLaneChange::isNearEndOfCurrentLanes(
 
   const auto & route_handler = getRouteHandler();
   const auto & current_pose = getEgoPose();
-  const auto lane_change_buffer = calcMinimumLaneChangeLength(
-    route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE);
+  const auto lane_change_buffer = calc_min_lane_change_length(common_data_ptr_, current_lanes);
 
   const auto distance_to_lane_change_end = std::invoke([&]() {
     auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes);
@@ -1075,8 +1073,8 @@ void NormalLaneChange::filterAheadTerminalObjects(
 {
   const auto & current_pose = getEgoPose();
   const auto & route_handler = getRouteHandler();
-  const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength(
-    route_handler, current_lanes.back(), *lane_change_parameters_, direction_);
+  const auto minimum_lane_change_length =
+    calc_min_lane_change_length(common_data_ptr_, current_lanes);
 
   const auto dist_ego_to_current_lanes_center =
     lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose);
@@ -1235,9 +1233,8 @@ bool NormalLaneChange::hasEnoughLength(
   const auto current_pose = getEgoPose();
   const auto & route_handler = getRouteHandler();
   const auto overall_graphs_ptr = route_handler->getOverallGraphPtr();
-  const auto minimum_lane_change_length_to_preferred_lane = calcMinimumLaneChangeLength(
-    route_handler, target_lanes.back(), *lane_change_parameters_, direction);
-
+  const auto minimum_lane_change_length_to_preferred_lane =
+    calc_min_lane_change_length(common_data_ptr_, target_lanes, direction);
   const double lane_change_length = path.info.length.sum();
   if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) {
     return false;
@@ -1342,12 +1339,9 @@ bool NormalLaneChange::getLaneChangePaths(
 
   const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back());
 
-  const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength(
-    *lane_change_parameters_,
-    route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()));
-  const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength(
-    *lane_change_parameters_,
-    route_handler.getLateralIntervalsToPreferredLane(target_lanes.back()));
+  const double lane_change_buffer = calc_min_lane_change_length(common_data_ptr_, current_lanes);
+  const double next_lane_change_buffer =
+    calc_min_lane_change_length(common_data_ptr_, target_lanes);
 
   const auto dist_to_end_of_current_lanes =
     utils::getDistanceToEndOfLane(getEgoPose(), current_lanes);
@@ -1638,12 +1632,9 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
 
   const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back());
 
-  const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength(
-    *lane_change_parameters_,
-    route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()));
-  const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength(
-    *lane_change_parameters_,
-    route_handler.getLateralIntervalsToPreferredLane(target_lanes.back()));
+  const double lane_change_buffer = calc_min_lane_change_length(common_data_ptr_, current_lanes);
+  const double next_lane_change_buffer =
+    calc_min_lane_change_length(common_data_ptr_, target_lanes);
 
   const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes);
 
@@ -1884,8 +1875,8 @@ bool NormalLaneChange::calcAbortPath()
   const auto ego_nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold;
 
   const auto direction = getDirection();
-  const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength(
-    route_handler, selected_path.info.current_lanes.back(), *lane_change_parameters_, direction);
+  const auto minimum_lane_change_length =
+    calc_min_lane_change_length(common_data_ptr_, selected_path.info.current_lanes, direction);
 
   const auto & lane_changing_path = selected_path.path;
   const auto lane_changing_end_pose_idx = std::invoke([&]() {
@@ -1932,8 +1923,7 @@ bool NormalLaneChange::calcAbortPath()
   }
 
   if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort(
-        route_handler, reference_lanelets, current_pose, abort_return_dist,
-        *lane_change_parameters_, direction)) {
+        common_data_ptr_, abort_return_dist, direction)) {
     RCLCPP_ERROR(logger_, "insufficient distance to abort.");
     return false;
   }
@@ -2132,8 +2122,7 @@ bool NormalLaneChange::isVehicleStuck(
     route_handler->isInGoalRouteSection(current_lanes.back())
       ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes)
       : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes);
-  const auto lane_change_buffer = calcMinimumLaneChangeLength(
-    route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE);
+  const auto lane_change_buffer = calc_min_lane_change_length(common_data_ptr_, current_lanes);
   const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane;
   const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0;
   if (distance_to_terminal < terminal_judge_buffer) {