Skip to content

feat(autoware): Add PointCloud messages and safe access abstractions - #783

Closed
romancardenas wants to merge 6 commits into
tier4:mainfrom
romancardenas:autoware/pointcloud
Closed

romancardenas wants to merge 6 commits into
tier4:mainfrom
romancardenas:autoware/pointcloud

Conversation

@romancardenas

Copy link
Copy Markdown
Contributor

Description

This PR is part of #748.

It includes ROS2 messages used in autoware for the crop box filter, including:

It also provides Rust safe abstractions to manipulate point clouds:

  • PointFieldError: errors in the definition of point fields.
  • PointCloudError: errors in the definition of point clouds.
  • PointCloudAccessor: safe abstraction to manipulate a point cloud. The PointCloudReader and PointCloudWriter aliases provide inmutable and mutable access.
  • PointAccessor: safe abstraction to manipulate a single point. The PointReader and PointWriter aliases provide inmutable and mutable access.
  • FieldAccessor: safe abstraction to manipulate a single field of a point.

It also provides typed versions that provide safe access to underlying data types without constantly checking that point fields are properly defined. The PR provides a README.md file with further details and block diagrams to better understand how it works.

With #780 and #781 , these PRs provide all the required blocks for implementing the crop box filter.

How was this PR tested?

  • scripts/ci.sh passes on my machine.

Comment on lines +101 to +114
// Check for valid row step (row padding allowed)
if min_row_step > row_step {
return Err(PointCloudError::InvalidRowStep {
step: row_step,
min: min_row_step,
});
}
// Check for valid data size (must match exactly the expected size)
if message.data.len() != expected_size {
return Err(PointCloudError::InvalidDataSize {
size: message.data.len(),
expected: expected_size,
});
}

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

Fidelity: the original ignores width/height/row_step; the iterator walks all of data by point_step (impl/point_cloud2_iterator.hpp L263-269), and crop_box_filter only checks w * h * point_step <= data.size() (crop_box_filter.cpp L56-67).
So extra data, a wrong row_step, or padded rows are processed there but rejected (or skipped) here.
Since an Awkernel reactor can't skip publishing like the C++ node (crop_box_filter_node.cpp L129-133), is this deviation intentional?

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

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

Good question. Do we want faithful support even for faulty point cloud messages? Or do we want to enforce proper format using Rust error handling mechanisms?

A problem of just checking w * h * point_step <= data.size() is that, if row_step is correct and w * point_step < row_step (i.e., there is padding per row), we are not considering the row padding and we are not successfully detecting a faulty message.

We can soften the check to row_step * height <= data.len(). In that case, we are ignoring trailing data.

Regarding reactors that can't skip publishing, the final goal would be to ensure that point cloud messages are always correctly formatted, and these checks should never fail.

In fact, once we have Autoware running on Awkernel, we can move part of the checks to the point cloud message constructor itself, so we just check that the point cloud is correct once.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

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

I relaxed the check to row_step * height <= data.len().

Let me know if you prefer a different requirement.

width: usize,
height: usize,
point_count: usize,
fields: Vec<CheckedField>,

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

RT: every accessor construction allocates a Vec<CheckedField> (also via try_new_typed, L155-163), i.e. heap allocations every period in the crop_box port.
For typed selections the field count is known at compile time, so S::Checked alone could hold the metadata.

});
}

let mut fields = Vec::new();

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

Same for L54.

Comment on lines +24 to +29
fn field_info(&self, name: &'static str) -> Result<&'a CheckedField, PointCloudError> {
self.fields
.iter()
.find(|field| field.name() == name)
.ok_or(PointFieldError::FieldNotFound(name).into())
}

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

RT: untyped field(name) does a string search per call, while the original resolves names once before the loop (crop_box_filter.cpp L160-172).
Could the docs recommend try_new_typed for per-point loops?

/// This is a plain message: all fields are public and may contain an invalid layout
/// or either byte order. Validated access requires native-endian data.
/// Borrow it through [`PointCloudReader`] or [`PointCloudWriter`] before access.
pub struct PointCloud {

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

Question: is the missing Clone intentional? Awkernel pub/sub requires T: Clone (awkernel_async_lib/src/pubsub.rs L516) and clones per subscriber (L311, L363), so Clone would deep-copy data every period.
If the intent is to pass Arc<PointCloud>, a doc note would help. A manual Debug (dimensions and fields only) would also be useful for tests.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

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

Derived Clone.

Manually implemented Debug. I only skipped the data field. Let me know if you prefer to skip other fields as well.

Added a comment in the documentation to recommend users using Arc<CloudPoint> to avoid expensive cloning.

@romancardenas

Copy link
Copy Markdown
Contributor Author

Closing in favor of #801 and #802

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

Port Crop Box Filtering to Awkernel

2 participants