From b840acff9f9e006e969e879d439d7d003f45eb78 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Thu, 1 Aug 2024 10:32:21 +0900 Subject: [PATCH] fix(autoware_behavior_velocity_intersection_module): fix funcArgNamesDifferent (#8023) * fix:funcArgNamesDifferent Signed-off-by: kobayu858 * fix:funcArgNamesDifferent Signed-off-by: kobayu858 * refactor:clang format Signed-off-by: kobayu858 * fix:funcArgNamesDifferent Signed-off-by: kobayu858 --------- Signed-off-by: kobayu858 --- .../src/object_manager.cpp | 36 +++++++++---------- .../src/object_manager.hpp | 18 +++++----- .../src/scene_intersection.hpp | 6 ++-- 3 files changed, 30 insertions(+), 30 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp index b749207aa239..d93fbf271aa1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp @@ -74,12 +74,12 @@ ObjectInfo::ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid) : uuid_st void ObjectInfo::initialize( const autoware_perception_msgs::msg::PredictedObject & object, - std::optional attention_lanelet_opt_, - std::optional stopline_opt_) + std::optional attention_lanelet_opt, + std::optional stopline_opt) { predicted_object_ = object; - attention_lanelet_opt = attention_lanelet_opt_; - stopline_opt = stopline_opt_; + attention_lanelet_opt_ = attention_lanelet_opt; + stopline_opt_ = stopline_opt; unsafe_interval_ = std::nullopt; calc_dist_to_stopline(); } @@ -96,10 +96,10 @@ void ObjectInfo::update_safety( std::optional ObjectInfo::estimated_past_position( const double past_duration) const { - if (!attention_lanelet_opt) { + if (!attention_lanelet_opt_) { return std::nullopt; } - const auto attention_lanelet = attention_lanelet_opt.value(); + const auto attention_lanelet = attention_lanelet_opt_.value(); const auto current_arc_coords = lanelet::utils::getArcCoordinates( {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); const auto distance = current_arc_coords.distance; @@ -116,29 +116,29 @@ std::optional ObjectInfo::estimated_past_position( void ObjectInfo::calc_dist_to_stopline() { - if (!stopline_opt || !attention_lanelet_opt) { + if (!stopline_opt_ || !attention_lanelet_opt_) { return; } - const auto attention_lanelet = attention_lanelet_opt.value(); + const auto attention_lanelet = attention_lanelet_opt_.value(); const auto object_arc_coords = lanelet::utils::getArcCoordinates( {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); - const auto stopline = stopline_opt.value(); + const auto stopline = stopline_opt_.value(); geometry_msgs::msg::Pose stopline_center; stopline_center.position.x = (stopline.front().x() + stopline.back().x()) / 2.0; stopline_center.position.y = (stopline.front().y() + stopline.back().y()) / 2.0; stopline_center.position.z = (stopline.front().z() + stopline.back().z()) / 2.0; const auto stopline_arc_coords = lanelet::utils::getArcCoordinates({attention_lanelet}, stopline_center); - dist_to_stopline_opt = (stopline_arc_coords.length - object_arc_coords.length); + dist_to_stopline_opt_ = (stopline_arc_coords.length - object_arc_coords.length); } bool ObjectInfo::can_stop_before_stopline(const double brake_deceleration) const { - if (!dist_to_stopline_opt) { + if (!dist_to_stopline_opt_) { return false; } const double velocity = predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; - const double dist_to_stopline = dist_to_stopline_opt.value(); + const double dist_to_stopline = dist_to_stopline_opt_.value(); const double braking_distance = (velocity * velocity) / (2.0 * brake_deceleration); return dist_to_stopline > braking_distance; } @@ -147,17 +147,17 @@ bool ObjectInfo::can_stop_before_ego_lane( const double brake_deceleration, const double tolerable_overshoot, lanelet::ConstLanelet ego_lane) const { - if (!dist_to_stopline_opt || !stopline_opt || !attention_lanelet_opt) { + if (!dist_to_stopline_opt_ || !stopline_opt_ || !attention_lanelet_opt_) { return false; } - const double dist_to_stopline = dist_to_stopline_opt.value(); + const double dist_to_stopline = dist_to_stopline_opt_.value(); const double velocity = predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; const double braking_distance = (velocity * velocity) / (2.0 * brake_deceleration); if (dist_to_stopline > braking_distance) { return false; } - const auto attention_lanelet = attention_lanelet_opt.value(); - const auto stopline = stopline_opt.value(); + const auto attention_lanelet = attention_lanelet_opt_.value(); + const auto stopline = stopline_opt_.value(); const auto stopline_p1 = stopline.front(); const auto stopline_p2 = stopline.back(); const autoware::universe_utils::Point2d stopline_mid{ @@ -185,10 +185,10 @@ bool ObjectInfo::can_stop_before_ego_lane( bool ObjectInfo::before_stopline_by(const double margin) const { - if (!dist_to_stopline_opt) { + if (!dist_to_stopline_opt_) { return false; } - const double dist_to_stopline = dist_to_stopline_opt.value(); + const double dist_to_stopline = dist_to_stopline_opt_.value(); return dist_to_stopline < margin; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp index 0ba9947fdb31..d98f750dacf1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp @@ -126,7 +126,7 @@ class ObjectInfo * @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline */ void initialize( - const autoware_perception_msgs::msg::PredictedObject & predicted_object, + const autoware_perception_msgs::msg::PredictedObject & object, std::optional attention_lanelet_opt, std::optional stopline_opt); @@ -134,9 +134,8 @@ class ObjectInfo * @brief update unsafe_knowledge */ void update_safety( - const std::optional & unsafe_interval_opt, - const std::optional & safe_interval_opt, - const bool safe_under_traffic_control); + const std::optional & unsafe_interval, + const std::optional & safe_interval, const bool safe_under_traffic_control); /** * @brief find the estimated position of the object in the past @@ -196,13 +195,13 @@ class ObjectInfo autoware_perception_msgs::msg::PredictedObject predicted_object_; //! null if the object in intersection_area but not in attention_area - std::optional attention_lanelet_opt{std::nullopt}; + std::optional attention_lanelet_opt_{std::nullopt}; //! null if the object in intersection_area but not in attention_area - std::optional stopline_opt{std::nullopt}; + std::optional stopline_opt_{std::nullopt}; //! null if the object in intersection_area but not in attention_area - std::optional dist_to_stopline_opt{std::nullopt}; + std::optional dist_to_stopline_opt_{std::nullopt}; //! store the information if judged as UNSAFE std::optional unsafe_interval_{std::nullopt}; @@ -230,11 +229,12 @@ class ObjectInfoManager public: std::shared_ptr registerObject( const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, - const bool belong_intersection_area, const bool is_parked); + const bool belong_intersection_area, const bool is_parked_vehicle); void registerExistingObject( const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, - const bool belong_intersection_area, const bool is_parked, std::shared_ptr object); + const bool belong_intersection_area, const bool is_parked_vehicle, + std::shared_ptr object); void clearObjects(); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index ea2e1a7ae16d..03417a8410d2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -590,7 +590,7 @@ class IntersectionModule : public SceneModuleInterface * @brief generate discretized detection lane linestring. */ std::vector generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets, + lanelet::ConstLanelets detection_lanelets_all, const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const; /** @} */ @@ -752,7 +752,7 @@ class IntersectionModule : public SceneModuleInterface void cutPredictPathWithinDuration( const builtin_interfaces::msg::Time & object_stamp, const double time_thr, - autoware_perception_msgs::msg::PredictedPath * predicted_path) const; + autoware_perception_msgs::msg::PredictedPath * path) const; /** * @brief check if there are any objects around the stoplines on the attention areas when ego @@ -809,7 +809,7 @@ class IntersectionModule : public SceneModuleInterface TimeDistanceArray calcIntersectionPassingTime( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const IntersectionStopLines & intersection_stoplines, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const; + tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const; /** @} */ mutable DebugData debug_data_;