ground_control/decode.py at main · cpspacesystems/ground_control · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
import argparse
from base64 import b64decode
import csv
import foxglove
from NavPacket_pb2 import NavPacket
import os
from utils import build_file_descriptor_set


parser = argparse.ArgumentParser()
parser.add_argument('filename')
args = parser.parse_args()


def main() -> None:
    prefix = args.filename.split(".")[0]
    csvfilename = prefix + ".csv"
    mcapfilename = prefix + ".mcap"
    nav_channel = foxglove.Channel(
        topic="/navigation",
        message_encoding="protobuf",
        schema=foxglove.Schema(
            name=NavPacket.DESCRIPTOR.full_name,
            encoding="protobuf",
            data=build_file_descriptor_set(NavPacket).SerializeToString(),
        )
    )
    mode = "w" if os.path.exists(csvfilename) else "x"

    with (
        open(args.filename) as file,
        open(csvfilename, mode) as csvfile,
        foxglove.open_mcap(mcapfilename, allow_overwrite=True)
    ):
        writer = csv.writer(csvfile)
        writer.writerow([
            "timestamp.seconds",
            "timestamp.nanos",
            "gnss.latitude",
            "gnss.longitude",
            "imu.acc_x",
            "imu.acc_y",
            "imu.acc_z",
            "imu.gyr_x",
            "imu.gyr_y",
            "imu.gyr_z",
            "alt.altitude",
            "magn.x",
            "magn.y",
            "magn.z",
        ])
        for line in file.readlines():
            if len(line.strip()) == 0 or line.startswith("#"):
                continue

            packet = NavPacket()
            try:
                packet.ParseFromString(b64decode(line))
            except Exception as e:
                continue

            nav_channel.log(packet.SerializeToString(), log_time=int(packet.timestamp.seconds * 1e9 + packet.timestamp.nanos))
            writer.writerow([
                packet.timestamp.seconds,
                packet.timestamp.nanos,
                packet.gnss.latitude,
                packet.gnss.longitude,
                packet.imu.acc_x,
                packet.imu.acc_y,
                packet.imu.acc_z,
                packet.imu.gyr_x,
                packet.imu.gyr_y,
                packet.imu.gyr_z,
                packet.alt.altitude,
                packet.magn.x,
                packet.magn.y,
                packet.magn.z,
            ])


if __name__ == '__main__':
    main()