From 5a3c884a34ebe68127d434667973662f674c873d Mon Sep 17 00:00:00 2001 From: Wyatt Alt Date: Mon, 14 Mar 2022 10:12:09 -0700 Subject: [PATCH] CLI: JSON output option for cat (#289) Adds a ros1msg JSON transcoder to the ros utilities package, and calls it from the CLI's cat command when the --json switch is supplied. --- go/cli/mcap/Makefile | 3 + go/cli/mcap/cmd/cat.go | 96 +++- go/cli/mcap/cmd/cat_test.go | 45 ++ go/cli/mcap/utils/ros/json_transcoder.go | 482 ++++++++++++++++++ go/cli/mcap/utils/ros/json_transcoder_test.go | 437 ++++++++++++++++ go/mcap/parse.go | 2 +- go/ros/constants.go | 24 + go/ros/ros1msg/ros1msg_parser.go | 201 ++++++++ go/ros/ros1msg/ros1msg_parser_test.go | 259 ++++++++++ go/ros/ros2db3_to_mcap.go | 29 +- testdata/mcap/demo.mcap | 4 +- 11 files changed, 1545 insertions(+), 37 deletions(-) create mode 100644 go/cli/mcap/cmd/cat_test.go create mode 100644 go/cli/mcap/utils/ros/json_transcoder.go create mode 100644 go/cli/mcap/utils/ros/json_transcoder_test.go create mode 100644 go/ros/constants.go create mode 100644 go/ros/ros1msg/ros1msg_parser.go create mode 100644 go/ros/ros1msg/ros1msg_parser_test.go diff --git a/go/cli/mcap/Makefile b/go/cli/mcap/Makefile index 88663ff0a7..34bb8a5a73 100644 --- a/go/cli/mcap/Makefile +++ b/go/cli/mcap/Makefile @@ -24,3 +24,6 @@ lint: test: go test ./... + +bench: + make -C cmd bench diff --git a/go/cli/mcap/cmd/cat.go b/go/cli/mcap/cmd/cat.go index 8b4d616425..61e71523f3 100644 --- a/go/cli/mcap/cmd/cat.go +++ b/go/cli/mcap/cmd/cat.go @@ -1,28 +1,79 @@ package cmd import ( + "bytes" "context" + "encoding/json" "errors" "fmt" "io" "log" "math" "os" + "strconv" "strings" "github.com/foxglove/mcap/go/cli/mcap/utils" + "github.com/foxglove/mcap/go/cli/mcap/utils/ros" "github.com/foxglove/mcap/go/mcap" "github.com/spf13/cobra" ) var ( - topics string - start int64 - end int64 + topics string + start int64 + end int64 + formatJSON bool ) -func printMessages(ctx context.Context, w io.Writer, it mcap.MessageIterator) error { +type DecimalTime uint64 + +func digits(n uint64) int { + if n == 0 { + return 1 + } + count := 0 + for n != 0 { + n = n / 10 + count++ + } + return count +} + +func (t DecimalTime) MarshalJSON() ([]byte, error) { + seconds := uint64(t) / 1e9 + nanoseconds := uint64(t) % 1e9 + requiredLength := digits(seconds) + 1 + 9 + buf := make([]byte, 0, requiredLength) + buf = strconv.AppendInt(buf, int64(seconds), 10) + buf = append(buf, '.') + for i := 0; i < 9-digits(nanoseconds); i++ { + buf = append(buf, '0') + } + buf = strconv.AppendInt(buf, int64(nanoseconds), 10) + return buf, nil +} + +type Message struct { + Topic string `json:"topic"` + Sequence uint32 `json:"sequence"` + LogTime DecimalTime `json:"log_time"` + PublishTime DecimalTime `json:"publish_time"` + Data json.RawMessage `json:"data"` +} + +func printMessages( + ctx context.Context, + w io.Writer, + it mcap.MessageIterator, + formatJSON bool, +) error { + msg := &bytes.Buffer{} + msgReader := &bytes.Reader{} buf := make([]byte, 1024*1024) + transcoders := make(map[uint16]*ros.JSONTranscoder) + encoder := json.NewEncoder(w) + target := Message{} for { schema, channel, message, err := it.Next(buf) if err != nil { @@ -31,7 +82,37 @@ func printMessages(ctx context.Context, w io.Writer, it mcap.MessageIterator) er } log.Fatalf("Failed to read next message: %s", err) } - fmt.Fprintf(w, "%d %s [%s] %v...\n", message.LogTime, channel.Topic, schema.Name, message.Data[:10]) + if !formatJSON { + fmt.Fprintf(w, "%d %s [%s] %v...\n", message.LogTime, channel.Topic, schema.Name, message.Data[:10]) + continue + } + if schema.Encoding != "ros1msg" { + return fmt.Errorf("JSON output only supported for ros1msg schemas") + } + transcoder, ok := transcoders[channel.SchemaID] + if !ok { + packageName := strings.Split(schema.Name, "/")[0] + transcoder, err = ros.NewJSONTranscoder(packageName, schema.Data) + if err != nil { + return fmt.Errorf("failed to build transcoder for %s: %w", channel.Topic, err) + } + transcoders[channel.SchemaID] = transcoder + } + msgReader.Reset(message.Data) + err = transcoder.Transcode(msg, msgReader) + if err != nil { + return fmt.Errorf("failed to transcode %s record on %s: %w", schema.Name, channel.Topic, err) + } + target.Topic = channel.Topic + target.Sequence = message.Sequence + target.LogTime = DecimalTime(message.LogTime) + target.PublishTime = DecimalTime(message.PublishTime) + target.Data = msg.Bytes() + err = encoder.Encode(target) + if err != nil { + return fmt.Errorf("failed to write encoded message") + } + msg.Reset() } return nil } @@ -57,7 +138,7 @@ var catCmd = &cobra.Command{ if err != nil { log.Fatalf("Failed to read messages: %s", err) } - err = printMessages(ctx, os.Stdout, it) + err = printMessages(ctx, os.Stdout, it, formatJSON) if err != nil { log.Fatalf("Failed to print messages: %s", err) } @@ -79,7 +160,7 @@ var catCmd = &cobra.Command{ if err != nil { return fmt.Errorf("failed to read messages: %w", err) } - err = printMessages(ctx, os.Stdout, it) + err = printMessages(ctx, os.Stdout, it, formatJSON) if err != nil { return fmt.Errorf("failed to print messages: %w", err) } @@ -97,4 +178,5 @@ func init() { catCmd.PersistentFlags().Int64VarP(&start, "start-secs", "", 0, "start time") catCmd.PersistentFlags().Int64VarP(&end, "end-secs", "", math.MaxInt64, "end time") catCmd.PersistentFlags().StringVarP(&topics, "topics", "", "", "comma-separated list of topics") + catCmd.PersistentFlags().BoolVarP(&formatJSON, "json", "", false, "print messages as JSON") } diff --git a/go/cli/mcap/cmd/cat_test.go b/go/cli/mcap/cmd/cat_test.go new file mode 100644 index 0000000000..f0cca9f92e --- /dev/null +++ b/go/cli/mcap/cmd/cat_test.go @@ -0,0 +1,45 @@ +package cmd + +import ( + "bytes" + "context" + "io" + "io/ioutil" + "math" + "testing" + + "github.com/foxglove/mcap/go/mcap" + "github.com/stretchr/testify/assert" +) + +func BenchmarkCat(b *testing.B) { + ctx := context.Background() + cases := []struct { + assertion string + inputfile string + formatJSON bool + }{ + { + "demo.bag", + "../../../../testdata/mcap/demo.mcap", + true, + }, + } + for _, c := range cases { + input, err := ioutil.ReadFile(c.inputfile) + assert.Nil(b, err) + w := io.Discard + r := bytes.NewReader(input) + b.Run(c.assertion, func(b *testing.B) { + for i := 0; i < b.N; i++ { + reader, err := mcap.NewReader(r) + assert.Nil(b, err) + it, err := reader.Messages(0, math.MaxInt64, []string{}, true) + assert.Nil(b, err) + err = printMessages(ctx, w, it, c.formatJSON) + assert.Nil(b, err) + r.Reset(input) + } + }) + } +} diff --git a/go/cli/mcap/utils/ros/json_transcoder.go b/go/cli/mcap/utils/ros/json_transcoder.go new file mode 100644 index 0000000000..c3a07a8797 --- /dev/null +++ b/go/cli/mcap/utils/ros/json_transcoder.go @@ -0,0 +1,482 @@ +package ros + +import ( + "encoding/base64" + "encoding/binary" + "fmt" + "io" + "math" + "strconv" + + "github.com/foxglove/mcap/go/ros/ros1msg" +) + +var ( + trueBytes = []byte("true") + falseBytes = []byte("false") + asciizero = []byte{'0'} +) + +type converter func(io.Writer, io.Reader) error + +type recordField struct { + name string + converter converter +} + +type JSONTranscoder struct { + buf []byte + parentPackage string + converter converter + formattedNumber []byte +} + +func (t *JSONTranscoder) Transcode(w io.Writer, r io.Reader) error { + return t.converter(w, r) +} + +func (t *JSONTranscoder) recordFromFields(fields []ros1msg.Field) (converter, error) { + recordFields := []recordField{} + for _, field := range fields { + // record + if field.Type.IsRecord { + recordConverter, err := t.recordFromFields(field.Type.Fields) + if err != nil { + return nil, fmt.Errorf("failed to build dependent fields: %w", err) + } + recordFields = append(recordFields, recordField{ + name: field.Name, + converter: recordConverter, + }) + continue + } + // complex array + if field.Type.IsArray && field.Type.Items.IsRecord { + recordConverter, err := t.recordFromFields(field.Type.Items.Fields) + if err != nil { + return nil, fmt.Errorf("failed to build dependent fields: %w", err) + } + recordFields = append(recordFields, recordField{ + name: field.Name, + converter: t.array(recordConverter, field.Type.FixedSize, false), + }) + continue + } + converterType := field.Type.BaseType + // if it's still an array, must be primitive + if field.Type.IsArray { + converterType = field.Type.Items.BaseType + } + var converter converter + var isBytes bool + if !field.Type.IsRecord { + switch converterType { + case "bool": + converter = t.bool + case "int8": + converter = t.int8 + case "int16": + converter = t.int16 + case "int32": + converter = t.int32 + case "int64": + converter = t.int64 + case "uint8": + isBytes = true + converter = t.uint8 + case "uint16": + converter = t.uint16 + case "uint32": + converter = t.uint32 + case "uint64": + converter = t.uint64 + case "float32": + converter = t.float32 + case "float64": + converter = t.float64 + case "string": + converter = t.string + case "time": + converter = t.time + case "duration": + converter = t.duration + case "char": + converter = t.uint8 + case "byte": + converter = t.uint8 + default: + return nil, fmt.Errorf("unrecognized primitive %s", converterType) + } + } + if field.Type.IsArray { + converter = t.array(converter, field.Type.FixedSize, isBytes) + } + recordFields = append(recordFields, recordField{ + converter: converter, + name: field.Name, + }) + } + return t.record(recordFields), nil +} + +func NewJSONTranscoder(parentPackage string, data []byte) (*JSONTranscoder, error) { + fields, err := ros1msg.ParseMessageDefinition(parentPackage, data) + if err != nil { + return nil, fmt.Errorf("failed to parse message definition: %w", err) + } + t := &JSONTranscoder{ + buf: make([]byte, 8), + parentPackage: parentPackage, + } + converter, err := t.recordFromFields(fields) + if err != nil { + return nil, fmt.Errorf("failed to build root converter: %w", err) + } + t.converter = converter + return t, nil +} + +func (t *JSONTranscoder) bool(w io.Writer, r io.Reader) error { + if _, err := io.ReadFull(r, t.buf[:1]); err != nil { + return fmt.Errorf("unable to read byte: %w", err) + } + switch t.buf[0] { + case 0: + _, err := w.Write(falseBytes) + if err != nil { + return fmt.Errorf("unable to write bool: %w", err) + } + case 1: + _, err := w.Write(trueBytes) + if err != nil { + return fmt.Errorf("unable to write bool: %w", err) + } + default: + return fmt.Errorf("invalid bool: %d", t.buf[0]) + } + return nil +} + +func (t *JSONTranscoder) int8(w io.Writer, r io.Reader) error { + _, err := io.ReadFull(r, t.buf[:1]) + if err != nil { + return err + } + s := strconv.Itoa(int(t.buf[0])) + _, err = w.Write([]byte(s)) + if err != nil { + return err + } + return nil +} + +func (t *JSONTranscoder) uint8(w io.Writer, r io.Reader) error { + _, err := io.ReadFull(r, t.buf[:1]) + if err != nil { + return err + } + s := strconv.Itoa(int(t.buf[0])) + _, err = w.Write([]byte(s)) + if err != nil { + return err + } + return nil +} + +func (t *JSONTranscoder) int16(w io.Writer, r io.Reader) error { + _, err := io.ReadFull(r, t.buf[:2]) + if err != nil { + return err + } + x := int(binary.LittleEndian.Uint16(t.buf[:2])) + s := strconv.Itoa(x) + _, err = w.Write([]byte(s)) + if err != nil { + return err + } + return nil +} + +func (t *JSONTranscoder) string(w io.Writer, r io.Reader) error { + _, err := io.ReadFull(r, t.buf[:4]) + if err != nil { + return err + } + length := binary.LittleEndian.Uint32(t.buf[:4]) + 2 // for the quotes + if uint32(len(t.buf)) < length { + t.buf = make([]byte, length) + } + n, err := io.ReadFull(r, t.buf[1:length-1]) + if err != nil { + return err + } + t.buf[0] = '"' + t.buf[n+1] = '"' + _, err = w.Write(t.buf[:n+2]) + if err != nil { + return err + } + return nil +} + +func (t *JSONTranscoder) uint16(w io.Writer, r io.Reader) error { + _, err := io.ReadFull(r, t.buf[:2]) + if err != nil { + return err + } + x := int(binary.LittleEndian.Uint16(t.buf[:2])) + t.formattedNumber = strconv.AppendInt(t.formattedNumber, int64(x), 10) + _, err = w.Write(t.formattedNumber) + if err != nil { + return err + } + t.formattedNumber = t.formattedNumber[:0] + return nil +} + +func (t *JSONTranscoder) int32(w io.Writer, r io.Reader) error { + _, err := io.ReadFull(r, t.buf[:4]) + if err != nil { + return err + } + x := binary.LittleEndian.Uint32(t.buf[:4]) + t.formattedNumber = strconv.AppendInt(t.formattedNumber, int64(x), 10) + _, err = w.Write(t.formattedNumber) + if err != nil { + return err + } + t.formattedNumber = t.formattedNumber[:0] + return nil +} + +func (t *JSONTranscoder) uint32(w io.Writer, r io.Reader) error { + _, err := io.ReadFull(r, t.buf[:4]) + if err != nil { + return err + } + x := binary.LittleEndian.Uint32(t.buf[:4]) + t.formattedNumber = strconv.AppendInt(t.formattedNumber, int64(x), 10) + _, err = w.Write(t.formattedNumber) + if err != nil { + return err + } + t.formattedNumber = t.formattedNumber[:0] + return nil +} + +func (t *JSONTranscoder) int64(w io.Writer, r io.Reader) error { + _, err := io.ReadFull(r, t.buf[:8]) + if err != nil { + return err + } + x := binary.LittleEndian.Uint64(t.buf[:8]) + t.formattedNumber = strconv.AppendInt(t.formattedNumber, int64(x), 10) + _, err = w.Write(t.formattedNumber) + if err != nil { + return err + } + t.formattedNumber = t.formattedNumber[:0] + return nil +} + +func (t *JSONTranscoder) uint64(w io.Writer, r io.Reader) error { + _, err := io.ReadFull(r, t.buf[:4]) + if err != nil { + return err + } + x := binary.LittleEndian.Uint64(t.buf[:8]) + t.formattedNumber = strconv.AppendInt(t.formattedNumber, int64(x), 10) + _, err = w.Write(t.formattedNumber) + if err != nil { + return err + } + t.formattedNumber = t.formattedNumber[:0] + return nil +} + +func (t *JSONTranscoder) float32(w io.Writer, r io.Reader) error { + _, err := io.ReadFull(r, t.buf[:4]) + if err != nil { + return err + } + x := binary.LittleEndian.Uint32(t.buf[:4]) + float := float64(math.Float32frombits(x)) + t.formattedNumber = strconv.AppendFloat(t.formattedNumber, float, 'f', -1, 32) + _, err = w.Write(t.formattedNumber) + if err != nil { + return err + } + t.formattedNumber = t.formattedNumber[:0] + return nil +} + +func (t *JSONTranscoder) float64(w io.Writer, r io.Reader) error { + _, err := io.ReadFull(r, t.buf[:8]) + if err != nil { + return err + } + x := binary.LittleEndian.Uint64(t.buf[:8]) + float := math.Float64frombits(x) + t.formattedNumber = strconv.AppendFloat(t.formattedNumber, float, 'f', -1, 64) + _, err = w.Write(t.formattedNumber) + if err != nil { + return err + } + t.formattedNumber = t.formattedNumber[:0] + return nil +} + +func digits(n uint32) int { + if n == 0 { + return 1 + } + count := 0 + for n != 0 { + n = n / 10 + count++ + } + return count +} + +func (t *JSONTranscoder) formatTime(secs uint32, nsecs uint32) { + nanosecondsDigits := digits(nsecs) + t.formattedNumber = strconv.AppendInt(t.formattedNumber, int64(secs), 10) + t.formattedNumber = append(t.formattedNumber, '.') + for i := 0; i < 9-nanosecondsDigits; i++ { + t.formattedNumber = append(t.formattedNumber, '0') + } + t.formattedNumber = strconv.AppendInt(t.formattedNumber, int64(nsecs), 10) +} + +func (t *JSONTranscoder) time(w io.Writer, r io.Reader) error { + _, err := io.ReadFull(r, t.buf[:8]) + if err != nil { + return err + } + secs := binary.LittleEndian.Uint32(t.buf[:4]) + nsecs := binary.LittleEndian.Uint32(t.buf[4:]) + t.formatTime(secs, nsecs) + _, err = w.Write(t.formattedNumber) + if err != nil { + return err + } + t.formattedNumber = t.formattedNumber[:0] + return nil +} + +func (t *JSONTranscoder) duration(w io.Writer, r io.Reader) error { + _, err := io.ReadFull(r, t.buf[:8]) + if err != nil { + return err + } + secs := binary.LittleEndian.Uint32(t.buf[:4]) + nsecs := binary.LittleEndian.Uint32(t.buf[4:]) + t.formatTime(secs, nsecs) + _, err = w.Write(t.formattedNumber) + if err != nil { + return err + } + t.formattedNumber = t.formattedNumber[:0] + return nil +} + +func (t *JSONTranscoder) array(items converter, fixedSize int, isBytes bool) converter { + return func(w io.Writer, r io.Reader) error { + var arrayLength uint32 + if fixedSize > 0 { + arrayLength = uint32(fixedSize) + } else { + _, err := io.ReadFull(r, t.buf[:4]) + if err != nil { + return err + } + arrayLength = binary.LittleEndian.Uint32(t.buf[:4]) + } + + // if isBytes is set, we will base64 the content directly. Otherwise + // transcode elements as a JSON array. + if isBytes { + _, err := w.Write([]byte("\"")) + if err != nil { + return fmt.Errorf("error writing array start: %w", err) + } + encoder := base64.NewEncoder(base64.StdEncoding, w) + _, err = io.CopyN(encoder, r, int64(arrayLength)) + if err != nil { + return fmt.Errorf("failed to encode base64 array: %w", err) + } + err = encoder.Close() + if err != nil { + return fmt.Errorf("failed to close base64 encoder: %w", err) + } + _, err = w.Write([]byte("\"")) + if err != nil { + return fmt.Errorf("error writing array end: %w", err) + } + return nil + } + + _, err := w.Write([]byte("[")) + if err != nil { + return err + } + for i := uint32(0); i < arrayLength; i++ { + if i > 0 { + _, err := w.Write([]byte(",")) + if err != nil { + return err + } + } + err := items(w, r) + if err != nil { + return err + } + } + _, err = w.Write([]byte("]")) + if err != nil { + return err + } + return nil + } +} + +func (t *JSONTranscoder) record(fields []recordField) converter { + comma := []byte(",") + leftBracket := []byte("{") + rightBracket := []byte("}") + buf := []byte{} + return func(w io.Writer, r io.Reader) error { + _, err := w.Write(leftBracket) + if err != nil { + return err + } + for i, field := range fields { + if i > 0 { + _, err := w.Write(comma) + if err != nil { + return fmt.Errorf("failed to write comma: %w", err) + } + } + if len(buf) < 3+len(field.name) { + buf = make([]byte, 3+len(field.name)) + } + buf[0] = '"' + buf[1+len(field.name)] = '"' + buf[2+len(field.name)] = ':' + copy(buf[1:], field.name) + _, err := w.Write(buf[:3+len(field.name)]) + if err != nil { + return fmt.Errorf("failed to write field %s name: %w", field.name, err) + } + err = field.converter(w, r) + if err != nil { + return fmt.Errorf("failed to convert field %s: %w", field.name, err) + } + } + _, err = w.Write(rightBracket) + if err != nil { + return fmt.Errorf("failed to close record: %w", err) + } + return nil + } +} diff --git a/go/cli/mcap/utils/ros/json_transcoder_test.go b/go/cli/mcap/utils/ros/json_transcoder_test.go new file mode 100644 index 0000000000..321d1d1798 --- /dev/null +++ b/go/cli/mcap/utils/ros/json_transcoder_test.go @@ -0,0 +1,437 @@ +package ros + +import ( + "bytes" + "testing" + + "github.com/stretchr/testify/assert" +) + +func TestJSONTranscoding(t *testing.T) { + cases := []struct { + assertion string + parentPackage string + messageDefinition string + input []byte + output string + }{ + { + "simple string", + "", + "string foo", + []byte{0x03, 0x00, 0x00, 0x00, 'b', 'a', 'r'}, + `{"foo":"bar"}`, + }, + { + "empty string", + "", + "string foo", + []byte{0x00, 0x00, 0x00, 0x00}, + `{"foo":""}`, + }, + { + "two primitive fields", + "", + `string foo + int32 bar`, + []byte{0x03, 0x00, 0x00, 0x00, 'b', 'a', 'r', 0x01, 0x00, 0x00, 0x00}, + `{"foo":"bar","bar":1}`, + }, + { + "primitive variable-length array", + "", + `bool[] foo`, + []byte{0x01, 0x00, 0x00, 0x00, 0x01}, + `{"foo":[true]}`, + }, + { + "primitive fixed-length array", + "", + `bool[2] foo`, + []byte{0x01, 0x00}, + `{"foo":[true,false]}`, + }, + { + "empty primitive array", + "", + `bool[] foo`, + []byte{0x00, 0x00, 0x00, 0x00}, + `{"foo":[]}`, + }, + { + "empty byte array", + "", + `uint8[] foo`, + []byte{0x00, 0x00, 0x00, 0x00}, + `{"foo":""}`, + }, + { + "nonempty byte array", + "", + `uint8[] foo`, + []byte{0x05, 0x00, 0x00, 0x00, 'h', 'e', 'l', 'l', 'o'}, + `{"foo":"aGVsbG8="}`, + }, + { + "dependent type", + "", + `Foo foo + === + MSG: Foo + string bar + `, + []byte{0x03, 0x00, 0x00, 0x00, 'b', 'a', 'z'}, + `{"foo":{"bar":"baz"}}`, + }, + { + "2x dependent type", + "", + `Foo foo + === + MSG: Foo + Baz bar + === + MSG: Baz + string spam + `, + []byte{0x03, 0x00, 0x00, 0x00, 'b', 'a', 'z'}, + `{"foo":{"bar":{"spam":"baz"}}}`, + }, + { + "uses a header", + "", + `Header header + === + MSG: std_msgs/Header + uint32 seq + time stamp + string frame_id + `, + []byte{ + 0x01, 0x00, 0x00, 0x00, + 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, + 0x05, 0x00, 0x00, 0x00, 'h', 'e', 'l', 'l', 'o', + }, + `{"header":{"seq":1,"stamp":16843009.016843009,"frame_id":"hello"}}`, + }, + { + "uses a relative type", + "my_package", + `MyType foo + === + MSG: my_package/MyType + string bar + `, + []byte{ + 0x05, 0x00, 0x00, 0x00, 'h', 'e', 'l', 'l', 'o', + }, + `{"foo":{"bar":"hello"}}`, + }, + { + "relative type inherited from subdefinition", + "", + `my_package/MyType foo + === + MSG: my_package/MyType + MyOtherType bar + == + MSG: my_package/MyOtherType + string baz + `, + []byte{ + 0x05, 0x00, 0x00, 0x00, 'h', 'e', 'l', 'l', 'o', + }, + `{"foo":{"bar":{"baz":"hello"}}}`, + }, + } + for _, c := range cases { + t.Run(c.assertion, func(t *testing.T) { + definition := []byte(c.messageDefinition) + buf := &bytes.Buffer{} + transcoder, err := NewJSONTranscoder(c.parentPackage, definition) + assert.Nil(t, err) + err = transcoder.Transcode(buf, bytes.NewReader(c.input)) + assert.Nil(t, err) + assert.Equal(t, c.output, buf.String()) + }) + } +} + +func TestSingleRecordConversion(t *testing.T) { + transcoder, err := NewJSONTranscoder("", nil) + assert.Nil(t, err) + cases := []struct { + assertion string + parentPackage string + fields []recordField + input []byte + output string + }{ + { + "string", + "", + []recordField{ + { + name: "foo", + converter: transcoder.string, + }, + }, + []byte{0x03, 0x00, 0x00, 0x00, 'b', 'a', 'r'}, + `{"foo":"bar"}`, + }, + { + "bool", + "", + []recordField{ + { + name: "foo", + converter: transcoder.bool, + }, + }, + []byte{0x01}, + `{"foo":true}`, + }, + { + "int8", + "", + []recordField{ + { + name: "foo", + converter: transcoder.int8, + }, + }, + []byte{0x01}, + `{"foo":1}`, + }, + { + "int16", + "", + []recordField{ + { + name: "foo", + converter: transcoder.int16, + }, + }, + []byte{0x07, 0x07}, + `{"foo":1799}`, + }, + { + "int32", + "", + []recordField{ + { + name: "foo", + converter: transcoder.int32, + }, + }, + []byte{0x07, 0x07, 0x07, 0x07}, + `{"foo":117901063}`, + }, + { + "int64", + "", + []recordField{ + { + name: "foo", + converter: transcoder.int64, + }, + }, + []byte{0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07}, + `{"foo":506381209866536711}`, + }, + { + "uint8", + "", + []recordField{ + { + name: "foo", + converter: transcoder.uint8, + }, + }, + []byte{0x01}, + `{"foo":1}`, + }, + { + "uint16", + "", + []recordField{ + { + name: "foo", + converter: transcoder.uint16, + }, + }, + []byte{0x07, 0x07}, + `{"foo":1799}`, + }, + { + "uint32", + "", + []recordField{ + { + name: "foo", + converter: transcoder.uint32, + }, + }, + []byte{0x07, 0x07, 0x07, 0x07}, + `{"foo":117901063}`, + }, + { + "uint64", + "", + []recordField{ + { + name: "foo", + converter: transcoder.uint64, + }, + }, + []byte{0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07}, + `{"foo":506381209866536711}`, + }, + { + "float32", + "", + []recordField{ + { + name: "foo", + converter: transcoder.float32, + }, + }, + []byte{208, 15, 73, 64}, + `{"foo":3.14159}`, + }, + { + "float64", + "", + []recordField{ + { + name: "foo", + converter: transcoder.float64, + }, + }, + []byte{24, 106, 203, 110, 105, 118, 1, 64}, + `{"foo":2.18281828459045}`, + }, + { + "time", + "", + []recordField{ + { + name: "foo", + converter: transcoder.time, + }, + }, + []byte{0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00}, + `{"foo":1.000000001}`, + }, + { + "time zero", + "", + []recordField{ + { + name: "foo", + converter: transcoder.time, + }, + }, + []byte{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, + `{"foo":0.000000000}`, + }, + { + "duration", + "", + []recordField{ + { + name: "foo", + converter: transcoder.duration, + }, + }, + []byte{0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00}, + `{"foo":1.000000001}`, + }, + { + "two fields", + "", + []recordField{ + { + name: "foo", + converter: transcoder.bool, + }, + { + name: "bar", + converter: transcoder.bool, + }, + }, + []byte{0x01, 0x01}, + `{"foo":true,"bar":true}`, + }, + { + "variable-length array", + "", + []recordField{ + { + name: "foo", + converter: transcoder.array(transcoder.bool, 0, false), + }, + }, + []byte{0x02, 0x00, 0x00, 0x00, 0x01, 0x00}, + `{"foo":[true,false]}`, + }, + { + "fixed-length array", + "", + []recordField{ + { + name: "foo", + converter: transcoder.array(transcoder.bool, 2, false), + }, + }, + []byte{0x01, 0x00}, + `{"foo":[true,false]}`, + }, + { + "byte array", + "", + []recordField{ + { + name: "foo", + converter: transcoder.array(transcoder.uint8, 0, true), + }, + }, + []byte{ + 0x05, 0x00, 0x00, 0x00, + 'h', 'e', 'l', 'l', 'o', + }, + `{"foo":"aGVsbG8="}`, + }, + { + "array of record", + "", + []recordField{ + { + name: "foo", + converter: transcoder.array( + transcoder.record([]recordField{ + { + name: "bar", + converter: transcoder.bool, + }, + }), + 0, + false, + ), + }, + }, + []byte{0x01, 0x00, 0x00, 0x00, 0x01}, + `{"foo":[{"bar":true}]}`, + }, + } + for _, c := range cases { + t.Run(c.assertion, func(t *testing.T) { + transcoder.parentPackage = c.parentPackage + buf := &bytes.Buffer{} + converter := transcoder.record(c.fields) + err := converter(buf, bytes.NewBuffer(c.input)) + assert.Nil(t, err) + assert.Equal(t, c.output, buf.String()) + }) + } +} diff --git a/go/mcap/parse.go b/go/mcap/parse.go index 3bad5cca4d..efda0539c1 100644 --- a/go/mcap/parse.go +++ b/go/mcap/parse.go @@ -64,7 +64,7 @@ func ParseSchema(buf []byte) (*Schema, error) { ID: schemaID, Name: name, Encoding: encoding, - Data: data, + Data: append([]byte{}, data...), }, nil } diff --git a/go/ros/constants.go b/go/ros/constants.go new file mode 100644 index 0000000000..ad6e5b98b0 --- /dev/null +++ b/go/ros/constants.go @@ -0,0 +1,24 @@ +package ros + +var Primitives = map[string]bool{ + "bool": true, + "int8": true, + "uint8": true, + "int16": true, + "uint16": true, + "int32": true, + "uint32": true, + "int64": true, + "uint64": true, + "float32": true, + "float64": true, + "string": true, + "time": true, + "duration": true, + "char": true, + "byte": true, +} + +var MessageDefinitionSeparator = []byte( + "================================================================================\n", +) diff --git a/go/ros/ros1msg/ros1msg_parser.go b/go/ros/ros1msg/ros1msg_parser.go new file mode 100644 index 0000000000..0719d58f56 --- /dev/null +++ b/go/ros/ros1msg/ros1msg_parser.go @@ -0,0 +1,201 @@ +package ros1msg + +import ( + "fmt" + "strconv" + "strings" + + "github.com/foxglove/mcap/go/ros" +) + +type Type struct { + BaseType string + IsArray bool + FixedSize int + IsRecord bool + Items *Type + Fields []Field +} + +type Field struct { + Name string + Type Type +} + +func resolveDependentFields( + parentPackage string, + dependencies map[string]string, + subdefinition string, +) ([]Field, error) { + fields := []Field{} + for i, line := range strings.Split(subdefinition, "\n") { + line := strings.TrimSpace(line) + // empty line + if line == "" { + continue + } + // comment + if strings.HasPrefix(line, "#") { + continue + } + // constant + if strings.Contains(strings.Split(line, "#")[0], "=") { + continue + } + + // must be a field + parts := strings.FieldsFunc(line, func(c rune) bool { return c == ' ' }) + if len(parts) < 2 { + return nil, fmt.Errorf("malformed field on line %d: %s", i, line) + } + fieldType := parts[0] + fieldName := parts[1] + + var isRecord bool + var recordFields []Field + var arrayItems *Type + var err error + inputType := fieldType + + // check if this is an array + isArray, baseType, fixedSize := parseArrayType(fieldType) + if isArray { + fieldType = baseType + } + + if _, ok := ros.Primitives[fieldType]; !ok { + // There are three ways the field type can relate to the type + // names listed in dependencies. + // 1. They can match exactly, either as qualified (including package) or unqualified types. + // 2. The type can be unqualified in the fieldType and qualified in + // the dependency. In this situation we need to qualify the field + // type with its parent package. + // 3. The type may be "Header". This is a special case that needs to + // translate to std_msgs/Header. + typeIsQualified := strings.Contains(fieldType, "/") + if typeIsQualified { + parentPackage = strings.Split(fieldType, "/")[0] + } + subdefinition, typeIsPresent := dependencies[fieldType] + switch { + case typeIsPresent: + break + case fieldType == "Header": + subdefinition, ok = dependencies["std_msgs/Header"] + if !ok { + return nil, fmt.Errorf("dependency Header not found") + } + case !typeIsPresent && !typeIsQualified: + qualifiedType := parentPackage + "/" + fieldType + subdefinition, ok = dependencies[qualifiedType] + if !ok { + return nil, fmt.Errorf("dependency %s not found", qualifiedType) + } + } + recordFields, err = resolveDependentFields( + parentPackage, + dependencies, + subdefinition, + ) + if err != nil { + return nil, fmt.Errorf("failed to resolve dependent record: %w", err) + } + isRecord = true + } + + // if isArray, then the "record fields" above are for the array items. + // Otherwise we are dealing with a record and they are for the record + // itself. + if isArray { + arrayItems = &Type{ + BaseType: fieldType, + IsArray: false, + FixedSize: 0, + IsRecord: isRecord, + Items: nil, // nested arrays not allowed + Fields: recordFields, + } + fields = append(fields, Field{ + Name: fieldName, + Type: Type{ + BaseType: inputType, + IsArray: true, + FixedSize: fixedSize, + IsRecord: false, + Items: arrayItems, + }, + }) + } else { + fields = append(fields, Field{ + Name: fieldName, + Type: Type{ + BaseType: inputType, + IsArray: isArray, + FixedSize: fixedSize, + IsRecord: isRecord, + Items: arrayItems, + Fields: recordFields, + }, + }) + } + } + return fields, nil +} + +func ParseMessageDefinition(parentPackage string, data []byte) ([]Field, error) { + // split the definition on lines starting with =, and load each section + // after the first (the top-level definition) into a map. Then, mutually + // resolve the definitions. + definitions := splitLines(string(data), func(line string) bool { + return strings.HasPrefix(strings.TrimSpace(line), "=") + }) + definition := definitions[0] + subdefinitions := definitions[1:] + dependencies := make(map[string]string) + for _, subdefinition := range subdefinitions { + lines := strings.Split(subdefinition, "\n") + header := strings.TrimSpace(lines[0]) + rosType := strings.TrimPrefix(header, "MSG: ") + dependencies[rosType] = strings.Join(lines[1:], "\n") + } + fields, err := resolveDependentFields(parentPackage, dependencies, definition) + if err != nil { + return nil, fmt.Errorf("failed to build dependent records: %w", err) + } + return fields, nil +} + +func splitLines(s string, predicate func(string) bool) []string { + chunks := []string{} + chunk := &strings.Builder{} + for _, line := range strings.Split(s, "\n") { + if predicate(line) { + chunks = append(chunks, chunk.String()) + chunk.Reset() + continue + } + chunk.WriteString(line + "\n") + } + if chunk.Len() > 0 { + chunks = append(chunks, chunk.String()) + } + return chunks +} + +func parseArrayType(s string) (isArray bool, baseType string, fixedSize int) { + if !strings.Contains(s, "[") || !strings.Contains(s, "]") { + return false, "", 0 + } + leftBracketIndex := strings.Index(s, "[") + rightBracketIndex := strings.Index(s, "]") + baseType = s[:leftBracketIndex] + size := s[leftBracketIndex+1 : rightBracketIndex] + if size == "" { + return true, baseType, 0 + } + fixedSize, err := strconv.Atoi(size) + if err != nil { + return false, "", 0 + } + return true, baseType, fixedSize +} diff --git a/go/ros/ros1msg/ros1msg_parser_test.go b/go/ros/ros1msg/ros1msg_parser_test.go new file mode 100644 index 0000000000..f06721ca08 --- /dev/null +++ b/go/ros/ros1msg/ros1msg_parser_test.go @@ -0,0 +1,259 @@ +package ros1msg + +import ( + "testing" + + "github.com/stretchr/testify/assert" +) + +func TestROS1MSGParser(t *testing.T) { + cases := []struct { + assertion string + parentPackage string + messageDefinition string + fields []Field + }{ + { + "simple string", + "", + "string foo", + []Field{ + { + Name: "foo", + Type: Type{ + BaseType: "string", + }, + }, + }, + }, + { + "two primitive fields", + "", + `string foo + int32 bar`, + []Field{ + { + Name: "foo", + Type: Type{ + BaseType: "string", + }, + }, + { + Name: "bar", + Type: Type{ + BaseType: "int32", + }, + }, + }, + }, + { + "primitive variable-length array", + "", + `bool[] foo`, + []Field{ + { + Name: "foo", + Type: Type{ + BaseType: "bool[]", + IsArray: true, + Items: &Type{ + BaseType: "bool", + }, + }, + }, + }, + }, + { + "primitive fixed-length array", + "", + `bool[2] foo`, + []Field{ + { + Name: "foo", + Type: Type{ + BaseType: "bool[2]", + IsArray: true, + FixedSize: 2, + Items: &Type{ + BaseType: "bool", + }, + }, + }, + }, + }, + { + "dependent type", + "", + `Foo foo + === + MSG: Foo + string bar + `, + []Field{ + { + Name: "foo", + Type: Type{ + BaseType: "Foo", + IsRecord: true, + Fields: []Field{ + { + Name: "bar", + Type: Type{ + BaseType: "string", + }, + }, + }, + }, + }, + }, + }, + { + "2x dependent type", + "", + `Foo foo + === + MSG: Foo + Baz bar + === + MSG: Baz + string spam + `, + []Field{ + { + Name: "foo", + Type: Type{ + BaseType: "Foo", + IsRecord: true, + Fields: []Field{ + { + Name: "bar", + Type: Type{ + BaseType: "Baz", + IsRecord: true, + Fields: []Field{ + { + Name: "spam", + Type: Type{ + BaseType: "string", + }, + }, + }, + }, + }, + }, + }, + }, + }, + }, + { + "uses a header", + "", + `Header header + === + MSG: std_msgs/Header + uint32 seq + time stamp + string frame_id + `, + []Field{ + { + Name: "header", + Type: Type{ + BaseType: "Header", + IsRecord: true, + Fields: []Field{ + { + Name: "seq", + Type: Type{ + BaseType: "uint32", + }, + }, + { + Name: "stamp", + Type: Type{ + BaseType: "time", + }, + }, + { + Name: "frame_id", + Type: Type{ + BaseType: "string", + }, + }, + }, + }, + }, + }, + }, + { + "uses a relative type", + "my_package", + `MyType foo + === + MSG: my_package/MyType + string bar + `, + []Field{ + { + Name: "foo", + Type: Type{ + BaseType: "MyType", + IsRecord: true, + Fields: []Field{ + { + Name: "bar", + Type: Type{ + BaseType: "string", + }, + }, + }, + }, + }, + }, + }, + { + "relative type inherited from subdefinition", + "", + `my_package/MyType foo + === + MSG: my_package/MyType + MyOtherType bar + == + MSG: my_package/MyOtherType + string baz`, + []Field{ + { + Name: "foo", + Type: Type{ + BaseType: "my_package/MyType", + IsRecord: true, + Fields: []Field{ + { + Name: "bar", + Type: Type{ + BaseType: "MyOtherType", + IsRecord: true, + Fields: []Field{ + { + Name: "baz", + Type: Type{ + BaseType: "string", + }, + }, + }, + }, + }, + }, + }, + }, + }, + }, + } + for _, c := range cases { + t.Run(c.assertion, func(t *testing.T) { + fields, err := ParseMessageDefinition(c.parentPackage, []byte(c.messageDefinition)) + assert.Nil(t, err) + assert.Equal(t, c.fields, fields) + }) + } +} diff --git a/go/ros/ros2db3_to_mcap.go b/go/ros/ros2db3_to_mcap.go index a422567cad..ffb529dc22 100644 --- a/go/ros/ros2db3_to_mcap.go +++ b/go/ros/ros2db3_to_mcap.go @@ -13,31 +13,6 @@ import ( "github.com/foxglove/mcap/go/mcap" ) -var ( - messageDefinitionSeparator = []byte( - "================================================================================\n", - ) -) - -var rosPrimitives = map[string]bool{ - "bool": true, - "int8": true, - "uint8": true, - "int16": true, - "uint16": true, - "int32": true, - "uint32": true, - "int64": true, - "uint64": true, - "float32": true, - "float64": true, - "string": true, - "time": true, - "duration": true, - "char": true, - "byte": true, -} - func getSchema(encoding string, rosType string, directories []string) ([]byte, error) { parts := strings.FieldsFunc(rosType, func(c rune) bool { return c == '/' }) if len(parts) < 3 { @@ -92,7 +67,7 @@ func getSchemas(encoding string, directories []string, types []string) (map[stri for len(subdefinitions) > 0 { subdefinition := subdefinitions[0] if !first { - _, err := messageDefinition.Write(messageDefinitionSeparator) + _, err := messageDefinition.Write(MessageDefinitionSeparator) if err != nil { return nil, fmt.Errorf("failed to write separator: %w", err) } @@ -137,7 +112,7 @@ func getSchemas(encoding string, directories []string, types []string) (map[stri } // if it's a primitive, no action required - if rosPrimitives[baseType] { + if Primitives[baseType] { continue } diff --git a/testdata/mcap/demo.mcap b/testdata/mcap/demo.mcap index 3d18107141..88f8c566e3 100644 --- a/testdata/mcap/demo.mcap +++ b/testdata/mcap/demo.mcap @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:ab7290877183190cb9c3033babf6d5afdcdc1f97951746047f4aee9c001e997d -size 61501088 +oid sha256:f878642b6fc15d2e771ce530252e6454de296e6d99b18748e6cd7d09eaa80598 +size 61497068