Skip to content

Commit 7f36c52

Browse files
brkay54brkay54
authored
feat: add published_time publisher debug to packages (#6490)
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
1 parent 152b11e commit 7f36c52

File tree

57 files changed

+153
-6
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

57 files changed

+153
-6
lines changed

‎common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,8 @@ class PublishedTimePublisher
3737
{
3838
}
3939

40-
void publish(const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp)
40+
void publish_if_subscribed(
41+
const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp)
4142
{
4243
const auto & gid_key = publisher->get_gid();
4344

@@ -57,7 +58,7 @@ class PublishedTimePublisher
5758
}
5859
}
5960

60-
void publish(
61+
void publish_if_subscribed(
6162
const rclcpp::PublisherBase::ConstSharedPtr & publisher, const std_msgs::msg::Header & header)
6263
{
6364
const auto & gid_key = publisher->get_gid();

‎control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828

2929
#include <Eigen/Core>
3030
#include <Eigen/Geometry>
31+
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
3132

3233
#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
3334
#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp"
@@ -121,6 +122,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
121122

122123
std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
123124

125+
std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
126+
124127
void publishProcessingTime(
125128
const double t_ms, const rclcpp::Publisher<Float64Stamped>::SharedPtr pub);
126129
StopWatch<std::chrono::milliseconds> stop_watch_;

‎control/trajectory_follower_node/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@
3030
<depend>pure_pursuit</depend>
3131
<depend>rclcpp</depend>
3232
<depend>rclcpp_components</depend>
33+
<depend>tier4_autoware_utils</depend>
3334
<depend>trajectory_follower_base</depend>
3435
<depend>vehicle_info_util</depend>
3536
<depend>visualization_msgs</depend>

‎control/trajectory_follower_node/src/controller_node.cpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
9191
}
9292

9393
logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
94+
95+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
9496
}
9597

9698
Controller::LateralControllerMode Controller::getLateralControllerMode(
@@ -231,7 +233,8 @@ void Controller::callbackTimerControl()
231233
out.longitudinal = lon_out.control_cmd;
232234
control_cmd_pub_->publish(out);
233235

234-
// 6. publish debug marker
236+
// 6. publish debug
237+
published_time_publisher_->publish_if_subscribed(control_cmd_pub_, out.stamp);
235238
publishDebugMarker(*input_data, lat_out);
236239
}
237240

‎control/vehicle_cmd_gate/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828
<depend>rclcpp_components</depend>
2929
<depend>std_srvs</depend>
3030
<depend>tier4_api_utils</depend>
31+
<depend>tier4_autoware_utils</depend>
3132
<depend>tier4_control_msgs</depend>
3233
<depend>tier4_debug_msgs</depend>
3334
<depend>tier4_external_api_msgs</depend>

‎control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

+4
Original file line numberDiff line numberDiff line change
@@ -242,6 +242,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
242242
this, get_clock(), period_ns, std::bind(&VehicleCmdGate::publishStatus, this));
243243

244244
logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
245+
246+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
245247
}
246248

247249
bool VehicleCmdGate::isHeartbeatTimeout(
@@ -456,6 +458,8 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
456458
// Publish commands
457459
vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency);
458460
control_cmd_pub_->publish(filtered_commands.control);
461+
published_time_publisher_->publish_if_subscribed(
462+
control_cmd_pub_, filtered_commands.control.stamp);
459463
adapi_pause_->publish();
460464
moderate_stop_interface_->publish();
461465

‎control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include <diagnostic_updater/diagnostic_updater.hpp>
2424
#include <motion_utils/vehicle/vehicle_state_checker.hpp>
2525
#include <rclcpp/rclcpp.hpp>
26+
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
2627
#include <vehicle_cmd_gate/msg/is_filter_activated.hpp>
2728
#include <vehicle_info_util/vehicle_info_util.hpp>
2829

@@ -248,6 +249,8 @@ class VehicleCmdGate : public rclcpp::Node
248249
void publishMarkers(const IsFilterActivated & filter_activated);
249250

