diff --git a/bag2lanelet/scripts/bag2map.py b/bag2lanelet/scripts/bag2map.py index f3e6300a..f6075b37 100755 --- a/bag2lanelet/scripts/bag2map.py +++ b/bag2lanelet/scripts/bag2map.py @@ -6,6 +6,7 @@ from bag2way import bag2point_stamped import folium + from tools.bag2lanelet.scripts.lanelet_xml import LaneletMap diff --git a/planning/autoware_planning_data_analyzer/src/data_structs.cpp b/planning/autoware_planning_data_analyzer/src/data_structs.cpp index a4456b55..8543886f 100644 --- a/planning/autoware_planning_data_analyzer/src/data_structs.cpp +++ b/planning/autoware_planning_data_analyzer/src/data_structs.cpp @@ -369,7 +369,7 @@ double TrajectoryData::travel_distance(const size_t idx) const bool TrajectoryData::feasible() const { - const auto condition = [](const auto & p) { return p.longitudinal_velocity_mps > 0.0; }; + const auto condition = [](const auto & p) { return p.longitudinal_velocity_mps > -1e-3; }; return std::all_of(points.begin(), points.end(), condition); } @@ -412,6 +412,14 @@ SamplingTrajectoryData::SamplingTrajectoryData( sample); } + std::vector stop_points(parameters->resample_num); + for (auto & stop_point : stop_points) { + stop_point.pose = opt_odometry.value().pose.pose; + } + data.emplace_back( + bag_data, vehicle_info, parameters->resample_num, parameters->time_resolution, "stop", + stop_points); + std::sort( data.begin(), data.end(), [](const auto & a, const auto & b) { return a.total() > b.total(); }); diff --git a/planning/autoware_planning_data_analyzer/src/data_structs.hpp b/planning/autoware_planning_data_analyzer/src/data_structs.hpp index 812d0bf4..abdfc34a 100644 --- a/planning/autoware_planning_data_analyzer/src/data_structs.hpp +++ b/planning/autoware_planning_data_analyzer/src/data_structs.hpp @@ -255,13 +255,18 @@ struct SamplingTrajectoryData const vehicle_info_utils::VehicleInfo & vehicle_info, const std::shared_ptr & parameters); - auto best() const -> TrajectoryData { return data.front(); } + auto best() const -> std::optional + { + if (data.empty()) return std::nullopt; + return data.front(); + } - auto autoware() const -> TrajectoryData + auto autoware() const -> std::optional { const auto itr = std::find_if(data.begin(), data.end(), [](const auto & trajectory) { return trajectory.tag == "autoware"; }); + if (itr == data.end()) return std::nullopt; return *itr; } diff --git a/planning/autoware_planning_data_analyzer/src/node.cpp b/planning/autoware_planning_data_analyzer/src/node.cpp index 5c9e578e..d63829df 100644 --- a/planning/autoware_planning_data_analyzer/src/node.cpp +++ b/planning/autoware_planning_data_analyzer/src/node.cpp @@ -215,6 +215,8 @@ void BehaviorAnalyzerNode::process(const std::shared_ptr & bag_data) co score(data_set); visualize(data_set); + + print(data_set); } void BehaviorAnalyzerNode::metrics(const std::shared_ptr & data_set) const @@ -239,7 +241,8 @@ void BehaviorAnalyzerNode::metrics(const std::shared_ptr & data_set) co pub_manual_metrics_->publish(msg); } - { + const auto autoware_trajectory = data_set->sampling.autoware(); + if (autoware_trajectory.has_value()) { Float32MultiArrayStamped msg{}; msg.stamp = now(); @@ -251,10 +254,10 @@ void BehaviorAnalyzerNode::metrics(const std::shared_ptr & data_set) co std::copy(metric.begin(), metric.end(), msg.data.begin() + offset); }; - set_metrics(data_set->sampling.autoware(), METRIC::LATERAL_ACCEL); - set_metrics(data_set->sampling.autoware(), METRIC::LONGITUDINAL_JERK); - set_metrics(data_set->sampling.autoware(), METRIC::TRAVEL_DISTANCE); - set_metrics(data_set->sampling.autoware(), METRIC::MINIMUM_TTC); + set_metrics(autoware_trajectory.value(), METRIC::LATERAL_ACCEL); + set_metrics(autoware_trajectory.value(), METRIC::LONGITUDINAL_JERK); + set_metrics(autoware_trajectory.value(), METRIC::TRAVEL_DISTANCE); + set_metrics(autoware_trajectory.value(), METRIC::MINIMUM_TTC); pub_system_metrics_->publish(msg); } @@ -280,7 +283,8 @@ void BehaviorAnalyzerNode::score(const std::shared_ptr & data_set) cons pub_manual_score_->publish(msg); } - { + const auto autoware_trajectory = data_set->sampling.autoware(); + if (autoware_trajectory.has_value()) { Float32MultiArrayStamped msg{}; msg.stamp = now(); @@ -290,10 +294,10 @@ void BehaviorAnalyzerNode::score(const std::shared_ptr & data_set) cons msg.data.at(static_cast(score_type)) = static_cast(data.scores.at(score_type)); }; - set_reward(data_set->sampling.autoware(), SCORE::LONGITUDINAL_COMFORTABILITY); - set_reward(data_set->sampling.autoware(), SCORE::LATERAL_COMFORTABILITY); - set_reward(data_set->sampling.autoware(), SCORE::EFFICIENCY); - set_reward(data_set->sampling.autoware(), SCORE::SAFETY); + set_reward(autoware_trajectory.value(), SCORE::LONGITUDINAL_COMFORTABILITY); + set_reward(autoware_trajectory.value(), SCORE::LATERAL_COMFORTABILITY); + set_reward(autoware_trajectory.value(), SCORE::EFFICIENCY); + set_reward(autoware_trajectory.value(), SCORE::SAFETY); pub_system_score_->publish(msg); } @@ -312,14 +316,6 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr & data_set) msg.markers.push_back(marker); } - for (const auto & point : data_set->sampling.autoware().points) { - Marker marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "system", i++, Marker::ARROW, - createMarkerScale(0.7, 0.3, 0.3), createMarkerColor(1.0, 1.0, 0.0, 0.999)); - marker.pose = point.pose; - msg.markers.push_back(marker); - } - for (const auto & trajectory : data_set->sampling.data) { Marker marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "candidates", i++, Marker::LINE_STRIP, @@ -338,19 +334,50 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr & data_set) msg.markers.push_back(marker); } - { + const auto best_trajectory = data_set->sampling.best(); + if (best_trajectory.has_value()) { Marker marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "best", i++, Marker::LINE_STRIP, createMarkerScale(0.2, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - for (const auto & point : data_set->sampling.best().points) { + for (const auto & point : best_trajectory.value().points) { marker.points.push_back(point.pose.position); } msg.markers.push_back(marker); } + const auto autoware_trajectory = data_set->sampling.autoware(); + if (autoware_trajectory.has_value()) { + for (const auto & point : autoware_trajectory.value().points) { + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "system", i++, Marker::ARROW, + createMarkerScale(0.7, 0.3, 0.3), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + marker.pose = point.pose; + msg.markers.push_back(marker); + } + } + pub_marker_->publish(msg); } +void BehaviorAnalyzerNode::print(const std::shared_ptr & data_set) const +{ + const auto autoware_trajectory = data_set->sampling.autoware(); + if (!autoware_trajectory.has_value()) { + return; + } + + const auto best_trajectory = data_set->sampling.best(); + if (!best_trajectory.has_value()) { + return; + } + + std::cout << "---result---" << std::endl; + std::cout << "[HUMAN] SCORE:" << data_set->manual.total() << std::endl; + std::cout << "[AUTOWARE] SCORE:" << autoware_trajectory.value().total() << std::endl; + std::cout << "[SAMPLING] BEST SCORE:" << best_trajectory.value().total() << "(" + << best_trajectory.value().tag << ")" << std::endl; +} + void BehaviorAnalyzerNode::on_timer() { if (!is_ready_) { diff --git a/planning/autoware_planning_data_analyzer/src/node.hpp b/planning/autoware_planning_data_analyzer/src/node.hpp index 37ec90b6..6ead5e12 100644 --- a/planning/autoware_planning_data_analyzer/src/node.hpp +++ b/planning/autoware_planning_data_analyzer/src/node.hpp @@ -56,6 +56,8 @@ class BehaviorAnalyzerNode : public rclcpp::Node void visualize(const std::shared_ptr & data_set) const; + void print(const std::shared_ptr & data_set) const; + rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr pub_marker_; rclcpp::Publisher::SharedPtr pub_odometry_;