| 9 | |
| 10 | # TODO(marcus): add support for choosing EuRoC standard or SWE format. |
| 11 | class GTLoggerNode: |
| 12 | def __init__(self): |
| 13 | self.gt_topic = rospy.get_param("~gt_topic") |
| 14 | self.output_dir = rospy.get_param("~output_dir") |
| 15 | self.output_csv_file = os.path.join(self.output_dir, "output_gt_poses.csv") |
| 16 | |
| 17 | # rospy.Subscriber(self.gt_topic, TransformStamped, self.gt_cb_tsf) |
| 18 | rospy.Subscriber(self.gt_topic, Odometry, self.gt_cb_odom, queue_size=20) |
| 19 | self.setup_gt_file() |
| 20 | |
| 21 | def gt_cb_tfs(self, msg): |
| 22 | """ Callback for ground-truth poses as they come in as TransformStamped |
| 23 | msgs. |
| 24 | |
| 25 | Writes each pose to a csv file in the swe format. This format is |
| 26 | as follows: |
| 27 | timestamp[ns], x, y, z, qw, qx, qy, qz |
| 28 | x, y, z are position coordinates in the world frame and qx-qw are |
| 29 | quaternion values, also in the world frame. Everything afterwards |
| 30 | is zero. |
| 31 | |
| 32 | Args: |
| 33 | // TODO: this should be a odometry msg... |
| 34 | msg: A geometry_msgs/TransformStamped object representing |
| 35 | the current ground-truth body transform. |
| 36 | """ |
| 37 | with open(self.output_csv_file, mode='a') as file: |
| 38 | # Because csv only accepts one-char delimiters, we add the space |
| 39 | # in each datum. |
| 40 | writer = csv.writer(file, delimiter=",") |
| 41 | writer.writerow([str(msg.header.stamp.to_nsec()), |
| 42 | str(msg.transform.translation.x), |
| 43 | str(msg.transform.translation.y), |
| 44 | str(msg.transform.translation.z), |
| 45 | str(msg.transform.rotation.w), |
| 46 | str(msg.transform.rotation.x), |
| 47 | str(msg.transform.rotation.y), |
| 48 | str(msg.transform.rotation.z)]) |
| 49 | |
| 50 | def gt_cb_odom(self, msg): |
| 51 | """ Callback for ground-truth poses as they come in as Odometry msgs. |
| 52 | |
| 53 | Writes each pose to a csv file in the swe format. This format is |
| 54 | as follows: |
| 55 | timestamp[ns], x, y, z, qw, qx, qy, qz |
| 56 | x, y, z are position coordinates in the world frame and qx-qw are |
| 57 | quaternion values, also in the world frame. Everything afterwards |
| 58 | is zero. |
| 59 | |
| 60 | Args: |
| 61 | msg: A geometry_msgs/TransformStamped object representing |
| 62 | the current ground-truth body transform. |
| 63 | """ |
| 64 | with open(self.output_csv_file, mode='a') as file: |
| 65 | # Because csv only accepts one-char delimiters, we add the space |
| 66 | # in each datum. |
| 67 | writer = csv.writer(file, delimiter=",") |
| 68 | writer.writerow([str(msg.header.stamp.to_nsec()), |