Skip to content

Commit

Permalink
feat(planning_evaluator): add planning evaluator polling sub (#7827)
Browse files Browse the repository at this point in the history 
* WIP add polling subs

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* WIP

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* update functions

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* remove semicolon

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* use last data for modified goal

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

---------

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
  • Loading branch information
@danielsanchezaran
danielsanchezaran authored Jul 5, 2024
1 parent 134b5c2 commit 0f60c62
Show file tree
Hide file tree
Showing 2 changed files with 106 additions and 86 deletions.
46 changes: 33 additions & 13 deletions ...toware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,8 @@ class PlanningEvaluatorNode : public rclcpp::Node
* @brief callback on receiving a trajectory
* @param [in] traj_msg received trajectory message
*/
void onTrajectory(const Trajectory::ConstSharedPtr traj_msg);
void onTrajectory(
const Trajectory::ConstSharedPtr traj_msg, const Odometry::ConstSharedPtr ego_state_ptr);

/**
* @brief callback on receiving a reference trajectory
Expand All @@ -88,7 +89,9 @@ class PlanningEvaluatorNode : public rclcpp::Node
* @brief callback on receiving a modified goal
* @param [in] modified_goal_msg received modified goal message
*/
void onModifiedGoal(const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg);
void onModifiedGoal(
const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg,
const Odometry::ConstSharedPtr ego_state_ptr);

/**
* @brief publish the given metric statistic
Expand All @@ -99,26 +102,43 @@ class PlanningEvaluatorNode : public rclcpp::Node
/**
* @brief publish current ego lane info
*/
DiagnosticStatus generateLaneletDiagnosticStatus();
DiagnosticStatus generateLaneletDiagnosticStatus(const Odometry::ConstSharedPtr ego_state_ptr);

/**
* @brief publish current ego kinematic state
*/
DiagnosticStatus generateKinematicStateDiagnosticStatus(
const AccelWithCovarianceStamped & accel_stamped);
const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr);

private:
static bool isFinite(const TrajectoryPoint & p);
void publishModifiedGoalDeviationMetrics();
// update Route Handler

/**
* @brief update route handler data
*/
void getRouteData();

/**
* @brief fetch data and publish diagnostics
*/
void onTimer();

/**
* @brief fetch topic data
*/
void fetchData();

// ROS
rclcpp::Subscription<Trajectory>::SharedPtr traj_sub_;
rclcpp::Subscription<Trajectory>::SharedPtr ref_sub_;
rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
rclcpp::Subscription<PoseWithUuidStamped>::SharedPtr modified_goal_sub_;
rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> traj_sub_{
this, "~/input/trajectory"};
autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> ref_sub_{
this, "~/input/reference_trajectory"};
autoware::universe_utils::InterProcessPollingSubscriber<PredictedObjects> objects_sub_{
this, "~/input/objects"};
autoware::universe_utils::InterProcessPollingSubscriber<PoseWithUuidStamped> modified_goal_sub_{
this, "~/input/modified_goal"};
autoware::universe_utils::InterProcessPollingSubscriber<Odometry> odometry_sub_{
this, "~/input/odometry"};
autoware::universe_utils::InterProcessPollingSubscriber<LaneletRoute> route_subscriber_{
this, "~/input/route", rclcpp::QoS{1}.transient_local()};
autoware::universe_utils::InterProcessPollingSubscriber<LaneletMapBin> vector_map_subscriber_{
Expand All @@ -131,6 +151,7 @@ class PlanningEvaluatorNode : public rclcpp::Node
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
autoware::route_handler::RouteHandler route_handler_;

DiagnosticArray metrics_msg_;
// Parameters
std::string output_file_str_;
std::string ego_frame_str_;
Expand All @@ -142,8 +163,7 @@ class PlanningEvaluatorNode : public rclcpp::Node
std::deque<rclcpp::Time> stamps_;
std::array<std::deque<Stat<double>>, static_cast<size_t>(Metric::SIZE)> metric_stats_;

Odometry::ConstSharedPtr ego_state_ptr_;
PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_;
rclcpp::TimerBase::SharedPtr timer_;
std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
};
} // namespace planning_diagnostics
Expand Down
146 changes: 73 additions & 73 deletions evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,26 +35,13 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op
: Node("planning_evaluator", node_options)
{
using std::placeholders::_1;

traj_sub_ = create_subscription<Trajectory>(
"~/input/trajectory", 1, std::bind(&PlanningEvaluatorNode::onTrajectory, this, _1));

ref_sub_ = create_subscription<Trajectory>(
"~/input/reference_trajectory", 1,
std::bind(&PlanningEvaluatorNode::onReferenceTrajectory, this, _1));

objects_sub_ = create_subscription<PredictedObjects>(
"~/input/objects", 1, std::bind(&PlanningEvaluatorNode::onObjects, this, _1));

modified_goal_sub_ = create_subscription<PoseWithUuidStamped>(
"~/input/modified_goal", 1, std::bind(&PlanningEvaluatorNode::onModifiedGoal, this, _1));

odom_sub_ = create_subscription<Odometry>(
"~/input/odometry", 1, std::bind(&PlanningEvaluatorNode::onOdometry, this, _1));

tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

// Timer callback to publish evaluator diagnostics
using namespace std::literals::chrono_literals;
timer_ = rclcpp::create_timer(
this, get_clock(), 100ms, std::bind(&PlanningEvaluatorNode::onTimer, this));
// Parameters
metrics_calculator_.parameters.trajectory.min_point_dist_m =
declare_parameter<double>("trajectory.min_point_dist_m");
Expand Down Expand Up @@ -133,9 +120,10 @@ void PlanningEvaluatorNode::getRouteData()
}
}

DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus()
DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus(
const Odometry::ConstSharedPtr ego_state_ptr)
{
const auto & ego_pose = ego_state_ptr_->pose.pose;
const auto & ego_pose = ego_state_ptr->pose.pose;
const auto current_lanelets = [&]() {
lanelet::ConstLanelet closest_route_lanelet;
route_handler_.getClosestLaneletWithinRoute(ego_pose, &closest_route_lanelet);
Expand Down Expand Up @@ -166,14 +154,14 @@ DiagnosticStatus PlanningEvaluatorNode::generateLaneletDiagnosticStatus()
}

DiagnosticStatus PlanningEvaluatorNode::generateKinematicStateDiagnosticStatus(
const AccelWithCovarianceStamped & accel_stamped)
const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr)
{
DiagnosticStatus status;
status.name = "kinematic_state";
status.level = status.OK;
diagnostic_msgs::msg::KeyValue key_value;
key_value.key = "vel";
key_value.value = std::to_string(ego_state_ptr_->twist.twist.linear.x);
key_value.value = std::to_string(ego_state_ptr->twist.twist.linear.x);
status.values.push_back(key_value);
key_value.key = "acc";
const auto & acc = accel_stamped.accel.accel.linear.x;
Expand Down Expand Up @@ -220,9 +208,40 @@ DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus(
return status;
}

void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_msg)
void PlanningEvaluatorNode::onTimer()
{
metrics_msg_.header.stamp = now();

const auto ego_state_ptr = odometry_sub_.takeData();
onOdometry(ego_state_ptr);
{
const auto objects_msg = objects_sub_.takeData();
onObjects(objects_msg);
}

{
const auto ref_traj_msg = ref_sub_.takeData();
onReferenceTrajectory(ref_traj_msg);
}

{
const auto traj_msg = traj_sub_.takeData();
onTrajectory(traj_msg, ego_state_ptr);
}
{
const auto modified_goal_msg = modified_goal_sub_.takeData();
onModifiedGoal(modified_goal_msg, ego_state_ptr);
}
if (!metrics_msg_.status.empty()) {
metrics_pub_->publish(metrics_msg_);
}
metrics_msg_ = DiagnosticArray{};
}

