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(autoware_sensing_msgs): implemented the proposed universal radar messages #120

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
8 changes: 7 additions & 1 deletion autoware_sensing_msgs/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,13 @@ ament_auto_find_build_dependencies()

set(msg_files
"msg/GnssInsOrientation.msg"
"msg/GnssInsOrientationStamped.msg")
"msg/GnssInsOrientationStamped.msg"
"msg/RadarClassification.msg"
"msg/RadarFieldInfo.msg"
"msg/RadarInfo.msg"
"msg/RadarObject.msg"
"msg/RadarObjects.msg"
)

set(msg_dependencies
geometry_msgs
Expand Down
18 changes: 18 additions & 0 deletions autoware_sensing_msgs/msg/RadarClassification.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
# Common classes with autoware_perception_msgs/msg/ObjectClassification.msg
uint32 UNKNOWN = 0
uint32 CAR = 1
uint32 TRUCK = 2
uint32 BUS = 3
uint32 TRAILER = 4
uint32 MOTORCYCLE = 5
uint32 BICYCLE = 6
uint32 PEDESTRIAN = 7

# Radar specific classes
uint32 ANIMAL = 8
uint32 HAZARD = 9
uint32 OVER_DRIVABLE = 10
uint32 UNDER_DRIVABLE = 11
Comment on lines +12 to +15
Copy link
Collaborator

Choose a reason for hiding this comment

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

What about updating https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/ObjectClassification.msg
list with these items? We could avoid creating a new classification message and duplicating information.

Also, what are hazard, over drivable, under drivable? And why are they radar specific?

Copy link
Author

Choose a reason for hiding this comment

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

@xmfcx
Thank you for the comments.
I agree with duplicating information, but using perception msgs in the sensing package seems unnatural. Would you prefer moving the ObjectClassification message to sensing ? using perception msgs as a dependency in sensing ? moving it outside to a common one?

The radar interfaces that I saw define them, but there is nothing that explains them. We could omit them, and update the universal interface. AFAIK:

  • hazard: unknown but that is dangerous for AD
  • overdrivable: leaves, plstic bags, etc
  • underdrivable: overpasses (?)

Copy link
Collaborator

Choose a reason for hiding this comment

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

This makes me consider even merging sensing and perception messages under perception as a general category.

Because sensing layer is doing some of the perception tasks here.

@mitsudome-r what do you think?
We don't have many sensing messages too, do you think these could be merged?

Also I see that https://github.com/tier4/tier4_autoware_msgs there is no tier4_sensing_msgs too. So I'd suggest merging.

Choose a reason for hiding this comment

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

Even for perception point-of-view, I think those new classes are necessary. Current perception do not distinguishes obstacles can be ignored or not ignored for driving, but just classified as "Unknown".


uint32 label
float32 probability
8 changes: 8 additions & 0 deletions autoware_sensing_msgs/msg/RadarFieldInfo.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
std_msgs/String field_name
bool min_value_available
Copy link

Choose a reason for hiding this comment

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

For the availability bools, I remember there was a discussion about using NaN (I guess the above suggestion by @xmfcx , using -1.0f may work for resolution, but it's a bit dangerous as requires the user to check an apparently irrelevant field for availability). I'm guessing having the bools as proposed is cleaner?

Copy link

Choose a reason for hiding this comment

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

Another way of doing this would be to use a field like
float32[<=1] min_value
which is functionally equivalent to an std::optional<float32>.
This would remove the need for the boolean field, and the user is forced to check if the value is present. Probably a bit ugly to use since it shows up as a dynamic bounded array, not an optional 🥲

bool max_value_available
bool resolution_available

float32 min_value
float32 max_value
float32 resolution
6 changes: 6 additions & 0 deletions autoware_sensing_msgs/msg/RadarInfo.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
std_msgs/Header header
RadarFieldInfo[] object_fields_info
RadarFieldInfo[] detection_fields_info

uint32[] available_classes
bool absolute_dynamics # absolute or relative dynamics
35 changes: 35 additions & 0 deletions autoware_sensing_msgs/msg/RadarObject.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
float32 INVALID_COV_VALUE = 100000000.0
Copy link

Choose a reason for hiding this comment

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

Instead of a dummy value we should most probably use NaN

Copy link
Author

Choose a reason for hiding this comment

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

We discussed this internally, and the architects decision was not to use NaNs or infs

Copy link

@mojomex mojomex Mar 11, 2025

Choose a reason for hiding this comment

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

So, if the user of these messages makes a mistake, the decision is to produce values that are valid numbers in a floating-point sense (not NaN) but invalid in a semantic sense (nonsensical value). This rids us of any chance to detect faulty assumptions about the content of the messages.

What were the reasons for this choice?

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
float32 INVALID_COV_VALUE = 100000000.0
float32 INVALID_COV_VALUE = -1.0f;

https://en.wikipedia.org/wiki/Covariance_matrix

Any covariance matrix is symmetric and positive semi-definite and its main diagonal contains variances (i.e., the covariance of each element with itself).

And since the message contains the diagonal of this matrix in the arrays below, we could make the invalid value -1.0f.

Copy link

Choose a reason for hiding this comment

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

@xmfcx That's at least a value that is not semantically valid. I still don't get the aversion to using NaN as one of the many reasons for its existence is exactly this use case: to mark invalid entries.

Copy link
Collaborator

Choose a reason for hiding this comment

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

I'd guess it's because of varying definitions of inf and nans across multiple systems.


uint8 MEASUREMENT_STATUS_MEASURED = 0
uint8 MEASUREMENT_STATUS_PREDICTED = 1
uint8 MEASUREMENT_STATUS_NEW = 2
uint8 MEASUREMENT_STATUS_UNKNOWN = 3
uint8 MEASUREMENT_STATUS_INVALID = 255
Copy link

Choose a reason for hiding this comment

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

Not sure if these values are chosen for backward compatibility, but e.g. Protobuf recommends to make 0 the invalid option such that any zero-initialized message is invalid by default.


uint8 MOVEMENT_STATUS_DYNAMIC = 0
uint8 MOVEMENT_STATUS_STATIC = 1
uint8 MOVEMENT_STATUS_UNKNOWN = 2
uint8 MOVEMENT_STATUS_INVALID = 255

uint32 object_id
uint16 age
uint8 measurement_status
uint8 movement_status

geometry_msgs/Point position
geometry_msgs/Vector3 velocity
geometry_msgs/Vector3 acceleration
geometry_msgs/Vector3 shape

Choose a reason for hiding this comment

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

Is shape means dimension of 3d bounding box?

Copy link
Author

Choose a reason for hiding this comment

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

Yes. That being said, the perception package has has one that is more general https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/Shape.msg
In the past I though that is was more inefficient (more bloat), but it seems that is not the case.
However, Shape is in the perception package. Assuming that radars can really only detect bboxes, would you prefer to keep it as is (with a better name like box_shape) or reuse Shape by redefining the message in the sensing package?

Copy link

@technolojin technolojin Mar 11, 2025

Choose a reason for hiding this comment

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

If the message only support bbox, just rename the vector3 "shape" to "size" or "dimensions" or something else would be enough.


float32[6] position_cov
float32[6] velocity_cov
float32[6] acceleration_cov
float32[6] shape_cov

float32 orientation
float32 orientation_std
float32 orientation_rate
float32 orientation_rate_std

float32 existence_probability
RadarClassification[] classifications
2 changes: 2 additions & 0 deletions autoware_sensing_msgs/msg/RadarObjects.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
std_msgs/Header header
RadarObject[] objects