Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(perception): add pipeline_latency_ms publisher #6181

Merged
merged 7 commits into from
Feb 9, 2024
Merged

feat(perception): add pipeline_latency_ms publisher #6181

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
4214d00
feat(perception): add pipeline_latency publishers
kminoda Jan 26, 2024
a52ce29
fix bug
kminoda Jan 29, 2024
9227726
feat: add pipeline latency publisher in centerpoint
kminoda Jan 29, 2024
f829880
Merge branch 'main' into feat/perception/add_accumulated_time_publisher
kminoda Jan 29, 2024
4d1fc10
fix: fix bug in euclidean_cluster
kminoda Jan 29, 2024
0306187
fix(object_lanelet_filter): add include
kminoda Jan 29, 2024
be2e218
fix(euclidean_cluster): fix bug
kminoda Jan 29, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions ...ation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@

#include <rclcpp/rclcpp.hpp>
#include <tier4_autoware_utils/geometry/geometry.hpp>
#include <tier4_autoware_utils/ros/debug_publisher.hpp>

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
Expand All @@ -27,6 +28,7 @@
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <memory>
#include <string>

namespace object_lanelet_filter
Expand All @@ -48,6 +50,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node
rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_pub_;
rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_sub_;
rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_sub_;
std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_{nullptr};

lanelet::LaneletMapPtr lanelet_map_ptr_;
lanelet::ConstLanelets road_lanelets_;
Expand Down
2 changes: 2 additions & 0 deletions ...ct_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include "detected_object_validation/obstacle_pointcloud_based_validator/debugger.hpp"

#include <rclcpp/rclcpp.hpp>
#include <tier4_autoware_utils/ros/debug_publisher.hpp>

#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
Expand Down Expand Up @@ -133,6 +134,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node
rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr objects_pub_;
message_filters::Subscriber<autoware_auto_perception_msgs::msg::DetectedObjects> objects_sub_;
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> obstacle_pointcloud_sub_;
std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_{nullptr};
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;

Expand Down
12 changes: 12 additions & 0 deletions perception/detected_object_validation/src/object_lanelet_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,9 @@
"input/object", rclcpp::QoS{1}, std::bind(&ObjectLaneletFilterNode::objectCallback, this, _1));
object_pub_ = this->create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
"output/object", rclcpp::QoS{1});

debug_publisher_ =
std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "object_lanelet_filter");

Check warning on line 57 in perception/detected_object_validation/src/object_lanelet_filter.cpp

View check run for this annotation

Codecov / codecov/patch

perception/detected_object_validation/src/object_lanelet_filter.cpp#L57 

Added line #L57 was not covered by tests
}

void ObjectLaneletFilterNode::mapCallback(
Expand Down Expand Up @@ -116,6 +119,15 @@
++index;
}
object_pub_->publish(output_object_msg);

// Publish debug info
const double pipeline_latency =
std::chrono::duration<double, std::milli>(
std::chrono::nanoseconds(
(this->get_clock()->now() - output_object_msg.header.stamp).nanoseconds()))
.count();
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(

Check warning on line 129 in perception/detected_object_validation/src/object_lanelet_filter.cpp

View check run for this annotation

Codecov / codecov/patch

perception/detected_object_validation/src/object_lanelet_filter.cpp#L127-L129 

Added lines #L127 - L129 were not covered by tests
"debug/pipeline_latency_ms", pipeline_latency);
}

geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint(
Expand Down
10 changes: 10 additions & 0 deletions perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -304,6 +304,8 @@

objects_pub_ = create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
"~/output/objects", rclcpp::QoS{1});
debug_publisher_ = std::make_unique<tier4_autoware_utils::DebugPublisher>(
this, "obstacle_pointcloud_based_validator");

Check warning on line 308 in perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

View check run for this annotation

Codecov / codecov/patch

perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp#L307-L308 

Added lines #L307 - L308 were not covered by tests

const bool enable_debugger = declare_parameter<bool>("enable_debugger", false);
if (enable_debugger) debugger_ = std::make_shared<Debugger>(this);
Expand Down Expand Up @@ -350,6 +352,14 @@
debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header);
debugger_->publishPointcloudWithinPolygon(input_obstacle_pointcloud->header);
}

// Publish processing time info
const double pipeline_latency =
std::chrono::duration<double, std::milli>(
std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds()))
.count();
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(

Check warning on line 361 in perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

View check run for this annotation

Codecov / codecov/patch

perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp#L359-L361 

Added lines #L359 - L361 were not covered by tests
"debug/pipeline_latency_ms", pipeline_latency);
}

} // namespace obstacle_pointcloud_based_validator
Expand Down
11 changes: 7 additions & 4 deletions perception/euclidean_cluster/src/euclidean_cluster_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,21 +64,24 @@
cluster_pub_->publish(output);

