diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 4b88eb669a2cf..be8bcb3fb970d 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -19,12 +19,11 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include #include -#include #include #include @@ -34,6 +33,7 @@ #include #include #include +#include #include namespace behavior_path_planner @@ -48,7 +48,6 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using geometry_msgs::msg::TransformStamped; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; @@ -329,8 +328,13 @@ struct AvoidanceParameters struct ObjectData // avoidance target { ObjectData() = default; - ObjectData(const PredictedObject & obj, double lat, double lon, double len, double overhang) - : object(obj), to_centerline(lat), longitudinal(lon), length(len), overhang_dist(overhang) + + ObjectData(PredictedObject obj, double lat, double lon, double len, double overhang) + : object(std::move(obj)), + to_centerline(lat), + longitudinal(lon), + length(len), + overhang_dist(overhang) { } @@ -345,16 +349,17 @@ struct ObjectData // avoidance target Behavior behavior{Behavior::NONE}; // lateral position of the CoM, in Frenet coordinate from ego-pose - double to_centerline; + + double to_centerline{0.0}; // longitudinal position of the CoM, in Frenet coordinate from ego-pose - double longitudinal; + double longitudinal{0.0}; // longitudinal length of vehicle, in Frenet coordinate - double length; + double length{0.0}; // lateral distance to the closest footprint, in Frenet coordinate - double overhang_dist; + double overhang_dist{0.0}; // lateral shiftable ratio double shiftable_ratio{0.0}; @@ -414,7 +419,7 @@ struct ObjectData // avoidance target Direction direction{Direction::NONE}; // unavoidable reason - std::string reason{""}; + std::string reason{}; // lateral avoid margin std::optional avoid_margin{std::nullopt}; @@ -457,8 +462,8 @@ using AvoidLineArray = std::vector; struct AvoidOutline { - AvoidOutline(const AvoidLine & avoid_line, const AvoidLine & return_line) - : avoid_line{avoid_line}, return_line{return_line} + AvoidOutline(AvoidLine avoid_line, AvoidLine return_line) + : avoid_line{std::move(avoid_line)}, return_line{std::move(return_line)} { } diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp index 2705af5af4020..f49f457ddd066 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp @@ -16,33 +16,23 @@ #define BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ #include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" #include -#include #include -#include #include -#include - #include -#include namespace marker_utils::avoidance_marker { -using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::AvoidancePlanningData; -using behavior_path_planner::AvoidanceState; using behavior_path_planner::AvoidLineArray; using behavior_path_planner::DebugData; using behavior_path_planner::ObjectDataArray; using behavior_path_planner::PathShifter; using behavior_path_planner::ShiftLineArray; -using geometry_msgs::msg::Point; -using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; using visualization_msgs::msg::MarkerArray; @@ -50,7 +40,7 @@ MarkerArray createEgoStatusMarkerArray( const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns); MarkerArray createAvoidLineMarkerArray( - const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g, + const AvoidLineArray & shift_lines, std::string && ns, const float & r, const float & g, const float & b, const double & w); MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::string && ns); @@ -63,11 +53,11 @@ MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug); } // namespace marker_utils::avoidance_marker -std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sp_arr); +std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr); std::string toStrInfo(const behavior_path_planner::AvoidLineArray & ap_arr); -std::string toStrInfo(const behavior_path_planner::ShiftLine & sp); +std::string toStrInfo(const behavior_path_planner::ShiftLine & sl); std::string toStrInfo(const behavior_path_planner::AvoidLine & ap); diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index df9640fa75626..f5b797978b744 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -60,7 +60,7 @@ class AvoidanceHelper geometry_msgs::msg::Pose getEgoPose() const { return data_->self_odometry->pose.pose; } - size_t getConstraintsMapIndex(const double velocity, const std::vector & values) const + static size_t getConstraintsMapIndex(const double velocity, const std::vector & values) { const auto itr = std::find_if( values.begin(), values.end(), [&](const auto value) { return velocity < value; }); diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp index 72cfbe6f0a1ed..e23a8a96ee7da 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp @@ -19,11 +19,10 @@ #include "behavior_path_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include +#include +#include #include -#include -#include #include namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 473b1d18c59ef..91c115706e030 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -20,9 +20,10 @@ #include "behavior_path_avoidance_module/shift_line_generator.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include +#include +#include +#include #include #include @@ -30,11 +31,9 @@ #include #include -#include #include #include #include -#include #include namespace behavior_path_planner @@ -374,8 +373,8 @@ class AvoidanceModule : public SceneModuleInterface */ void removeRegisteredShiftLines() { - constexpr double THRESHOLD = 0.1; - if (std::abs(path_shifter_.getBaseOffset()) > THRESHOLD) { + constexpr double threshold = 0.1; + if (std::abs(path_shifter_.getBaseOffset()) > threshold) { RCLCPP_INFO(getLogger(), "base offset is not zero. can't reset registered shift lines."); return; } @@ -396,7 +395,7 @@ class AvoidanceModule : public SceneModuleInterface * @brief remove behind shift lines. * @param path shifter. */ - void postProcess() + void postProcess() override { const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); path_shifter_.removeBehindShiftLineAndSetBaseOffset(idx); @@ -411,8 +410,6 @@ class AvoidanceModule : public SceneModuleInterface using RegisteredShiftLineArray = std::vector; - bool is_avoidance_maneuver_starts; - bool arrived_path_end_{false}; bool safe_{true}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp index edfe9ab9663ce..efdda622a664c 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp @@ -17,11 +17,8 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/helper.hpp" -#include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include - #include namespace behavior_path_planner::utils::avoidance @@ -29,8 +26,6 @@ namespace behavior_path_planner::utils::avoidance using behavior_path_planner::PathShifter; using behavior_path_planner::helper::avoidance::AvoidanceHelper; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; class ShiftLineGenerator { diff --git a/planning/behavior_path_avoidance_module/package.xml b/planning/behavior_path_avoidance_module/package.xml index 642f83497acfb..f72adf0591493 100644 --- a/planning/behavior_path_avoidance_module/package.xml +++ b/planning/behavior_path_avoidance_module/package.xml @@ -27,7 +27,6 @@ behavior_path_planner_common geometry_msgs lanelet2_extension - libboost-dev magic_enum motion_utils objects_of_interest_marker_interface diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 9d0492829c53b..ee36f8c33149d 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -14,6 +14,7 @@ #include "behavior_path_avoidance_module/debug.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include @@ -22,9 +23,6 @@ #include -#include -#include - #include #include diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 952747cd64791..a05bcd90bdfb9 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -28,14 +29,11 @@ #include #include #include -#include -#include -#include -#include +#include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" +#include #include -#include #include #include @@ -1234,7 +1232,6 @@ void AvoidanceModule::initVariables() debug_marker_.markers.clear(); resetPathCandidate(); resetPathReference(); - is_avoidance_maneuver_starts = false; arrived_path_end_ = false; } @@ -1271,8 +1268,8 @@ void AvoidanceModule::updateRTCData() CandidateOutput output; - const auto sl_front = candidates.front(); - const auto sl_back = candidates.back(); + const auto & sl_front = candidates.front(); + const auto & sl_back = candidates.back(); output.path_candidate = data.candidate_path.path; output.lateral_shift = helper_->getRelativeShiftToPath(shift_line); diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 4332008b95e70..01bfc375e9515 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -19,7 +19,7 @@ #include -#include +#include namespace behavior_path_planner::utils::avoidance { diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index f28ad324267cc..6abfc2e107348 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -21,29 +21,23 @@ #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include -#include -#include #include -#include #include -#include +#include +#include -#include +#include #include #include #include -#include #include -#include #include #include #include #include -#include #include #include @@ -53,6 +47,7 @@ namespace behavior_path_planner::utils::avoidance using autoware_perception_msgs::msg::TrafficSignalElement; using behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight; using behavior_path_planner::utils::traffic_light::getDistanceToNextTrafficLight; +using geometry_msgs::msg::TransformStamped; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -65,7 +60,6 @@ using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::getPose; using tier4_autoware_utils::pose2transform; -using tier4_autoware_utils::toHexString; using tier4_planning_msgs::msg::AvoidanceDebugFactor; using tier4_planning_msgs::msg::AvoidanceDebugMsg; diff --git a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 58bf36d978cea..7b9a608aecec3 100644 --- a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -17,9 +17,6 @@ #include "planning_interface_test_manager/planning_interface_test_manager.hpp" #include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" -#include - -#include #include using behavior_path_planner::BehaviorPathPlannerNode;