From fc5a16c6ee93182c7170e95318cd427e4e832060 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Tue, 12 Mar 2024 11:39:01 +0300 Subject: [PATCH 1/4] feat: add published_time publisher debug to packages Signed-off-by: Berkay Karaman --- .../trajectory_follower_node/controller_node.hpp | 3 +++ .../src/controller_node.cpp | 7 ++++++- control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 9 +++++++++ control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp | 3 +++ .../detected_object_feature_remover/README.md | 2 -- .../detected_object_feature_remover.hpp | 2 ++ .../detected_object_feature_remover/package.xml | 1 + .../src/detected_object_feature_remover.cpp | 5 +++++ .../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 | 5 +++++ .../src/object_position_filter.cpp | 5 +++++ .../src/obstacle_pointcloud_based_validator.cpp | 6 ++++++ .../src/occupancy_grid_based_validator.cpp | 5 +++++ .../detection_by_tracker_core.hpp | 3 +++ .../src/detection_by_tracker_core.cpp | 9 +++++++++ .../include/lidar_centerpoint/node.hpp | 3 +++ perception/lidar_centerpoint/src/node.cpp | 5 +++++ .../map_based_prediction_node.hpp | 7 +++++++ .../src/map_based_prediction_node.cpp | 6 ++++++ .../multi_object_tracker_core.hpp | 3 +++ .../src/multi_object_tracker_core.cpp | 5 +++++ .../object_merger/include/object_merger/node.hpp | 3 +++ .../src/object_association_merger/node.cpp | 5 +++++ .../occupancy_grid_map_outlier_filter_nodelet.hpp | 2 ++ .../occupancy_grid_map_outlier_filter_nodelet.cpp | 5 +++++ perception/shape_estimation/src/node.cpp | 5 ++++- perception/shape_estimation/src/node.hpp | 2 ++ .../decorative_tracker_merger.hpp | 3 +++ .../src/decorative_tracker_merger.cpp | 6 ++++++ .../behavior_path_planner_node.hpp | 4 ++++ .../src/behavior_path_planner_node.cpp | 6 ++++++ planning/behavior_velocity_planner/src/node.cpp | 4 ++++ planning/behavior_velocity_planner/src/node.hpp | 3 +++ .../motion_velocity_smoother_node.hpp | 3 +++ .../src/motion_velocity_smoother_node.cpp | 4 ++++ .../include/obstacle_avoidance_planner/node.hpp | 4 ++++ planning/obstacle_avoidance_planner/src/node.cpp | 8 ++++++++ .../include/obstacle_cruise_planner/node.hpp | 3 +++ planning/obstacle_cruise_planner/src/node.cpp | 3 +++ .../include/obstacle_stop_planner/node.hpp | 3 +++ planning/obstacle_stop_planner/src/node.cpp | 8 ++++++++ .../obstacle_velocity_limiter_node.hpp | 3 +++ .../src/obstacle_velocity_limiter_node.cpp | 5 +++++ planning/path_smoother/README.md | 2 ++ .../path_smoother/elastic_band_smoother.hpp | 4 ++++ .../path_smoother/src/elastic_band_smoother.cpp | 6 ++++++ .../planning_validator/planning_validator.hpp | 3 +++ .../planning_validator/src/planning_validator.cpp | 14 ++++++++++++++ .../scenario_selector/scenario_selector_node.hpp | 2 ++ .../scenario_selector_node.cpp | 6 ++++++ .../include/pointcloud_preprocessor/filter.hpp | 2 ++ sensing/pointcloud_preprocessor/src/filter.cpp | 8 ++++++++ 55 files changed, 239 insertions(+), 4 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..c769e3127b65a 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_ = std::make_unique(this); } Controller::LateralControllerMode Controller::getLateralControllerMode( @@ -231,7 +233,10 @@ void Controller::callbackTimerControl() out.longitudinal = lon_out.control_cmd; control_cmd_pub_->publish(out); - // 6. publish debug marker + // 6. publish published time only if there are subscribers more than 1 + 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..ef0c179f4afc3 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_ = std::make_unique(this); } bool VehicleCmdGate::isHeartbeatTimeout( @@ -456,7 +458,14 @@ 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 there are subscribers more than 1 + 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/README.md b/perception/detected_object_feature_remover/README.md index 21a8bd9a00541..5395bc9bdde70 100644 --- a/perception/detected_object_feature_remover/README.md +++ b/perception/detected_object_feature_remover/README.md @@ -22,6 +22,4 @@ The `detected_object_feature_remover` is a package to convert topic-type from `D ## Parameters -None - ## Assumptions / Known limits 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..f28453980fb54 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,8 @@ 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_ = std::make_unique(this); } void DetectedObjectFeatureRemover::objectCallback( @@ -31,6 +33,9 @@ void DetectedObjectFeatureRemover::objectCallback( DetectedObjects output; convert(*input, output); pub_->publish(output); + + // Publish published time only if there are subscribers more than 1 + 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..1c0d8ffd65a2c 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -55,6 +55,8 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod debug_publisher_ = std::make_unique(this, "object_lanelet_filter"); + + published_time_publisher_ = std::make_unique(this); } void ObjectLaneletFilterNode::mapCallback( @@ -120,6 +122,9 @@ void ObjectLaneletFilterNode::objectCallback( } object_pub_->publish(output_object_msg); + // Publish published time only if there are subscribers more than 1 + 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..76a6aa1858d23 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -42,6 +42,8 @@ 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_ = std::make_unique(this); } void ObjectPositionFilterNode::objectCallback( @@ -65,6 +67,9 @@ void ObjectPositionFilterNode::objectCallback( } object_pub_->publish(output_object_msg); + + // Publish published time only if there are subscribers more than 1 + 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..8fdd7c7976f33 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,8 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); + + published_time_publisher_ = std::make_unique(this); } void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, @@ -347,6 +349,10 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( } objects_pub_->publish(output); + + // Publish published time only if there are subscribers more than 1 + 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..801010017bb2f 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,8 @@ OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptio mean_threshold_ = declare_parameter("mean_threshold", 0.6); enable_debug_ = declare_parameter("enable_debug", false); + + published_time_publisher_ = std::make_unique(this); } void OccupancyGridBasedValidator::onObjectsAndOccGrid( @@ -86,6 +88,9 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid( objects_pub_->publish(output); + // Publish published time only if there are subscribers more than 1 + 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 7afc3e41e4683..4e0496bd8c528 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,8 @@ 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_ = std::make_unique(this); } void DetectionByTracker::setMaxSearchRange() @@ -221,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); + + // Publish published time only if there are subscribers more than 1 + published_time_publisher_->publish(objects_pub_, detected_objects.header.stamp); + return; } // to simplify post processes, convert tracked_objects to DetectedObjects message. @@ -252,6 +258,9 @@ void DetectionByTracker::onObjects( objects_pub_->publish(detected_objects); debugger_->publishProcessingTime(); + + // Publish published time only if there are subscribers more than 1 + 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..9907ae42ce3ce 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -126,6 +126,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); } + + published_time_publisher_ = std::make_unique(this); } void LidarCenterPointNode::pointCloudCallback( @@ -163,6 +165,9 @@ void LidarCenterPointNode::pointCloudCallback( if (objects_sub_count > 0) { objects_pub_->publish(output_msg); + + // Publish published time only if there are subscribers more than 1 + 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 26dee3eacb08d..e3a1276c8dd40 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 @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -199,6 +200,12 @@ class MapBasedPredictionNode : public rclcpp::Node std::vector distance_set_for_no_intention_to_walk_; std::vector timeout_set_for_no_intention_to_walk_; + // 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 549619950cf82..e5726715a0c73 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -811,6 +811,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ processing_time_publisher_ = std::make_unique(this, "map_based_prediction"); + published_time_publisher_ = std::make_unique(this); + set_param_res_ = this->add_on_set_parameters_callback( std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); @@ -1112,6 +1114,10 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Publish Results pub_objects_->publish(output); + + // Publish published time only if there are subscribers more than 1 + published_time_publisher_->publish(pub_objects_, output.header.stamp); + pub_debug_markers_->publish(debug_markers); const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true); const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); 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 3b05af9601f1e..ca59f994a3b0e 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 1e684e78fc45a..8632d5efdcf5e 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,8 @@ 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_ = std::make_unique(this); } void MultiObjectTracker::onMeasurement( @@ -469,6 +471,9 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const // Publish tracked_objects_pub_->publish(output_msg); + // Publish published time only if there are subscribers more than 1 + 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 ba89a04553476..1194de2558f58 100644 --- a/perception/object_merger/include/object_merger/node.hpp +++ b/perception/object_merger/include/object_merger/node.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -87,6 +88,8 @@ class ObjectAssociationMergerNode : public rclcpp::Node // debug publisher std::unique_ptr processing_time_publisher_; std::unique_ptr> stop_watch_ptr_; + + 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 15b1939911394..f3bca450802b7 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -125,6 +125,8 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio stop_watch_ptr_ = std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); + + published_time_publisher_ = std::make_unique(this); } void ObjectAssociationMergerNode::objectsCallback( @@ -228,6 +230,9 @@ void ObjectAssociationMergerNode::objectsCallback( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); + + // Publish published time only if there are subscribers more than 1 + 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 78f76d4f980c3..2cb5a34b76d61 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_ = std::make_unique(this); } void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( @@ -328,6 +330,9 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( return; } pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr)); + + // Publish published time only if there are subscribers more than 1 + 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/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 423dda764492b..83823f3383a26 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -58,6 +58,8 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option stop_watch_ptr_ = std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); + + published_time_publisher_ = std::make_unique(this); } void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg) @@ -120,7 +122,8 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared // Publish pub_->publish(output_msg); - + // Publish published time only if there are subscribers more than 1 + published_time_publisher_->publish(pub_, output_msg.header.stamp); processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); processing_time_publisher_->publish( diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/node.hpp index a1c30446605b0..012261e79e8dd 100644 --- a/perception/shape_estimation/src/node.hpp +++ b/perception/shape_estimation/src/node.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -34,6 +35,7 @@ class ShapeEstimationNode : public rclcpp::Node // ros rclcpp::Publisher::SharedPtr pub_; rclcpp::Subscription::SharedPtr sub_; + std::unique_ptr published_time_publisher_; // debug publisher std::unique_ptr> stop_watch_ptr_; 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 56843f4c5e1e0..357ed9c986301 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 @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -110,6 +111,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 af0de2a02bbe7..e0efc496afd0d 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -158,6 +158,8 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio stop_watch_ptr_ = std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); + + published_time_publisher_ = std::make_unique(this); } void DecorativeTrackerMergerNode::set3dDataAssociation( @@ -220,8 +222,12 @@ 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)); + // Publish published time only if there are subscribers more than 1 + published_time_publisher_->publish(merged_object_pub_, tracked_objects.header.stamp); processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); processing_time_publisher_->publish( 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..f170cf4d6d401 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,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod } logger_configure_ = std::make_unique(this); + + published_time_publisher_ = std::make_unique(this); } std::vector BehaviorPathPlannerNode::getWaitingApprovalModules() @@ -413,6 +415,10 @@ void BehaviorPathPlannerNode::run() if (!path->points.empty()) { path_publisher_->publish(*path); + + // Publish published time only if there are subscribers more than 1 + 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..3791141dcc8df 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -159,6 +159,8 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio } logger_configure_ = std::make_unique(this); + + published_time_publisher_ = std::make_unique(this); } void BehaviorVelocityPlannerNode::onLoadPlugin( @@ -399,6 +401,8 @@ void BehaviorVelocityPlannerNode::onTrigger( lk.unlock(); path_pub_->publish(output_path_msg); + // Publish published time only if there are subscribers more than 1 + 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..6d68189a7672c 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,8 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions clock_ = get_clock(); logger_configure_ = std::make_unique(this); + + published_time_publisher_ = std::make_unique(this); } void MotionVelocitySmootherNode::setupSmoother(const double wheelbase) @@ -314,6 +316,8 @@ 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); + // Publish published time only if there are subscribers more than 1 + 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..11d0c46021edb 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -153,6 +153,8 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n std::bind(&ObstacleAvoidancePlanner::onParam, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); + + published_time_publisher_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( @@ -239,6 +241,9 @@ 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); + + // Publish published time only if there are subscribers more than 1 + published_time_publisher_->publish(traj_pub_, output_traj_msg.header.stamp); return; } @@ -271,6 +276,9 @@ 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); + + // Publish published time only if there are subscribers more than 1 + 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..190b53cb7d510 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -442,6 +442,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); + + published_time_publisher_ = std::make_unique(this); } ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlanningAlgorithmType( @@ -538,6 +540,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms trajectory_pub_->publish(output_traj); // 8. Publish debug data + 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..448189ff604b1 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -246,6 +246,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod createSubscriptionOptions(this)); logger_configure_ = std::make_unique(this); + + published_time_publisher_ = std::make_unique(this); } void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) @@ -330,6 +332,9 @@ 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); + + // Publish published time only if there are subscribers more than 1 + published_time_publisher_->publish(pub_trajectory_, input_msg->header.stamp); return; } @@ -381,6 +386,9 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m trajectory.header = input_msg->header; pub_trajectory_->publish(trajectory); + // Publish published time only if there are subscribers more than 1 + 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..818340882321d 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,8 @@ 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_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ObstacleVelocityLimiterNode::onParameter( @@ -222,6 +224,9 @@ void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr pub_trajectory_->publish(safe_trajectory); + // Publish published time only if there are subscribers more than 1 + 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/README.md b/planning/path_smoother/README.md index 3521ba754081f..7bcc6ef70731c 100644 --- a/planning/path_smoother/README.md +++ b/planning/path_smoother/README.md @@ -9,3 +9,5 @@ This package contains code to smooth a path or trajectory. ### Elastic Band More details about the elastic band can be found [here](docs/eb.md). + +## Node parameters 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..639707a857448 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -108,6 +108,8 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option std::bind(&ElasticBandSmoother::onParam, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); + + published_time_publisher_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( @@ -171,6 +173,8 @@ 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); + // Publish published time only if there are subscribers more than 1 + published_time_publisher_->publish(path_pub_, path_ptr->header.stamp); return; } @@ -225,6 +229,8 @@ 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); + // Publish published time only if there are subscribers more than 1 + 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..5747973b321c6 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -48,6 +48,8 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) setupParameters(); logger_configure_ = std::make_unique(this); + + published_time_publisher_ = std::make_unique(this); } void PlanningValidator::setupParameters() @@ -219,6 +221,10 @@ void PlanningValidator::publishTrajectory() // Validation check is all green. Publish the trajectory. if (isAllValid(validation_status_)) { pub_traj_->publish(*current_trajectory_); + + // Publish published time only if there are subscribers more than 1 + published_time_publisher_->publish(pub_traj_, current_trajectory_->header.stamp); + previous_published_trajectory_ = current_trajectory_; return; } @@ -227,6 +233,10 @@ void PlanningValidator::publishTrajectory() if (invalid_trajectory_handling_type_ == InvalidTrajectoryHandlingType::PUBLISH_AS_IT_IS) { pub_traj_->publish(*current_trajectory_); + + // Publish published time only if there are subscribers more than 1 + published_time_publisher_->publish(pub_traj_, current_trajectory_->header.stamp); + RCLCPP_ERROR(get_logger(), "Caution! Invalid Trajectory published."); return; } @@ -239,6 +249,10 @@ void PlanningValidator::publishTrajectory() if (invalid_trajectory_handling_type_ == InvalidTrajectoryHandlingType::USE_PREVIOUS_RESULT) { if (previous_published_trajectory_) { pub_traj_->publish(*previous_published_trajectory_); + + // Publish published time only if there are subscribers more than 1 + 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..c1534a7272121 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,10 @@ void ScenarioSelectorNode::publishTrajectory( const auto delay_sec = (now - msg->header.stamp).seconds(); if (delay_sec <= th_max_message_delay_sec_) { pub_trajectory_->publish(*msg); + + // Publish published time only if there are subscribers more than 1 + 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 +376,8 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); + + published_time_publisher_ = std::make_unique(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 c4364ce1b06e3..daad22763fdf2 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -102,6 +102,8 @@ 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_ = std::make_unique(this); + RCLCPP_DEBUG(this->get_logger(), "[Filter Constructor] successfully created."); } @@ -192,6 +194,9 @@ void pointcloud_preprocessor::Filter::computePublish( // Publish a boost shared ptr pub_output_->publish(std::move(output)); + + // Publish published time only if there are subscribers more than 1 + published_time_publisher_->publish(pub_output_, input->header.stamp); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -456,6 +461,9 @@ void pointcloud_preprocessor::Filter::faster_input_indices_callback( output->header.stamp = cloud->header.stamp; pub_output_->publish(std::move(output)); + + // Publish published time only if there are subscribers more than 1 + published_time_publisher_->publish(pub_output_, cloud->header.stamp); } // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform From 187f73d10c1fbd7c9e2ca4185879bf3587a4dad9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Wed, 13 Mar 2024 15:30:03 +0300 Subject: [PATCH 2/4] add `#include ` to missing files MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: M. Fatih Cırıt --- .../detected_object_feature_remover.hpp | 2 ++ .../detected_object_filter/object_position_filter.hpp | 1 + 2 files changed, 3 insertions(+) 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 d14952cdda54e..8476cd1262a58 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 @@ -21,6 +21,8 @@ #include #include +#include + namespace detected_object_feature_remover { using autoware_auto_perception_msgs::msg::DetectedObjects; 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 9cc9c51747c52..fda36f2c6b6d9 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 @@ -27,6 +27,7 @@ #include #include +#include #include namespace object_position_filter From 752bb0b1eb93d8a590aec1754dbfb78d10816034 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Thu, 14 Mar 2024 13:21:49 +0300 Subject: [PATCH 3/4] feat: change publish() function name and improve readability Signed-off-by: Berkay Karaman --- .../ros/published_time_publisher.hpp | 5 +++-- .../src/controller_node.cpp | 6 ++---- .../vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 9 ++------- .../detected_object_feature_remover/README.md | 2 ++ .../src/detected_object_feature_remover.cpp | 5 +---- .../src/object_lanelet_filter.cpp | 5 +---- .../src/object_position_filter.cpp | 5 +---- .../src/obstacle_pointcloud_based_validator.cpp | 6 +----- .../src/occupancy_grid_based_validator.cpp | 5 +---- .../src/detection_by_tracker_core.cpp | 10 ++-------- perception/lidar_centerpoint/src/node.cpp | 5 +---- .../map_based_prediction_node.hpp | 4 ---- .../src/map_based_prediction_node.cpp | 6 +----- .../src/multi_object_tracker_core.cpp | 5 +---- .../src/object_association_merger/node.cpp | 5 +---- ...ccupancy_grid_map_outlier_filter_nodelet.cpp | 6 ++---- perception/shape_estimation/src/node.cpp | 4 +--- .../src/decorative_tracker_merger.cpp | 7 ++----- .../src/behavior_path_planner_node.cpp | 6 +----- planning/behavior_velocity_planner/src/node.cpp | 4 +--- .../src/motion_velocity_smoother_node.cpp | 5 ++--- .../obstacle_avoidance_planner/src/node.cpp | 9 ++------- planning/obstacle_cruise_planner/src/node.cpp | 3 +-- planning/obstacle_stop_planner/src/node.cpp | 9 ++------- .../src/obstacle_velocity_limiter_node.cpp | 5 +---- planning/path_smoother/README.md | 2 -- .../path_smoother/src/elastic_band_smoother.cpp | 7 ++----- .../src/planning_validator.cpp | 17 ++++------------- .../scenario_selector_node.cpp | 6 +----- sensing/pointcloud_preprocessor/src/filter.cpp | 9 ++------- 30 files changed, 44 insertions(+), 138 deletions(-) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp index 2b6c2997f2caf..e8546e2db2b9f 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp @@ -37,7 +37,8 @@ class PublishedTimePublisher { } - void publish(const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp) + void publish_if_subscribed( + const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp) { const auto & gid_key = publisher->get_gid(); @@ -57,7 +58,7 @@ class PublishedTimePublisher } } - void publish( + void publish_if_subscribed( const rclcpp::PublisherBase::ConstSharedPtr & publisher, const std_msgs::msg::Header & header) { const auto & gid_key = publisher->get_gid(); diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index c769e3127b65a..801f42ad9a470 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -233,10 +233,8 @@ void Controller::callbackTimerControl() out.longitudinal = lon_out.control_cmd; control_cmd_pub_->publish(out); - // 6. publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(control_cmd_pub_, out.stamp); - - // 7. publish debug marker + // 6. publish debug + published_time_publisher_->publish_if_subscribed(control_cmd_pub_, out.stamp); 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 ef0c179f4afc3..27592b9405fc5 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -458,14 +458,9 @@ 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 there are subscribers more than 1 - published_time_publisher_->publish(control_cmd_pub_, filtered_commands.control.stamp); - - // Publish pause state to api + published_time_publisher_->publish_if_subscribed( + control_cmd_pub_, filtered_commands.control.stamp); 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/perception/detected_object_feature_remover/README.md b/perception/detected_object_feature_remover/README.md index 5395bc9bdde70..21a8bd9a00541 100644 --- a/perception/detected_object_feature_remover/README.md +++ b/perception/detected_object_feature_remover/README.md @@ -22,4 +22,6 @@ The `detected_object_feature_remover` is a package to convert topic-type from `D ## Parameters +None + ## Assumptions / Known limits 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 f28453980fb54..c5f977a4a3354 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,7 +23,6 @@ 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_ = std::make_unique(this); } @@ -33,9 +32,7 @@ void DetectedObjectFeatureRemover::objectCallback( DetectedObjects output; convert(*input, output); pub_->publish(output); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(pub_, output.header.stamp); + published_time_publisher_->publish_if_subscribed(pub_, output.header.stamp); } void DetectedObjectFeatureRemover::convert( diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 1c0d8ffd65a2c..50a81e95d5a9b 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -55,7 +55,6 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod debug_publisher_ = std::make_unique(this, "object_lanelet_filter"); - published_time_publisher_ = std::make_unique(this); } @@ -121,9 +120,7 @@ void ObjectLaneletFilterNode::objectCallback( ++index; } object_pub_->publish(output_object_msg); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(object_pub_, output_object_msg.header.stamp); + published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp); // Publish debug info const double pipeline_latency = diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp index 76a6aa1858d23..eb3b42c83f125 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -42,7 +42,6 @@ ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & n "input/object", rclcpp::QoS{1}, std::bind(&ObjectPositionFilterNode::objectCallback, this, _1)); object_pub_ = this->create_publisher( "output/object", rclcpp::QoS{1}); - published_time_publisher_ = std::make_unique(this); } @@ -67,9 +66,7 @@ void ObjectPositionFilterNode::objectCallback( } object_pub_->publish(output_object_msg); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(object_pub_, output_object_msg.header.stamp); + published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp); } bool ObjectPositionFilterNode::isObjectInBounds( diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index 8fdd7c7976f33..e39de639a6121 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -309,7 +309,6 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); - published_time_publisher_ = std::make_unique(this); } void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( @@ -349,10 +348,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( } objects_pub_->publish(output); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(objects_pub_, output.header.stamp); - + published_time_publisher_->publish_if_subscribed(objects_pub_, output.header.stamp); if (debugger_) { debugger_->publishRemovedObjects(removed_objects); debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header); diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp index 801010017bb2f..93bdb8eeee74c 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -52,7 +52,6 @@ OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptio mean_threshold_ = declare_parameter("mean_threshold", 0.6); enable_debug_ = declare_parameter("enable_debug", false); - published_time_publisher_ = std::make_unique(this); } @@ -87,9 +86,7 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid( } objects_pub_->publish(output); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(objects_pub_, output.header.stamp); + published_time_publisher_->publish_if_subscribed(objects_pub_, output.header.stamp); if (enable_debug_) showDebugImage(*input_occ_grid, transformed_objects, occ_grid); } diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 4e0496bd8c528..b1cc97ef3c391 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -177,7 +177,6 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); - published_time_publisher_ = std::make_unique(this); } @@ -223,10 +222,7 @@ void DetectionByTracker::onObjects( !object_recognition_utils::transformObjects( objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) { objects_pub_->publish(detected_objects); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(objects_pub_, detected_objects.header.stamp); - + published_time_publisher_->publish_if_subscribed(objects_pub_, detected_objects.header.stamp); return; } // to simplify post processes, convert tracked_objects to DetectedObjects message. @@ -257,10 +253,8 @@ void DetectionByTracker::onObjects( } objects_pub_->publish(detected_objects); + published_time_publisher_->publish_if_subscribed(objects_pub_, detected_objects.header.stamp); debugger_->publishProcessingTime(); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(objects_pub_, detected_objects.header.stamp); } void DetectionByTracker::divideUnderSegmentedObjects( diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index 9907ae42ce3ce..65bdafe94fe7d 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -126,7 +126,6 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); } - published_time_publisher_ = std::make_unique(this); } @@ -165,9 +164,7 @@ void LidarCenterPointNode::pointCloudCallback( if (objects_sub_count > 0) { objects_pub_->publish(output_msg); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(objects_pub_, output_msg.header.stamp); + published_time_publisher_->publish_if_subscribed(objects_pub_, output_msg.header.stamp); } // add processing time for debug diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index e3a1276c8dd40..829c4d6e4a114 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 @@ -200,10 +200,6 @@ class MapBasedPredictionNode : public rclcpp::Node std::vector distance_set_for_no_intention_to_walk_; std::vector timeout_set_for_no_intention_to_walk_; - // Stop watch - StopWatch stop_watch_; - - // PublishedTime Publisher std::unique_ptr published_time_publisher_; // Member Functions 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 e5726715a0c73..e12b7c54c3a9f 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -812,7 +812,6 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ std::make_unique(this, "map_based_prediction"); published_time_publisher_ = std::make_unique(this); - set_param_res_ = this->add_on_set_parameters_callback( std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); @@ -1114,10 +1113,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Publish Results pub_objects_->publish(output); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(pub_objects_, output.header.stamp); - + published_time_publisher_->publish_if_subscribed(pub_objects_, output.header.stamp); pub_debug_markers_->publish(debug_markers); const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true); const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); 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 8632d5efdcf5e..fa609ed9ff9c7 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -236,7 +236,6 @@ 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_ = std::make_unique(this); } @@ -470,9 +469,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const // Publish tracked_objects_pub_->publish(output_msg); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(tracked_objects_pub_, output_msg.header.stamp); + published_time_publisher_->publish_if_subscribed(tracked_objects_pub_, output_msg.header.stamp); // Debugger Publish if enabled debugger_->publishProcessingTime(); diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index f3bca450802b7..ec5ad62b52a29 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -125,7 +125,6 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio stop_watch_ptr_ = std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); - published_time_publisher_ = std::make_unique(this); } @@ -225,14 +224,12 @@ void ObjectAssociationMergerNode::objectsCallback( // publish output msg merged_object_pub_->publish(output_msg); + published_time_publisher_->publish_if_subscribed(merged_object_pub_, output_msg.header.stamp); // publish processing time processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(merged_object_pub_, output_msg.header.stamp); } } // namespace object_association 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 2cb5a34b76d61..65eb336269fe4 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,7 +234,6 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( if (enable_debugger) { debugger_ptr_ = std::make_shared(*this); } - published_time_publisher_ = std::make_unique(this); } @@ -330,9 +329,8 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( return; } pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr)); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(pointcloud_pub_, ogm_frame_filtered_pc.header.stamp); + published_time_publisher_->publish_if_subscribed( + 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/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 83823f3383a26..9d7e8b4d4729d 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -58,7 +58,6 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option stop_watch_ptr_ = std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); - published_time_publisher_ = std::make_unique(this); } @@ -122,8 +121,7 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared // Publish pub_->publish(output_msg); - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(pub_, output_msg.header.stamp); + published_time_publisher_->publish_if_subscribed(pub_, output_msg.header.stamp); processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); processing_time_publisher_->publish( diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index e0efc496afd0d..2273ad6504e2a 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -158,7 +158,6 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio stop_watch_ptr_ = std::make_unique>(); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); - published_time_publisher_ = std::make_unique(this); } @@ -224,10 +223,8 @@ void DecorativeTrackerMergerNode::mainObjectsCallback( 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)); - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(merged_object_pub_, tracked_objects.header.stamp); + published_time_publisher_->publish_if_subscribed( + merged_object_pub_, tracked_objects.header.stamp); processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); processing_time_publisher_->publish( 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 f170cf4d6d401..ad0e4f94caf75 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -172,7 +172,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod } logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); } @@ -415,10 +414,7 @@ void BehaviorPathPlannerNode::run() if (!path->points.empty()) { path_publisher_->publish(*path); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(path_publisher_, path->header.stamp); - + published_time_publisher_->publish_if_subscribed(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 3791141dcc8df..aa9613a35dd7c 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -159,7 +159,6 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio } logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); } @@ -401,8 +400,7 @@ void BehaviorVelocityPlannerNode::onTrigger( lk.unlock(); path_pub_->publish(output_path_msg); - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(path_pub_, output_path_msg.header.stamp); + published_time_publisher_->publish_if_subscribed(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/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 6d68189a7672c..eb8592bb99637 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -104,7 +104,6 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions clock_ = get_clock(); logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); } @@ -316,8 +315,8 @@ 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); - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(pub_trajectory_, publishing_trajectory.header.stamp); + published_time_publisher_->publish_if_subscribed( + pub_trajectory_, publishing_trajectory.header.stamp); } void MotionVelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg) diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 11d0c46021edb..c1da988eb5c25 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -153,7 +153,6 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n std::bind(&ObstacleAvoidancePlanner::onParam, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); } @@ -241,9 +240,7 @@ 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); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(traj_pub_, output_traj_msg.header.stamp); + published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); return; } @@ -276,9 +273,7 @@ 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); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(traj_pub_, output_traj_msg.header.stamp); + published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); } bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock clock) const diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 190b53cb7d510..2d1b70d1745f9 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -442,7 +442,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); } @@ -540,7 +539,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms trajectory_pub_->publish(output_traj); // 8. Publish debug data - published_time_publisher_->publish(trajectory_pub_, output_traj.header.stamp); + published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp); publishDebugMarker(); publishDebugInfo(); diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 448189ff604b1..8f57b3e0f051a 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -246,7 +246,6 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod createSubscriptionOptions(this)); logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); } @@ -332,9 +331,7 @@ 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); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(pub_trajectory_, input_msg->header.stamp); + published_time_publisher_->publish_if_subscribed(pub_trajectory_, input_msg->header.stamp); return; } @@ -385,9 +382,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m trajectory.header = input_msg->header; pub_trajectory_->publish(trajectory); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(pub_trajectory_, trajectory.header.stamp); + published_time_publisher_->publish_if_subscribed(pub_trajectory_, trajectory.header.stamp); Float64Stamped processing_time_ms; processing_time_ms.stamp = now(); 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 818340882321d..ea08c2f7cf552 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -82,7 +82,6 @@ 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_ = std::make_unique(this); } @@ -223,9 +222,7 @@ void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr safe_trajectory.header.stamp = now(); pub_trajectory_->publish(safe_trajectory); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(pub_trajectory_, safe_trajectory.header.stamp); + published_time_publisher_->publish_if_subscribed(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); diff --git a/planning/path_smoother/README.md b/planning/path_smoother/README.md index 7bcc6ef70731c..3521ba754081f 100644 --- a/planning/path_smoother/README.md +++ b/planning/path_smoother/README.md @@ -9,5 +9,3 @@ This package contains code to smooth a path or trajectory. ### Elastic Band More details about the elastic band can be found [here](docs/eb.md). - -## Node parameters diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 639707a857448..6fb732309da4b 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -108,7 +108,6 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option std::bind(&ElasticBandSmoother::onParam, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); } @@ -173,8 +172,7 @@ 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); - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(path_pub_, path_ptr->header.stamp); + published_time_publisher_->publish_if_subscribed(path_pub_, path_ptr->header.stamp); return; } @@ -229,8 +227,7 @@ 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); - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(path_pub_, path_ptr->header.stamp); + published_time_publisher_->publish_if_subscribed(path_pub_, path_ptr->header.stamp); } bool ElasticBandSmoother::isDataReady(const Path & path, rclcpp::Clock clock) const diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 5747973b321c6..1e5f6fc53acf0 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -48,7 +48,6 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) setupParameters(); logger_configure_ = std::make_unique(this); - published_time_publisher_ = std::make_unique(this); } @@ -221,10 +220,7 @@ void PlanningValidator::publishTrajectory() // Validation check is all green. Publish the trajectory. if (isAllValid(validation_status_)) { pub_traj_->publish(*current_trajectory_); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(pub_traj_, current_trajectory_->header.stamp); - + published_time_publisher_->publish_if_subscribed(pub_traj_, current_trajectory_->header.stamp); previous_published_trajectory_ = current_trajectory_; return; } @@ -233,10 +229,7 @@ void PlanningValidator::publishTrajectory() if (invalid_trajectory_handling_type_ == InvalidTrajectoryHandlingType::PUBLISH_AS_IT_IS) { pub_traj_->publish(*current_trajectory_); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(pub_traj_, current_trajectory_->header.stamp); - + published_time_publisher_->publish_if_subscribed(pub_traj_, current_trajectory_->header.stamp); RCLCPP_ERROR(get_logger(), "Caution! Invalid Trajectory published."); return; } @@ -249,10 +242,8 @@ void PlanningValidator::publishTrajectory() if (invalid_trajectory_handling_type_ == InvalidTrajectoryHandlingType::USE_PREVIOUS_RESULT) { if (previous_published_trajectory_) { pub_traj_->publish(*previous_published_trajectory_); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(pub_traj_, previous_published_trajectory_->header.stamp); - + published_time_publisher_->publish_if_subscribed( + pub_traj_, previous_published_trajectory_->header.stamp); RCLCPP_ERROR(get_logger(), "Invalid Trajectory detected. Use previous trajectory."); return; } 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 c1534a7272121..7cc251a03296c 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,10 +320,7 @@ void ScenarioSelectorNode::publishTrajectory( const auto delay_sec = (now - msg->header.stamp).seconds(); if (delay_sec <= th_max_message_delay_sec_) { pub_trajectory_->publish(*msg); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(pub_trajectory_, msg->header.stamp); - + published_time_publisher_->publish_if_subscribed(pub_trajectory_, msg->header.stamp); } else { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), @@ -376,7 +373,6 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); - published_time_publisher_ = std::make_unique(this); } diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index daad22763fdf2..5584fd65c9acb 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -103,7 +103,6 @@ pointcloud_preprocessor::Filter::Filter( std::bind(&Filter::filterParamCallback, this, std::placeholders::_1)); published_time_publisher_ = std::make_unique(this); - RCLCPP_DEBUG(this->get_logger(), "[Filter Constructor] successfully created."); } @@ -194,9 +193,7 @@ void pointcloud_preprocessor::Filter::computePublish( // Publish a boost shared ptr pub_output_->publish(std::move(output)); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(pub_output_, input->header.stamp); + published_time_publisher_->publish_if_subscribed(pub_output_, input->header.stamp); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -461,9 +458,7 @@ void pointcloud_preprocessor::Filter::faster_input_indices_callback( output->header.stamp = cloud->header.stamp; pub_output_->publish(std::move(output)); - - // Publish published time only if there are subscribers more than 1 - published_time_publisher_->publish(pub_output_, cloud->header.stamp); + published_time_publisher_->publish_if_subscribed(pub_output_, cloud->header.stamp); } // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform From b1724f540546d2961ddccba90089ce368b0c56ce Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Thu, 14 Mar 2024 13:50:51 +0300 Subject: [PATCH 4/4] feat: add missed package dependency Signed-off-by: Berkay Karaman --- control/trajectory_follower_node/package.xml | 1 + control/vehicle_cmd_gate/package.xml | 1 + planning/scenario_selector/package.xml | 1 + 3 files changed, 3 insertions(+) diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml index 34e9fb45424a7..36b4d0108de78 100644 --- a/control/trajectory_follower_node/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -30,6 +30,7 @@ pure_pursuit rclcpp rclcpp_components + tier4_autoware_utils trajectory_follower_base vehicle_info_util visualization_msgs diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index deb988d61738f..22ae9da6d222e 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -28,6 +28,7 @@ rclcpp_components std_srvs tier4_api_utils + tier4_autoware_utils tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs diff --git a/planning/scenario_selector/package.xml b/planning/scenario_selector/package.xml index 53c29613a1de8..b985cac0b6ae7 100644 --- a/planning/scenario_selector/package.xml +++ b/planning/scenario_selector/package.xml @@ -27,6 +27,7 @@ tf2 tf2_geometry_msgs tf2_ros + tier4_autoware_utils tier4_planning_msgs ros2cli