feat(freespace_planner): add processing time pub

Signed-off-by: Kasunori-Nakajima <[email protected]>
autowarefoundation · Nov 15, 2024 · c3ce5e1 · c3ce5e1
1 parent 8652963
commit c3ce5e1
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 0 deletions.
diff --git a/.../autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp b/.../autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp
@@ -45,6 +45,7 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <std_msgs/msg/bool.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 #include <tier4_planning_msgs/msg/scenario.hpp>
 
 #ifdef ROS_DISTRO_GALACTIC
@@ -111,6 +112,7 @@ class FreespacePlannerNode : public rclcpp::Node
   rclcpp::Publisher<PoseArray>::SharedPtr debug_pose_array_pub_;
   rclcpp::Publisher<PoseArray>::SharedPtr debug_partial_pose_array_pub_;
   rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr parking_state_pub_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_pub_;
 
   rclcpp::Subscription<LaneletRoute>::SharedPtr route_sub_;
 

diff --git a/...ning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/...ning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp
@@ -36,6 +36,7 @@
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/geometry/pose_deviation.hpp>
+#include <autoware/universe_utils/system/stop_watch.hpp>
 
 #include <algorithm>
 #include <deque>
@@ -94,6 +95,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti
     debug_pose_array_pub_ = create_publisher<PoseArray>("~/debug/pose_array", qos);
     debug_partial_pose_array_pub_ = create_publisher<PoseArray>("~/debug/partial_pose_array", qos);
     parking_state_pub_ = create_publisher<std_msgs::msg::Bool>("is_completed", qos);
+    processing_time_pub_ =
+      create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
   }
 
   // TF
@@ -277,6 +280,8 @@ bool FreespacePlannerNode::isDataReady()
 
 void FreespacePlannerNode::onTimer()
 {
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
+
   scenario_ = scenario_sub_.takeData();
   if (!utils::is_active(scenario_)) {
     reset();
@@ -355,6 +360,12 @@ void FreespacePlannerNode::onTimer()
   debug_partial_pose_array_pub_->publish(utils::trajectory_to_pose_array(partial_trajectory_));
 
   is_new_parking_cycle_ = false;
+
+  // Publish ProcessingTime
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = stop_watch.toc();
+  processing_time_pub_->publish(processing_time_msg);
 }
 
 void FreespacePlannerNode::planTrajectory()