250251
std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
252+
253+
std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
251254
};
252255

253256
} // namespace vehicle_cmd_gate

‎perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -16,10 +16,13 @@
1616
#define DETECTED_OBJECT_FEATURE_REMOVER__DETECTED_OBJECT_FEATURE_REMOVER_HPP_
1717

1818
#include <rclcpp/rclcpp.hpp>
19+
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
1920

2021
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
2122
#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
2223

24+
#include <memory>
25+
2326
namespace detected_object_feature_remover
2427
{
2528
using autoware_auto_perception_msgs::msg::DetectedObjects;
@@ -33,6 +36,7 @@ class DetectedObjectFeatureRemover : public rclcpp::Node
3336
private:
3437
rclcpp::Subscription<DetectedObjectsWithFeature>::SharedPtr sub_;
3538
rclcpp::Publisher<DetectedObjects>::SharedPtr pub_;
39+
std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
3640
void objectCallback(const DetectedObjectsWithFeature::ConstSharedPtr input);
3741
void convert(const DetectedObjectsWithFeature & objs_with_feature, DetectedObjects & objs);
3842
};

‎perception/detected_object_feature_remover/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
<depend>autoware_auto_perception_msgs</depend>
1414
<depend>rclcpp</depend>
1515
<depend>rclcpp_components</depend>
16+
<depend>tier4_autoware_utils</depend>
1617
<depend>tier4_perception_msgs</depend>
1718

1819
<test_depend>ament_cmake_ros</test_depend>

‎perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ DetectedObjectFeatureRemover::DetectedObjectFeatureRemover(const rclcpp::NodeOpt
2323
pub_ = this->create_publisher<DetectedObjects>("~/output", rclcpp::QoS(1));
2424
sub_ = this->create_subscription<DetectedObjectsWithFeature>(
2525
"~/input", 1, std::bind(&DetectedObjectFeatureRemover::objectCallback, this, _1));
26+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
2627
}
2728

2829
void DetectedObjectFeatureRemover::objectCallback(
@@ -31,6 +32,7 @@ void DetectedObjectFeatureRemover::objectCallback(
3132
DetectedObjects output;
3233
convert(*input, output);
3334
pub_->publish(output);
35+
published_time_publisher_->publish_if_subscribed(pub_, output.header.stamp);
3436
}
3537

3638
void DetectedObjectFeatureRemover::convert(

‎perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include <rclcpp/rclcpp.hpp>
2121
#include <tier4_autoware_utils/geometry/geometry.hpp>
2222
#include <tier4_autoware_utils/ros/debug_publisher.hpp>
23+
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
2324

2425
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
2526
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
@@ -68,6 +69,8 @@ class ObjectLaneletFilterNode : public rclcpp::Node
6869
bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &);
6970
geometry_msgs::msg::Polygon setFootprint(
7071
const autoware_auto_perception_msgs::msg::DetectedObject &);
72+
73+
std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
7174
};
7275

7376
} // namespace object_lanelet_filter

‎perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -19,13 +19,15 @@
1919

2020
#include <rclcpp/rclcpp.hpp>
2121
#include <tier4_autoware_utils/geometry/geometry.hpp>
22+
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
2223

2324
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
2425
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
2526

2627
#include <tf2_ros/buffer.h>
2728
#include <tf2_ros/transform_listener.h>
2829

30+
#include <memory>
2931
#include <string>
3032

3133
namespace object_position_filter
@@ -51,6 +53,8 @@ class ObjectPositionFilterNode : public rclcpp::Node
5153
float lower_bound_y_;
5254
utils::FilterTargetLabel filter_target_;
5355
bool isObjectInBounds(const autoware_auto_perception_msgs::msg::DetectedObject & object) const;
56+
57+
std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
5458
};
5559

5660
} // namespace object_position_filter

‎perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121

2222
#include <rclcpp/rclcpp.hpp>
2323
#include <tier4_autoware_utils/ros/debug_publisher.hpp>
24+
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
2425

