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

fix(tier4_autoware_utils): fix covariance enum name #1950

Merged
merged 6 commits into from
Sep 27, 2022
Merged

fix(tier4_autoware_utils): fix covariance enum name #1950

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
bbab695
fix(tier4_autoware_utils): change XYZRPY covariance enum name
scepter914 Sep 25, 2022
9baf78d
fix comment
scepter914 Sep 25, 2022
20ab65a
fix covariance index
scepter914 Sep 25, 2022
21b7e44
fix covariance index
scepter914 Sep 25, 2022
4c8c9b2
fix typo
scepter914 Sep 25, 2022
d2d66d3
fix covariance variable name
scepter914 Sep 26, 2022
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: 4 additions & 4 deletions common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,14 +54,14 @@ enum RPY_COV_IDX {
};
} // namespace rpy_covariance_index

namespace pose_covariance_index
namespace xyzrpy_covariance_index
{
/// Covariance for 6-DOF pose.
/// Covariance for 6-DOF.
/// Used at
/// - geometry_msgs/msg/AccelWithCovariance.msg: msg.covariance
/// - geometry_msgs/msg/TwistWithCovariance.msg: msg.covariance
/// - geometry_msgs/msg/PoseWithCovariance.msg: msg.covariance
enum POSE_COV_IDX {
enum XYZRPY_COV_IDX {
X_X = 0,
X_Y = 1,
X_Z = 2,
Expand Down Expand Up @@ -99,7 +99,7 @@ enum POSE_COV_IDX {
YAW_PITCH = 34,
YAW_YAW = 35
};
} // namespace pose_covariance_index
} // namespace xyzrpy_covariance_index

namespace xyz_upper_covariance_index
{
Expand Down
2 changes: 1 addition & 1 deletion perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -133,7 +133,7 @@ RadarFusionToDetectedObject::Output RadarFusionToDetectedObject::update(
bool RadarFusionToDetectedObject::hasTwistCovariance(
const TwistWithCovariance & twist_with_covariance)
{
using IDX = tier4_autoware_utils::pose_covariance_index::POSE_COV_IDX;
using IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
auto covariance = twist_with_covariance.covariance;
if (covariance[IDX::X_X] == 0.0 && covariance[IDX::Y_Y] == 0.0 && covariance[IDX::Z_Z] == 0.0) {
return false;
Expand Down
67 changes: 35 additions & 32 deletions ..._msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -162,8 +162,8 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects()
TrackedObjects tracked_objects;
tracked_objects.header = radar_data_->header;
tracked_objects.header.frame_id = node_param_.new_frame_id;
using P_IDX = tier4_autoware_utils::pose_covariance_index::POSE_COV_IDX;
using R_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX;
using POSE_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
using RADAR_IDX = tier4_autoware_utils::xyz_upper_covariance_index::XYZ_UPPER_COV_IDX;

for (auto & radar_track : radar_data_->tracks) {
TrackedObject tracked_object;
Expand All @@ -187,16 +187,17 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects()
kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose;

{
auto & covariance = kinematics.pose_with_covariance.covariance;
covariance[P_IDX::X_X] = radar_track.position_covariance[R_IDX::X_X];
covariance[P_IDX::X_Y] = radar_track.position_covariance[R_IDX::X_Y];
covariance[P_IDX::X_Z] = radar_track.position_covariance[R_IDX::X_Z];
covariance[P_IDX::Y_X] = radar_track.position_covariance[R_IDX::X_Y];
covariance[P_IDX::Y_Y] = radar_track.position_covariance[R_IDX::Y_Y];
covariance[P_IDX::Y_Z] = radar_track.position_covariance[R_IDX::Y_Z];
covariance[P_IDX::Z_X] = radar_track.position_covariance[R_IDX::X_Z];
covariance[P_IDX::Z_Y] = radar_track.position_covariance[R_IDX::Y_Z];
covariance[P_IDX::Z_Z] = radar_track.position_covariance[R_IDX::Z_Z];
auto & pose_cov = kinematics.pose_with_covariance.covariance;
auto & radar_position_cov = radar_track.position_covariance;
pose_cov[POSE_IDX::X_X] = radar_position_cov[RADAR_IDX::X_X];
pose_cov[POSE_IDX::X_Y] = radar_position_cov[RADAR_IDX::X_Y];
pose_cov[POSE_IDX::X_Z] = radar_position_cov[RADAR_IDX::X_Z];
pose_cov[POSE_IDX::Y_X] = radar_position_cov[RADAR_IDX::X_Y];
pose_cov[POSE_IDX::Y_Y] = radar_position_cov[RADAR_IDX::Y_Y];
pose_cov[POSE_IDX::Y_Z] = radar_position_cov[RADAR_IDX::Y_Z];
pose_cov[POSE_IDX::Z_X] = radar_position_cov[RADAR_IDX::X_Z];
pose_cov[POSE_IDX::Z_Y] = radar_position_cov[RADAR_IDX::Y_Z];
pose_cov[POSE_IDX::Z_Z] = radar_position_cov[RADAR_IDX::Z_Z];
}

// convert by tf
Expand All @@ -218,28 +219,30 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects()
}

{
auto & covariance = kinematics.twist_with_covariance.covariance;
covariance[P_IDX::X_X] = radar_track.velocity_covariance[R_IDX::X_X];
covariance[P_IDX::X_Y] = radar_track.velocity_covariance[R_IDX::X_Y];
covariance[P_IDX::X_Z] = radar_track.velocity_covariance[R_IDX::X_Z];
covariance[P_IDX::Y_X] = radar_track.velocity_covariance[R_IDX::X_Y];
covariance[P_IDX::Y_Y] = radar_track.velocity_covariance[R_IDX::Y_Y];
covariance[P_IDX::Y_Z] = radar_track.velocity_covariance[R_IDX::Y_Z];
covariance[P_IDX::Z_X] = radar_track.velocity_covariance[R_IDX::X_Z];
covariance[P_IDX::Z_Y] = radar_track.velocity_covariance[R_IDX::Y_Z];
covariance[P_IDX::Z_Z] = radar_track.velocity_covariance[R_IDX::Z_Z];
auto & twist_cov = kinematics.twist_with_covariance.covariance;
auto & radar_vel_cov = radar_track.velocity_covariance;
twist_cov[POSE_IDX::X_X] = radar_vel_cov[RADAR_IDX::X_X];
twist_cov[POSE_IDX::X_Y] = radar_vel_cov[RADAR_IDX::X_Y];
twist_cov[POSE_IDX::X_Z] = radar_vel_cov[RADAR_IDX::X_Z];
twist_cov[POSE_IDX::Y_X] = radar_vel_cov[RADAR_IDX::X_Y];
twist_cov[POSE_IDX::Y_Y] = radar_vel_cov[RADAR_IDX::Y_Y];
twist_cov[POSE_IDX::Y_Z] = radar_vel_cov[RADAR_IDX::Y_Z];
twist_cov[POSE_IDX::Z_X] = radar_vel_cov[RADAR_IDX::X_Z];
twist_cov[POSE_IDX::Z_Y] = radar_vel_cov[RADAR_IDX::Y_Z];
twist_cov[POSE_IDX::Z_Z] = radar_vel_cov[RADAR_IDX::Z_Z];
}
{
auto & covariance = kinematics.acceleration_with_covariance.covariance;
covariance[P_IDX::X_X] = radar_track.acceleration_covariance[R_IDX::X_X];
covariance[P_IDX::X_Y] = radar_track.acceleration_covariance[R_IDX::X_Y];
covariance[P_IDX::X_Z] = radar_track.acceleration_covariance[R_IDX::X_Z];
covariance[P_IDX::Y_X] = radar_track.acceleration_covariance[R_IDX::X_Y];
covariance[P_IDX::Y_Y] = radar_track.acceleration_covariance[R_IDX::Y_Y];
covariance[P_IDX::Y_Z] = radar_track.acceleration_covariance[R_IDX::Y_Z];
covariance[P_IDX::Z_X] = radar_track.acceleration_covariance[R_IDX::X_Z];
covariance[P_IDX::Z_Y] = radar_track.acceleration_covariance[R_IDX::Y_Z];
covariance[P_IDX::Z_Z] = radar_track.acceleration_covariance[R_IDX::Z_Z];
auto & accel_cov = kinematics.acceleration_with_covariance.covariance;
auto & radar_accel_cov = radar_track.acceleration_covariance;
accel_cov[POSE_IDX::X_X] = radar_accel_cov[RADAR_IDX::X_X];
accel_cov[POSE_IDX::X_Y] = radar_accel_cov[RADAR_IDX::X_Y];
accel_cov[POSE_IDX::X_Z] = radar_accel_cov[RADAR_IDX::X_Z];
accel_cov[POSE_IDX::Y_X] = radar_accel_cov[RADAR_IDX::X_Y];
accel_cov[POSE_IDX::Y_Y] = radar_accel_cov[RADAR_IDX::Y_Y];
accel_cov[POSE_IDX::Y_Z] = radar_accel_cov[RADAR_IDX::Y_Z];
accel_cov[POSE_IDX::Z_X] = radar_accel_cov[RADAR_IDX::X_Z];
accel_cov[POSE_IDX::Z_Y] = radar_accel_cov[RADAR_IDX::Y_Z];
accel_cov[POSE_IDX::Z_Z] = radar_accel_cov[RADAR_IDX::Z_Z];
}

tracked_object.kinematics = kinematics;
Expand Down