From 6c375495d826ddf86aa4aeac26d59204bf33530e Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 28 Jun 2024 15:53:19 +0900 Subject: [PATCH] chore(autonomous_emergency_braking): apply clangd suggestions to aeb (#7703) * apply clangd suggestions Signed-off-by: Daniel Sanchez * add maintainer Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .github/CODEOWNERS | 2 +- .../autoware/autonomous_emergency_braking/node.hpp | 14 +++++++------- .../autonomous_emergency_braking/utils.hpp | 3 +-- .../src/node.cpp | 1 + .../src/utils.cpp | 11 +++++------ 5 files changed, 15 insertions(+), 16 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index d47a999ad9254..16056ba7448b3 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -45,7 +45,7 @@ common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomo common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp kyoichi.sugahara@tier4.jp control/autoware_control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/autoware_joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 885779a0e0b3d..f6df46440d841 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -60,7 +60,6 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; using PointCloud = pcl::PointCloud; -using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using diagnostic_updater::DiagnosticStatusWrapper; @@ -120,12 +119,12 @@ class CollisionDataKeeper return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_); } - ObjectData get() const + [[nodiscard]] ObjectData get() const { return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); } - ObjectData getPreviousObjectData() const + [[nodiscard]] ObjectData getPreviousObjectData() const { return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData(); } @@ -156,20 +155,21 @@ class CollisionDataKeeper }), obstacle_velocity_history_.end()); obstacle_velocity_history_.emplace_back( - std::make_pair(current_object_velocity, current_object_velocity_time_stamp)); + current_object_velocity, current_object_velocity_time_stamp); } - std::optional getMedianObstacleVelocity() const + [[nodiscard]] std::optional getMedianObstacleVelocity() const { if (obstacle_velocity_history_.empty()) return std::nullopt; std::vector raw_velocities; + raw_velocities.reserve(obstacle_velocity_history_.size()); for (const auto & vel_time_pair : obstacle_velocity_history_) { raw_velocities.emplace_back(vel_time_pair.first); } const size_t med1 = (raw_velocities.size() % 2 == 0) ? (raw_velocities.size()) / 2 - 1 - : (raw_velocities.size()) / 2.0; - const size_t med2 = (raw_velocities.size()) / 2.0; + : (raw_velocities.size()) / 2; + const size_t med2 = (raw_velocities.size()) / 2; std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med1, raw_velocities.end()); const double vel1 = raw_velocities.at(med1); std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med2, raw_velocities.end()); diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index 64bedd2163912..4a6bc0ce2c11f 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -37,7 +37,6 @@ namespace autoware::motion::control::autonomous_emergency_braking::utils { -using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; @@ -51,7 +50,7 @@ using geometry_msgs::msg::TransformStamped; * @param transform_stamped the tf2 transform */ PredictedObject transformObjectFrame( - const PredictedObject & input, geometry_msgs::msg::TransformStamped transform_stamped); + const PredictedObject & input, geometry_msgs::msg::TransformStamped & transform_stamped); /** * @brief Get the predicted objects polygon as a geometry polygon diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index fbbaf2e4e63ae..fb0fc2d043cc5 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -54,6 +54,7 @@ namespace autoware::motion::control::autonomous_emergency_braking { using autoware::motion::control::autonomous_emergency_braking::utils::convertObjToPolygon; +using autoware::universe_utils::Point2d; using diagnostic_msgs::msg::DiagnosticStatus; namespace bg = boost::geometry; diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp index c284ee3c99dbb..f3b5192855d84 100644 --- a/control/autoware_autonomous_emergency_braking/src/utils.cpp +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -16,7 +16,6 @@ namespace autoware::motion::control::autonomous_emergency_braking::utils { -using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; @@ -26,7 +25,7 @@ using geometry_msgs::msg::TransformStamped; using geometry_msgs::msg::Vector3; PredictedObject transformObjectFrame( - const PredictedObject & input, geometry_msgs::msg::TransformStamped transform_stamped) + const PredictedObject & input, geometry_msgs::msg::TransformStamped & transform_stamped) { PredictedObject output = input; const auto & linear_twist = input.kinematics.initial_twist_with_covariance.twist.linear; @@ -74,12 +73,12 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( const double obj_x = current_pose.position.x; const double obj_y = current_pose.position.y; - constexpr int N = 20; + constexpr int n = 20; const double r = obj_shape.dimensions.x / 2; - object_polygon.outer().reserve(N + 1); - for (int i = 0; i < N; ++i) { + object_polygon.outer().reserve(n + 1); + for (int i = 0; i < n; ++i) { object_polygon.outer().emplace_back( - obj_x + r * std::cos(2.0 * M_PI / N * i), obj_y + r * std::sin(2.0 * M_PI / N * i)); + obj_x + r * std::cos(2.0 * M_PI / n * i), obj_y + r * std::sin(2.0 * M_PI / n * i)); } object_polygon.outer().push_back(object_polygon.outer().front());