Skip to content

Commit f5d2208

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

File tree

76 files changed

+424
-103
lines changed

Some content is hidden

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

76 files changed

+424
-103
lines changed

‎control/trajectory_follower_node/README.md

+1
Original file line numberDiff line numberDiff line change
@@ -146,6 +146,7 @@ Giving the longitudinal controller information about steer convergence allows it
146146
2. The last received commands are not older than defined by `timeout_thr_sec`.
147147
- `lateral_controller_mode`: `mpc` or `pure_pursuit`
148148
- (currently there is only `PID` for longitudinal controller)
149+
- `use_published_time`: Node publishes its control command publishing time with pipeline header file. Default it is false.
149150
150151
## Debugging
151152

‎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

+8-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_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
9496
}
9597

9698
Controller::LateralControllerMode Controller::getLateralControllerMode(
@@ -231,7 +233,12 @@ 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 enabled by parameter
237+
if (published_time_publisher_) {
238+
published_time_publisher_->publish(control_cmd_pub_, out.stamp);
239+
}
240+
241+
// 7. publish debug marker
235242
publishDebugMarker(*input_data, lat_out);
236243
}
237244

‎control/vehicle_cmd_gate/README.md

+1
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,7 @@
6767
| `on_transition.lon_jerk_lim` | <double> | array of limits of longitudinal jerk (activated in TRANSITION operation mode) |
6868
| `on_transition.lat_acc_lim` | <double> | array of limits of lateral acceleration (activated in TRANSITION operation mode) |
6969
| `on_transition.lat_jerk_lim` | <double> | array of limits of lateral jerk (activated in TRANSITION operation mode) |
70+
| `use_published_time` | bool | Node publishes its control command publishing time with pipeline header file. Default it is false. |
7071

7172
## Filter function
7273

‎control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

+11
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_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
245247
}
246248

247249
bool VehicleCmdGate::isHeartbeatTimeout(
@@ -456,7 +458,16 @@ 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 enabled by parameter
463+
if (published_time_publisher_) {
464+
published_time_publisher_->publish(control_cmd_pub_, filtered_commands.control.stamp);
465+
}
466+
467+
// Publish pause state to api
459468
adapi_pause_->publish();
469+
470+
// Publish moderate stop state which is used for stop request
460471
moderate_stop_interface_->publish();
461472

462473
// 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

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

2323
## Parameters
2424

25-
None
25+
| Name | Type | Description |
26+
| -------------------- | ------ | -------------------------------------------------------------------------------------------- |
27+
| `use_published_time` | `bool` | Node publishes its DetectedObjects publishing time with pipeline header file. Default: false |
2628

2729
## 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

+8
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,9 @@ 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 enabled only if the use_published_time is true
28+
published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
2629
}
2730

