Skip to content

Commit

Permalink
fix: print
Browse files Browse the repository at this point in the history 
Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Aug 26, 2024
1 parent 8ac192d commit ec13949
Show file tree
Hide file tree
Showing 5 changed files with 66 additions and 23 deletions.
1 change: 1 addition & 0 deletions bag2lanelet/scripts/bag2map.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

from bag2way import bag2point_stamped
import folium

from tools.bag2lanelet.scripts.lanelet_xml import LaneletMap


Expand Down
10 changes: 9 additions & 1 deletion planning/autoware_planning_data_analyzer/src/data_structs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand Down Expand Up @@ -412,6 +412,14 @@ SamplingTrajectoryData::SamplingTrajectoryData(
sample);
}

std::vector<TrajectoryPoint> 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(); });

Expand Down
9 changes: 7 additions & 2 deletions planning/autoware_planning_data_analyzer/src/data_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -255,13 +255,18 @@ struct SamplingTrajectoryData
const vehicle_info_utils::VehicleInfo & vehicle_info,
const std::shared_ptr<Parameters> & parameters);

auto best() const -> TrajectoryData { return data.front(); }
auto best() const -> std::optional<TrajectoryData>
{
if (data.empty()) return std::nullopt;
return data.front();
}

auto autoware() const -> TrajectoryData
auto autoware() const -> std::optional<TrajectoryData>
{
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;
}

Expand Down
67 changes: 47 additions & 20 deletions planning/autoware_planning_data_analyzer/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -215,6 +215,8 @@ void BehaviorAnalyzerNode::process(const std::shared_ptr<BagData> & bag_data) co
score(data_set);

visualize(data_set);

print(data_set);
}

void BehaviorAnalyzerNode::metrics(const std::shared_ptr<DataSet> & data_set) const
Expand All @@ -239,7 +241,8 @@ void BehaviorAnalyzerNode::metrics(const std::shared_ptr<DataSet> & 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();
Expand All @@ -251,10 +254,10 @@ void BehaviorAnalyzerNode::metrics(const std::shared_ptr<DataSet> & 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);
}
Expand All @@ -280,7 +283,8 @@ void BehaviorAnalyzerNode::score(const std::shared_ptr<DataSet> & 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();
Expand All @@ -290,10 +294,10 @@ void BehaviorAnalyzerNode::score(const std::shared_ptr<DataSet> & data_set) cons
msg.data.at(static_cast<size_t>(score_type)) = static_cast<float>(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);
}
Expand All @@ -312,14 +316,6 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr<DataSet> & 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,
Expand All @@ -338,19 +334,50 @@ void BehaviorAnalyzerNode::visualize(const std::shared_ptr<DataSet> & 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<DataSet> & 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_) {
Expand Down
2 changes: 2 additions & 0 deletions planning/autoware_planning_data_analyzer/src/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,8 @@ class BehaviorAnalyzerNode : public rclcpp::Node

void visualize(const std::shared_ptr<DataSet> & data_set) const;

void print(const std::shared_ptr<DataSet> & data_set) const;

rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<MarkerArray>::SharedPtr pub_marker_;
rclcpp::Publisher<Odometry>::SharedPtr pub_odometry_;
Expand Down

0 comments on commit ec13949

Please sign in to comment.