2526
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
2627
#include <sensor_msgs/msg/point_cloud2.hpp>
@@ -148,6 +149,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node
148149
std::shared_ptr<Debugger> debugger_;
149150
bool using_2d_validator_;
150151
std::unique_ptr<Validator> validator_;
152+
std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
151153

152154
private:
153155
void onObjectsAndObstaclePointCloud(

‎perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <opencv2/imgproc/imgproc.hpp>
2020
#include <opencv2/opencv.hpp>
2121
#include <rclcpp/rclcpp.hpp>
22+
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
2223

2324
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
2425
#include <nav_msgs/msg/occupancy_grid.hpp>
@@ -42,6 +43,7 @@ class OccupancyGridBasedValidator : public rclcpp::Node
4243
message_filters::Subscriber<nav_msgs::msg::OccupancyGrid> occ_grid_sub_;
4344
tf2_ros::Buffer tf_buffer_;
4445
tf2_ros::TransformListener tf_listener_;
46+
std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
4547

4648
typedef message_filters::sync_policies::ApproximateTime<
4749
autoware_auto_perception_msgs::msg::DetectedObjects, nav_msgs::msg::OccupancyGrid>

‎perception/detected_object_validation/src/object_lanelet_filter.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,7 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
5555

5656
debug_publisher_ =
5757
std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "object_lanelet_filter");
58+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
5859
}
5960

6061
void ObjectLaneletFilterNode::mapCallback(
@@ -119,6 +120,7 @@ void ObjectLaneletFilterNode::objectCallback(
119120
++index;
120121
}
121122
object_pub_->publish(output_object_msg);
123+
published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp);
122124

123125
// Publish debug info
124126
const double pipeline_latency =

‎perception/detected_object_validation/src/object_position_filter.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@ ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & n
4242
"input/object", rclcpp::QoS{1}, std::bind(&ObjectPositionFilterNode::objectCallback, this, _1));
4343
object_pub_ = this->create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
4444
"output/object", rclcpp::QoS{1});
45+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
4546
}
4647

4748
void ObjectPositionFilterNode::objectCallback(
@@ -65,6 +66,7 @@ void ObjectPositionFilterNode::objectCallback(
6566
}
6667

6768
object_pub_->publish(output_object_msg);
69+
published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp);
6870
}
6971

7072
bool ObjectPositionFilterNode::isObjectInBounds(

‎perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -309,6 +309,7 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
309309

310310
const bool enable_debugger = declare_parameter<bool>("enable_debugger", false);
311311
if (enable_debugger) debugger_ = std::make_shared<Debugger>(this);
312+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
312313
}
313314
void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
314315
const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
@@ -347,6 +348,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
347348
}
348349

349350
objects_pub_->publish(output);
351+
published_time_publisher_->publish_if_subscribed(objects_pub_, output.header.stamp);
350352
if (debugger_) {
351353
debugger_->publishRemovedObjects(removed_objects);
352354
debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header);

‎perception/detected_object_validation/src/occupancy_grid_based_validator.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@ OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptio
5252

5353
mean_threshold_ = declare_parameter<float>("mean_threshold", 0.6);
5454
enable_debug_ = declare_parameter<bool>("enable_debug", false);
55+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
5556
}
5657

5758
void OccupancyGridBasedValidator::onObjectsAndOccGrid(
@@ -85,6 +86,7 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid(
8586
}
8687

8788
objects_pub_->publish(output);
89+
published_time_publisher_->publish_if_subscribed(objects_pub_, output.header.stamp);
8890

8991
if (enable_debug_) showDebugImage(*input_occ_grid, transformed_objects, occ_grid);
9092
}

‎perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
#include <euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp>
2424
#include <rclcpp/rclcpp.hpp>
2525
#include <shape_estimation/shape_estimator.hpp>
26+
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
2627

2728
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
2829
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
@@ -83,6 +84,8 @@ class DetectionByTracker : public rclcpp::Node
8384

8485
detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_;
8586

