fix(gyro_odometer): fix output frame (autowarefoundation#2562)

Signed-off-by: kminoda <koji.minoda@tier4.jp> Signed-off-by: kminoda <koji.minoda@tier4.jp>
sujithvemi · Dec 23, 2022 · 1f41e67 · 1f41e67
1 parent db37c87
commit 1f41e67
Showing 1 changed file with 2 additions and 0 deletions.
diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp
@@ -80,6 +80,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer(
   } else {
     twist_with_cov.header.stamp = latest_vehicle_twist_stamp;
   }
+  twist_with_cov.header.frame_id = gyro_queue.front().header.frame_id;
   twist_with_cov.twist.twist.linear.x = vx_mean;
   twist_with_cov.twist.twist.angular = gyro_mean;
 
@@ -211,6 +212,7 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m
 
   sensor_msgs::msg::Imu gyro_base_link;
   gyro_base_link.header = imu_msg_ptr->header;
+  gyro_base_link.header.frame_id = output_frame_;
   gyro_base_link.angular_velocity = transformed_angular_velocity.vector;
   gyro_base_link.angular_velocity_covariance =
     transformCovariance(imu_msg_ptr->angular_velocity_covariance);