feat: add published_time publisher debug to packages

Signed-off-by: Berkay Karaman <[email protected]>
autowarefoundation · Mar 5, 2024 · f5d2208 · f5d2208
1 parent 7ef61a9
commit f5d2208
Show file tree

Hide file tree

Showing 76 changed files with 424 additions and 103 deletions.
diff --git a/control/trajectory_follower_node/README.md b/control/trajectory_follower_node/README.md
@@ -146,6 +146,7 @@ Giving the longitudinal controller information about steer convergence allows it
     2. The last received commands are not older than defined by `timeout_thr_sec`.
 - `lateral_controller_mode`: `mpc` or `pure_pursuit`
   - (currently there is only `PID` for longitudinal controller)
+- `use_published_time`: Node publishes its control command publishing time with pipeline header file. Default it is false.
 
 ## Debugging
 

diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp
@@ -28,6 +28,7 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
 #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp"
@@ -121,6 +122,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
 
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
 
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
+
   void publishProcessingTime(
     const double t_ms, const rclcpp::Publisher<Float64Stamped>::SharedPtr pub);
   StopWatch<std::chrono::milliseconds> stop_watch_;

diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp
@@ -91,6 +91,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
   }
 
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
+
+  published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
 }
 
 Controller::LateralControllerMode Controller::getLateralControllerMode(
@@ -231,7 +233,12 @@ void Controller::callbackTimerControl()
   out.longitudinal = lon_out.control_cmd;
   control_cmd_pub_->publish(out);
 
-  // 6. publish debug marker
+  // 6. publish published time only if enabled by parameter
+  if (published_time_publisher_) {
+    published_time_publisher_->publish(control_cmd_pub_, out.stamp);
+  }
+
+  // 7. publish debug marker
   publishDebugMarker(*input_data, lat_out);
 }
 

diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md
@@ -67,6 +67,7 @@
 | `on_transition.lon_jerk_lim`                | <double> | array of limits of longitudinal jerk (activated in TRANSITION operation mode)                                                                                                               |
 | `on_transition.lat_acc_lim`                 | <double> | array of limits of lateral acceleration (activated in TRANSITION operation mode)                                                                                                            |
 | `on_transition.lat_jerk_lim`                | <double> | array of limits of lateral jerk (activated in TRANSITION operation mode)                                                                                                                    |
+| `use_published_time`                        | bool     | Node publishes its control command publishing time with pipeline header file. Default it is false.                                                                                          |
 
 ## Filter function
 

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -242,6 +242,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     this, get_clock(), period_ns, std::bind(&VehicleCmdGate::publishStatus, this));
 
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
+
+  published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
 }
 
 bool VehicleCmdGate::isHeartbeatTimeout(
@@ -456,7 +458,16 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
   // Publish commands
   vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency);
   control_cmd_pub_->publish(filtered_commands.control);
+
+  // Publish published time only if enabled by parameter
+  if (published_time_publisher_) {
+    published_time_publisher_->publish(control_cmd_pub_, filtered_commands.control.stamp);
+  }
+
+  // Publish pause state to api
   adapi_pause_->publish();
+
+  // Publish moderate stop state which is used for stop request
   moderate_stop_interface_->publish();
 
   // Save ControlCmd to steering angle when disengaged

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
@@ -23,6 +23,7 @@
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <motion_utils/vehicle/vehicle_state_checker.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 #include <vehicle_cmd_gate/msg/is_filter_activated.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
@@ -248,6 +249,8 @@ class VehicleCmdGate : public rclcpp::Node
   void publishMarkers(const IsFilterActivated & filter_activated);
 
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
+
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 };
 
 }  // namespace vehicle_cmd_gate

