Revert "feat: add published_time publisher debug to packages (#6490)"

This reverts commit 7f36c52.
autowarefoundation · Mar 15, 2024 · 48c5fe5 · 48c5fe5
1 parent 70eb8cf
commit 48c5fe5
Show file tree

Hide file tree

Showing 57 changed files with 6 additions and 153 deletions.
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp
@@ -37,8 +37,7 @@ class PublishedTimePublisher
   {
   }
 
-  void publish_if_subscribed(
-    const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp)
+  void publish(const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp)
   {
     const auto & gid_key = publisher->get_gid();
 
@@ -58,7 +57,7 @@ class PublishedTimePublisher
     }
   }
 
-  void publish_if_subscribed(
+  void publish(
     const rclcpp::PublisherBase::ConstSharedPtr & publisher, const std_msgs::msg::Header & header)
   {
     const auto & gid_key = publisher->get_gid();

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,7 +28,6 @@
 
 #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"
@@ -122,8 +121,6 @@ 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/package.xml b/control/trajectory_follower_node/package.xml
@@ -30,7 +30,6 @@
   <depend>pure_pursuit</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
-  <depend>tier4_autoware_utils</depend>
   <depend>trajectory_follower_base</depend>
   <depend>vehicle_info_util</depend>
   <depend>visualization_msgs</depend>

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

diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml
@@ -28,7 +28,6 @@
   <depend>rclcpp_components</depend>
   <depend>std_srvs</depend>
   <depend>tier4_api_utils</depend>
-  <depend>tier4_autoware_utils</depend>
   <depend>tier4_control_msgs</depend>
   <depend>tier4_debug_msgs</depend>
   <depend>tier4_external_api_msgs</depend>

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -242,8 +242,6 @@ 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_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
 bool VehicleCmdGate::isHeartbeatTimeout(
@@ -458,8 +456,6 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
   // Publish commands
   vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency);
   control_cmd_pub_->publish(filtered_commands.control);
-  published_time_publisher_->publish_if_subscribed(
-    control_cmd_pub_, filtered_commands.control.stamp);
   adapi_pause_->publish();
   moderate_stop_interface_->publish();
 

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
@@ -23,7 +23,6 @@
 #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>
 
@@ -249,8 +248,6 @@ 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/...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,13 +16,10 @@
 #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>
 
-#include <memory>
-
 namespace detected_object_feature_remover
 {
 using autoware_auto_perception_msgs::msg::DetectedObjects;
@@ -36,7 +33,6 @@ 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,7 +13,6 @@
   <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,7 +23,6 @@ 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_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
 void DetectedObjectFeatureRemover::objectCallback(
@@ -32,7 +31,6 @@ void DetectedObjectFeatureRemover::objectCallback(
   DetectedObjects output;
   convert(*input, output);
   pub_->publish(output);
-  published_time_publisher_->publish_if_subscribed(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,7 +20,6 @@
 #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>
@@ -69,8 +68,6 @@ 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,15 +19,13 @@
 
 #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>
 
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
-#include <memory>
 #include <string>
 
 namespace object_position_filter
@@ -53,8 +51,6 @@ 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,7 +21,6 @@
 
 #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>
@@ -149,7 +148,6 @@ 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,7 +19,6 @@
 #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>
@@ -43,7 +42,6 @@ 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/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp
@@ -55,7 +55,6 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
 
   debug_publisher_ =
     std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "object_lanelet_filter");
-  published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
 void ObjectLaneletFilterNode::mapCallback(
@@ -120,7 +119,6 @@ void ObjectLaneletFilterNode::objectCallback(
     ++index;
   }
   object_pub_->publish(output_object_msg);
-  published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp);
 
   // Publish debug info
   const double pipeline_latency =

diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp
@@ -42,7 +42,6 @@ ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & n
     "input/object", rclcpp::QoS{1}, std::bind(&ObjectPositionFilterNode::objectCallback, this, _1));
   object_pub_ = this->create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
     "output/object", rclcpp::QoS{1});
-  published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
 void ObjectPositionFilterNode::objectCallback(
@@ -66,7 +65,6 @@ void ObjectPositionFilterNode::objectCallback(
   }
 
   object_pub_->publish(output_object_msg);
-  published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp);
 }
 
 bool ObjectPositionFilterNode::isObjectInBounds(

diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp
@@ -309,7 +309,6 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
 
   const bool enable_debugger = declare_parameter<bool>("enable_debugger", false);
   if (enable_debugger) debugger_ = std::make_shared<Debugger>(this);
-  published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
   const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
@@ -348,7 +347,6 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
   }
 
   objects_pub_->publish(output);
-  published_time_publisher_->publish_if_subscribed(objects_pub_, output.header.stamp);
   if (debugger_) {
     debugger_->publishRemovedObjects(removed_objects);
     debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header);

diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp
@@ -52,7 +52,6 @@ OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptio
 
   mean_threshold_ = declare_parameter<float>("mean_threshold", 0.6);
   enable_debug_ = declare_parameter<bool>("enable_debug", false);
-  published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
 void OccupancyGridBasedValidator::onObjectsAndOccGrid(
@@ -86,7 +85,6 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid(
   }
 
   objects_pub_->publish(output);
-  published_time_publisher_->publish_if_subscribed(objects_pub_, output.header.stamp);
 
   if (enable_debug_) showDebugImage(*input_occ_grid, transformed_objects, occ_grid);
 }

diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp
@@ -23,7 +23,6 @@
 #include <euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <shape_estimation/shape_estimator.hpp>
-#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
@@ -84,8 +83,6 @@ class DetectionByTracker : public rclcpp::Node
 
   detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_;
 
-  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
-
   void setMaxSearchRange();
 
   void onObjects(

diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp
@@ -177,7 +177,6 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options)
   cluster_ = std::make_shared<euclidean_cluster::VoxelGridBasedEuclideanCluster>(
     false, 10, 10000, 0.7, 0.3, 0);
   debugger_ = std::make_shared<Debugger>(this);
-  published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
 void DetectionByTracker::setMaxSearchRange()
@@ -222,7 +221,6 @@ void DetectionByTracker::onObjects(
       !object_recognition_utils::transformObjects(
         objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) {
       objects_pub_->publish(detected_objects);
-      published_time_publisher_->publish_if_subscribed(objects_pub_, detected_objects.header.stamp);
       return;
     }
     // to simplify post processes, convert tracked_objects to DetectedObjects message.
@@ -253,7 +251,6 @@ void DetectionByTracker::onObjects(
   }
 
   objects_pub_->publish(detected_objects);
-  published_time_publisher_->publish_if_subscribed(objects_pub_, detected_objects.header.stamp);
   debugger_->publishProcessingTime();
 }
 

diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp
@@ -21,7 +21,6 @@
 #include <lidar_centerpoint/detection_class_remapper.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
-#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_object_kinematics.hpp>
@@ -64,8 +63,6 @@ class LidarCenterPointNode : public rclcpp::Node
   std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_{
     nullptr};
   std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_ptr_{nullptr};
-
-  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 };
 
 }  // namespace centerpoint

diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp
@@ -126,7 +126,6 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
     RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node.");
     rclcpp::shutdown();
   }
-  published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
 void LidarCenterPointNode::pointCloudCallback(
@@ -164,7 +163,6 @@ void LidarCenterPointNode::pointCloudCallback(
 
   if (objects_sub_count > 0) {
     objects_pub_->publish(output_msg);
-    published_time_publisher_->publish_if_subscribed(objects_pub_, output_msg.header.stamp);
   }
 
   // add processing time for debug

diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -22,7 +22,6 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
-#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 #include <tier4_autoware_utils/ros/transform_listener.hpp>
 #include <tier4_autoware_utils/ros/uuid_helper.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
@@ -200,8 +199,6 @@ class MapBasedPredictionNode : public rclcpp::Node
   std::vector<double> distance_set_for_no_intention_to_walk_;
   std::vector<double> timeout_set_for_no_intention_to_walk_;
 
-  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
-
   // Member Functions
   void mapCallback(const HADMapBin::ConstSharedPtr msg);
   void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg);

diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -811,7 +811,6 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
   processing_time_publisher_ =
     std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "map_based_prediction");
 
-  published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
   set_param_res_ = this->add_on_set_parameters_callback(
     std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1));
 
@@ -1113,7 +1112,6 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
 
   // Publish Results
   pub_objects_->publish(output);
-  published_time_publisher_->publish_if_subscribed(pub_objects_, output.header.stamp);
   pub_debug_markers_->publish(debug_markers);
   const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
   const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);