feat(evaluator): Add Trajectory Evaluator #9265
Conversation
github-actions
bot
added
the
component:evaluator
|
Thank you for contributing to the Autoware project! 🚧 If your pull request is in progress, switch it to draft mode. Please ensure:
|
|
This pull request has been automatically marked as stale because it has not had recent activity. |
| const double velocity = std::sqrt( | ||
| std::pow(odom_msg->twist.twist.linear.x, 2) + std::pow(odom_msg->twist.twist.linear.y, 2) + | ||
| std::pow(odom_msg->twist.twist.linear.z, 2)); |
There was a problem hiding this comment.
it is enough to directly use odom_msg->twist.twist.linear.x
| void TrajectoryEvaluatorNode::on_kinematic_state( | ||
| const nav_msgs::msg::Odometry::ConstSharedPtr odom_msg) | ||
| { | ||
| if (odom_msg != nullptr) { |
There was a problem hiding this comment.
Instead of having the whole function in the if block, you should do an early return.
| if (odom_msg != nullptr) { | |
| if (odom_msg == nullptr) { | |
| return; | |
| } |
| } | ||
| } | ||
|
|
||
| if (min_distance < 1e-6) { |
There was a problem hiding this comment.
if ego deviates from the trajectory by more than 1e-6m then the error is not measured ?
| double min_distance = std::numeric_limits<double>::max(); | ||
| size_t closest_index = trajectory.trajectory_points.size(); | ||
| const geometry_msgs::msg::Pose & pose = odom_msg->pose.pose; | ||
| for (size_t i = 0; i < trajectory.trajectory_points.size(); ++i) { | ||
| const auto & trajectory_point_pose = trajectory.trajectory_points[i].trajectory_point.pose; | ||
| double distance = autoware::universe_utils::calcDistance2d(trajectory_point_pose, pose); | ||
|
|
||
| if (distance < min_distance) { | ||
| min_distance = distance; | ||
| closest_index = i; | ||
| } | ||
| } |
There was a problem hiding this comment.
There is already a function autoware::motion_utils::findNearestIndex() to find the nearest index.
BUT you should calculate the expected time more accurately. Please calculate the arc length of the odom pose and interpolate the corresponding expected time.
You can use autoware::motion_utils::findNearestSegmentIndex().
| metrics_pub_->publish(metrics_msg_); | ||
|
|
||
| metrics_pub_->publish(metrics_msg_); |
There was a problem hiding this comment.
You are publishing twice, and for each stored trajectory.
This is too much so please only publish the metrics message once per iteration.
Description
Add trajectory evaluator to evaluate the accuracy of predicted or planned vehicle trajectories by comparing the expected versus actual arrival times at specific trajectory points.
Related links
Parent Issue:
How was this PR tested?
Unit and random test.
Notes for reviewers
None.
Interface changes
None.
Effects on system behavior
None.