From 1a4a19231352dbf86ea9a4444395fefae0e108e2 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 5 Jul 2024 13:13:57 +0900 Subject: [PATCH] feat(planning_evaluator): add planning evaluator polling sub (#7827) * WIP add polling subs Signed-off-by: Daniel Sanchez * WIP Signed-off-by: Daniel Sanchez * update functions Signed-off-by: Daniel Sanchez * remove semicolon Signed-off-by: Daniel Sanchez * use last data for modified goal Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez Signed-off-by: palas21 --- .../planning_evaluator_node.hpp | 46 ++++-- .../src/planning_evaluator_node.cpp | 146 +++++++++--------- 2 files changed, 106 insertions(+), 86 deletions(-) diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 74523398c3a77..91dc0386ae6c6 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -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 @@ -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 @@ -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::SharedPtr traj_sub_; - rclcpp::Subscription::SharedPtr ref_sub_; - rclcpp::Subscription::SharedPtr objects_sub_; - rclcpp::Subscription::SharedPtr modified_goal_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; + autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ + this, "~/input/trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber ref_sub_{ + this, "~/input/reference_trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber objects_sub_{ + this, "~/input/objects"}; + autoware::universe_utils::InterProcessPollingSubscriber modified_goal_sub_{ + this, "~/input/modified_goal"}; + autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ + this, "~/input/odometry"}; autoware::universe_utils::InterProcessPollingSubscriber route_subscriber_{ this, "~/input/route", rclcpp::QoS{1}.transient_local()}; autoware::universe_utils::InterProcessPollingSubscriber vector_map_subscriber_{ @@ -131,6 +151,7 @@ class PlanningEvaluatorNode : public rclcpp::Node std::unique_ptr tf_buffer_; autoware::route_handler::RouteHandler route_handler_; + DiagnosticArray metrics_msg_; // Parameters std::string output_file_str_; std::string ego_frame_str_; @@ -142,8 +163,7 @@ class PlanningEvaluatorNode : public rclcpp::Node std::deque stamps_; std::array>, static_cast(Metric::SIZE)> metric_stats_; - Odometry::ConstSharedPtr ego_state_ptr_; - PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_; + rclcpp::TimerBase::SharedPtr timer_; std::optional prev_acc_stamped_{std::nullopt}; }; } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index 5e959aed123af..9b65dbbc0b89e 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -35,26 +35,13 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op : Node("planning_evaluator", node_options) { using std::placeholders::_1; - - traj_sub_ = create_subscription( - "~/input/trajectory", 1, std::bind(&PlanningEvaluatorNode::onTrajectory, this, _1)); - - ref_sub_ = create_subscription( - "~/input/reference_trajectory", 1, - std::bind(&PlanningEvaluatorNode::onReferenceTrajectory, this, _1)); - - objects_sub_ = create_subscription( - "~/input/objects", 1, std::bind(&PlanningEvaluatorNode::onObjects, this, _1)); - - modified_goal_sub_ = create_subscription( - "~/input/modified_goal", 1, std::bind(&PlanningEvaluatorNode::onModifiedGoal, this, _1)); - - odom_sub_ = create_subscription( - "~/input/odometry", 1, std::bind(&PlanningEvaluatorNode::onOdometry, this, _1)); - tf_buffer_ = std::make_unique(this->get_clock()); transform_listener_ = std::make_shared(*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("trajectory.min_point_dist_m"); @@ -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); @@ -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; @@ -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; } @@ -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) { @@ -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(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); }