// build debug msg
if (debug_pub_->get_subscription_count() < 1) {
return;
}
{
if (debug_pub_->get_subscription_count() >= 1) {

Check warning on line 67 in perception/euclidean_cluster/src/euclidean_cluster_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/euclidean_cluster/src/euclidean_cluster_node.cpp#L67 

Added line #L67 was not covered by tests
sensor_msgs::msg::PointCloud2 debug;
convertObjectMsg2SensorMsg(output, debug);
debug_pub_->publish(debug);
}
if (debug_publisher_) {
const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
const double pipeline_latency_ms =
std::chrono::duration<double, std::milli>(
std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds()))
.count();

Check warning on line 78 in perception/euclidean_cluster/src/euclidean_cluster_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/euclidean_cluster/src/euclidean_cluster_node.cpp#L77-L78 

Added lines #L77 - L78 were not covered by tests
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/cyclic_time_ms", cyclic_time_ms);
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/processing_time_ms", processing_time_ms);
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(

Check warning on line 83 in perception/euclidean_cluster/src/euclidean_cluster_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/euclidean_cluster/src/euclidean_cluster_node.cpp#L83 

Added line #L83 was not covered by tests
"debug/pipeline_latency_ms", pipeline_latency_ms);
}
}

Expand Down
11 changes: 7 additions & 4 deletions perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,21 +76,24 @@
cluster_pub_->publish(output);

// build debug msg
if (debug_pub_->get_subscription_count() < 1) {
return;
}
{
if (debug_pub_->get_subscription_count() >= 1) {

Check warning on line 79 in perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp#L79 

Added line #L79 was not covered by tests
sensor_msgs::msg::PointCloud2 debug;
convertObjectMsg2SensorMsg(output, debug);
debug_pub_->publish(debug);
}
if (debug_publisher_) {
const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
const double pipeline_latency_ms =
std::chrono::duration<double, std::milli>(
std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds()))
.count();

Check warning on line 90 in perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp#L89-L90 

Added lines #L89 - L90 were not covered by tests
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/cyclic_time_ms", cyclic_time_ms);
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/processing_time_ms", processing_time_ms);
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(

Check warning on line 95 in perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp#L95 

Added line #L95 was not covered by tests
"debug/pipeline_latency_ms", pipeline_latency_ms);
}
}

Expand Down
7 changes: 7 additions & 0 deletions perception/image_projection_based_fusion/src/fusion_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -175,11 +175,18 @@
// add processing time for debug
if (debug_publisher_) {
const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
const double pipeline_latency_ms =

Check warning on line 178 in perception/image_projection_based_fusion/src/fusion_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/image_projection_based_fusion/src/fusion_node.cpp#L178 

Added line #L178 was not covered by tests
std::chrono::duration<double, std::milli>(
std::chrono::nanoseconds(
(this->get_clock()->now() - cached_msg_.second->header.stamp).nanoseconds()))

Check warning on line 181 in perception/image_projection_based_fusion/src/fusion_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/image_projection_based_fusion/src/fusion_node.cpp#L181 

Added line #L181 was not covered by tests
.count();
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/cyclic_time_ms", cyclic_time_ms);
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/processing_time_ms",
processing_time_ms + stop_watch_ptr_->toc("processing_time", true));
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(

Check warning on line 188 in perception/image_projection_based_fusion/src/fusion_node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/image_projection_based_fusion/src/fusion_node.cpp#L188 

Added line #L188 was not covered by tests
"debug/pipeline_latency_ms", pipeline_latency_ms);

Check warning on line 189 in perception/image_projection_based_fusion/src/fusion_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

subCallback already has high cyclomatic complexity, and now it increases in Lines of Code from 98 to 105. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
processing_time_ms = 0;
}
}
Expand Down
7 changes: 7 additions & 0 deletions perception/lidar_centerpoint/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -169,10 +169,17 @@
if (debug_publisher_ptr_ && stop_watch_ptr_) {
const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
const double pipeline_latency_ms =
std::chrono::duration<double, std::milli>(
std::chrono::nanoseconds(
(this->get_clock()->now() - output_msg.header.stamp).nanoseconds()))
.count();

Check warning on line 176 in perception/lidar_centerpoint/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/lidar_centerpoint/src/node.cpp#L175-L176 

Added lines #L175 - L176 were not covered by tests
debug_publisher_ptr_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/cyclic_time_ms", cyclic_time_ms);
debug_publisher_ptr_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/processing_time_ms", processing_time_ms);
debug_publisher_ptr_->publish<tier4_debug_msgs::msg::Float64Stamped>(

Check warning on line 181 in perception/lidar_centerpoint/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

perception/lidar_centerpoint/src/node.cpp#L181 

Added line #L181 was not covered by tests
"debug/pipeline_latency_ms", pipeline_latency_ms);
}
}

Expand Down
7 changes: 7 additions & 0 deletions ..._map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -206,10 +206,17 @@
if (debug_publisher_ptr_ && stop_watch_ptr_) {
const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
const double pipeline_latency_ms =
std::chrono::duration<double, std::milli>(
std::chrono::nanoseconds(
(this->get_clock()->now() - input_raw_msg->header.stamp).nanoseconds()))
.count();
debug_publisher_ptr_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/cyclic_time_ms", cyclic_time_ms);
debug_publisher_ptr_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/processing_time_ms", processing_time_ms);
debug_publisher_ptr_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/pipeline_latency_ms", pipeline_latency_ms);

Check warning on line 219 in perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw already has high cyclomatic complexity, and now it increases in Lines of Code from 70 to 77. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
}
}

Expand Down
Loading