2831
void DetectedObjectFeatureRemover::objectCallback(
@@ -31,6 +34,11 @@ void DetectedObjectFeatureRemover::objectCallback(
3134
DetectedObjects output;
3235
convert(*input, output);
3336
pub_->publish(output);
37+
38+
// Publish published time if enabled by parameter
39+
if (published_time_publisher_) {
40+
published_time_publisher_->publish(pub_, output.header.stamp);
41+
}
3442
}
3543

3644
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/object-lanelet-filter.md

+11-10
Original file line numberDiff line numberDiff line change
@@ -26,16 +26,17 @@ The objects only inside of the vector map will be published.
2626

2727
### Core Parameters
2828

29-
| Name | Type | Default Value | Description |
30-
| -------------------------------- | ---- | ------------- | ----------------------------------------- |
31-
| `filter_target_label.UNKNOWN` | bool | false | If true, unknown objects are filtered. |
32-
| `filter_target_label.CAR` | bool | false | If true, car objects are filtered. |
33-
| `filter_target_label.TRUCK` | bool | false | If true, truck objects are filtered. |
34-
| `filter_target_label.BUS` | bool | false | If true, bus objects are filtered. |
35-
| `filter_target_label.TRAILER` | bool | false | If true, trailer objects are filtered. |
36-
| `filter_target_label.MOTORCYCLE` | bool | false | If true, motorcycle objects are filtered. |
37-
| `filter_target_label.BICYCLE` | bool | false | If true, bicycle objects are filtered. |
38-
| `filter_target_label.PEDESTRIAN` | bool | false | If true, pedestrian objects are filtered. |
29+
| Name | Type | Default Value | Description |
30+
| -------------------------------- | ---- | ------------- | ----------------------------------------------------------------------------- |
31+
| `filter_target_label.UNKNOWN` | bool | false | If true, unknown objects are filtered. |
32+
| `filter_target_label.CAR` | bool | false | If true, car objects are filtered. |
33+
| `filter_target_label.TRUCK` | bool | false | If true, truck objects are filtered. |
34+
| `filter_target_label.BUS` | bool | false | If true, bus objects are filtered. |
35+
| `filter_target_label.TRAILER` | bool | false | If true, trailer objects are filtered. |
36+
| `filter_target_label.MOTORCYCLE` | bool | false | If true, motorcycle objects are filtered. |
37+
| `filter_target_label.BICYCLE` | bool | false | If true, bicycle objects are filtered. |
38+
| `filter_target_label.PEDESTRIAN` | bool | false | If true, pedestrian objects are filtered. |
39+
| `use_published_time` | bool | false | Node publishes its DetectedObjects publishing time with pipeline header file. |
3940

4041
## Assumptions / Known limits
4142

‎perception/detected_object_validation/object-position-filter.md

+15-14
Original file line numberDiff line numberDiff line change
@@ -25,20 +25,21 @@ The objects only inside of the x, y bound will be published.
2525

2626
### Core Parameters
2727

28-
| Name | Type | Default Value | Description |
29-
| -------------------------------- | ----- | ------------- | --------------------------------------------------------------- |
30-
| `filter_target_label.UNKNOWN` | bool | false | If true, unknown objects are filtered. |
31-
| `filter_target_label.CAR` | bool | false | If true, car objects are filtered. |
32-
| `filter_target_label.TRUCK` | bool | false | If true, truck objects are filtered. |
33-
| `filter_target_label.BUS` | bool | false | If true, bus objects are filtered. |
34-
| `filter_target_label.TRAILER` | bool | false | If true, trailer objects are filtered. |
35-
| `filter_target_label.MOTORCYCLE` | bool | false | If true, motorcycle objects are filtered. |
36-
| `filter_target_label.BICYCLE` | bool | false | If true, bicycle objects are filtered. |
37-
| `filter_target_label.PEDESTRIAN` | bool | false | If true, pedestrian objects are filtered. |
38-
| `upper_bound_x` | float | 100.00 | Bound for filtering. Only used if filter_by_xy_position is true |
39-
| `lower_bound_x` | float | 0.00 | Bound for filtering. Only used if filter_by_xy_position is true |
40-
| `upper_bound_y` | float | 50.00 | Bound for filtering. Only used if filter_by_xy_position is true |
41-
| `lower_bound_y` | float | -50.00 | Bound for filtering. Only used if filter_by_xy_position is true |
28+
| Name | Type | Default Value | Description |
29+
| -------------------------------- | ----- | ------------- | ----------------------------------------------------------------------------- |
30+
| `filter_target_label.UNKNOWN` | bool | false | If true, unknown objects are filtered. |
31+
| `filter_target_label.CAR` | bool | false | If true, car objects are filtered. |
32+
| `filter_target_label.TRUCK` | bool | false | If true, truck objects are filtered. |
33+
| `filter_target_label.BUS` | bool | false | If true, bus objects are filtered. |
34+
| `filter_target_label.TRAILER` | bool | false | If true, trailer objects are filtered. |
35+
| `filter_target_label.MOTORCYCLE` | bool | false | If true, motorcycle objects are filtered. |
36+
| `filter_target_label.BICYCLE` | bool | false | If true, bicycle objects are filtered. |
37+
| `filter_target_label.PEDESTRIAN` | bool | false | If true, pedestrian objects are filtered. |
38+
| `upper_bound_x` | float | 100.00 | Bound for filtering. Only used if filter_by_xy_position is true |
39+
| `lower_bound_x` | float | 0.00 | Bound for filtering. Only used if filter_by_xy_position is true |
40+
| `upper_bound_y` | float | 50.00 | Bound for filtering. Only used if filter_by_xy_position is true |
41+
| `lower_bound_y` | float | -50.00 | Bound for filtering. Only used if filter_by_xy_position is true |
42+
| `use_published_time` | bool | false | Node publishes its DetectedObjects publishing time with pipeline header file. |
4243

4344
## Assumptions / Known limits
4445

‎perception/detected_object_validation/obstacle-pointcloud-based-validator.md

+1
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@ In the debug image above, the red DetectedObject is the validated object. The bl
3333
| `max_points_num` | int | The max number of obstacle point clouds in DetectedObjects |
3434
| `min_points_and_distance_ratio` | float | Threshold value of the number of point clouds per object when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink. |
3535
| `enable_debugger` | bool | Whether to create debug topics or not? |
36+
| `use_published_time` | bool | Node publishes its DetectedObjects publishing time with pipeline header file. Default it is false. |
3637

3738
## Assumptions / Known limits
3839

‎perception/detected_object_validation/occupancy-grid-based-validator.md

+5-4
Original file line numberDiff line numberDiff line change
@@ -28,10 +28,11 @@ If the percentage is low, it is deleted.
2828

2929
## Parameters
3030

31-
| Name | Type | Description |
32-
| ---------------- | ----- | -------------------------------------------------- |
33-
| `mean_threshold` | float | The percentage threshold of allowed non-freespace. |
34-
| `enable_debug` | bool | Whether to display debug images or not? |
31+
| Name | Type | Description |
32+
| -------------------- | ----- | -------------------------------------------------------------------------------------------------- |
33+
| `mean_threshold` | float | The percentage threshold of allowed non-freespace. |
34+
| `enable_debug` | bool | Whether to display debug images or not? |
35+
| `use_published_time` | bool | Node publishes its DetectedObjects publishing time with pipeline header file. Default it is false. |
3536

3637
## Assumptions / Known limits
3738

‎perception/detected_object_validation/src/object_lanelet_filter.cpp

+7
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,9 @@ 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 enabled only if the use_published_time is true
60+
published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
5861
}
5962

6063
void ObjectLaneletFilterNode::mapCallback(
@@ -120,6 +123,10 @@ void ObjectLaneletFilterNode::objectCallback(
120123
}
121124
object_pub_->publish(output_object_msg);
122125

126+
if (published_time_publisher_) {
127+
published_time_publisher_->publish(object_pub_, output_object_msg.header.stamp);
128+
}
129+
123130
// Publish debug info
124131
const double pipeline_latency =
125132
std::chrono::duration<double, std::milli>(

0 commit comments

Comments
 (0)