From c84d1b02417383314353160ef2fef53872f9a690 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Sun, 25 Feb 2024 19:45:23 +0300 Subject: [PATCH] feat: add published_time publisher debug to packages Signed-off-by: Berkay Karaman --- .../controller_node.hpp | 3 +++ .../src/controller_node.cpp | 9 ++++++++- .../vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 11 +++++++++++ .../vehicle_cmd_gate/src/vehicle_cmd_gate.hpp | 3 +++ .../detected_object_feature_remover.hpp | 2 ++ .../package.xml | 1 + .../src/detected_object_feature_remover.cpp | 8 ++++++++ .../object_lanelet_filter.hpp | 3 +++ .../object_position_filter.hpp | 3 +++ .../obstacle_pointcloud_based_validator.hpp | 2 ++ .../occupancy_grid_based_validator.hpp | 2 ++ .../src/object_lanelet_filter.cpp | 7 +++++++ .../src/object_position_filter.cpp | 7 +++++++ .../obstacle_pointcloud_based_validator.cpp | 8 ++++++++ .../src/occupancy_grid_based_validator.cpp | 7 +++++++ .../detection_by_tracker_core.hpp | 3 +++ .../src/detection_by_tracker_core.cpp | 11 +++++++++++ .../include/lidar_centerpoint/node.hpp | 3 +++ perception/lidar_centerpoint/src/node.cpp | 7 +++++++ .../map_based_prediction_node.hpp | 4 ++++ .../src/map_based_prediction_node.cpp | 8 ++++++++ .../multi_object_tracker_core.hpp | 3 +++ .../src/multi_object_tracker_core.cpp | 7 +++++++ .../include/object_merger/node.hpp | 3 +++ .../src/object_association_merger/node.cpp | 8 ++++++++ ...upancy_grid_map_outlier_filter_nodelet.hpp | 2 ++ ...upancy_grid_map_outlier_filter_nodelet.cpp | 6 ++++++ .../decorative_tracker_merger.hpp | 3 +++ .../src/decorative_tracker_merger.cpp | 10 +++++++++- .../motion_velocity_smoother_node.hpp | 3 +++ .../src/motion_velocity_smoother_node.cpp | 6 ++++++ .../include/obstacle_stop_planner/node.hpp | 3 +++ planning/obstacle_stop_planner/src/node.cpp | 11 +++++++++++ .../planning_validator/planning_validator.hpp | 3 +++ .../src/planning_validator.cpp | 19 +++++++++++++++++++ .../scenario_selector_node.hpp | 2 ++ .../scenario_selector_node.cpp | 8 ++++++++ .../pointcloud_preprocessor/filter.hpp | 2 ++ .../pointcloud_preprocessor/src/filter.cpp | 10 ++++++++++ 39 files changed, 219 insertions(+), 2 deletions(-) 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 index cc43da7114630..be4fe885ae8c1 100644 --- 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 #include +#include #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 logger_configure_; + std::unique_ptr published_time_publisher_; + void publishProcessingTime( const double t_ms, const rclcpp::Publisher::SharedPtr pub); StopWatch stop_watch_; diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 322aa9eef5a79..5b1388e3065f1 100644 --- 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(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/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index aec2ede53d9e7..90cdd7670968f 100644 --- 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(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 index b79c58d120443..1907cf123632f 100644 --- 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 #include #include +#include #include #include @@ -248,6 +249,8 @@ class VehicleCmdGate : public rclcpp::Node void publishMarkers(const IsFilterActivated & filter_activated); std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace vehicle_cmd_gate diff --git a/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp b/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp index 68a15113254d9..d14952cdda54e 100644 --- a/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp +++ b/perception/detected_object_feature_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 +#include #include #include @@ -33,6 +34,7 @@ class DetectedObjectFeatureRemover : public rclcpp::Node private: rclcpp::Subscription::SharedPtr sub_; rclcpp::Publisher::SharedPtr pub_; + std::unique_ptr 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 index 44571a3c18dd4..80d07291e17ce 100644 --- a/perception/detected_object_feature_remover/package.xml +++ b/perception/detected_object_feature_remover/package.xml @@ -13,6 +13,7 @@ autoware_auto_perception_msgs rclcpp rclcpp_components + tier4_autoware_utils tier4_perception_msgs ament_cmake_ros 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 index d1e007d7ef65f..3f06883c42e0d 100644 --- 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("~/output", rclcpp::QoS(1)); sub_ = this->create_subscription( "~/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/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index e3c3263466033..f3871aaf98117 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -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 published_time_publisher_; }; } // namespace object_lanelet_filter diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp index be25eb6a353e6..9cc9c51747c52 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -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 published_time_publisher_; }; } // namespace object_position_filter diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index eb6da6bc45b24..742455c60a42c 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -148,6 +149,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node std::shared_ptr debugger_; bool using_2d_validator_; std::unique_ptr validator_; + std::unique_ptr published_time_publisher_; private: void onObjectsAndObstaclePointCloud( diff --git a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp index 5fd866d09e725..9be990ebc1787 100644 --- a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -42,6 +43,7 @@ class OccupancyGridBasedValidator : public rclcpp::Node message_filters::Subscriber occ_grid_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; + std::unique_ptr 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 index 74000a91e60fc..dd65c4e628b49 100644 --- 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(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( diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp index a509fc7571dd5..3927db81a602f 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -42,6 +42,9 @@ ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & n "input/object", rclcpp::QoS{1}, std::bind(&ObjectPositionFilterNode::objectCallback, this, _1)); object_pub_ = this->create_publisher( "output/object", rclcpp::QoS{1}); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void ObjectPositionFilterNode::objectCallback( @@ -65,6 +68,10 @@ void ObjectPositionFilterNode::objectCallback( } object_pub_->publish(output_object_msg); + + if (published_time_publisher_) { + published_time_publisher_->publish(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 index f3f56652792e9..e005b3327b893 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -309,6 +309,9 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, @@ -347,6 +350,11 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( } objects_pub_->publish(output); + + if (published_time_publisher_) { + published_time_publisher_->publish(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 index 80e4dc66d35c2..22a2e2b90cc0f 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -52,6 +52,9 @@ OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptio mean_threshold_ = declare_parameter("mean_threshold", 0.6); enable_debug_ = declare_parameter("enable_debug", false); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void OccupancyGridBasedValidator::onObjectsAndOccGrid( @@ -86,6 +89,10 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid( objects_pub_->publish(output); + if (published_time_publisher_) { + published_time_publisher_->publish(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 index 771747c80bb95..7747a37ee2f29 100644 --- 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,6 +23,7 @@ #include #include #include +#include #include #include @@ -83,6 +84,8 @@ class DetectionByTracker : public rclcpp::Node detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_; + std::unique_ptr 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 index 2595315e1f9b2..d8d55d97f427e 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -177,6 +177,9 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void DetectionByTracker::setMaxSearchRange() @@ -220,6 +223,10 @@ void DetectionByTracker::onObjects( !object_recognition_utils::transformObjects( objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) { objects_pub_->publish(detected_objects); + + if (published_time_publisher_) { + published_time_publisher_->publish(objects_pub_, detected_objects.header.stamp); + } return; } // to simplify post processes, convert tracked_objects to DetectedObjects message. @@ -250,6 +257,10 @@ void DetectionByTracker::onObjects( } objects_pub_->publish(detected_objects); + + if (published_time_publisher_) { + published_time_publisher_->publish(objects_pub_, detected_objects.header.stamp); + } } void DetectionByTracker::divideUnderSegmentedObjects( diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index 474c9884eb36f..963579e5763c2 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -63,6 +64,8 @@ class LidarCenterPointNode : public rclcpp::Node std::unique_ptr> stop_watch_ptr_{ nullptr}; std::unique_ptr debug_publisher_ptr_{nullptr}; + + std::unique_ptr published_time_publisher_; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index d370a60a26aa5..eafc129b4d00f 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -126,6 +126,9 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); } + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void LidarCenterPointNode::pointCloudCallback( @@ -163,6 +166,10 @@ void LidarCenterPointNode::pointCloudCallback( if (objects_sub_count > 0) { objects_pub_->publish(output_msg); + + if (published_time_publisher_) { + published_time_publisher_->publish(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 index 321342c3d8367..1b531e5a4f7fa 100644 --- 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 @@ -21,6 +21,7 @@ #include "tier4_autoware_utils/ros/update_param.hpp" #include +#include #include #include #include @@ -199,6 +200,9 @@ class MapBasedPredictionNode : public rclcpp::Node // Stop watch StopWatch stop_watch_; + // PublishedTime Publisher + std::unique_ptr 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 index 21697bb1b5f1a..c253cc77116a3 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -818,6 +818,9 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ this->create_publisher("maneuver", rclcpp::QoS{1}); pub_calculation_time_ = create_publisher("~/debug/calculation_time", 1); + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); + set_param_res_ = this->add_on_set_parameters_callback( std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); } @@ -1115,6 +1118,11 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Publish Results pub_objects_->publish(output); + + if (published_time_publisher_) { + published_time_publisher_->publish(pub_objects_, output.header.stamp); + } + pub_debug_markers_->publish(debug_markers); const auto calculation_time_msg = createStringStamped(now(), stop_watch_.toc()); pub_calculation_time_->publish(calculation_time_msg); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 95d33b78ff184..09499ac49467d 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -107,6 +108,8 @@ class MultiObjectTracker : public rclcpp::Node std::map tracker_map_; + std::unique_ptr published_time_publisher_; + void onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 1d4d50c7eab4c..d3e566a2339c3 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -236,6 +236,9 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) data_association_ = std::make_unique( can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, min_iou_matrix); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void MultiObjectTracker::onMeasurement( @@ -469,6 +472,10 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) // Publish tracked_objects_pub_->publish(output_msg); + if (published_time_publisher_) { + published_time_publisher_->publish(tracked_objects_pub_, output_msg.header.stamp); + } + // Debugger Publish if enabled debugger_->publishProcessingTime(); debugger_->publishTentativeObjects(tentative_objects_msg); diff --git a/perception/object_merger/include/object_merger/node.hpp b/perception/object_merger/include/object_merger/node.hpp index 8341ce490ca72..c884a4730e330 100644 --- a/perception/object_merger/include/object_merger/node.hpp +++ b/perception/object_merger/include/object_merger/node.hpp @@ -18,6 +18,7 @@ #include "object_merger/data_association/data_association.hpp" #include +#include #include @@ -81,6 +82,8 @@ class ObjectAssociationMergerNode : public rclcpp::Node std::map generalized_iou_threshold; std::map distance_threshold_map; } overlapped_judge_param_; + + std::unique_ptr published_time_publisher_; }; } // namespace object_association diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 1ffc2791e8f4e..c23234665bc5a 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -118,6 +118,9 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio merged_object_pub_ = create_publisher( "output/object", rclcpp::QoS{1}); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void ObjectAssociationMergerNode::objectsCallback( @@ -215,6 +218,11 @@ void ObjectAssociationMergerNode::objectsCallback( // publish output msg merged_object_pub_->publish(output_msg); + + // Publish published time if enabled by parameter + if (published_time_publisher_) { + published_time_publisher_->publish(merged_object_pub_, output_msg.header.stamp); + } } } // namespace object_association diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index 953307e9d5380..1d7d01bfbab55 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -120,6 +121,7 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node std::shared_ptr debugger_ptr_; std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; + std::unique_ptr published_time_publisher_; // ROS Parameters std::string map_frame_; diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 691c13a6d4701..b4c08ac44e204 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -234,6 +234,8 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( if (enable_debugger) { debugger_ptr_ = std::make_shared(*this); } + /* published time publisher enabled only if the use_published_time is true */ + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( @@ -305,6 +307,10 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( return; } pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr)); + + if (published_time_publisher_) { + published_time_publisher_->publish(pointcloud_pub_, ogm_frame_filtered_pc.header.stamp); + } } if (debugger_ptr_) { debugger_ptr_->publishHighConfidence(high_confidence_pc, ogm_frame_pc.header); diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp index 0509fb2a07dc5..aab9e51886350 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp @@ -20,6 +20,7 @@ #include "tracking_object_merger/utils/utils.hpp" #include +#include #include #include @@ -106,6 +107,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node // tracker default settings TrackerStateParameter tracker_state_parameter_; + std::unique_ptr published_time_publisher_; + // merge policy (currently not used) struct { diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index b22b8d5fa2948..ba0e75e218535 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -151,6 +151,9 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio set3dDataAssociation("lidar-radar", data_association_map_); // radar-radar association matrix set3dDataAssociation("radar-radar", data_association_map_); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void DecorativeTrackerMergerNode::set3dDataAssociation( @@ -212,8 +215,13 @@ void DecorativeTrackerMergerNode::mainObjectsCallback( // try to merge main object this->decorativeMerger(main_sensor_type_, main_objects); + const auto & tracked_objects = getTrackedObjects(main_objects->header); + merged_object_pub_->publish(tracked_objects); - merged_object_pub_->publish(getTrackedObjects(main_objects->header)); + // Published Time Publisher enabled only if the use_published_time is true + if (published_time_publisher_) { + published_time_publisher_->publish(merged_object_pub_, tracked_objects.header.stamp); + } } /** diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index fc347cb11f5cd..868c1ab12cce8 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -34,6 +34,8 @@ #include "tier4_autoware_utils/ros/self_pose_listener.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" +#include + #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -271,6 +273,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node void publishStopWatchTime(); std::unique_ptr logger_configure_; + std::unique_ptr published_time_publisher_; }; } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 5b9f84112f97a..6854609c2f409 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -104,6 +104,9 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions clock_ = get_clock(); logger_configure_ = std::make_unique(this); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void MotionVelocitySmootherNode::setupSmoother(const double wheelbase) @@ -314,6 +317,9 @@ void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & traj Trajectory publishing_trajectory = motion_utils::convertToTrajectory(trajectory); publishing_trajectory.header = base_traj_raw_ptr_->header; pub_trajectory_->publish(publishing_trajectory); + if (published_time_publisher_) { + published_time_publisher_->publish(pub_trajectory_, publishing_trajectory.header.stamp); + } } void MotionVelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg) diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 4e356eab6f736..d894544a67fe1 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -315,6 +316,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node } std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 1b34e85b87a50..78f61b3735493 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -211,6 +211,9 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod pub_processing_time_ms_ = this->create_publisher("~/debug/processing_time_ms", 1); + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); + // Subscribers if (!node_param_.use_predicted_objects) { // No need to point cloud while using predicted objects @@ -330,6 +333,10 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), 3000, "Backward path is NOT supported. publish input as it is."); pub_trajectory_->publish(*input_msg); + + if (published_time_publisher_) { + published_time_publisher_->publish(pub_trajectory_, input_msg->header.stamp); + } return; } @@ -381,6 +388,10 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m trajectory.header = input_msg->header; pub_trajectory_->publish(trajectory); + if (published_time_publisher_) { + published_time_publisher_->publish(pub_trajectory_, trajectory.header.stamp); + } + Float64Stamped processing_time_ms; processing_time_ms.stamp = now(); processing_time_ms.data = stop_watch_.toc(__func__); diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index 4e27def784b12..36954c267b2a9 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -136,6 +137,8 @@ class PlanningValidator : public rclcpp::Node std::unique_ptr logger_configure_; + std::unique_ptr published_time_publisher_; + StopWatch stop_watch_; }; } // namespace planning_validator diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 8b8b7d4566f08..4db45592d3849 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -48,6 +48,9 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) setupParameters(); logger_configure_ = std::make_unique(this); + + // Published time publisher only enabled if "use_published_time" parameter is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void PlanningValidator::setupParameters() @@ -219,6 +222,12 @@ void PlanningValidator::publishTrajectory() // Validation check is all green. Publish the trajectory. if (isAllValid(validation_status_)) { pub_traj_->publish(*current_trajectory_); + + // Publish published time if enabled by parameter + if (published_time_publisher_) { + published_time_publisher_->publish(pub_traj_, current_trajectory_->header.stamp); + } + previous_published_trajectory_ = current_trajectory_; return; } @@ -227,6 +236,11 @@ void PlanningValidator::publishTrajectory() if (invalid_trajectory_handling_type_ == InvalidTrajectoryHandlingType::PUBLISH_AS_IT_IS) { pub_traj_->publish(*current_trajectory_); + + if (published_time_publisher_) { + published_time_publisher_->publish(pub_traj_, current_trajectory_->header.stamp); + } + RCLCPP_ERROR(get_logger(), "Caution! Invalid Trajectory published."); return; } @@ -239,6 +253,11 @@ void PlanningValidator::publishTrajectory() if (invalid_trajectory_handling_type_ == InvalidTrajectoryHandlingType::USE_PREVIOUS_RESULT) { if (previous_published_trajectory_) { pub_traj_->publish(*previous_published_trajectory_); + + if (published_time_publisher_) { + published_time_publisher_->publish(pub_traj_, previous_published_trajectory_->header.stamp); + } + RCLCPP_ERROR(get_logger(), "Invalid Trajectory detected. Use previous trajectory."); return; } diff --git a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp index 94969570d6b53..3651f6c76b8bc 100644 --- a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp +++ b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp @@ -16,6 +16,7 @@ #define SCENARIO_SELECTOR__SCENARIO_SELECTOR_NODE_HPP_ #include +#include #include #include @@ -91,6 +92,7 @@ class ScenarioSelectorNode : public rclcpp::Node std::shared_ptr routing_graph_ptr_; std::shared_ptr traffic_rules_ptr_; std::shared_ptr route_handler_; + std::unique_ptr published_time_publisher_; // Parameters double update_rate_; diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp index 734ff11aaf4a1..fa071ae054222 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -320,6 +320,11 @@ void ScenarioSelectorNode::publishTrajectory( const auto delay_sec = (now - msg->header.stamp).seconds(); if (delay_sec <= th_max_message_delay_sec_) { pub_trajectory_->publish(*msg); + + if (published_time_publisher_) { + published_time_publisher_->publish(pub_trajectory_, msg->header.stamp); + } + } else { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), @@ -372,6 +377,9 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); + + // Published time publisher only enabled if "use_published_time" parameter is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp index 47c0f2b403faa..1f23c975a88af 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp @@ -81,6 +81,7 @@ // Include tier4 autoware utils #include +#include #include namespace pointcloud_preprocessor @@ -178,6 +179,7 @@ class Filter : public rclcpp::Node /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; + std::unique_ptr published_time_publisher_; /** \brief Virtual abstract filter method. To be implemented by every child. * \param input the input point cloud dataset. diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index d5843be395adf..a03172f5319cf 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -192,6 +192,11 @@ void pointcloud_preprocessor::Filter::computePublish( // Publish a boost shared ptr pub_output_->publish(std::move(output)); + + // Publish published time only if enabled by parameter + if (published_time_publisher_) { + published_time_publisher_->publish(pub_output_, input->header.stamp); + } } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -456,6 +461,11 @@ void pointcloud_preprocessor::Filter::faster_input_indices_callback( output->header.stamp = cloud->header.stamp; pub_output_->publish(std::move(output)); + + // Publish published time if enabled by parameter + if (published_time_publisher_) { + published_time_publisher_->publish(pub_output_, cloud->header.stamp); + } } // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform