@@ -70,7 +70,8 @@ class PlanningEvaluatorNode : public rclcpp::Node
7070 * @brief callback on receiving a trajectory
7171 * @param [in] traj_msg received trajectory message
7272 */
73- void onTrajectory (const Trajectory::ConstSharedPtr traj_msg);
73+ void onTrajectory (
74+ const Trajectory::ConstSharedPtr traj_msg, const Odometry::ConstSharedPtr ego_state_ptr);
7475
7576 /* *
7677 * @brief callback on receiving a reference trajectory
@@ -88,7 +89,9 @@ class PlanningEvaluatorNode : public rclcpp::Node
8889 * @brief callback on receiving a modified goal
8990 * @param [in] modified_goal_msg received modified goal message
9091 */
91- void onModifiedGoal (const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg);
92+ void onModifiedGoal (
93+ const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg,
94+ const Odometry::ConstSharedPtr ego_state_ptr);
9295
9396 /* *
9497 * @brief publish the given metric statistic
@@ -99,26 +102,43 @@ class PlanningEvaluatorNode : public rclcpp::Node
99102 /* *
100103 * @brief publish current ego lane info
101104 */
102- DiagnosticStatus generateLaneletDiagnosticStatus ();
105+ DiagnosticStatus generateLaneletDiagnosticStatus (const Odometry::ConstSharedPtr ego_state_ptr );
103106
104107 /* *
105108 * @brief publish current ego kinematic state
106109 */
107110 DiagnosticStatus generateKinematicStateDiagnosticStatus (
108- const AccelWithCovarianceStamped & accel_stamped);
111+ const AccelWithCovarianceStamped & accel_stamped, const Odometry::ConstSharedPtr ego_state_ptr );
109112
110113private:
111114 static bool isFinite (const TrajectoryPoint & p);
112- void publishModifiedGoalDeviationMetrics ();
113- // update Route Handler
115+
116+ /* *
117+ * @brief update route handler data
118+ */
114119 void getRouteData ();
115120
121+ /* *
122+ * @brief fetch data and publish diagnostics
123+ */
124+ void onTimer ();
125+
126+ /* *
127+ * @brief fetch topic data
128+ */
129+ void fetchData ();
130+
116131 // ROS
117- rclcpp::Subscription<Trajectory>::SharedPtr traj_sub_;
118- rclcpp::Subscription<Trajectory>::SharedPtr ref_sub_;
119- rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
120- rclcpp::Subscription<PoseWithUuidStamped>::SharedPtr modified_goal_sub_;
121- rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
132+ autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> traj_sub_{
133+ this , " ~/input/trajectory"
134+ autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> ref_sub_{
135+ this , " ~/input/reference_trajectory"
136+ autoware::universe_utils::InterProcessPollingSubscriber<PredictedObjects> objects_sub_{
137+ this , " ~/input/objects"
138+ autoware::universe_utils::InterProcessPollingSubscriber<PoseWithUuidStamped> modified_goal_sub_{
139+ this , " ~/input/modified_goal"
140+ autoware::universe_utils::InterProcessPollingSubscriber<Odometry> odometry_sub_{
141+ this , " ~/input/odometry"
122142 autoware::universe_utils::InterProcessPollingSubscriber<LaneletRoute> route_subscriber_{
123143 this , " ~/input/route" 1 }.transient_local ()};
124144 autoware::universe_utils::InterProcessPollingSubscriber<LaneletMapBin> vector_map_subscriber_{
@@ -131,6 +151,7 @@ class PlanningEvaluatorNode : public rclcpp::Node
131151 std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
132152 autoware::route_handler::RouteHandler route_handler_;
133153
154+ DiagnosticArray metrics_msg_;
134155 // Parameters
135156 std::string output_file_str_;
136157 std::string ego_frame_str_;
@@ -142,8 +163,7 @@ class PlanningEvaluatorNode : public rclcpp::Node
142163 std::deque<rclcpp::Time> stamps_;
143164 std::array<std::deque<Stat<double >>, static_cast <size_t >(Metric::SIZE)> metric_stats_;
144165
145- Odometry::ConstSharedPtr ego_state_ptr_;
146- PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_;
166+ rclcpp::TimerBase::SharedPtr timer_;
147167 std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
148168};
149169} // namespace planning_diagnostics
0 commit comments