87+
std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
88+
8689
void setMaxSearchRange();
8790

8891
void onObjects(

‎perception/detection_by_tracker/src/detection_by_tracker_core.cpp

+3
Original file line numberDiff line numberDiff line change
@@ -177,6 +177,7 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options)
177177
cluster_ = std::make_shared<euclidean_cluster::VoxelGridBasedEuclideanCluster>(
178178
false, 10, 10000, 0.7, 0.3, 0);
179179
debugger_ = std::make_shared<Debugger>(this);
180+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
180181
}
181182

182183
void DetectionByTracker::setMaxSearchRange()
@@ -221,6 +222,7 @@ void DetectionByTracker::onObjects(
221222
!object_recognition_utils::transformObjects(
222223
objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) {
223224
objects_pub_->publish(detected_objects);
225+
published_time_publisher_->publish_if_subscribed(objects_pub_, detected_objects.header.stamp);
224226
return;
225227
}
226228
// to simplify post processes, convert tracked_objects to DetectedObjects message.
@@ -251,6 +253,7 @@ void DetectionByTracker::onObjects(
251253
}
252254

253255
objects_pub_->publish(detected_objects);
256+
published_time_publisher_->publish_if_subscribed(objects_pub_, detected_objects.header.stamp);
254257
debugger_->publishProcessingTime();
255258
}
256259

‎perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include <lidar_centerpoint/detection_class_remapper.hpp>
2222
#include <rclcpp/rclcpp.hpp>
2323
#include <tier4_autoware_utils/ros/debug_publisher.hpp>
24+
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
2425
#include <tier4_autoware_utils/system/stop_watch.hpp>
2526

2627
#include <autoware_auto_perception_msgs/msg/detected_object_kinematics.hpp>
@@ -63,6 +64,8 @@ class LidarCenterPointNode : public rclcpp::Node
6364
std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_{
6465
nullptr};
6566
std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_ptr_{nullptr};
67+
68+
std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
6669
};
6770

6871
} // namespace centerpoint

‎perception/lidar_centerpoint/src/node.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -126,6 +126,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
126126
RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node.");
127127
rclcpp::shutdown();
128128
}
129+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
129130
}
130131

131132
void LidarCenterPointNode::pointCloudCallback(
@@ -163,6 +164,7 @@ void LidarCenterPointNode::pointCloudCallback(
163164

164165
if (objects_sub_count > 0) {
165166
objects_pub_->publish(output_msg);
167+
published_time_publisher_->publish_if_subscribed(objects_pub_, output_msg.header.stamp);
166168
}
167169

168170
// add processing time for debug

‎perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222

2323
#include <rclcpp/rclcpp.hpp>
2424
#include <tier4_autoware_utils/ros/debug_publisher.hpp>
25+
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
2526
#include <tier4_autoware_utils/ros/transform_listener.hpp>
2627
#include <tier4_autoware_utils/ros/uuid_helper.hpp>
2728
#include <tier4_autoware_utils/system/stop_watch.hpp>
@@ -199,6 +200,8 @@ class MapBasedPredictionNode : public rclcpp::Node
199200
std::vector<double> distance_set_for_no_intention_to_walk_;
200201
std::vector<double> timeout_set_for_no_intention_to_walk_;
201202

203+
std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
204+
202205
// Member Functions
203206
void mapCallback(const HADMapBin::ConstSharedPtr msg);
204207
void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg);

‎perception/map_based_prediction/src/map_based_prediction_node.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -811,6 +811,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
811811
processing_time_publisher_ =
812812
std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "map_based_prediction");
813813

814+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
814815
set_param_res_ = this->add_on_set_parameters_callback(
815816
std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1));
816817

@@ -1112,6 +1113,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
11121113

11131114
// Publish Results
11141115
pub_objects_->publish(output);
1116+
published_time_publisher_->publish_if_subscribed(pub_objects_, output.header.stamp);
11151117
pub_debug_markers_->publish(debug_markers);
11161118
const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
11171119
const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);

0 commit comments

Comments
 (0)