void PlanningEvaluatorNode::onTrajectory(
const Trajectory::ConstSharedPtr traj_msg, const Odometry::ConstSharedPtr ego_state_ptr)
{
if (!ego_state_ptr_) {
if (!ego_state_ptr || !traj_msg) {
return;
}

Expand All @@ -231,8 +250,6 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m
stamps_.push_back(traj_msg->header.stamp);
}

DiagnosticArray metrics_msg;
metrics_msg.header.stamp = now();
for (Metric metric : metrics_) {
const auto metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg);
if (!metric_stat) {
Expand All @@ -244,88 +261,71 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m
}

if (metric_stat->count() > 0) {
metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat));
metrics_msg_.status.push_back(generateDiagnosticStatus(metric, *metric_stat));
}
}
if (!metrics_msg.status.empty()) {
metrics_pub_->publish(metrics_msg);
}

metrics_calculator_.setPreviousTrajectory(*traj_msg);
auto runtime = (now() - start).seconds();
RCLCPP_DEBUG(get_logger(), "Planning evaluation calculation time: %2.2f ms", runtime * 1e3);
}

void PlanningEvaluatorNode::onModifiedGoal(
const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg)
const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg,
const Odometry::ConstSharedPtr ego_state_ptr)
{
modified_goal_ptr_ = modified_goal_msg;
if (ego_state_ptr_) {
publishModifiedGoalDeviationMetrics();
}
}

void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg)
{
ego_state_ptr_ = odometry_msg;
metrics_calculator_.setEgoPose(odometry_msg->pose.pose);
{
DiagnosticArray metrics_msg;
metrics_msg.header.stamp = now();

getRouteData();
if (route_handler_.isHandlerReady() && ego_state_ptr_) {
metrics_msg.status.push_back(generateLaneletDiagnosticStatus());
}

const auto acc = accel_sub_.takeData();

if (acc && ego_state_ptr_) {
metrics_msg.status.push_back(generateKinematicStateDiagnosticStatus(*acc));
}

if (!metrics_msg.status.empty()) {
metrics_pub_->publish(metrics_msg);
}
}

if (modified_goal_ptr_) {
publishModifiedGoalDeviationMetrics();
if (!modified_goal_msg || !ego_state_ptr) {
return;
}
}

void PlanningEvaluatorNode::publishModifiedGoalDeviationMetrics()
{
auto start = now();

DiagnosticArray metrics_msg;
metrics_msg.header.stamp = now();
for (Metric metric : metrics_) {
const auto metric_stat = metrics_calculator_.calculate(
Metric(metric), modified_goal_ptr_->pose, ego_state_ptr_->pose.pose);
Metric(metric), modified_goal_msg->pose, ego_state_ptr->pose.pose);
if (!metric_stat) {
continue;
}
metric_stats_[static_cast<size_t>(metric)].push_back(*metric_stat);
if (metric_stat->count() > 0) {
metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat));
metrics_msg_.status.push_back(generateDiagnosticStatus(metric, *metric_stat));
}
}
if (!metrics_msg.status.empty()) {
metrics_pub_->publish(metrics_msg);
}
auto runtime = (now() - start).seconds();
RCLCPP_DEBUG(
get_logger(), "Planning evaluation modified goal deviation calculation time: %2.2f ms",
runtime * 1e3);
}

void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg)
{
if (!odometry_msg) return;
metrics_calculator_.setEgoPose(odometry_msg->pose.pose);
{
getRouteData();
if (route_handler_.isHandlerReady() && odometry_msg) {
metrics_msg_.status.push_back(generateLaneletDiagnosticStatus(odometry_msg));
}

const auto acc_msg = accel_sub_.takeData();
if (acc_msg && odometry_msg) {
metrics_msg_.status.push_back(generateKinematicStateDiagnosticStatus(*acc_msg, odometry_msg));
}
}
}

void PlanningEvaluatorNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr traj_msg)
{
if (!traj_msg) {
return;
}
metrics_calculator_.setReferenceTrajectory(*traj_msg);
}

void PlanningEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg)
{
if (!objects_msg) {
return;
}
metrics_calculator_.setPredictedObjects(*objects_msg);
}

Expand Down

0 comments on commit 0f60c62

Please sign in to comment.