From 1689a4943432be3d1bc699b493ef7454b7644590 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 30 Sep 2024 18:06:06 +0900 Subject: [PATCH] refactor: split file Signed-off-by: satoshi-ota --- .../package.xml | 1 + .../src/data_structs.hpp | 58 ---- .../src/evaluation.cpp | 277 +----------------- .../src/evaluation.hpp | 110 +------ .../src/node.cpp | 28 +- .../src/node.hpp | 3 +- .../src/utils.cpp | 84 +----- .../src/utils.hpp | 9 +- 8 files changed, 36 insertions(+), 534 deletions(-) diff --git a/planning/autoware_planning_data_analyzer/package.xml b/planning/autoware_planning_data_analyzer/package.xml index d2265b04..296a8b81 100644 --- a/planning/autoware_planning_data_analyzer/package.xml +++ b/planning/autoware_planning_data_analyzer/package.xml @@ -22,6 +22,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_trajectory_evaluator autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs diff --git a/planning/autoware_planning_data_analyzer/src/data_structs.hpp b/planning/autoware_planning_data_analyzer/src/data_structs.hpp index 336da335..9e13c370 100644 --- a/planning/autoware_planning_data_analyzer/src/data_structs.hpp +++ b/planning/autoware_planning_data_analyzer/src/data_structs.hpp @@ -22,27 +22,6 @@ namespace autoware::behavior_analyzer { - -enum class METRIC { - LATERAL_ACCEL = 0, - LONGITUDINAL_JERK = 1, - TRAVEL_DISTANCE = 2, - MINIMUM_TTC = 3, - LATERAL_DEVIATION = 4, - TRAJECTORY_DEVIATION = 5, - SIZE -}; - -enum class SCORE { - LATERAL_COMFORTABILITY = 0, - LONGITUDINAL_COMFORTABILITY = 1, - EFFICIENCY = 2, - SAFETY = 3, - ACHIEVABILITY = 4, - CONSISTENCY = 5, - SIZE -}; - struct TOPIC { static std::string TF; @@ -87,18 +66,6 @@ struct EvaluatorParameters TargetStateParameters target_state{}; }; -struct SelectorParameters -{ - explicit SelectorParameters(const size_t sample_num) - : time_decay_weight(static_cast(METRIC::SIZE), std::vector(sample_num, 0.0)), - score_weight(static_cast(SCORE::SIZE), 0.0) - { - } - - std::vector> time_decay_weight; - std::vector score_weight; -}; - struct Result { Result( @@ -111,31 +78,6 @@ struct Result std::vector weight; double loss{0.0}; }; - -struct CoreData -{ - CoreData( - const std::shared_ptr & points, - const std::shared_ptr & previous_points, - const std::shared_ptr & objects, const std::string & tag) - : points{points}, previous_points{previous_points}, objects{objects}, tag{tag} - { - } - - std::shared_ptr points; - - std::shared_ptr previous_points; - - std::shared_ptr objects; - - std::string tag; -}; - -struct DataSet -{ - std::shared_ptr points; -}; - } // namespace autoware::behavior_analyzer #endif // DATA_STRUCTS_HPP_ diff --git a/planning/autoware_planning_data_analyzer/src/evaluation.cpp b/planning/autoware_planning_data_analyzer/src/evaluation.cpp index 06d65019..5e03dcd4 100644 --- a/planning/autoware_planning_data_analyzer/src/evaluation.cpp +++ b/planning/autoware_planning_data_analyzer/src/evaluation.cpp @@ -30,275 +30,13 @@ namespace autoware::behavior_analyzer { - -DataInterface::DataInterface( - const std::shared_ptr & core_data, const std::shared_ptr & route_handler, - const std::shared_ptr & vehicle_info) -: core_data_{core_data}, - route_handler_{route_handler}, - vehicle_info_{vehicle_info}, - metrics_(static_cast(METRIC::SIZE), std::vector(core_data->points->size(), 0.0)), - scores_(static_cast(SCORE::SIZE), 0.0) -{ - evaluate(); -} - -void DataInterface::evaluate() -{ - for (size_t i = 0; i < core_data_->points->size(); i++) { - metrics_.at(static_cast(METRIC::LATERAL_ACCEL)).at(i) = lateral_accel(i); - metrics_.at(static_cast(METRIC::LONGITUDINAL_JERK)).at(i) = longitudinal_jerk(i); - metrics_.at(static_cast(METRIC::MINIMUM_TTC)).at(i) = minimum_ttc(i); - metrics_.at(static_cast(METRIC::TRAVEL_DISTANCE)).at(i) = travel_distance(i); - metrics_.at(static_cast(METRIC::LATERAL_DEVIATION)).at(i) = lateral_deviation(i); - metrics_.at(static_cast(METRIC::TRAJECTORY_DEVIATION)).at(i) = trajectory_deviation(i); - } -} - -void DataInterface::compress(const std::vector> & weight) -{ - scores_.at(static_cast(SCORE::LATERAL_COMFORTABILITY)) = - compress(weight, METRIC::LATERAL_ACCEL); - scores_.at(static_cast(SCORE::LONGITUDINAL_COMFORTABILITY)) = - compress(weight, METRIC::LONGITUDINAL_JERK); - scores_.at(static_cast(SCORE::EFFICIENCY)) = compress(weight, METRIC::TRAVEL_DISTANCE); - scores_.at(static_cast(SCORE::SAFETY)) = compress(weight, METRIC::MINIMUM_TTC); - scores_.at(static_cast(SCORE::ACHIEVABILITY)) = - compress(weight, METRIC::LATERAL_DEVIATION); - scores_.at(static_cast(SCORE::CONSISTENCY)) = - compress(weight, METRIC::TRAJECTORY_DEVIATION); -} - -double DataInterface::lateral_accel(const size_t idx) const -{ - const auto radius = - vehicle_info_->wheel_base_m / std::tan(core_data_->points->at(idx).front_wheel_angle_rad); - const auto speed = core_data_->points->at(idx).longitudinal_velocity_mps; - return std::abs(speed * speed / radius); -} - -double DataInterface::longitudinal_jerk(const size_t idx) const -{ - if (idx + 2 > core_data_->points->size()) return 0.0; - - const auto jerk = (core_data_->points->at(idx + 1).acceleration_mps2 - - core_data_->points->at(idx).acceleration_mps2) / - 0.5; - return std::abs(jerk); -} - -double DataInterface::minimum_ttc(const size_t idx) const -{ - // TODO: linear interpolation - return utils::time_to_collision(core_data_->points, core_data_->objects, idx); -} - -double DataInterface::travel_distance(const size_t idx) const -{ - return autoware::motion_utils::calcSignedArcLength(*core_data_->points, 0L, idx); -} - -double DataInterface::lateral_deviation(const size_t idx) const -{ - lanelet::ConstLanelet nearest{}; - if (!route_handler_->getClosestPreferredLaneletWithinRoute( - autoware::universe_utils::getPose(core_data_->points->at(idx)), &nearest)) { - return std::numeric_limits::max(); - } - const auto arc_coordinates = lanelet::utils::getArcCoordinates( - {nearest}, autoware::universe_utils::getPose(core_data_->points->at(idx))); - return std::abs(arc_coordinates.distance); -} - -double DataInterface::trajectory_deviation(const size_t idx) const -{ - if (core_data_->previous_points == nullptr) return 0.0; - - if (idx + 1 > core_data_->previous_points->size()) return 0.0; - - const auto & p1 = autoware::universe_utils::getPose(core_data_->points->at(idx)); - const auto & p2 = autoware::universe_utils::getPose(core_data_->previous_points->at(idx)); - return autoware::universe_utils::calcSquaredDistance2d(p1, p2); -} - -bool DataInterface::feasible() const -{ - const auto condition = [](const auto & p) { return p.longitudinal_velocity_mps >= 0.0; }; - return std::all_of(core_data_->points->begin(), core_data_->points->end(), condition); -} - -void DataInterface::normalize( - const double min, const double max, const size_t score_type, const bool flip) -{ - scores_.at(score_type) = flip ? (max - scores_.at(score_type)) / (max - min) - : (scores_.at(score_type) - min) / (max - min); -} - -auto DataInterface::compress( - const std::vector> & weight, const METRIC & metric_type) const -> double -{ - const auto w = weight.at(static_cast(metric_type)); - const auto metric = metrics_.at(static_cast(metric_type)); - return std::inner_product(w.begin(), w.end(), metric.begin(), 0.0); -} - -auto DataInterface::score(const SCORE & score_type) const -> double -{ - return scores_.at(static_cast(score_type)); -} - -void DataInterface::weighting(const std::vector & weight) -{ - total_ = std::inner_product(weight.begin(), weight.end(), scores_.begin(), 0.0); -} - -void Evaluator::normalize() -{ - const auto range = [this](const size_t idx) { - const auto min_itr = std::min_element( - results_.begin(), results_.end(), - [&idx](const auto & a, const auto & b) { return a->scores().at(idx) < b->scores().at(idx); }); - const auto max_itr = std::max_element( - results_.begin(), results_.end(), - [&idx](const auto & a, const auto & b) { return a->scores().at(idx) < b->scores().at(idx); }); - - return std::make_pair((*min_itr)->scores().at(idx), (*max_itr)->scores().at(idx)); - }; - - const auto [s0_min, s0_max] = range(static_cast(SCORE::LATERAL_COMFORTABILITY)); - const auto [s1_min, s1_max] = range(static_cast(SCORE::LONGITUDINAL_COMFORTABILITY)); - const auto [s2_min, s2_max] = range(static_cast(SCORE::EFFICIENCY)); - const auto [s3_min, s3_max] = range(static_cast(SCORE::SAFETY)); - const auto [s4_min, s4_max] = range(static_cast(SCORE::ACHIEVABILITY)); - const auto [s5_min, s5_max] = range(static_cast(SCORE::CONSISTENCY)); - - for (auto & data : results_) { - data->normalize(s0_min, s0_max, static_cast(SCORE::LATERAL_COMFORTABILITY), true); - data->normalize(s1_min, s1_max, static_cast(SCORE::LONGITUDINAL_COMFORTABILITY), true); - data->normalize(s2_min, s2_max, static_cast(SCORE::EFFICIENCY)); - data->normalize(s3_min, s3_max, static_cast(SCORE::SAFETY)); - data->normalize(s4_min, s4_max, static_cast(SCORE::ACHIEVABILITY), true); - data->normalize(s5_min, s5_max, static_cast(SCORE::CONSISTENCY), true); - } -} - -void Evaluator::pruning() -{ - const auto itr = - std::remove_if(results_.begin(), results_.end(), [](const auto & d) { return !d->feasible(); }); - - results_.erase(itr, results_.end()); -} - -void Evaluator::compress(const std::vector> & weight) -{ - std::for_each( - results_.begin(), results_.end(), [&weight](auto & data) { data->compress(weight); }); -} - -void Evaluator::weighting(const std::vector & weight) -{ - std::for_each( - results_.begin(), results_.end(), [&weight](auto & data) { data->weighting(weight); }); - - std::sort(results_.begin(), results_.end(), [](const auto & a, const auto & b) { - return a->total() > b->total(); - }); -} - -auto Evaluator::get(const std::string & tag) const -> std::shared_ptr -{ - const auto itr = std::find_if( - results_.begin(), results_.end(), [&tag](const auto & data) { return data->tag() == tag; }); - - return itr != results_.end() ? *itr : nullptr; -} - -void Evaluator::add(const std::shared_ptr & core_data) -{ - const auto ptr = std::make_shared(core_data, route_handler_, vehicle_info_); - results_.push_back(ptr); -} - -auto Evaluator::best(const std::shared_ptr & parameters) - -> std::shared_ptr -{ - pruning(); - - compress(parameters->time_decay_weight); - - normalize(); - - weighting(parameters->score_weight); - - return best(); -} - -auto Evaluator::best() const -> std::shared_ptr -{ - if (results_.empty()) return nullptr; - - if (!results_.front()->feasible()) return nullptr; - - return results_.front(); -} - -auto Evaluator::statistics(const SCORE & score_type) const -> std::pair -{ - double ave = 0.0; - double dev = 0.0; - - const auto update = [](const double ave, const double dev, const double value, const size_t i) { - const auto new_ave = (i * ave + value) / (i + 1); - const auto new_dev = - (i * (ave * ave + dev * dev) + value * value) / (i + 1) - new_ave * new_ave; - return std::make_pair(new_ave, new_dev); - }; - - for (size_t i = 0; i < results_.size(); i++) { - std::tie(ave, dev) = update(ave, dev, results_.at(i)->score(score_type), i); - } - - return std::make_pair(ave, dev); -} - -void Evaluator::show() const -{ - const auto best_data = best(); - - if (best_data == nullptr) { - return; - } - - const auto s0 = statistics(SCORE::LATERAL_COMFORTABILITY); - const auto s1 = statistics(SCORE::LONGITUDINAL_COMFORTABILITY); - const auto s2 = statistics(SCORE::EFFICIENCY); - const auto s3 = statistics(SCORE::SAFETY); - const auto s4 = statistics(SCORE::ACHIEVABILITY); - const auto s5 = statistics(SCORE::CONSISTENCY); - - std::stringstream ss; - ss << std::fixed << std::setprecision(2) << "\n"; - // clang-format off - ss << " tag :" << best_data->tag() << "\n"; - ss << " lat comfortability:" << best_data->score(SCORE::LATERAL_COMFORTABILITY) << " mean:" << s0.first << " std:" << std::sqrt(s0.second) << "\n"; // NOLINT - ss << " lon comfortability:" << best_data->score(SCORE::LONGITUDINAL_COMFORTABILITY) << " mean:" << s1.first << " std:" << std::sqrt(s1.second) << "\n"; // NOLINT - ss << " efficiency :" << best_data->score(SCORE::EFFICIENCY) << " mean:" << s2.first << " std:" << std::sqrt(s2.second) << "\n"; // NOLINT - ss << " safety :" << best_data->score(SCORE::SAFETY) << " mean:" << s3.first << " std:" << std::sqrt(s3.second) << "\n"; // NOLINT - ss << " achievability :" << best_data->score(SCORE::ACHIEVABILITY) << " mean:" << s4.first << " std:" << std::sqrt(s4.second) << "\n"; // NOLINT - ss << " consistency :" << best_data->score(SCORE::CONSISTENCY) << " mean:" << s5.first << " std:" << std::sqrt(s5.second) << "\n"; // NOLINT - ss << " total :" << best_data->total(); - // clang-format on - RCLCPP_INFO_STREAM(rclcpp::get_logger(__func__), ss.str()); -} - BagEvaluator::BagEvaluator( const std::shared_ptr & bag_data, const std::shared_ptr & previous_points, const std::shared_ptr & route_handler, const std::shared_ptr & vehicle_info, const std::shared_ptr & evaluator_parameters) -: Evaluator{route_handler, vehicle_info} +: autoware::trajectory_selector::trajectory_evaluator::Evaluator{route_handler, vehicle_info} { const auto odometry_buffer_ptr = std::dynamic_pointer_cast>(bag_data->buffers.at(TOPIC::ODOMETRY)); @@ -377,7 +115,7 @@ BagEvaluator::BagEvaluator( } { - const auto core_data = std::make_shared( + const auto core_data = std::make_shared( std::make_shared(points), previous_points, current_objects, "ground_truth"); @@ -411,7 +149,7 @@ BagEvaluator::BagEvaluator( for (const auto & points : utils::sampling( *trajectory_ptr, odometry_ptr->pose.pose, odometry_ptr->twist.twist.linear.x, accel_ptr->accel.accel.linear.x, vehicle_info, evaluator_parameters)) { - const auto core_data = std::make_shared( + const auto core_data = std::make_shared( std::make_shared(points), previous_points, current_objects, "candidates"); add(core_data); @@ -419,15 +157,10 @@ BagEvaluator::BagEvaluator( } } -auto BagEvaluator::loss(const std::shared_ptr & parameters) +auto BagEvaluator::loss( + const std::shared_ptr & parameters) -> std::pair> { - // pruning(); - - // normalize(); - - // weighting(weight); - const auto best_data = best(parameters); if (best_data == nullptr) { diff --git a/planning/autoware_planning_data_analyzer/src/evaluation.hpp b/planning/autoware_planning_data_analyzer/src/evaluation.hpp index 141df341..d8c5cce4 100644 --- a/planning/autoware_planning_data_analyzer/src/evaluation.hpp +++ b/planning/autoware_planning_data_analyzer/src/evaluation.hpp @@ -18,6 +18,8 @@ #include "bag_handler.hpp" #include "type_alias.hpp" +#include + #include #include #include @@ -26,108 +28,7 @@ namespace autoware::behavior_analyzer { - -class DataInterface -{ -public: - DataInterface( - const std::shared_ptr & core_data, - const std::shared_ptr & route_handler, - const std::shared_ptr & vehicle_info); - - void compress(const std::vector> & weight); - - void normalize( - const double min, const double max, const size_t score_type, const bool flip = false); - - void weighting(const std::vector & weight); - - auto total() const -> double { return total_; }; - - bool feasible() const; - - auto score(const SCORE & score_type) const -> double; - - auto scores() const -> std::vector { return scores_; } - - auto points() const -> std::shared_ptr { return core_data_->points; } - - auto tag() const -> std::string { return core_data_->tag; } - -private: - void evaluate(); - - auto lateral_accel(const size_t idx) const -> double; - - auto longitudinal_jerk(const size_t idx) const -> double; - - auto minimum_ttc(const size_t idx) const -> double; - - auto travel_distance(const size_t idx) const -> double; - - auto lateral_deviation(const size_t idx) const -> double; - - auto trajectory_deviation(const size_t idx) const -> double; - - auto compress(const std::vector> & weight, const METRIC & metric_type) const - -> double; - - std::shared_ptr core_data_; - - std::shared_ptr route_handler_; - - std::shared_ptr vehicle_info_; - - std::vector> metrics_; - - std::vector scores_; - - double total_; -}; - -class Evaluator -{ -public: - explicit Evaluator( - const std::shared_ptr & route_handler, - const std::shared_ptr & vehicle_info) - : route_handler_{route_handler}, vehicle_info_{vehicle_info} - { - } - - void add(const std::shared_ptr & core_data); - - auto best(const std::shared_ptr & parameters) - -> std::shared_ptr; - - auto results() const -> std::vector> { return results_; } - - auto get(const std::string & tag) const -> std::shared_ptr; - - auto statistics(const SCORE & score_type) const -> std::pair; - - void show() const; - -protected: - void pruning(); - - void compress(const std::vector> & weight); - - void normalize(); - - void weighting(const std::vector & weight); - - auto best() const -> std::shared_ptr; - -private: - std::vector> results_; - - std::shared_ptr route_handler_; - - std::shared_ptr vehicle_info_; -}; - -class BagEvaluator : public Evaluator +class BagEvaluator : public trajectory_selector::trajectory_evaluator::Evaluator { public: BagEvaluator( @@ -137,10 +38,9 @@ class BagEvaluator : public Evaluator const std::shared_ptr & vehicle_info, const std::shared_ptr & evaluator_parameters); - auto loss(const std::shared_ptr & parameters) - -> std::pair>; + auto loss(const std::shared_ptr & + parameters) -> std::pair>; }; - } // namespace autoware::behavior_analyzer #endif // EVALUATION_HPP_ diff --git a/planning/autoware_planning_data_analyzer/src/node.cpp b/planning/autoware_planning_data_analyzer/src/node.cpp index 2b937388..b1b2d398 100644 --- a/planning/autoware_planning_data_analyzer/src/node.cpp +++ b/planning/autoware_planning_data_analyzer/src/node.cpp @@ -32,7 +32,7 @@ BehaviorAnalyzerNode::BehaviorAnalyzerNode(const rclcpp::NodeOptions & node_opti : Node("path_selector_node", node_options), route_handler_{std::make_shared()}, previous_{nullptr}, - buffer_{static_cast(SCORE::SIZE)} + buffer_{static_cast(trajectory_selector::trajectory_evaluator::SCORE::SIZE)} { using namespace std::literals::chrono_literals; timer_ = @@ -104,7 +104,9 @@ BehaviorAnalyzerNode::BehaviorAnalyzerNode(const rclcpp::NodeOptions & node_opti evaluator_parameters_->target_state.lon_accelerations = declare_parameter>("target_state.longitudinal_accelerations"); - selector_parameters_ = std::make_shared(evaluator_parameters_->resample_num); + selector_parameters_ = + std::make_shared( + evaluator_parameters_->resample_num); selector_parameters_->time_decay_weight.at(0) = declare_parameter>("time_decay_weight.s0"); selector_parameters_->time_decay_weight.at(1) = @@ -333,7 +335,8 @@ void BehaviorAnalyzerNode::weight( // TODO: set time_decay_weight const auto update = [&weight_grid, &grid_mutex](const auto & bag_evaluator, const auto idx) { - const auto selector_parameters = std::make_shared(20); + const auto selector_parameters = + std::make_shared(20); { std::lock_guard lock(grid_mutex); if (idx + 1 > weight_grid.size()) return; @@ -533,12 +536,19 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr & bag_e const auto results = bag_evaluator->results(); for (size_t i = 0; i < results.size(); ++i) { - msg.markers.push_back(utils::to_marker(results.at(i), SCORE::LATERAL_COMFORTABILITY, i)); - msg.markers.push_back(utils::to_marker(results.at(i), SCORE::LONGITUDINAL_COMFORTABILITY, i)); - msg.markers.push_back(utils::to_marker(results.at(i), SCORE::EFFICIENCY, i)); - msg.markers.push_back(utils::to_marker(results.at(i), SCORE::SAFETY, i)); - msg.markers.push_back(utils::to_marker(results.at(i), SCORE::ACHIEVABILITY, i)); - msg.markers.push_back(utils::to_marker(results.at(i), SCORE::CONSISTENCY, i)); + msg.markers.push_back(utils::to_marker( + results.at(i), trajectory_selector::trajectory_evaluator::SCORE::LATERAL_COMFORTABILITY, i)); + msg.markers.push_back(utils::to_marker( + results.at(i), trajectory_selector::trajectory_evaluator::SCORE::LONGITUDINAL_COMFORTABILITY, + i)); + msg.markers.push_back(utils::to_marker( + results.at(i), trajectory_selector::trajectory_evaluator::SCORE::EFFICIENCY, i)); + msg.markers.push_back( + utils::to_marker(results.at(i), trajectory_selector::trajectory_evaluator::SCORE::SAFETY, i)); + msg.markers.push_back(utils::to_marker( + results.at(i), trajectory_selector::trajectory_evaluator::SCORE::ACHIEVABILITY, i)); + msg.markers.push_back(utils::to_marker( + results.at(i), trajectory_selector::trajectory_evaluator::SCORE::CONSISTENCY, i)); } { diff --git a/planning/autoware_planning_data_analyzer/src/node.hpp b/planning/autoware_planning_data_analyzer/src/node.hpp index 8771c1b7..b2e98b2b 100644 --- a/planning/autoware_planning_data_analyzer/src/node.hpp +++ b/planning/autoware_planning_data_analyzer/src/node.hpp @@ -93,7 +93,8 @@ class BehaviorAnalyzerNode : public rclcpp::Node std::shared_ptr evaluator_parameters_; - std::shared_ptr selector_parameters_; + std::shared_ptr + selector_parameters_; mutable std::shared_ptr previous_; diff --git a/planning/autoware_planning_data_analyzer/src/utils.cpp b/planning/autoware_planning_data_analyzer/src/utils.cpp index 69367e4a..805c82ef 100644 --- a/planning/autoware_planning_data_analyzer/src/utils.cpp +++ b/planning/autoware_planning_data_analyzer/src/utils.cpp @@ -38,87 +38,6 @@ using autoware::universe_utils::createDefaultMarker; using autoware::universe_utils::createMarkerColor; using autoware::universe_utils::createMarkerScale; -Point vector2point(const geometry_msgs::msg::Vector3 & v) -{ - return autoware::universe_utils::createPoint(v.x, v.y, v.z); -} - -tf2::Vector3 from_msg(const Point & p) -{ - return tf2::Vector3(p.x, p.y, p.z); -} - -tf2::Vector3 get_velocity_in_world_coordinate(const PredictedObjectKinematics & kinematics) -{ - const auto pose = kinematics.initial_pose_with_covariance.pose; - const auto v_local = kinematics.initial_twist_with_covariance.twist.linear; - const auto v_world = autoware::universe_utils::transformPoint(vector2point(v_local), pose); - - return from_msg(v_world) - from_msg(pose.position); -} - -tf2::Vector3 get_velocity_in_world_coordinate(const Odometry & odometry) -{ - const auto pose = odometry.pose.pose; - const auto v_local = odometry.twist.twist.linear; - const auto v_world = autoware::universe_utils::transformPoint(vector2point(v_local), pose); - - return from_msg(v_world) - from_msg(pose.position); -} - -tf2::Vector3 get_velocity_in_world_coordinate(const TrajectoryPoint & point) -{ - const auto pose = point.pose; - const auto v_local = - geometry_msgs::build().x(point.longitudinal_velocity_mps).y(0.0).z(0.0); - const auto v_world = autoware::universe_utils::transformPoint(vector2point(v_local), pose); - - return from_msg(v_world) - from_msg(pose.position); -} - -auto time_to_collision( - const std::shared_ptr & points, - const std::shared_ptr & objects, const size_t idx) -> double -{ - if (objects->objects.empty()) { - return std::numeric_limits::max(); - } - - const auto p_ego = points->at(idx).pose; - const auto v_ego = utils::get_velocity_in_world_coordinate(points->at(idx)); - - std::vector time_to_collisions(objects->objects.size()); - - for (const auto & object : objects->objects) { - const auto max_confidence_path = std::max_element( - object.kinematics.predicted_paths.begin(), object.kinematics.predicted_paths.end(), - [](const auto & a, const auto & b) { return a.confidence < b.confidence; }); - - if (max_confidence_path == object.kinematics.predicted_paths.end()) continue; - - if (max_confidence_path->path.size() < idx + 1) continue; - - const auto & p_object = max_confidence_path->path.at(idx); - const auto v_ego2object = - autoware::universe_utils::point2tfVector(p_ego.position, p_object.position); - - const auto v_object = get_velocity_in_world_coordinate(object.kinematics); - const auto v_relative = tf2::tf2Dot(v_ego2object.normalized(), v_ego) - - tf2::tf2Dot(v_ego2object.normalized(), v_object); - - time_to_collisions.push_back(v_ego2object.length() / v_relative); - } - - const auto itr = std::remove_if( - time_to_collisions.begin(), time_to_collisions.end(), - [](const auto & value) { return value < 1e-3; }); - time_to_collisions.erase(itr, time_to_collisions.end()); - - std::sort(time_to_collisions.begin(), time_to_collisions.end()); - - return time_to_collisions.front(); -} - auto convertToTrajectoryPoints( const autoware::sampler_common::Trajectory & trajectory, const std::shared_ptr & vehicle_info, const double z) -> std::vector @@ -297,7 +216,8 @@ auto sampling( } auto to_marker( - const std::shared_ptr & data, const SCORE & score_type, const size_t id) -> Marker + const std::shared_ptr & data, + const trajectory_selector::trajectory_evaluator::SCORE & score_type, const size_t id) -> Marker { if (data == nullptr) return Marker{}; diff --git a/planning/autoware_planning_data_analyzer/src/utils.hpp b/planning/autoware_planning_data_analyzer/src/utils.hpp index 88f39a7e..a5c6b7dd 100644 --- a/planning/autoware_planning_data_analyzer/src/utils.hpp +++ b/planning/autoware_planning_data_analyzer/src/utils.hpp @@ -26,11 +26,6 @@ namespace autoware::behavior_analyzer::utils { - -auto time_to_collision( - const std::shared_ptr & points, - const std::shared_ptr & objects, const size_t idx) -> double; - auto convertToTrajectoryPoints( const autoware::sampler_common::Trajectory & trajectory, const std::shared_ptr & vehicle_info, const double z) @@ -58,8 +53,8 @@ auto sampling( -> std::vector>; auto to_marker( - const std::shared_ptr & data, const SCORE & score_type, const size_t id) -> Marker; - + const std::shared_ptr & data, + const trajectory_selector::trajectory_evaluator::SCORE & score_type, const size_t id) -> Marker; } // namespace autoware::behavior_analyzer::utils #endif // UTILS_HPP_