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.

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(hesai): add filtered pointcloud counter function #247

Open
wants to merge 14 commits into
base: main
Choose a base branch
from
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,10 @@
#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>

#include <sys/types.h>

#include <algorithm>
#include <array>
#include <cmath>
#include <cstdint>
#include <memory>
#include <tuple>
#include <utility>
Expand All @@ -41,84 +40,73 @@

struct HesaiDecodeFilteredInfo
mojomex marked this conversation as resolved.
Show resolved Hide resolved
{
uint16_t distance_filtered_count = 0;
uint16_t fov_filtered_count = 0;
uint16_t timestamp_filtered_count = 0;
uint16_t invalid_point_count = 0;
uint16_t multiple_return_point_count = 0;
uint16_t mutliple_return_point_count = 0;
uint16_t total_kept_point_count = 0;
uint16_t invalid_packet_count = 0;
float cloud_distance_min_m = 0;
float cloud_distance_max_m = 0;
float cloud_azimuth_min_rad = 0;
float cloud_azimuth_max_rad = 0;
uint64_t packet_timestamp_min_ns = 0;
uint64_t packet_timestamp_max_ns = 0;

void clear()
{
distance_filtered_count = 0;
fov_filtered_count = 0;
timestamp_filtered_count = 0;
invalid_point_count = 0;
multiple_return_point_count = 0;
mutliple_return_point_count = 0;
total_kept_point_count = 0;
invalid_packet_count = 0;
cloud_distance_min_m = 0;
cloud_distance_max_m = 0;
cloud_azimuth_min_rad = 0;
cloud_azimuth_max_rad = 0;
packet_timestamp_min_ns = 0;
packet_timestamp_max_ns = 0;
}
uint64_t distance_filtered_count = 0;
uint64_t fov_filtered_count = 0;
uint64_t invalid_point_count = 0;
uint64_t identical_filtered_count = 0;
uint64_t multiple_return_filtered_count = 0;
uint64_t total_kept_point_count = 0;
uint64_t invalid_packet_count = 0;
float cloud_distance_min_m = std::numeric_limits<float>::infinity();
float cloud_distance_max_m = std::numeric_limits<float>::lowest();
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

While also okay, lowest() represents the lowest finite number, so for consistency, and to signal that the initial value is not a valid one, let's change this to -infinity() instead:

Suggested change
float cloud_distance_max_m = std::numeric_limits<float>::lowest();
float cloud_distance_max_m = -std::numeric_limits<float>::infinity();

float cloud_azimuth_min_deg = std::numeric_limits<float>::infinity();
float cloud_azimuth_max_rad = std::numeric_limits<float>::lowest();
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
float cloud_azimuth_max_rad = std::numeric_limits<float>::lowest();
float cloud_azimuth_max_rad = -std::numeric_limits<float>::infinity();

uint64_t packet_timestamp_min_ns = std::numeric_limits<uint64_t>::max();
uint64_t packet_timestamp_max_ns = std::numeric_limits<uint64_t>::min();

[[nodiscard]] nlohmann::ordered_json to_json() const
{
nlohmann::json distance_j;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please use ordered_json throughout your code to preserve the field ordering when printing.

distance_j["filter"] = "distance";
distance_j["distance_filtered_count"] = distance_filtered_count;
distance_j["cloud_distance_min_m"] = cloud_distance_min_m;
distance_j["cloud_distance_max_m"] = cloud_distance_max_m;
distance_j["name"] = "distance";
distance_j["filtered_count"] = distance_filtered_count;
// distance_j["cloud_distance_min_m"] = cloud_distance_min_m;
// distance_j["cloud_distance_max_m"] = cloud_distance_max_m;
nlohmann::json fov_j;
fov_j["filter"] = "fov";
fov_j["fov_filtered_count"] = fov_filtered_count;
fov_j["cloud_azimuth_min_rad"] = cloud_azimuth_min_rad;
fov_j["cloud_azimuth_max_rad"] = cloud_azimuth_max_rad;
nlohmann::json timestamp_j;
timestamp_j["filter"] = "timestamp";
timestamp_j["timestamp_filtered_count"] = timestamp_filtered_count;
timestamp_j["packet_timestamp_min_ns"] = packet_timestamp_min_ns;
timestamp_j["packet_timestamp_max_ns"] = packet_timestamp_max_ns;
fov_j["name"] = "fov";
fov_j["filtered_count"] = fov_filtered_count;
// fov_j["cloud_azimuth_min_deg"] = cloud_azimuth_min_deg;
// fov_j["cloud_azimuth_max_rad"] = cloud_azimuth_max_rad;
nlohmann::json identical_j;
identical_j["name"] = "identical";
identical_j["filtered_count"] = identical_filtered_count;
nlohmann::json multiple_j;
multiple_j["name"] = "multiple";
multiple_j["filtered_count"] = multiple_return_filtered_count;
nlohmann::json invalid_j;
invalid_j["filter"] = "invalid";
invalid_j["name"] = "invalid";
invalid_j["invalid_point_count"] = invalid_point_count;
invalid_j["invalid_packet_count"] = invalid_packet_count;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Invalid points and packets are different concepts:

  • while invalid points are normal (the sensor sends them when there was no object hit, or when an object is too close),
  • invalid packets are an error (the size does not match our expectations)

So, please make the invalid points as part of the filter pipeline, and move invalid packets to the top level.
Also see this previous comment.

nlohmann::json identical_j;
identical_j["filter"] = "identical";
identical_j["multiple_return_point_count"] = multiple_return_point_count;
nlohmann::json multiple_j;
multiple_j["filter"] = "multiple";
multiple_j["mutliple_return_point_count"] = mutliple_return_point_count;
nlohmann::json pointcloud_bounds_azimuth_j;
pointcloud_bounds_azimuth_j["min"] = cloud_azimuth_min_deg;
pointcloud_bounds_azimuth_j["max"] = cloud_azimuth_max_rad;
nlohmann::json pointcloud_bounds_distance_j;
pointcloud_bounds_distance_j["min"] = cloud_distance_min_m;
pointcloud_bounds_distance_j["max"] = cloud_distance_max_m;
nlohmann::json pointcloud_bounds_timestamp_j;
pointcloud_bounds_timestamp_j["min"] = packet_timestamp_min_ns;
pointcloud_bounds_timestamp_j["max"] = packet_timestamp_max_ns;

nlohmann::json j;
j["filter_pipeline"] = nlohmann::json::array({
distance_j,
fov_j,
timestamp_j,
invalid_j,
identical_j,
multiple_j,
});
j["pointcloud_bounds"] = {
j["azimuth_deg"] = pointcloud_bounds_azimuth_j,
j["distance_m"] = pointcloud_bounds_distance_j,
j["timestamp_ns"] = pointcloud_bounds_timestamp_j,
Comment on lines +97 to +99
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This does not work as-is (see the 0/1/2 instead of azimuth_deg/distance_m/timestamp_nsin your self-evaluation). Please check the nlohmann json documentation for how to specify JSON {"key": value} pairs.

};
j["invalid_filter"] = invalid_j;
j["total_kept_point_count"] = total_kept_point_count;

return j;
}

void get_minmax_info(const NebulaPoint & point)
void update_pointcloud_bounds(const NebulaPoint & point)
{
cloud_azimuth_min_rad = std::min(cloud_azimuth_min_rad, point.azimuth);
cloud_azimuth_min_deg = std::min(cloud_azimuth_min_deg, point.azimuth);
cloud_azimuth_max_rad = std::max(cloud_azimuth_max_rad, point.azimuth);
packet_timestamp_min_ns =
std::min(packet_timestamp_min_ns, static_cast<uint64_t>(point.time_stamp));
Expand Down Expand Up @@ -245,7 +233,7 @@

// Keep only last of multiple identical points
if (return_type == ReturnType::IDENTICAL && block_offset != n_blocks - 1) {
decode_filtered_info_.multiple_return_point_count++;
decode_filtered_info_.identical_filtered_count++;
continue;
}

Expand All @@ -267,7 +255,7 @@
}

if (is_below_multi_return_threshold) {
decode_filtered_info_.mutliple_return_point_count++;
decode_filtered_info_.multiple_return_filtered_count++;
continue;
}
}
Expand Down Expand Up @@ -316,7 +304,7 @@
point.azimuth = corrected_angle_data.azimuth_rad;
point.elevation = corrected_angle_data.elevation_rad;

decode_filtered_info_.get_minmax_info(point);
decode_filtered_info_.update_pointcloud_bounds(point);
decode_filtered_info_.total_kept_point_count++;
}
}
Expand Down Expand Up @@ -373,7 +361,7 @@
int unpack(const std::vector<uint8_t> & packet) override
{
if (!parse_packet(packet)) {
decode_filtered_info_.invalid_packet_count++;

Check warning on line 364 in nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp

View check run for this annotation

Codecov / codecov/patch

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp#L364

Added line #L364 was not covered by tests
return -1;
}

Expand Down Expand Up @@ -434,7 +422,7 @@
}
}
std::cout << "=======================" << std::endl;
decode_filtered_info_.clear();
decode_filtered_info_ = HesaiDecodeFilteredInfo{};
}

last_azimuth_ = block_azimuth;
Expand Down
1 change: 0 additions & 1 deletion ros2_socketcan
mojomex marked this conversation as resolved.
Outdated
Show resolved Hide resolved
Submodule ros2_socketcan deleted from 4ced52
1 change: 0 additions & 1 deletion transport_drivers
mojomex marked this conversation as resolved.
Outdated
Show resolved Hide resolved
Submodule transport_drivers deleted from 86b9aa
Loading