diff --git a/perception/detected_object_feature_remover/README.md b/perception/detected_object_feature_remover/README.md
@@ -22,6 +22,8 @@ The `detected_object_feature_remover` is a package to convert topic-type from `D
 
 ## Parameters
 
-None
+| Name                 | Type   | Description                                                                                  |
+| -------------------- | ------ | -------------------------------------------------------------------------------------------- |
+| `use_published_time` | `bool` | Node publishes its DetectedObjects publishing time with pipeline header file. Default: false |
 
 ## Assumptions / Known limits
diff --git a/...ature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp b/...ature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp
@@ -16,6 +16,7 @@
 #define DETECTED_OBJECT_FEATURE_REMOVER__DETECTED_OBJECT_FEATURE_REMOVER_HPP_
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
@@ -33,6 +34,7 @@ class DetectedObjectFeatureRemover : public rclcpp::Node
 private:
   rclcpp::Subscription<DetectedObjectsWithFeature>::SharedPtr sub_;
   rclcpp::Publisher<DetectedObjects>::SharedPtr pub_;
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
   void objectCallback(const DetectedObjectsWithFeature::ConstSharedPtr input);
   void convert(const DetectedObjectsWithFeature & objs_with_feature, DetectedObjects & objs);
 };

diff --git a/perception/detected_object_feature_remover/package.xml b/perception/detected_object_feature_remover/package.xml
@@ -13,6 +13,7 @@
   <depend>autoware_auto_perception_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tier4_autoware_utils</depend>
   <depend>tier4_perception_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>

diff --git a/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp b/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp
@@ -23,6 +23,9 @@ DetectedObjectFeatureRemover::DetectedObjectFeatureRemover(const rclcpp::NodeOpt
   pub_ = this->create_publisher<DetectedObjects>("~/output", rclcpp::QoS(1));
   sub_ = this->create_subscription<DetectedObjectsWithFeature>(
     "~/input", 1, std::bind(&DetectedObjectFeatureRemover::objectCallback, this, _1));
+
+  // Published Time Publisher enabled only if the use_published_time is true
+  published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
 }
 
 void DetectedObjectFeatureRemover::objectCallback(
@@ -31,6 +34,11 @@ void DetectedObjectFeatureRemover::objectCallback(
   DetectedObjects output;
   convert(*input, output);
   pub_->publish(output);
+
+  // Publish published time if enabled by parameter
+  if (published_time_publisher_) {
+    published_time_publisher_->publish(pub_, output.header.stamp);
+  }
 }
 
 void DetectedObjectFeatureRemover::convert(

diff --git a/...ation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/...ation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp
@@ -20,6 +20,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
@@ -68,6 +69,8 @@ class ObjectLaneletFilterNode : public rclcpp::Node
   bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &);
   geometry_msgs::msg::Polygon setFootprint(
     const autoware_auto_perception_msgs::msg::DetectedObject &);
+
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 };
 
 }  // namespace object_lanelet_filter

diff --git a/...tion/include/detected_object_validation/detected_object_filter/object_position_filter.hpp b/...tion/include/detected_object_validation/detected_object_filter/object_position_filter.hpp
@@ -19,6 +19,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
@@ -51,6 +52,8 @@ class ObjectPositionFilterNode : public rclcpp::Node
   float lower_bound_y_;
   utils::FilterTargetLabel filter_target_;
   bool isObjectInBounds(const autoware_auto_perception_msgs::msg::DetectedObject & object) const;
+
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 };
 
 }  // namespace object_position_filter

diff --git a/...ct_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/...ct_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp
@@ -21,6 +21,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -148,6 +149,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node
   std::shared_ptr<Debugger> debugger_;
   bool using_2d_validator_;
   std::unique_ptr<Validator> validator_;
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 
 private:
   void onObjectsAndObstaclePointCloud(

diff --git a/...ected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/...ected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp
@@ -19,6 +19,7 @@
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/opencv.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
@@ -42,6 +43,7 @@ class OccupancyGridBasedValidator : public rclcpp::Node
   message_filters::Subscriber<nav_msgs::msg::OccupancyGrid> occ_grid_sub_;
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 
   typedef message_filters::sync_policies::ApproximateTime<
     autoware_auto_perception_msgs::msg::DetectedObjects, nav_msgs::msg::OccupancyGrid>

diff --git a/perception/detected_object_validation/object-lanelet-filter.md b/perception/detected_object_validation/object-lanelet-filter.md
@@ -26,16 +26,17 @@ The objects only inside of the vector map will be published.
 
 ### Core Parameters
 
-| Name                             | Type | Default Value | Description                               |
-| -------------------------------- | ---- | ------------- | ----------------------------------------- |
-| `filter_target_label.UNKNOWN`    | bool | false         | If true, unknown objects are filtered.    |
-| `filter_target_label.CAR`        | bool | false         | If true, car objects are filtered.        |
-| `filter_target_label.TRUCK`      | bool | false         | If true, truck objects are filtered.      |
-| `filter_target_label.BUS`        | bool | false         | If true, bus objects are filtered.        |
-| `filter_target_label.TRAILER`    | bool | false         | If true, trailer objects are filtered.    |
-| `filter_target_label.MOTORCYCLE` | bool | false         | If true, motorcycle objects are filtered. |
-| `filter_target_label.BICYCLE`    | bool | false         | If true, bicycle objects are filtered.    |
-| `filter_target_label.PEDESTRIAN` | bool | false         | If true, pedestrian objects are filtered. |
+| Name                             | Type | Default Value | Description                                                                   |
+| -------------------------------- | ---- | ------------- | ----------------------------------------------------------------------------- |
+| `filter_target_label.UNKNOWN`    | bool | false         | If true, unknown objects are filtered.                                        |
+| `filter_target_label.CAR`        | bool | false         | If true, car objects are filtered.                                            |
+| `filter_target_label.TRUCK`      | bool | false         | If true, truck objects are filtered.                                          |
+| `filter_target_label.BUS`        | bool | false         | If true, bus objects are filtered.                                            |
+| `filter_target_label.TRAILER`    | bool | false         | If true, trailer objects are filtered.                                        |
+| `filter_target_label.MOTORCYCLE` | bool | false         | If true, motorcycle objects are filtered.                                     |
+| `filter_target_label.BICYCLE`    | bool | false         | If true, bicycle objects are filtered.                                        |
+| `filter_target_label.PEDESTRIAN` | bool | false         | If true, pedestrian objects are filtered.                                     |
+| `use_published_time`             | bool | false         | Node publishes its DetectedObjects publishing time with pipeline header file. |
 
 ## Assumptions / Known limits
 

diff --git a/perception/detected_object_validation/object-position-filter.md b/perception/detected_object_validation/object-position-filter.md
@@ -25,20 +25,21 @@ The objects only inside of the x, y bound will be published.
 
 ### Core Parameters
 
-| Name                             | Type  | Default Value | Description                                                     |
-| -------------------------------- | ----- | ------------- | --------------------------------------------------------------- |
-| `filter_target_label.UNKNOWN`    | bool  | false         | If true, unknown objects are filtered.                          |
-| `filter_target_label.CAR`        | bool  | false         | If true, car objects are filtered.                              |
-| `filter_target_label.TRUCK`      | bool  | false         | If true, truck objects are filtered.                            |
-| `filter_target_label.BUS`        | bool  | false         | If true, bus objects are filtered.                              |
-| `filter_target_label.TRAILER`    | bool  | false         | If true, trailer objects are filtered.                          |
-| `filter_target_label.MOTORCYCLE` | bool  | false         | If true, motorcycle objects are filtered.                       |
-| `filter_target_label.BICYCLE`    | bool  | false         | If true, bicycle objects are filtered.                          |
-| `filter_target_label.PEDESTRIAN` | bool  | false         | If true, pedestrian objects are filtered.                       |
-| `upper_bound_x`                  | float | 100.00        | Bound for filtering. Only used if filter_by_xy_position is true |
-| `lower_bound_x`                  | float | 0.00          | Bound for filtering. Only used if filter_by_xy_position is true |
-| `upper_bound_y`                  | float | 50.00         | Bound for filtering. Only used if filter_by_xy_position is true |
-| `lower_bound_y`                  | float | -50.00        | Bound for filtering. Only used if filter_by_xy_position is true |
+| Name                             | Type  | Default Value | Description                                                                   |
+| -------------------------------- | ----- | ------------- | ----------------------------------------------------------------------------- |
+| `filter_target_label.UNKNOWN`    | bool  | false         | If true, unknown objects are filtered.                                        |
+| `filter_target_label.CAR`        | bool  | false         | If true, car objects are filtered.                                            |
+| `filter_target_label.TRUCK`      | bool  | false         | If true, truck objects are filtered.                                          |
+| `filter_target_label.BUS`        | bool  | false         | If true, bus objects are filtered.                                            |
+| `filter_target_label.TRAILER`    | bool  | false         | If true, trailer objects are filtered.                                        |
+| `filter_target_label.MOTORCYCLE` | bool  | false         | If true, motorcycle objects are filtered.                                     |
+| `filter_target_label.BICYCLE`    | bool  | false         | If true, bicycle objects are filtered.                                        |
+| `filter_target_label.PEDESTRIAN` | bool  | false         | If true, pedestrian objects are filtered.                                     |
+| `upper_bound_x`                  | float | 100.00        | Bound for filtering. Only used if filter_by_xy_position is true               |
+| `lower_bound_x`                  | float | 0.00          | Bound for filtering. Only used if filter_by_xy_position is true               |
+| `upper_bound_y`                  | float | 50.00         | Bound for filtering. Only used if filter_by_xy_position is true               |
+| `lower_bound_y`                  | float | -50.00        | Bound for filtering. Only used if filter_by_xy_position is true               |
+| `use_published_time`             | bool  | false         | Node publishes its DetectedObjects publishing time with pipeline header file. |
 
 ## Assumptions / Known limits
 

diff --git a/perception/detected_object_validation/obstacle-pointcloud-based-validator.md b/perception/detected_object_validation/obstacle-pointcloud-based-validator.md
@@ -33,6 +33,7 @@ In the debug image above, the red DetectedObject is the validated object. The bl
 | `max_points_num`                | int   | The max number of obstacle point clouds in DetectedObjects                                                                                                                 |
 | `min_points_and_distance_ratio` | float | Threshold value of the number of point clouds per object when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink. |
 | `enable_debugger`               | bool  | Whether to create debug topics or not?                                                                                                                                     |
+| `use_published_time`            | bool  | Node publishes its DetectedObjects publishing time with pipeline header file. Default it is false.                                                                         |
 
 ## Assumptions / Known limits
 

diff --git a/perception/detected_object_validation/occupancy-grid-based-validator.md b/perception/detected_object_validation/occupancy-grid-based-validator.md
@@ -28,10 +28,11 @@ If the percentage is low, it is deleted.
 
 ## Parameters
 
-| Name             | Type  | Description                                        |
-| ---------------- | ----- | -------------------------------------------------- |
-| `mean_threshold` | float | The percentage threshold of allowed non-freespace. |
-| `enable_debug`   | bool  | Whether to display debug images or not?            |
+| Name                 | Type  | Description                                                                                        |
+| -------------------- | ----- | -------------------------------------------------------------------------------------------------- |
+| `mean_threshold`     | float | The percentage threshold of allowed non-freespace.                                                 |
+| `enable_debug`       | bool  | Whether to display debug images or not?                                                            |
+| `use_published_time` | bool  | Node publishes its DetectedObjects publishing time with pipeline header file. Default it is false. |
 
 ## Assumptions / Known limits
 

diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp
@@ -55,6 +55,9 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
 
   debug_publisher_ =
     std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "object_lanelet_filter");
+
+  // Published Time Publisher enabled only if the use_published_time is true
+  published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
 }
 
 void ObjectLaneletFilterNode::mapCallback(
@@ -120,6 +123,10 @@ void ObjectLaneletFilterNode::objectCallback(
   }
   object_pub_->publish(output_object_msg);
 
+  if (published_time_publisher_) {
+    published_time_publisher_->publish(object_pub_, output_object_msg.header.stamp);
+  }
+
   // Publish debug info
   const double pipeline_latency =
     std::chrono::duration<double, std::milli>(