diff --git a/control/trajectory_follower_node/README.md b/control/trajectory_follower_node/README.md index c3ea3ddf6b7e8..2847831d4d2db 100644 --- a/control/trajectory_follower_node/README.md +++ b/control/trajectory_follower_node/README.md @@ -146,6 +146,7 @@ Giving the longitudinal controller information about steer convergence allows it 2. The last received commands are not older than defined by `timeout_thr_sec`. - `lateral_controller_mode`: `mpc` or `pure_pursuit` - (currently there is only `PID` for longitudinal controller) +- `use_published_time`: Node publishes its control command publishing time with pipeline header file. Default it is false. ## Debugging diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index cc43da7114630..be4fe885ae8c1 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -28,6 +28,7 @@ #include #include +#include #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" @@ -121,6 +122,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node std::unique_ptr logger_configure_; + std::unique_ptr published_time_publisher_; + void publishProcessingTime( const double t_ms, const rclcpp::Publisher::SharedPtr pub); StopWatch stop_watch_; diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 322aa9eef5a79..5b1388e3065f1 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -91,6 +91,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control } logger_configure_ = std::make_unique(this); + + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } Controller::LateralControllerMode Controller::getLateralControllerMode( @@ -231,7 +233,12 @@ void Controller::callbackTimerControl() out.longitudinal = lon_out.control_cmd; control_cmd_pub_->publish(out); - // 6. publish debug marker + // 6. publish published time only if enabled by parameter + if (published_time_publisher_) { + published_time_publisher_->publish(control_cmd_pub_, out.stamp); + } + + // 7. publish debug marker publishDebugMarker(*input_data, lat_out); } diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index 6ec2da84a7b6c..ebc5674377016 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/control/vehicle_cmd_gate/README.md @@ -67,6 +67,7 @@ | `on_transition.lon_jerk_lim` | | array of limits of longitudinal jerk (activated in TRANSITION operation mode) | | `on_transition.lat_acc_lim` | | array of limits of lateral acceleration (activated in TRANSITION operation mode) | | `on_transition.lat_jerk_lim` | | array of limits of lateral jerk (activated in TRANSITION operation mode) | +| `use_published_time` | bool | Node publishes its control command publishing time with pipeline header file. Default it is false. | ## Filter function diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index aec2ede53d9e7..90cdd7670968f 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -242,6 +242,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) this, get_clock(), period_ns, std::bind(&VehicleCmdGate::publishStatus, this)); logger_configure_ = std::make_unique(this); + + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } bool VehicleCmdGate::isHeartbeatTimeout( @@ -456,7 +458,16 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) // Publish commands vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency); control_cmd_pub_->publish(filtered_commands.control); + + // Publish published time only if enabled by parameter + if (published_time_publisher_) { + published_time_publisher_->publish(control_cmd_pub_, filtered_commands.control.stamp); + } + + // Publish pause state to api adapi_pause_->publish(); + + // Publish moderate stop state which is used for stop request moderate_stop_interface_->publish(); // Save ControlCmd to steering angle when disengaged diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index b79c58d120443..1907cf123632f 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -248,6 +249,8 @@ class VehicleCmdGate : public rclcpp::Node void publishMarkers(const IsFilterActivated & filter_activated); std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace vehicle_cmd_gate diff --git a/perception/detected_object_feature_remover/README.md b/perception/detected_object_feature_remover/README.md index 21a8bd9a00541..3e18fb4ab0023 100644 --- a/perception/detected_object_feature_remover/README.md +++ b/perception/detected_object_feature_remover/README.md @@ -22,6 +22,8 @@ The `detected_object_feature_remover` is a package to convert topic-type from `D ## Parameters -None +| Name | Type | Description | +| -------------------- | ------ | -------------------------------------------------------------------------------------------- | +| `use_published_time` | `bool` | Node publishes its DetectedObjects publishing time with pipeline header file. Default: false | ## 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..3f06883c42e0d 100644 --- a/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp +++ b/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp @@ -23,6 +23,9 @@ DetectedObjectFeatureRemover::DetectedObjectFeatureRemover(const rclcpp::NodeOpt pub_ = this->create_publisher("~/output", rclcpp::QoS(1)); sub_ = this->create_subscription( "~/input", 1, std::bind(&DetectedObjectFeatureRemover::objectCallback, this, _1)); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void DetectedObjectFeatureRemover::objectCallback( @@ -31,6 +34,11 @@ void DetectedObjectFeatureRemover::objectCallback( DetectedObjects output; convert(*input, output); pub_->publish(output); + + // Publish published time if enabled by parameter + if (published_time_publisher_) { + published_time_publisher_->publish(pub_, output.header.stamp); + } } void DetectedObjectFeatureRemover::convert( diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index e3c3263466033..f3871aaf98117 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -68,6 +69,8 @@ class ObjectLaneletFilterNode : public rclcpp::Node bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &); geometry_msgs::msg::Polygon setFootprint( const autoware_auto_perception_msgs::msg::DetectedObject &); + + std::unique_ptr published_time_publisher_; }; } // namespace object_lanelet_filter diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp index be25eb6a353e6..9cc9c51747c52 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -51,6 +52,8 @@ class ObjectPositionFilterNode : public rclcpp::Node float lower_bound_y_; utils::FilterTargetLabel filter_target_; bool isObjectInBounds(const autoware_auto_perception_msgs::msg::DetectedObject & object) const; + + std::unique_ptr published_time_publisher_; }; } // namespace object_position_filter diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index eb6da6bc45b24..742455c60a42c 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -148,6 +149,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node std::shared_ptr debugger_; bool using_2d_validator_; std::unique_ptr validator_; + std::unique_ptr published_time_publisher_; private: void onObjectsAndObstaclePointCloud( diff --git a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp index 5fd866d09e725..9be990ebc1787 100644 --- a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -42,6 +43,7 @@ class OccupancyGridBasedValidator : public rclcpp::Node message_filters::Subscriber occ_grid_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; + std::unique_ptr published_time_publisher_; typedef message_filters::sync_policies::ApproximateTime< autoware_auto_perception_msgs::msg::DetectedObjects, nav_msgs::msg::OccupancyGrid> diff --git a/perception/detected_object_validation/object-lanelet-filter.md b/perception/detected_object_validation/object-lanelet-filter.md index eebd8ede79270..e7d5dc8c1b9ed 100644 --- a/perception/detected_object_validation/object-lanelet-filter.md +++ b/perception/detected_object_validation/object-lanelet-filter.md @@ -26,16 +26,17 @@ The objects only inside of the vector map will be published. ### Core Parameters -| Name | Type | Default Value | Description | -| -------------------------------- | ---- | ------------- | ----------------------------------------- | -| `filter_target_label.UNKNOWN` | bool | false | If true, unknown objects are filtered. | -| `filter_target_label.CAR` | bool | false | If true, car objects are filtered. | -| `filter_target_label.TRUCK` | bool | false | If true, truck objects are filtered. | -| `filter_target_label.BUS` | bool | false | If true, bus objects are filtered. | -| `filter_target_label.TRAILER` | bool | false | If true, trailer objects are filtered. | -| `filter_target_label.MOTORCYCLE` | bool | false | If true, motorcycle objects are filtered. | -| `filter_target_label.BICYCLE` | bool | false | If true, bicycle objects are filtered. | -| `filter_target_label.PEDESTRIAN` | bool | false | If true, pedestrian objects are filtered. | +| Name | Type | Default Value | Description | +| -------------------------------- | ---- | ------------- | ----------------------------------------------------------------------------- | +| `filter_target_label.UNKNOWN` | bool | false | If true, unknown objects are filtered. | +| `filter_target_label.CAR` | bool | false | If true, car objects are filtered. | +| `filter_target_label.TRUCK` | bool | false | If true, truck objects are filtered. | +| `filter_target_label.BUS` | bool | false | If true, bus objects are filtered. | +| `filter_target_label.TRAILER` | bool | false | If true, trailer objects are filtered. | +| `filter_target_label.MOTORCYCLE` | bool | false | If true, motorcycle objects are filtered. | +| `filter_target_label.BICYCLE` | bool | false | If true, bicycle objects are filtered. | +| `filter_target_label.PEDESTRIAN` | bool | false | If true, pedestrian objects are filtered. | +| `use_published_time` | bool | false | Node publishes its DetectedObjects publishing time with pipeline header file. | ## Assumptions / Known limits diff --git a/perception/detected_object_validation/object-position-filter.md b/perception/detected_object_validation/object-position-filter.md index a47379a778f44..f91571b09c419 100644 --- a/perception/detected_object_validation/object-position-filter.md +++ b/perception/detected_object_validation/object-position-filter.md @@ -25,20 +25,21 @@ The objects only inside of the x, y bound will be published. ### Core Parameters -| Name | Type | Default Value | Description | -| -------------------------------- | ----- | ------------- | --------------------------------------------------------------- | -| `filter_target_label.UNKNOWN` | bool | false | If true, unknown objects are filtered. | -| `filter_target_label.CAR` | bool | false | If true, car objects are filtered. | -| `filter_target_label.TRUCK` | bool | false | If true, truck objects are filtered. | -| `filter_target_label.BUS` | bool | false | If true, bus objects are filtered. | -| `filter_target_label.TRAILER` | bool | false | If true, trailer objects are filtered. | -| `filter_target_label.MOTORCYCLE` | bool | false | If true, motorcycle objects are filtered. | -| `filter_target_label.BICYCLE` | bool | false | If true, bicycle objects are filtered. | -| `filter_target_label.PEDESTRIAN` | bool | false | If true, pedestrian objects are filtered. | -| `upper_bound_x` | float | 100.00 | Bound for filtering. Only used if filter_by_xy_position is true | -| `lower_bound_x` | float | 0.00 | Bound for filtering. Only used if filter_by_xy_position is true | -| `upper_bound_y` | float | 50.00 | Bound for filtering. Only used if filter_by_xy_position is true | -| `lower_bound_y` | float | -50.00 | Bound for filtering. Only used if filter_by_xy_position is true | +| Name | Type | Default Value | Description | +| -------------------------------- | ----- | ------------- | ----------------------------------------------------------------------------- | +| `filter_target_label.UNKNOWN` | bool | false | If true, unknown objects are filtered. | +| `filter_target_label.CAR` | bool | false | If true, car objects are filtered. | +| `filter_target_label.TRUCK` | bool | false | If true, truck objects are filtered. | +| `filter_target_label.BUS` | bool | false | If true, bus objects are filtered. | +| `filter_target_label.TRAILER` | bool | false | If true, trailer objects are filtered. | +| `filter_target_label.MOTORCYCLE` | bool | false | If true, motorcycle objects are filtered. | +| `filter_target_label.BICYCLE` | bool | false | If true, bicycle objects are filtered. | +| `filter_target_label.PEDESTRIAN` | bool | false | If true, pedestrian objects are filtered. | +| `upper_bound_x` | float | 100.00 | Bound for filtering. Only used if filter_by_xy_position is true | +| `lower_bound_x` | float | 0.00 | Bound for filtering. Only used if filter_by_xy_position is true | +| `upper_bound_y` | float | 50.00 | Bound for filtering. Only used if filter_by_xy_position is true | +| `lower_bound_y` | float | -50.00 | Bound for filtering. Only used if filter_by_xy_position is true | +| `use_published_time` | bool | false | Node publishes its DetectedObjects publishing time with pipeline header file. | ## Assumptions / Known limits diff --git a/perception/detected_object_validation/obstacle-pointcloud-based-validator.md b/perception/detected_object_validation/obstacle-pointcloud-based-validator.md index 36b3815e7e689..cbf8e2be16506 100644 --- a/perception/detected_object_validation/obstacle-pointcloud-based-validator.md +++ b/perception/detected_object_validation/obstacle-pointcloud-based-validator.md @@ -33,6 +33,7 @@ In the debug image above, the red DetectedObject is the validated object. The bl | `max_points_num` | int | The max number of obstacle point clouds in DetectedObjects | | `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. | | `enable_debugger` | bool | Whether to create debug topics or not? | +| `use_published_time` | bool | Node publishes its DetectedObjects publishing time with pipeline header file. Default it is false. | ## Assumptions / Known limits diff --git a/perception/detected_object_validation/occupancy-grid-based-validator.md b/perception/detected_object_validation/occupancy-grid-based-validator.md index 3b084d51e9988..2fbfcd997efed 100644 --- a/perception/detected_object_validation/occupancy-grid-based-validator.md +++ b/perception/detected_object_validation/occupancy-grid-based-validator.md @@ -28,10 +28,11 @@ If the percentage is low, it is deleted. ## Parameters -| Name | Type | Description | -| ---------------- | ----- | -------------------------------------------------- | -| `mean_threshold` | float | The percentage threshold of allowed non-freespace. | -| `enable_debug` | bool | Whether to display debug images or not? | +| Name | Type | Description | +| -------------------- | ----- | -------------------------------------------------------------------------------------------------- | +| `mean_threshold` | float | The percentage threshold of allowed non-freespace. | +| `enable_debug` | bool | Whether to display debug images or not? | +| `use_published_time` | bool | Node publishes its DetectedObjects publishing time with pipeline header file. Default it is false. | ## Assumptions / Known limits diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 74000a91e60fc..dd65c4e628b49 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -55,6 +55,9 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod debug_publisher_ = std::make_unique(this, "object_lanelet_filter"); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void ObjectLaneletFilterNode::mapCallback( @@ -120,6 +123,10 @@ void ObjectLaneletFilterNode::objectCallback( } object_pub_->publish(output_object_msg); + if (published_time_publisher_) { + published_time_publisher_->publish(object_pub_, output_object_msg.header.stamp); + } + // Publish debug info const double pipeline_latency = std::chrono::duration( diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp index a509fc7571dd5..3927db81a602f 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -42,6 +42,9 @@ ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & n "input/object", rclcpp::QoS{1}, std::bind(&ObjectPositionFilterNode::objectCallback, this, _1)); object_pub_ = this->create_publisher( "output/object", rclcpp::QoS{1}); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void ObjectPositionFilterNode::objectCallback( @@ -65,6 +68,10 @@ void ObjectPositionFilterNode::objectCallback( } object_pub_->publish(output_object_msg); + + if (published_time_publisher_) { + published_time_publisher_->publish(object_pub_, output_object_msg.header.stamp); + } } bool ObjectPositionFilterNode::isObjectInBounds( diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index f3f56652792e9..e005b3327b893 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -309,6 +309,9 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, @@ -347,6 +350,11 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( } objects_pub_->publish(output); + + if (published_time_publisher_) { + published_time_publisher_->publish(objects_pub_, output.header.stamp); + } + if (debugger_) { debugger_->publishRemovedObjects(removed_objects); debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header); diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp index 80e4dc66d35c2..22a2e2b90cc0f 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -52,6 +52,9 @@ OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptio mean_threshold_ = declare_parameter("mean_threshold", 0.6); enable_debug_ = declare_parameter("enable_debug", false); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void OccupancyGridBasedValidator::onObjectsAndOccGrid( @@ -86,6 +89,10 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid( objects_pub_->publish(output); + if (published_time_publisher_) { + published_time_publisher_->publish(objects_pub_, output.header.stamp); + } + if (enable_debug_) showDebugImage(*input_occ_grid, transformed_objects, occ_grid); } diff --git a/perception/detection_by_tracker/README.md b/perception/detection_by_tracker/README.md index 0cff97c4d3ab4..0f0807f71e48f 100644 --- a/perception/detection_by_tracker/README.md +++ b/perception/detection_by_tracker/README.md @@ -46,16 +46,17 @@ Simply looking at the overlap between the unknown object and the tracker does no ## Parameters -| Name | Type | Description | Default value | -| --------------------------------- | ------ | --------------------------------------------------------------------- | ------------- | -| `tracker_ignore_label.UNKNOWN` | `bool` | If true, the node will ignore the tracker if its label is unknown. | `true` | -| `tracker_ignore_label.CAR` | `bool` | If true, the node will ignore the tracker if its label is CAR. | `false` | -| `tracker_ignore_label.PEDESTRIAN` | `bool` | If true, the node will ignore the tracker if its label is pedestrian. | `false` | -| `tracker_ignore_label.BICYCLE` | `bool` | If true, the node will ignore the tracker if its label is bicycle. | `false` | -| `tracker_ignore_label.MOTORCYCLE` | `bool` | If true, the node will ignore the tracker if its label is MOTORCYCLE. | `false` | -| `tracker_ignore_label.BUS` | `bool` | If true, the node will ignore the tracker if its label is bus. | `false` | -| `tracker_ignore_label.TRUCK` | `bool` | If true, the node will ignore the tracker if its label is truck. | `false` | -| `tracker_ignore_label.TRAILER` | `bool` | If true, the node will ignore the tracker if its label is TRAILER. | `false` | +| Name | Type | Description | Default value | +| --------------------------------- | ------ | ----------------------------------------------------------------------------- | ------------- | +| `tracker_ignore_label.UNKNOWN` | `bool` | If true, the node will ignore the tracker if its label is unknown. | `true` | +| `tracker_ignore_label.CAR` | `bool` | If true, the node will ignore the tracker if its label is CAR. | `false` | +| `tracker_ignore_label.PEDESTRIAN` | `bool` | If true, the node will ignore the tracker if its label is pedestrian. | `false` | +| `tracker_ignore_label.BICYCLE` | `bool` | If true, the node will ignore the tracker if its label is bicycle. | `false` | +| `tracker_ignore_label.MOTORCYCLE` | `bool` | If true, the node will ignore the tracker if its label is MOTORCYCLE. | `false` | +| `tracker_ignore_label.BUS` | `bool` | If true, the node will ignore the tracker if its label is bus. | `false` | +| `tracker_ignore_label.TRUCK` | `bool` | If true, the node will ignore the tracker if its label is truck. | `false` | +| `tracker_ignore_label.TRAILER` | `bool` | If true, the node will ignore the tracker if its label is TRAILER. | `false` | +| `use_published_time` | `bool` | Node publishes its DetectedObjects publishing time with pipeline header file. | `false` | ## Assumptions / Known limits diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 771747c80bb95..7747a37ee2f29 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -83,6 +84,8 @@ class DetectionByTracker : public rclcpp::Node detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_; + std::unique_ptr published_time_publisher_; + void setMaxSearchRange(); void onObjects( diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 2595315e1f9b2..d8d55d97f427e 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -177,6 +177,9 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void DetectionByTracker::setMaxSearchRange() @@ -220,6 +223,10 @@ void DetectionByTracker::onObjects( !object_recognition_utils::transformObjects( objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) { objects_pub_->publish(detected_objects); + + if (published_time_publisher_) { + published_time_publisher_->publish(objects_pub_, detected_objects.header.stamp); + } return; } // to simplify post processes, convert tracked_objects to DetectedObjects message. @@ -250,6 +257,10 @@ void DetectionByTracker::onObjects( } objects_pub_->publish(detected_objects); + + if (published_time_publisher_) { + published_time_publisher_->publish(objects_pub_, detected_objects.header.stamp); + } } void DetectionByTracker::divideUnderSegmentedObjects( diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index 2a9adedad9074..72f3e79ab8e23 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -30,20 +30,21 @@ We trained the models using . ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------------- | ------------ | ------------- | ------------------------------------------------------------- | -| `score_threshold` | float | `0.4` | detected objects with score less than threshold are ignored | -| `densification_world_frame_id` | string | `map` | the world frame id to fuse multi-frame pointcloud | -| `densification_num_past_frames` | int | `1` | the number of past frames to fuse with the current frame | -| `trt_precision` | string | `fp16` | TensorRT inference precision: `fp32` or `fp16` | -| `encoder_onnx_path` | string | `""` | path to VoxelFeatureEncoder ONNX file | -| `encoder_engine_path` | string | `""` | path to VoxelFeatureEncoder TensorRT Engine file | -| `head_onnx_path` | string | `""` | path to DetectionHead ONNX file | -| `head_engine_path` | string | `""` | path to DetectionHead TensorRT Engine file | -| `nms_iou_target_class_names` | list[string] | - | target classes for IoU-based Non Maximum Suppression | -| `nms_iou_search_distance_2d` | double | - | If two objects are farther than the value, NMS isn't applied. | -| `nms_iou_threshold` | double | - | IoU threshold for the IoU-based Non Maximum Suppression | -| `build_only` | bool | `false` | shutdown the node after TensorRT engine file is built | +| Name | Type | Default Value | Description | +| ------------------------------- | ------------ | ------------- | ----------------------------------------------------------------------------- | +| `score_threshold` | float | `0.4` | detected objects with score less than threshold are ignored | +| `densification_world_frame_id` | string | `map` | the world frame id to fuse multi-frame pointcloud | +| `densification_num_past_frames` | int | `1` | the number of past frames to fuse with the current frame | +| `trt_precision` | string | `fp16` | TensorRT inference precision: `fp32` or `fp16` | +| `encoder_onnx_path` | string | `""` | path to VoxelFeatureEncoder ONNX file | +| `encoder_engine_path` | string | `""` | path to VoxelFeatureEncoder TensorRT Engine file | +| `head_onnx_path` | string | `""` | path to DetectionHead ONNX file | +| `head_engine_path` | string | `""` | path to DetectionHead TensorRT Engine file | +| `nms_iou_target_class_names` | list[string] | - | target classes for IoU-based Non Maximum Suppression | +| `nms_iou_search_distance_2d` | double | - | If two objects are farther than the value, NMS isn't applied. | +| `nms_iou_threshold` | double | - | IoU threshold for the IoU-based Non Maximum Suppression | +| `build_only` | bool | `false` | shutdown the node after TensorRT engine file is built | +| `use_published_time` | bool | `false` | Node publishes its DetectedObjects publishing time with pipeline header file. | ### The `build_only` option diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index 474c9884eb36f..963579e5763c2 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -63,6 +64,8 @@ class LidarCenterPointNode : public rclcpp::Node std::unique_ptr> stop_watch_ptr_{ nullptr}; std::unique_ptr debug_publisher_ptr_{nullptr}; + + std::unique_ptr published_time_publisher_; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index d370a60a26aa5..eafc129b4d00f 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -126,6 +126,9 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); } + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void LidarCenterPointNode::pointCloudCallback( @@ -163,6 +166,10 @@ void LidarCenterPointNode::pointCloudCallback( if (objects_sub_count > 0) { objects_pub_->publish(output_msg); + + if (published_time_publisher_) { + published_time_publisher_->publish(objects_pub_, output_msg.header.stamp); + } } // add processing time for debug diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index a58e19db70304..6fb7f9479d43c 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -242,6 +242,7 @@ If the target object is inside the road or crosswalk, this module outputs one or | `object_buffer_time_length` | [s] | double | Time span of object history to store the information | | `history_time_length` | [s] | double | Time span of object information used for prediction | | `prediction_time_horizon_rate_for_validate_shoulder_lane_length` | [-] | double | prediction path will disabled when the estimated path length exceeds lanelet length. This parameter control the estimated path length | +| `use_published_time` | [-] | bool | Node publishes its PredictedObjects publishing time with pipeline header file. Default it is false. | ## Assumptions / Known limits diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 321342c3d8367..1b531e5a4f7fa 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -21,6 +21,7 @@ #include "tier4_autoware_utils/ros/update_param.hpp" #include +#include #include #include #include @@ -199,6 +200,9 @@ class MapBasedPredictionNode : public rclcpp::Node // Stop watch StopWatch stop_watch_; + // PublishedTime Publisher + std::unique_ptr published_time_publisher_; + // Member Functions void mapCallback(const HADMapBin::ConstSharedPtr msg); void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg); diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 21697bb1b5f1a..c253cc77116a3 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -818,6 +818,9 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ this->create_publisher("maneuver", rclcpp::QoS{1}); pub_calculation_time_ = create_publisher("~/debug/calculation_time", 1); + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); + set_param_res_ = this->add_on_set_parameters_callback( std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); } @@ -1115,6 +1118,11 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Publish Results pub_objects_->publish(output); + + if (published_time_publisher_) { + published_time_publisher_->publish(pub_objects_, output.header.stamp); + } + pub_debug_markers_->publish(debug_markers); const auto calculation_time_msg = createStringStamped(now(), stop_watch_.toc()); pub_calculation_time_->publish(calculation_time_msg); diff --git a/perception/multi_object_tracker/README.md b/perception/multi_object_tracker/README.md index 311fe7b6da716..9c36e3dc8cb11 100644 --- a/perception/multi_object_tracker/README.md +++ b/perception/multi_object_tracker/README.md @@ -80,6 +80,7 @@ Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_ob | `world_frame_id` | double | object kinematics definition frame | | `enable_delay_compensation` | bool | if True, tracker use timers to schedule publishers and use prediction step to extrapolate object state at desired timestamp | | `publish_rate` | double | Timer frequency to output with delay compensation | +| `use_published_time` | bool | Node publishes its DetectedObjects publishing time with pipeline header file. Default it is false. | #### Association parameters diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 95d33b78ff184..09499ac49467d 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -107,6 +108,8 @@ class MultiObjectTracker : public rclcpp::Node std::map tracker_map_; + std::unique_ptr published_time_publisher_; + void onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 1d4d50c7eab4c..d3e566a2339c3 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -236,6 +236,9 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) data_association_ = std::make_unique( can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, min_iou_matrix); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void MultiObjectTracker::onMeasurement( @@ -469,6 +472,10 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) // Publish tracked_objects_pub_->publish(output_msg); + if (published_time_publisher_) { + published_time_publisher_->publish(tracked_objects_pub_, output_msg.header.stamp); + } + // Debugger Publish if enabled debugger_->publishProcessingTime(); debugger_->publishTentativeObjects(tentative_objects_msg); diff --git a/perception/object_merger/README.md b/perception/object_merger/README.md index 18bf12fa8cdb8..33530f4229370 100644 --- a/perception/object_merger/README.md +++ b/perception/object_merger/README.md @@ -35,6 +35,7 @@ The successive shortest path algorithm is used to solve the data association pro | `base_link_frame_id` | double | association frame | | `distance_threshold_list` | `std::vector` | Distance threshold for each class used in judging overlap. The class order depends on [ObjectClassification](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/ObjectClassification.idl). | | `generalized_iou_threshold` | `std::vector` | Generalized IoU threshold for each class | +| `use_published_time` | bool | Node publishes its DetectedObjects publishing time with pipeline header file. Default it is false. | ## Tips diff --git a/perception/object_merger/include/object_merger/node.hpp b/perception/object_merger/include/object_merger/node.hpp index 8341ce490ca72..c884a4730e330 100644 --- a/perception/object_merger/include/object_merger/node.hpp +++ b/perception/object_merger/include/object_merger/node.hpp @@ -18,6 +18,7 @@ #include "object_merger/data_association/data_association.hpp" #include +#include #include @@ -81,6 +82,8 @@ class ObjectAssociationMergerNode : public rclcpp::Node std::map generalized_iou_threshold; std::map distance_threshold_map; } overlapped_judge_param_; + + std::unique_ptr published_time_publisher_; }; } // namespace object_association diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 1ffc2791e8f4e..c23234665bc5a 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -118,6 +118,9 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio merged_object_pub_ = create_publisher( "output/object", rclcpp::QoS{1}); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void ObjectAssociationMergerNode::objectsCallback( @@ -215,6 +218,11 @@ void ObjectAssociationMergerNode::objectsCallback( // publish output msg merged_object_pub_->publish(output_msg); + + // Publish published time if enabled by parameter + if (published_time_publisher_) { + published_time_publisher_->publish(merged_object_pub_, output_msg.header.stamp); + } } } // namespace object_association diff --git a/perception/occupancy_grid_map_outlier_filter/README.md b/perception/occupancy_grid_map_outlier_filter/README.md index fd064af19902f..1b8e6ee520136 100644 --- a/perception/occupancy_grid_map_outlier_filter/README.md +++ b/perception/occupancy_grid_map_outlier_filter/README.md @@ -51,6 +51,7 @@ The following video is a sample. Yellow points are high occupancy probability, g | `radius_search_2d_filter/min_points_and_distance_ratio` | float | Threshold value of the number of point clouds per radius when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink. | | `radius_search_2d_filter/min_points` | int | Minimum number of point clouds per radius | | `radius_search_2d_filter/max_points` | int | Maximum number of point clouds per radius | +| `use_published_time` | bool | Node publishes its PointCloud2 publishing time with pipeline header file. Default it is false. | ## Assumptions / Known limits diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index 953307e9d5380..1d7d01bfbab55 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -120,6 +121,7 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node std::shared_ptr debugger_ptr_; std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; + std::unique_ptr published_time_publisher_; // ROS Parameters std::string map_frame_; diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 691c13a6d4701..b4c08ac44e204 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -234,6 +234,8 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( if (enable_debugger) { debugger_ptr_ = std::make_shared(*this); } + /* published time publisher enabled only if the use_published_time is true */ + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( @@ -305,6 +307,10 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( return; } pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr)); + + if (published_time_publisher_) { + published_time_publisher_->publish(pointcloud_pub_, ogm_frame_filtered_pc.header.stamp); + } } if (debugger_ptr_) { debugger_ptr_->publishHighConfidence(high_confidence_pc, ogm_frame_pc.header); diff --git a/perception/tracking_object_merger/README.md b/perception/tracking_object_merger/README.md index d55622f89181e..a7d2671cde9fb 100644 --- a/perception/tracking_object_merger/README.md +++ b/perception/tracking_object_merger/README.md @@ -96,6 +96,7 @@ Default parameters are set in [config/decorative_tracker_merger.param.yaml](./co | `main_sensor_type` | main sensor type. This is used to determine the dominant tracking object. | "lidar" | | `sub_sensor_type` | sub sensor type. This is used to determine the sub tracking object. | "radar" | | `tracker_state_parameter` | tracker state parameter. This is used to manage the tracklet. | | +| `use_published_time` | node publishes its TrackedObjects publishing time with pipeline header file. | false | - the detail of `tracker_state_parameter` is described in [tracklet management](#tracklet-management) diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp index 0509fb2a07dc5..aab9e51886350 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp @@ -20,6 +20,7 @@ #include "tracking_object_merger/utils/utils.hpp" #include +#include #include #include @@ -106,6 +107,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node // tracker default settings TrackerStateParameter tracker_state_parameter_; + std::unique_ptr published_time_publisher_; + // merge policy (currently not used) struct { diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index b22b8d5fa2948..ba0e75e218535 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -151,6 +151,9 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio set3dDataAssociation("lidar-radar", data_association_map_); // radar-radar association matrix set3dDataAssociation("radar-radar", data_association_map_); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void DecorativeTrackerMergerNode::set3dDataAssociation( @@ -212,8 +215,13 @@ void DecorativeTrackerMergerNode::mainObjectsCallback( // try to merge main object this->decorativeMerger(main_sensor_type_, main_objects); + const auto & tracked_objects = getTrackedObjects(main_objects->header); + merged_object_pub_->publish(tracked_objects); - merged_object_pub_->publish(getTrackedObjects(main_objects->header)); + // Published Time Publisher enabled only if the use_published_time is true + if (published_time_publisher_) { + published_time_publisher_->publish(merged_object_pub_, tracked_objects.header.stamp); + } } /** diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 03f8c8d2a5696..4c405b817ca24 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -118,15 +118,16 @@ The Planner Manager's responsibilities include: ### Debug -| Name | Type | Description | QoS Durability | -| :-------------------------------------- | :-------------------------------------------------- | :---------------------------------------------------------------------------------------- | -------------- | -| ~/debug/avoidance_debug_message_array | `tier4_planning_msgs::msg::AvoidanceDebugMsgArray` | debug message for avoidance. notify users reasons for avoidance path cannot be generated. | `volatile` | -| ~/debug/lane_change_debug_message_array | `tier4_planning_msgs::msg::LaneChangeDebugMsgArray` | debug message for lane change. notify users unsafe reason during lane changing process | `volatile` | -| ~/debug/maximum_drivable_area | `visualization_msgs::msg::MarkerArray` | shows maximum static drivable area. | `volatile` | -| ~/debug/turn_signal_info | `visualization_msgs::msg::MarkerArray` | TBA | `volatile` | -| ~/debug/bound | `visualization_msgs::msg::MarkerArray` | debug for static drivable area | `volatile` | -| ~/planning/path_candidate/\* | `autoware_auto_planning_msgs::msg::Path` | the path before approval. | `volatile` | -| ~/planning/path_reference/\* | `autoware_auto_planning_msgs::msg::Path` | reference path generated by each modules. | `volatile` | +| Name | Type | Description | QoS Durability | +| :-------------------------------------- | :-------------------------------------------------- | :---------------------------------------------------------------------------------------------------------- | -------------- | +| ~/debug/avoidance_debug_message_array | `tier4_planning_msgs::msg::AvoidanceDebugMsgArray` | debug message for avoidance. notify users reasons for avoidance path cannot be generated. | `volatile` | +| ~/debug/lane_change_debug_message_array | `tier4_planning_msgs::msg::LaneChangeDebugMsgArray` | debug message for lane change. notify users unsafe reason during lane changing process | `volatile` | +| ~/debug/maximum_drivable_area | `visualization_msgs::msg::MarkerArray` | shows maximum static drivable area. | `volatile` | +| ~/debug/turn_signal_info | `visualization_msgs::msg::MarkerArray` | TBA | `volatile` | +| ~/debug/bound | `visualization_msgs::msg::MarkerArray` | debug for static drivable area | `volatile` | +| ~/planning/path_candidate/\* | `autoware_auto_planning_msgs::msg::Path` | the path before approval. | `volatile` | +| ~/planning/path_reference/\* | `autoware_auto_planning_msgs::msg::Path` | reference path generated by each modules. | `volatile` | +| ~/output/path/debug/published_time | `autoware_internal_msgs::msg::PublishedTime` | Node publishes its Path publishing time with pipeline header file when `use_published_time` parameter true. | `volatile` | !!! note diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 5d40f2a8614ed..99e312f98f100 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -21,6 +21,8 @@ #include "behavior_path_planner_common/interface/steering_factor_interface.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include + #include #include #include @@ -215,6 +217,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node const std::shared_ptr & planner_data); std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index d2f58e7d7e017..9ef26b03ed991 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -172,6 +172,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod } logger_configure_ = std::make_unique(this); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } std::vector BehaviorPathPlannerNode::getWaitingApprovalModules() @@ -413,6 +416,11 @@ void BehaviorPathPlannerNode::run() if (!path->points.empty()) { path_publisher_->publish(*path); + + if (published_time_publisher_) { + published_time_publisher_->publish(path_publisher_, path->header.stamp); + } + } else { RCLCPP_ERROR_THROTTLE( get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish."); diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md index 519e68764b450..07ffc4d326399 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -48,11 +48,12 @@ So for example, in order to stop at a stop line with the vehicles' front on the ## Node parameters -| Parameter | Type | Description | -| ---------------------- | -------------------- | ----------------------------------------------------------------------------------- | -| `launch_modules` | vector<string> | module names to launch | -| `forward_path_length` | double | forward path length | -| `backward_path_length` | double | backward path length | -| `max_accel` | double | (to be a global parameter) max acceleration of the vehicle | -| `system_delay` | double | (to be a global parameter) delay time until output control command | -| `delay_response_time` | double | (to be a global parameter) delay time of the vehicle's response to control commands | +| Parameter | Type | Description | +| ---------------------- | -------------------- | --------------------------------------------------------------------------------------- | +| `launch_modules` | vector<string> | module names to launch | +| `forward_path_length` | double | forward path length | +| `backward_path_length` | double | backward path length | +| `max_accel` | double | (to be a global parameter) max acceleration of the vehicle | +| `system_delay` | double | (to be a global parameter) delay time until output control command | +| `delay_response_time` | double | (to be a global parameter) delay time of the vehicle's response to control commands | +| `use_published_time` | bool | node publishes its Path publishing time with pipeline header file. Default it is false. | diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 854f3c8852c29..19879899c5bd9 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -159,6 +159,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio } logger_configure_ = std::make_unique(this); + + // Published Time Publisher enabled only if the use_published_time is true + published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this); } void BehaviorVelocityPlannerNode::onLoadPlugin( @@ -399,6 +402,9 @@ void BehaviorVelocityPlannerNode::onTrigger( lk.unlock(); path_pub_->publish(output_path_msg); + if (published_time_publisher_) { + published_time_publisher_->publish(path_pub_, output_path_msg.header.stamp); + } stop_reason_diag_pub_->publish(planner_manager_.getStopReasonDiag()); if (debug_viz_pub_->get_subscription_count() > 0) { diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index a345f1200f721..dd6edf6ae0bc0 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -130,6 +131,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node const PlannerData & planner_data); std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace behavior_velocity_planner diff --git a/planning/motion_velocity_smoother/README.md b/planning/motion_velocity_smoother/README.md index 1ff7f5982b4eb..f394f923d9f91 100644 --- a/planning/motion_velocity_smoother/README.md +++ b/planning/motion_velocity_smoother/README.md @@ -238,6 +238,7 @@ After the optimization, a resampling called `post resampling` is performed befor | Name | Type | Description | Default value | | :---------------------------- | :------- | :------------------------------------------------------------------------------------------------ | :------------ | | `over_stop_velocity_warn_thr` | `double` | Threshold to judge that the optimized velocity exceeds the input velocity on the stop point [m/s] | 1.389 | +| `use_published_time` | `bool` | Node publishes its trajectory publishing time with pipeline header file. [-] | false |