Skip to content

Commit 92ca393

Browse files
brkay54brkay54
committed
feat: add published_time publisher debug to packages
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
1 parent b7fdccc commit 92ca393

File tree

55 files changed

+239
-4
lines changed

Some content is hidden

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

55 files changed

+239
-4
lines changed

‎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/src/controller_node.cpp

+6-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,10 @@ 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 published time only if there are subscribers more than 1
237+
published_time_publisher_->publish(control_cmd_pub_, out.stamp);
238+
239+
// 7. publish debug marker
235240
publishDebugMarker(*input_data, lat_out);
236241
}
237242

‎control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

+9
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,7 +458,14 @@ 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+
462+
// Publish published time only if there are subscribers more than 1
463+
published_time_publisher_->publish(control_cmd_pub_, filtered_commands.control.stamp);
464+
465+
// Publish pause state to api
459466
adapi_pause_->publish();
467+
468+
// Publish moderate stop state which is used for stop request
460469
moderate_stop_interface_->publish();
461470

462471
// Save ControlCmd to steering angle when disengaged

‎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/README.md

-2
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,4 @@ The `detected_object_feature_remover` is a package to convert topic-type from `D
2222

2323
## Parameters
2424

25-
None
26-
2725
## Assumptions / Known limits

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

+2
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
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>
@@ -33,6 +34,7 @@ class DetectedObjectFeatureRemover : public rclcpp::Node
3334
private:
3435
rclcpp::Subscription<DetectedObjectsWithFeature>::SharedPtr sub_;
3536
rclcpp::Publisher<DetectedObjects>::SharedPtr pub_;
37+
std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
3638
void objectCallback(const DetectedObjectsWithFeature::ConstSharedPtr input);
3739
void convert(const DetectedObjectsWithFeature & objs_with_feature, DetectedObjects & objs);
3840
};

‎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

+5
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,8 @@ 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+
27+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
2628
}
2729

2830
void DetectedObjectFeatureRemover::objectCallback(
@@ -31,6 +33,9 @@ void DetectedObjectFeatureRemover::objectCallback(
3133
DetectedObjects output;
3234
convert(*input, output);
3335
pub_->publish(output);
36+
37+
// Publish published time only if there are subscribers more than 1
38+
published_time_publisher_->publish(pub_, output.header.stamp);
3439
}
3540

3641
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

+3
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
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>
@@ -51,6 +52,8 @@ class ObjectPositionFilterNode : public rclcpp::Node
5152
float lower_bound_y_;
5253
utils::FilterTargetLabel filter_target_;
5354
bool isObjectInBounds(const autoware_auto_perception_msgs::msg::DetectedObject & object) const;
55+
56+
std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
5457
};
5558

5659
} // 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

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

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

6062
void ObjectLaneletFilterNode::mapCallback(
@@ -120,6 +122,9 @@ void ObjectLaneletFilterNode::objectCallback(
120122
}
121123
object_pub_->publish(output_object_msg);
122124

125+
// Publish published time only if there are subscribers more than 1
126+
published_time_publisher_->publish(object_pub_, output_object_msg.header.stamp);
127+
123128
// Publish debug info
124129
const double pipeline_latency =
125130
std::chrono::duration<double, std::milli>(

‎perception/detected_object_validation/src/object_position_filter.cpp

+5
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,8 @@ 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+
46+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
4547
}
4648

4749
void ObjectPositionFilterNode::objectCallback(
@@ -65,6 +67,9 @@ void ObjectPositionFilterNode::objectCallback(
6567
}
6668

6769
object_pub_->publish(output_object_msg);
70+
71+
// Publish published time only if there are subscribers more than 1
72+
published_time_publisher_->publish(object_pub_, output_object_msg.header.stamp);
6873
}
6974

7075
bool ObjectPositionFilterNode::isObjectInBounds(

‎perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

+6
Original file line numberDiff line numberDiff line change
@@ -309,6 +309,8 @@ 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+
313+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
312314
}
313315
void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
314316
const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
@@ -347,6 +349,10 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
347349
}
348350

349351
objects_pub_->publish(output);
352+
353+
// Publish published time only if there are subscribers more than 1
354+
published_time_publisher_->publish(objects_pub_, output.header.stamp);
355+
350356
if (debugger_) {
351357
debugger_->publishRemovedObjects(removed_objects);
352358
debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header);

‎perception/detected_object_validation/src/occupancy_grid_based_validator.cpp

+5
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,8 @@ 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+
56+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
5557
}
5658

5759
void OccupancyGridBasedValidator::onObjectsAndOccGrid(
@@ -86,6 +88,9 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid(
8688

8789
objects_pub_->publish(output);
8890

91+
// Publish published time only if there are subscribers more than 1
92+
published_time_publisher_->publish(objects_pub_, output.header.stamp);
93+
8994
if (enable_debug_) showDebugImage(*input_occ_grid, transformed_objects, occ_grid);
9095
}
9196

‎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

+9
Original file line numberDiff line numberDiff line change
@@ -177,6 +177,8 @@ 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+
181+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
180182
}
181183

182184
void DetectionByTracker::setMaxSearchRange()
@@ -221,6 +223,10 @@ void DetectionByTracker::onObjects(
221223
!object_recognition_utils::transformObjects(
222224
objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) {
223225
objects_pub_->publish(detected_objects);
226+
227+
// Publish published time only if there are subscribers more than 1
228+
published_time_publisher_->publish(objects_pub_, detected_objects.header.stamp);
229+
224230
return;
225231
}
226232
// to simplify post processes, convert tracked_objects to DetectedObjects message.
@@ -252,6 +258,9 @@ void DetectionByTracker::onObjects(
252258

253259
objects_pub_->publish(detected_objects);
254260
debugger_->publishProcessingTime();
261+
262+
// Publish published time only if there are subscribers more than 1
263+
published_time_publisher_->publish(objects_pub_, detected_objects.header.stamp);
255264
}
256265

257266
void DetectionByTracker::divideUnderSegmentedObjects(

‎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

+5
Original file line numberDiff line numberDiff line change
@@ -126,6 +126,8 @@ 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+
130+
published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
129131
}
130132

131133
void LidarCenterPointNode::pointCloudCallback(
@@ -163,6 +165,9 @@ void LidarCenterPointNode::pointCloudCallback(
163165

164166
if (objects_sub_count > 0) {
165167
objects_pub_->publish(output_msg);
168+
169+
// Publish published time only if there are subscribers more than 1
170+
published_time_publisher_->publish(objects_pub_, output_msg.header.stamp);
166171
}
167172

168173
// add processing time for debug

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

+7
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,12 @@ 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+
// Stop watch
204+
StopWatch<std::chrono::milliseconds> stop_watch_;
205+
206+
// PublishedTime Publisher
207+
std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
208+
202209
// Member Functions
203210
void mapCallback(const HADMapBin::ConstSharedPtr msg);
204211
void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg);

‎perception/map_based_prediction/src/map_based_prediction_node.cpp

+6
Original file line numberDiff line numberDiff line change
@@ -811,6 +811,8 @@ 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);
815+
814816
set_param_res_ = this->add_on_set_parameters_callback(
815817
std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1));
816818

@@ -1112,6 +1114,10 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
11121114

11131115
// Publish Results
11141116
pub_objects_->publish(output);
1117+
1118+
// Publish published time only if there are subscribers more than 1
1119+
published_time_publisher_->publish(pub_objects_, output.header.stamp);
1120+
11151121
pub_debug_markers_->publish(debug_markers);
11161122
const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
11171123
const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);

0 commit comments

Comments
 (0)