Fix TargetPose flatbuffer conversion
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I3ecec7c5f84876edf29531cf9f65a915508344fa
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index ccaa805..5a7b400 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -343,9 +343,9 @@
for (const auto &[id, pose] : target_poses_) {
TargetPoseFbs::Builder target_pose_builder(fbb);
target_pose_builder.add_id(id);
- target_pose_builder.add_x(id);
- target_pose_builder.add_y(id);
- target_pose_builder.add_yaw(id);
+ target_pose_builder.add_x(pose.x);
+ target_pose_builder.add_y(pose.y);
+ target_pose_builder.add_yaw(pose.yaw_radians);
target_poses_fbs.emplace_back(target_pose_builder.Finish());
}