Skip to content

Commit

Permalink
chore(autonomous_emergency_braking): apply clangd suggestions to aeb (a…
Browse files Browse the repository at this point in the history
…utowarefoundation#7703)

* apply clangd suggestions

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* add maintainer

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

---------

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
  • Loading branch information
danielsanchezaran authored and tby-udel committed Jul 14, 2024
1 parent c428daf commit 6c37549
Show file tree
Hide file tree
Showing 5 changed files with 15 additions and 16 deletions.
2 changes: 1 addition & 1 deletion .github/CODEOWNERS
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,6 @@ using nav_msgs::msg::Odometry;
using sensor_msgs::msg::Imu;
using sensor_msgs::msg::PointCloud2;
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
using autoware::universe_utils::Point2d;
using autoware::universe_utils::Polygon2d;
using autoware::vehicle_info_utils::VehicleInfo;
using diagnostic_updater::DiagnosticStatusWrapper;
Expand Down Expand Up @@ -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();
}
Expand Down Expand Up @@ -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<double> getMedianObstacleVelocity() const
[[nodiscard]] std::optional<double> getMedianObstacleVelocity() const
{
if (obstacle_velocity_history_.empty()) return std::nullopt;
std::vector<double> 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());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
Expand Down
1 change: 1 addition & 0 deletions control/autoware_autonomous_emergency_braking/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
11 changes: 5 additions & 6 deletions control/autoware_autonomous_emergency_braking/src/utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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());
Expand Down

0 comments on commit 6c37549

Please sign in to comment.