From fe433ba930f9ec55ac91050d04ec50d3dbc70ef6 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 27 Sep 2024 17:15:23 +0900 Subject: [PATCH] fixup! refactor: commonize --- .../src/evaluation.cpp | 45 ++++++++++--------- .../src/evaluation.hpp | 10 +++-- .../src/node.cpp | 10 ++--- .../src/utils.cpp | 4 +- 4 files changed, 36 insertions(+), 33 deletions(-) diff --git a/planning/autoware_planning_data_analyzer/src/evaluation.cpp b/planning/autoware_planning_data_analyzer/src/evaluation.cpp index a67803bd..2e086cd5 100644 --- a/planning/autoware_planning_data_analyzer/src/evaluation.cpp +++ b/planning/autoware_planning_data_analyzer/src/evaluation.cpp @@ -35,13 +35,13 @@ DataInterface::DataInterface( const std::shared_ptr & bag_data, const std::shared_ptr & previous, const vehicle_info_utils::VehicleInfo & vehicle_info, const std::shared_ptr & parameters, const std::string & tag, - const TrajectoryPoints & points) + const std::shared_ptr & points) : previous_{previous}, vehicle_info{vehicle_info}, route_handler{bag_data->route_handler}, parameters{parameters}, tag{tag}, - points{points} + points_{points} { objects_history.reserve(parameters->resample_num); @@ -114,38 +114,38 @@ void DataInterface::calculate() double DataInterface::lateral_accel(const size_t idx) const { - const auto radius = vehicle_info.wheel_base_m / std::tan(points.at(idx).front_wheel_angle_rad); - const auto speed = points.at(idx).longitudinal_velocity_mps; + const auto radius = vehicle_info.wheel_base_m / std::tan(points_->at(idx).front_wheel_angle_rad); + const auto speed = points_->at(idx).longitudinal_velocity_mps; return speed * speed / radius; } double DataInterface::longitudinal_jerk(const size_t idx) const { - return (points.at(idx + 1).acceleration_mps2 - points.at(idx).acceleration_mps2) / 0.5; + return (points_->at(idx + 1).acceleration_mps2 - points_->at(idx).acceleration_mps2) / 0.5; } double DataInterface::minimum_ttc(const size_t idx) const { - const auto p_ego = points.at(idx).pose; - const auto v_ego = utils::get_velocity_in_world_coordinate(points.at(idx)); + const auto p_ego = points_->at(idx).pose; + const auto v_ego = utils::get_velocity_in_world_coordinate(points_->at(idx)); return utils::time_to_collision(*objects_history.at(idx), p_ego, v_ego); } double DataInterface::travel_distance(const size_t idx) const { - return autoware::motion_utils::calcSignedArcLength(points, 0L, idx); + return autoware::motion_utils::calcSignedArcLength(*points_, 0L, idx); } double DataInterface::lateral_deviation(const size_t idx) const { lanelet::ConstLanelet nearest{}; if (!route_handler->getClosestPreferredLaneletWithinRoute( - autoware::universe_utils::getPose(points.at(idx)), &nearest)) { + autoware::universe_utils::getPose(points_->at(idx)), &nearest)) { return std::numeric_limits::max(); } - const auto arc_coordinates = - lanelet::utils::getArcCoordinates({nearest}, autoware::universe_utils::getPose(points.at(idx))); + const auto arc_coordinates = lanelet::utils::getArcCoordinates( + {nearest}, autoware::universe_utils::getPose(points_->at(idx))); return arc_coordinates.distance; } @@ -153,7 +153,7 @@ double DataInterface::trajectory_deviation(const size_t idx) const { if (previous_ == nullptr) return 0.0; - const auto & p1 = autoware::universe_utils::getPose(points.at(idx)); + const auto & p1 = autoware::universe_utils::getPose(points_->at(idx)); const auto & p2 = previous_->at(idx); return autoware::universe_utils::calcSquaredDistance2d(p1, p2); } @@ -161,7 +161,7 @@ double DataInterface::trajectory_deviation(const size_t idx) const bool DataInterface::feasible() const { const auto condition = [](const auto & p) { return p.longitudinal_velocity_mps >= 0.0; }; - return std::all_of(points.begin(), points.end(), condition); + return std::all_of(points_->begin(), points_->end(), condition); } bool DataInterface::ready() const @@ -170,7 +170,7 @@ bool DataInterface::ready() const return false; } - if (points.size() < parameters->resample_num) { + if (points_->size() < parameters->resample_num) { return false; } @@ -220,7 +220,7 @@ GroundTruth::GroundTruth( auto GroundTruth::to_points( const std::shared_ptr & bag_data, const std::shared_ptr & parameters) - -> TrajectoryPoints + -> std::shared_ptr { TrajectoryPoints points; @@ -264,7 +264,7 @@ auto GroundTruth::to_points( points.push_back(point); } - return points; + return std::make_shared(points); } TrajectoryData::TrajectoryData( @@ -272,7 +272,7 @@ TrajectoryData::TrajectoryData( const std::shared_ptr & prev_best_data, const vehicle_info_utils::VehicleInfo & vehicle_info, const std::shared_ptr & parameters, const std::string & tag, - const std::vector & points) + const std::shared_ptr & points) : DataInterface(bag_data, prev_best_data, vehicle_info, parameters, tag, points) { calculate(); @@ -307,11 +307,12 @@ Evaluator::Evaluator( } // frenet planner - for (const auto & sample : utils::sampling( + for (const auto & points : utils::sampling( *opt_trajectory, opt_odometry->pose.pose, opt_odometry->twist.twist.linear.x, opt_accel->accel.accel.linear.x, vehicle_info, parameters)) { const auto ptr = std::make_shared( - bag_data, prev_best_data, vehicle_info, parameters, "frenet", sample); + bag_data, prev_best_data, vehicle_info, parameters, "frenet", + std::make_shared(points)); data_set.push_back(static_cast>(ptr)); } @@ -396,12 +397,12 @@ auto Evaluator::loss(const std::vector & weight) const -> double } const auto ground_truth = get("ground_truth"); - const auto min_size = std::min(ground_truth->points.size(), best_data->points.size()); + const auto min_size = std::min(ground_truth->points()->size(), best_data->points()->size()); double mse = 0.0; for (size_t i = 0; i < min_size; i++) { - const auto & p1 = autoware::universe_utils::getPose(ground_truth->points.at(i)); - const auto & p2 = autoware::universe_utils::getPose(best_data->points.at(i)); + const auto & p1 = autoware::universe_utils::getPose(ground_truth->points()->at(i)); + const auto & p2 = autoware::universe_utils::getPose(best_data->points()->at(i)); mse = (mse * i + autoware::universe_utils::calcSquaredDistance2d(p1, p2)) / (i + 1); } diff --git a/planning/autoware_planning_data_analyzer/src/evaluation.hpp b/planning/autoware_planning_data_analyzer/src/evaluation.hpp index 455a82c6..fb3ef92d 100644 --- a/planning/autoware_planning_data_analyzer/src/evaluation.hpp +++ b/planning/autoware_planning_data_analyzer/src/evaluation.hpp @@ -34,7 +34,7 @@ class DataInterface const std::shared_ptr & bag_data, const std::shared_ptr & previous, const vehicle_info_utils::VehicleInfo & vehicle_info, const std::shared_ptr & parameters, const std::string & tag, - const TrajectoryPoints & points); + const std::shared_ptr & points); void calculate(); @@ -65,6 +65,8 @@ class DataInterface bool ready() const; + auto points() const -> std::shared_ptr { return points_; } + std::shared_ptr previous_; std::vector objects_history; @@ -79,7 +81,7 @@ class DataInterface std::shared_ptr parameters; - TrajectoryPoints points; + std::shared_ptr points_; std::string tag{""}; }; @@ -96,7 +98,7 @@ class GroundTruth : public DataInterface private: static auto to_points( const std::shared_ptr & bag_data, const std::shared_ptr & parameters) - -> TrajectoryPoints; + -> std::shared_ptr; }; class TrajectoryData : public DataInterface @@ -107,7 +109,7 @@ class TrajectoryData : public DataInterface const std::shared_ptr & prev_best_data, const vehicle_info_utils::VehicleInfo & vehicle_info, const std::shared_ptr & parameters, const std::string & tag, - const std::vector & points); + const std::shared_ptr & points); }; class Evaluator diff --git a/planning/autoware_planning_data_analyzer/src/node.cpp b/planning/autoware_planning_data_analyzer/src/node.cpp index d3a468ec..bcced252 100644 --- a/planning/autoware_planning_data_analyzer/src/node.cpp +++ b/planning/autoware_planning_data_analyzer/src/node.cpp @@ -359,7 +359,7 @@ void BehaviorAnalyzerNode::weight( if (best_data == nullptr) { best = nullptr; } else { - best = std::make_shared(best_data->points); + best = best_data->points(); } std::cout << "IDX:" << i << " GRID:" << weight_grid.size() << std::endl; @@ -503,11 +503,11 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr & data_set const auto ground_truth = data_set->get("ground_truth"); if (ground_truth != nullptr) { - for (size_t i = 0; i < ground_truth->points.size(); ++i) { + for (size_t i = 0; i < ground_truth->points()->size(); ++i) { Marker marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "ground_truth", i, Marker::ARROW, createMarkerScale(0.7, 0.3, 0.3), createMarkerColor(1.0, 0.0, 0.0, 0.999)); - marker.pose = ground_truth->points.at(i).pose; + marker.pose = ground_truth->points()->at(i).pose; msg.markers.push_back(marker); } } @@ -517,11 +517,11 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr & data_set Marker marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "best_score", 0L, Marker::LINE_STRIP, createMarkerScale(0.2, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - for (const auto & point : best_data->points) { + for (const auto & point : *best_data->points()) { marker.points.push_back(point.pose.position); } msg.markers.push_back(marker); - previous_ = std::make_shared(best_data->points); + previous_ = best_data->points(); } else { previous_ = nullptr; } diff --git a/planning/autoware_planning_data_analyzer/src/utils.cpp b/planning/autoware_planning_data_analyzer/src/utils.cpp index f9656d19..5bea31d8 100644 --- a/planning/autoware_planning_data_analyzer/src/utils.cpp +++ b/planning/autoware_planning_data_analyzer/src/utils.cpp @@ -295,12 +295,12 @@ auto to_marker( createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); if (!data->feasible()) { - for (const auto & point : data->points) { + for (const auto & point : *data->points()) { marker.points.push_back(point.pose.position); marker.colors.push_back(createMarkerColor(0.1, 0.1, 0.1, 0.3)); } } else { - for (const auto & point : data->points) { + for (const auto & point : *data->points()) { marker.points.push_back(point.pose.position); marker.colors.push_back(createMarkerColor(1.0 - score, score, 0.0, std::min(0.5, score))); }