From 7263bb31fbea9dc5fa18ad35ae09e6505452402e Mon Sep 17 00:00:00 2001 From: Gui Date: Thu, 28 Sep 2023 11:14:14 -0700 Subject: [PATCH] Relsponed & pvt msgs for playback example --- py/examples/file_reader_gps/main.py | 123 ++++++++++++++++++---------- 1 file changed, 81 insertions(+), 42 deletions(-) diff --git a/py/examples/file_reader_gps/main.py b/py/examples/file_reader_gps/main.py index 85d020c1..24f66088 100644 --- a/py/examples/file_reader_gps/main.py +++ b/py/examples/file_reader_gps/main.py @@ -21,45 +21,75 @@ from farm_ng.gps import gps_pb2 -def unpack_gps_message(msg: gps_pb2.RelativePositionFrame) -> tuple: +def unpack_gps_message(msg: gps_pb2.RelativePositionFrame | gps_pb2.GpsFrame, msg_type: str) -> tuple: """Unpacks a gps_pb2.RelativePositionFrame message into a list of values. Args: - msg (gps_pb2.RelativePositionFrame): The message to unpack. + msg (gps_pb2.RelativePositionFrame | gps_pb2.GpsFrame): The message to unpack. Returns: tuple: The unpacked message. """ - stamp: float = msg.stamp.stamp - gps_time: float = msg.gps_time.stamp - relative_pose_north: float = msg.relative_pose_north - relative_pose_east: float = msg.relative_pose_east - relative_pose_down: float = msg.relative_pose_down - relative_pose_length: float = msg.relative_pose_length - accuracy_north: float = msg.accuracy_north - accuracy_east: float = msg.accuracy_east - accuracy_down: float = msg.accuracy_down - carr_soln: int = msg.carr_soln - gnss_fix_ok: bool = msg.gnss_fix_ok - - gps_message = ( - stamp, - gps_time, - relative_pose_north, - relative_pose_east, - relative_pose_down, - relative_pose_length, - accuracy_north, - accuracy_east, - accuracy_down, - carr_soln, - gnss_fix_ok, - ) + if msg_type == "relposned": + + stamp: float = msg.stamp.stamp + gps_time: float = msg.gps_time.stamp + relative_pose_north: float = msg.relative_pose_north + relative_pose_east: float = msg.relative_pose_east + relative_pose_down: float = msg.relative_pose_down + relative_pose_length: float = msg.relative_pose_length + accuracy_north: float = msg.accuracy_north + accuracy_east: float = msg.accuracy_east + accuracy_down: float = msg.accuracy_down + carr_soln: int = msg.carr_soln + gnss_fix_ok: bool = msg.gnss_fix_ok + + gps_message = ( + stamp, + gps_time, + relative_pose_north, + relative_pose_east, + relative_pose_down, + relative_pose_length, + accuracy_north, + accuracy_east, + accuracy_down, + carr_soln, + gnss_fix_ok, + ) + + else: + stamp: float = msg.stamp.stamp + gps_time: float = msg.gps_time.stamp + latitude: float = msg.latitude + longitude: float = msg.longitude + altitude: float = msg.altitude + ground_speed: float = msg.ground_speed + speed_accuracy: float = msg.speed_accuracy + horizontal_accuracy: float = msg.horizontal_accuracy + vertical_accuracy: float = msg.vertical_accuracy + p_dop: float = msg.p_dop + + gps_message = ( + stamp, + gps_time, + latitude, + longitude, + altitude, + ground_speed, + speed_accuracy, + horizontal_accuracy, + vertical_accuracy, + p_dop, + ) return gps_message -def main(file_name: str, gps_interface: str) -> None: +def main(file_name: str, msg_type: str) -> None: + if msg_type not in ["relposned", "pvt"]: + raise RuntimeError(f"Message type not recognized: {msg_type}") + # create the file reader reader = EventsFileReader(file_name) success: bool = reader.open() @@ -73,22 +103,31 @@ def main(file_name: str, gps_interface: str) -> None: events_dict: dict[str, list[EventLogPosition]] = build_events_dict(events_index) print(f"All available topics: {sorted(events_dict.keys())}") - gps_events = events_dict["/gps/relposned"] - print(f"Found {len(gps_events)} packets of gps_pb2.RelativePositionFrame\n") + gps_events = events_dict[f"/gps/{msg_type}"] + print(f"Found {len(gps_events)} packets of gps/{msg_type}\n") for event_log in gps_events: # parse the message - msg: gps_pb2.RelativePositionFrame = event_log.read_message() - gps_msg = unpack_gps_message(msg) - print( - f"Message stamp: {gps_msg[0]}, GPS time: {gps_msg[1]}, " - f"Relative pose north: {gps_msg[2]}, Relative pose east: {gps_msg[3]}, " - f"Relative pose down: {gps_msg[4]}, Relative pose length: {gps_msg[5]}," - f"Accuracy north: {gps_msg[6]}, Accuracy east: {gps_msg[7]}, " - f"Accuracy down: {gps_msg[8]}, Carr soln: {gps_msg[9]}, " - f"GNSS fix ok: {gps_msg[10]}\n" - ) + msg: gps_pb2.RelativePositionFrame | gps_pb2.GpsFrame = event_log.read_message() + gps_msg = unpack_gps_message(msg, msg_type) + + if msg_type == "pvt": + print( + f"Message stamp: {gps_msg[0]}, GPS time: {gps_msg[1]}, " + f"Latitude: {gps_msg[2]}, Longitude: {gps_msg[3]}, Altitude: {gps_msg[4]},\n" + f"Ground speed: {gps_msg[5]}, Speed accuracy: {gps_msg[6]}, " + f"Horizontal accuracy: {gps_msg[7]}, Vertical accuracy: {gps_msg[8]}, P dop: {gps_msg[9]}\n" + ) + else: + print( + f"Message stamp: {gps_msg[0]}, GPS time: {gps_msg[1]}, " + f"Relative pose north: {gps_msg[2]}, Relative pose east: {gps_msg[3]}, " + f"Relative pose down: {gps_msg[4]},\n" + f"Relative pose length: {gps_msg[5]}, Accuracy north: {gps_msg[6]}, " + f"Accuracy east: {gps_msg[7]}, Accuracy down: {gps_msg[8]}, " + f"Carr soln: {gps_msg[9]}, GNSS fix ok: {gps_msg[10]}\n" + ) assert reader.close() @@ -97,7 +136,7 @@ def main(file_name: str, gps_interface: str) -> None: parser = argparse.ArgumentParser(prog="Event file reader example for parsing GPS messages.") parser.add_argument("--file-name", type=str, required=True, help="Path to the `events.bin` file.") parser.add_argument( - "--gps-interface", type=str, default="relposed", help="The name of the gps interface to read: relposned or pvt." + "--msg-type", type=str, default="relposned", help="The name of the gps interface to read: relposned or pvt." ) args = parser.parse_args() - main(args.file_name, args.gps_interface) + main(args.file_name, args.msg_type)