From d942222c5c861d4b287b883988f46f2f0ac5e96f Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Mon, 26 Feb 2024 09:51:56 +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 +++++++++- .../behavior_path_planner_node.hpp | 4 ++++ .../src/behavior_path_planner_node.cpp | 8 ++++++++ .../behavior_velocity_planner/src/node.cpp | 6 ++++++ .../behavior_velocity_planner/src/node.hpp | 3 +++ .../motion_velocity_smoother_node.hpp | 3 +++ .../src/motion_velocity_smoother_node.cpp | 6 ++++++ .../obstacle_avoidance_planner/node.hpp | 4 ++++ .../obstacle_avoidance_planner/src/node.cpp | 11 +++++++++++ .../include/obstacle_cruise_planner/node.hpp | 3 +++ planning/obstacle_cruise_planner/src/node.cpp | 6 ++++++ .../include/obstacle_stop_planner/node.hpp | 3 +++ planning/obstacle_stop_planner/src/node.cpp | 11 +++++++++++ .../obstacle_velocity_limiter_node.hpp | 3 +++ .../src/obstacle_velocity_limiter_node.cpp | 9 +++++++++ .../path_smoother/elastic_band_smoother.hpp | 4 ++++ .../src/elastic_band_smoother.cpp | 9 +++++++++ .../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 | 13 +++++++++++++ 51 files changed, 292 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/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 5d40f2a8614ed..99e312f98f100 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -21,6 +21,8 @@ #include "behavior_path_planner_common/interface/steering_factor_interface.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include + #include #include #include @@ -215,6 +217,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node const std::shared_ptr & planner_data); std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index d2f58e7d7e017..9ef26b03ed991 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -172,6 +172,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod } 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); } std::vector BehaviorPathPlannerNode::getWaitingApprovalModules() @@ -413,6 +416,11 @@ void BehaviorPathPlannerNode::run() if (!path->points.empty()) { path_publisher_->publish(*path); + + if (published_time_publisher_) { + published_time_publisher_->publish(path_publisher_, path->header.stamp); + } + } else { RCLCPP_ERROR_THROTTLE( get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish."); diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 854f3c8852c29..19879899c5bd9 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -159,6 +159,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio } 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 BehaviorVelocityPlannerNode::onLoadPlugin( @@ -399,6 +402,9 @@ void BehaviorVelocityPlannerNode::onTrigger( lk.unlock(); path_pub_->publish(output_path_msg); + if (published_time_publisher_) { + published_time_publisher_->publish(path_pub_, output_path_msg.header.stamp); + } stop_reason_diag_pub_->publish(planner_manager_.getStopReasonDiag()); if (debug_viz_pub_->get_subscription_count() > 0) { diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index a345f1200f721..dd6edf6ae0bc0 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -130,6 +131,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node const PlannerData & planner_data); std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace behavior_velocity_planner 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_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 9ca2f31b6ec5a..2dbefffe67738 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -24,6 +24,8 @@ #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" +#include + #include #include #include @@ -136,6 +138,8 @@ class ObstacleAvoidancePlanner : public rclcpp::Node double vehicle_stop_margin_outside_drivable_area_; std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 1dcabf202970c..27ddf7aef277a 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -153,6 +153,9 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n std::bind(&ObstacleAvoidancePlanner::onParam, this, std::placeholders::_1)); 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); } rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( @@ -239,6 +242,10 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); const auto output_traj_msg = motion_utils::convertToTrajectory(traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); + + if (published_time_publisher_) { + published_time_publisher_->publish(traj_pub_, output_traj_msg.header.stamp); + } return; } @@ -271,6 +278,10 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) const auto output_traj_msg = motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); + + if (published_time_publisher_) { + published_time_publisher_->publish(traj_pub_, output_traj_msg.header.stamp); + } } bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock clock) const diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index a8328d536f13c..d1a9d0ff56b52 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -24,6 +24,7 @@ #include "tier4_autoware_utils/system/stop_watch.hpp" #include +#include #include #include @@ -269,6 +270,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node std::shared_ptr prev_closest_stop_obstacle_ptr_{nullptr}; std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace motion_planning diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index e4e6cdf9e0873..00ffb813fdd81 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -442,6 +442,9 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1)); 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); } ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlanningAlgorithmType( @@ -538,6 +541,9 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms trajectory_pub_->publish(output_traj); // 8. Publish debug data + if (published_time_publisher_) { + published_time_publisher_->publish(trajectory_pub_, output_traj.header.stamp); + } publishDebugMarker(); publishDebugInfo(); 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..09b6b4099bd63 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -246,6 +246,9 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod createSubscriptionOptions(this)); 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 ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) @@ -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/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index cbdd390183d61..63eec0f2cf378 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -21,6 +21,7 @@ #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include +#include #include #include @@ -104,6 +105,8 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node bool validInputs(); std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index 086d76e9ef0fe..e9340bd2644d4 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -82,6 +82,9 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio add_on_set_parameters_callback([this](const auto & params) { return onParameter(params); }); 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); } rcl_interfaces::msg::SetParametersResult ObstacleVelocityLimiterNode::onParameter( @@ -162,6 +165,8 @@ rcl_interfaces::msg::SetParametersResult ObstacleVelocityLimiterNode::onParamete projection_params_.steering_angle_offset = parameter.as_double(); } else if (parameter.get_name() == ProjectionParameters::DISTANCE_METHOD_PARAM) { projection_params_.updateDistanceMethod(*this, parameter.as_string()); + } else if (parameter.get_name() == "use_published_time") { + void(); } else { RCLCPP_WARN(get_logger(), "Unknown parameter %s", parameter.get_name().c_str()); result.successful = false; @@ -222,6 +227,10 @@ void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr pub_trajectory_->publish(safe_trajectory); + if (published_time_publisher_) { + published_time_publisher_->publish(pub_trajectory_, safe_trajectory.header.stamp); + } + const auto t_end = std::chrono::system_clock::now(); const auto runtime = std::chrono::duration_cast(t_end - t_start); pub_runtime_->publish(tier4_debug_msgs::msg::Float64Stamped().set__stamp(now()).set__data( diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index 364e1e3cc43d8..eb2d651cc967d 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -23,6 +23,8 @@ #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include + #include #include #include @@ -112,6 +114,8 @@ class ElasticBandSmoother : public rclcpp::Node const std::vector & optimized_points) const; std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace path_smoother diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index a81a92f3fcc96..f0ff9d6c3eb85 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -108,6 +108,9 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option std::bind(&ElasticBandSmoother::onParam, this, std::placeholders::_1)); 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); } rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( @@ -171,6 +174,9 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) const auto output_traj_msg = motion_utils::convertToTrajectory(traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); path_pub_->publish(*path_ptr); + if (published_time_publisher_) { + published_time_publisher_->publish(path_pub_, path_ptr->header.stamp); + } return; } @@ -225,6 +231,9 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) traj_pub_->publish(output_traj_msg); const auto output_path_msg = trajectory_utils::create_path(*path_ptr, full_traj_points); path_pub_->publish(output_path_msg); + if (published_time_publisher_) { + published_time_publisher_->publish(path_pub_, path_ptr->header.stamp); + } } bool ElasticBandSmoother::isDataReady(const Path & path, rclcpp::Clock clock) const 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..61b160317ee96 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -102,6 +102,9 @@ pointcloud_preprocessor::Filter::Filter( set_param_res_filter_ = this->add_on_set_parameters_callback( std::bind(&Filter::filterParamCallback, this, std::placeholders::_1)); + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); + RCLCPP_DEBUG(this->get_logger(), "[Filter Constructor] successfully created."); } @@ -192,6 +195,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 +464,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