Skip to content

Commit

Permalink
refactor(radar_tracks_msgs_converter): refactor to use covariance ind…
Browse files Browse the repository at this point in the history 
…ex library in tier4_autoware_util (autowarefoundation#1883)

Signed-off-by: scepter914 <scepter914@gmail.com>

Signed-off-by: scepter914 <scepter914@gmail.com>
  • Loading branch information
@scepter914 @boyali
scepter914 authored and boyali committed Oct 19, 2022
1 parent 73a622d commit 9e7393c
Showing 1 changed file with 26 additions and 22 deletions.
48 changes: 26 additions & 22 deletions ..._msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -162,6 +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;

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

{
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;

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];
Expand Down Expand Up @@ -218,25 +217,30 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects()
}
}

kinematics.twist_with_covariance.covariance[0] = radar_track.velocity_covariance[0];
kinematics.twist_with_covariance.covariance[1] = radar_track.velocity_covariance[1];
kinematics.twist_with_covariance.covariance[2] = radar_track.velocity_covariance[2];
kinematics.twist_with_covariance.covariance[6] = radar_track.velocity_covariance[1];
kinematics.twist_with_covariance.covariance[7] = radar_track.velocity_covariance[3];
kinematics.twist_with_covariance.covariance[8] = radar_track.velocity_covariance[4];
kinematics.twist_with_covariance.covariance[12] = radar_track.velocity_covariance[2];
kinematics.twist_with_covariance.covariance[13] = radar_track.velocity_covariance[4];
kinematics.twist_with_covariance.covariance[14] = radar_track.velocity_covariance[5];

kinematics.acceleration_with_covariance.covariance[0] = radar_track.acceleration_covariance[0];
kinematics.acceleration_with_covariance.covariance[1] = radar_track.acceleration_covariance[1];
kinematics.acceleration_with_covariance.covariance[2] = radar_track.acceleration_covariance[2];
kinematics.acceleration_with_covariance.covariance[6] = radar_track.acceleration_covariance[1];
kinematics.acceleration_with_covariance.covariance[7] = radar_track.acceleration_covariance[3];
kinematics.acceleration_with_covariance.covariance[8] = radar_track.acceleration_covariance[4];
kinematics.acceleration_with_covariance.covariance[12] = radar_track.acceleration_covariance[2];
kinematics.acceleration_with_covariance.covariance[13] = radar_track.acceleration_covariance[4];
kinematics.acceleration_with_covariance.covariance[14] = radar_track.acceleration_covariance[5];
{
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 & 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];
}

tracked_object.kinematics = kinematics;

Expand Down

0 comments on commit 9e7393c

Please sign in to comment.