Parsing CDR Messages

Welcome to the 4th part of our series on parsing MCAP data from a ROS2 bag in Haskell. We’ve spent the previous parts understanding the record structure in the bag and extracting the message data for particular topics. Today we will parse that data into Haskell types! Unfortunately, this will actually require us to learn a bit about yet another data encoding (CDR), but luckily we’ll still be able to use some of our previous parsing code!

If you sign up for our Solve.hs course, you’ll learn all about parsing techniques in Haskell in the 4th module! You’ll learn how to use the Megaparsec library, as well as other concepts like regular expressions.

What is CDR?

CDR stands for Common Data Representation, and it is a serialization format for primitive data. It is designed to work with an Interface Definition Language (IDL). ROS2 provides an IDL with its system of defining message types, and it uses CDR as the default encoding for message data in bags.

Now, most of the primitives we’ll see in our CDR data are encoded in a similar way to MCAP data. For example, fix-width integer values are still represented exactly how you would expect. Strings are similar in that they are size-prefixed by a 32-bit integer, but they are different in that CDR strings must have a null byte at the end.

What really distinguishes CDR though is the concept of alignment. Different primitive types must start at a position that is a certain multiple from the start of the data, and padding must be added if they would not.

The simplest example of this is to imagine we are encoding a boolean followed by a 64-bit integer (little endian). If we have the boolean true and the number 18, we could represent this data with 9 bytes:

0x01 0xf7 0x06 0x00 0x00 0x00 0x00 0x00 0x00

The first byte (0x01) indicates the boolean True, the remaining 8 bytes give us the number 1783.

However, in CDR, a 64-bit integer must start at a position that is a multiple of 8. This means we would need to “pad” the boolean to have 7 additional null bytes:

0x01 0x00 0x00 0x00 0x00 0x00 0x00 0x00
0xf7 0x06 0x00 0x00 0x00 0x00 0x00 0x00

A 32-bit integer has an alignment multiple of 4, and since strings are prefixed by a 32-bit integer, this means they do as well. So to represent the boolean true, the string “Jacky” and the string “Carl”, we would have the following:

0x01 0x00 0x00 0x00 # Bool `true` padded to 4
0x06 0x00 0x00 0x00 # Uint32 6 (size of “Jacky” + 1 for null byte)
0x4a 0x61 0x63 0x6b # “Jack”
0x79 0x00 0x00 0x00 # “y”, null byte to terminate, 2 bytes of padding
0x05 0x00 0x00 0x00 # Uint32 5 (size of “Carl” + 1 for null byte)
0x43 0x61 0x72 0x63 # “Carl”
0x00                # Null byte to terminate

Now let’s start bringing this to Haskell.

Representing our Messages

First let’s define some Haskell types for our data. We’ve already done this with a Simple message type. Here is its message definition:

int64 num
float64 value
string name

And here is a Haskell type for it:

data SimpleMsg = SimpleMsg
  { num :: Int64
  , value :: Double
  , name :: String
  } deriving (Show, Eq)

But since the first two types in here are aligned to 8, we could actually parse this without worrying about alignment! To make things more challenging (and show the range of data types available in ROS2), let’s make a Complex message like this:

bool is_good
float64[] measurements
string complex_name
Simple msg1
Simple msg2

This has a boolean, an array, and it references two instances of our Simple message above! Here’s how we could represent this type in Haskell:

data ComplexMsg = ComplexMsg
  { isGood :: Bool
  , measurements :: [Double]
  , complexName :: String
  , msg1 :: SimpleMsg
  , msg2 :: SimpleMsg
  } deriving (Show, Eq)

Now let’s figure out how to organize our parsing code.

A New Parser Monad

So we won’t be parsing the message data in the context of the rest of the bag. Our bag parser organizes messages by topic, and we’ll parse the data after the fact. So while we still want a ParsecT monad, we don’t need StateT TopicState. Instead of tracking the full bag state, we need to track the number of bytes we’ve parsed so far so that we can perform alignment correctly. So let’s create this monad alias:

type CDRParser = ParsecT Void ByteString (StateT Word64 IO) a

And now we can already write a crucial function to help us align our parser. We’ll call this alignTo, and it will move our parser forward until the saved alignment value matches the input:

alignTo :: Word64 -> CDRParser ()
alignTo n = do
  align <- lift get
  unless (align `mod` n == 0) $ do
    _ <- anySingle
    lift $ modify (+ 1)
    alignTo n

We inspect the current alignment value. If it is evenly divided by the input, then we are done. If not, we consume a single byte with anySingle, and then use modify to bump the saved alignment value by 1. Then we recurse so that we keep aligning until it’s done. We’ll use this function for each of our primitive parsers!

Making our Monads Flexible

Now ideally, we also want to make use of some of our prior parsers. After all, they use Megaparsec machinery. However, we’ve written them in the Parser monad, not the CDRParser monad.

Lucky for us, Megaparsec allows ParsecT to have a monad transformer class MonadParsec that enables us to reuse this code. We just have to change some type signatures. Let’s consider this original function:

parseUintLE :: (Bits a, Integral a) => Int -> Parser (Word64, a)

All we have to do to make this generalizable is add a MonadParsec constraint and use a generic monad type m:

parseUintLE :: (Bits a, Integral a, MonadParsec Void ByteString m) => Int -> m (Word64, a)

The MonadParsec class is still parameterized by the error type (Void) and the stream type (ByteString), so we need flexible contexts. We’ll do the same thing with our parsers for Word64, Word32 and the string parser since we’ll use those in the next section:

parseUint64LE :: (MonadParsec Void ByteString m) => m (Word64, Word64)

parseUint32LE :: (MonadParsec Void ByteString m) => m (Word64, Word32)

parseString :: (MonadParsec Void ByteString m) => m (Word64, ByteString)

One last thing we’ll want to generalize is guard’. This will allow us to do validation with our CDR parsers as well! We don’t need this to be MonadParsec. Any MonadFail will do.

guard' :: (MonadFail m) => String -> Bool -> m ()

Now that these functions are generic, let’s apply them to our CDR case!

Writing Primitive CDR Parsers

First we’ll adapt our integer parsers. We can simply refer to our existing parsers, except we first align the parser and we update the alignment count.

parseCdrUint64LE :: CDRParser Word64
parseCdrUint64LE = do
  alignTo 8
  (len, n) <- parseUint64LE
  lift $ modify (+ len)
  return n

parseCdrUint32LE :: CDRParser Word32
parseCdrUint32LE = do
  alignTo 4
  (len, n) <- parseUint32LE
  lift $ modify (+ len)
  return n

Our string parser is a similar story. However, we’ll do an extra check and manipulation because of the null byte. We want to use guard’ to verify that the null byte is there, but then we use init later to remove it. We also convert it from ByteString to String.

import qualified Data.ByteString.Lazy.Char8 as BSC

parseCdrString :: CDRParser String
parseCdrString = do
  alignTo 4
  (len, str) <- parseString
  lift $ modify (+ (fromIntegral len))
  guard' ("CDR String did not null terminate: " <> show str) (BS.last str == 0)
  return (init $ BSC.unpack str)

To parse a Double, we’ll actually start by treating it like a 64-bit integer. Then we’ll just use Data.Binary.IEEE754.coerceWordToDouble to treat these exact bytes as a Double instead!

import Data.Binary.IEEE754 (wordToDouble)

parseCdrDoubleLE :: CDRParser Double
parseCdrDoubleLE = wordToDouble <$> parseCdrUint64LE

Finally there’s boolean values. This one is easy with what we already know, as there’s no alignment necessary:

parseCdrBool :: CDRParser Bool
parseCdrBool = do
  b <- anySingle
  lift $ modify (+ 1)
  return (b == 1)

These are all the primitives we’ll need for our types, but there’s two more pieces we need. First, we need to be able to parse an Array of values. We’ll follow a similar pattern to the MCAP array parser by passing in Parser a as an argument. After that, things are much easier because CDR prefixes the array by the number of elements (32-bit integer), not the number of bytes.

parseCdrArray :: CDRParser a -> CDRParser [a]
parseCdrArray parser = do
  n <- parseCdrUint32LE
  forM [1..n] $ \_ -> parser

The nice part about this is that we’ve built alignment into all of our primitive parsers, so we don’t have to think about it when building macro parsers!

Writing CDR Message Parsers

Now it’s time to parse our messages. The good news is that we’ve done most of the hard work already! We just need one last helper, and that is for the CDR encapsulation header. This comes at the start of every message. It consists of 4 bytes, but we only care about the second byte. If this byte is 1, then our data is little endian. Otherwise it is big endian. We’ll add a validation check that it’s little endian, since all our parsers assume this:

parseCdrHeader :: CDRParser ()
parseCdrHeader = do
  _ <- anySingle
  leBit <- anySingle
  _ <- anySingle
  _ <- anySingle
  guard' ("ROS2 CDR does not indicate little endian: " <> show leBit) (leBit == 1)

This header comes at the start of every message, but we’ll write a parser for SimpleMsg that doesn’t include the header. We do this because there is not a separate header when Complex includes instances of Simple. This parser is quite simple, just using all our primitive parsers:

parseSimple :: CDRParser SimpleMsg
parseSimple = do
  n <- parseCdrUint64LE
  v <- parseCdrDoubleLE
  s <- parseCdrString
  return $ SimpleMsg n v s

Now we also include a version with the header:

parseSimpleMsg :: CDRParser SimpleMsg
parseSimpleMsg = do
  parseCdrHeader
  parseSimple

Then Complex isn’t much harder. We just have more fields using our different primitives, plus parseSimple:

parseComplex :: CDRParser ComplexMsg
parseComplex = do
  b <- parseCdrBool
  ms <- parseCdrArray parseCdrDoubleLE
  n <- parseCdrString
  s1 <- parseSimple
  s2 <- parseSimple
  return $ ComplexMsg b ms n s1 s2

parseComplexMsg :: CDRParser ComplexMsg
parseComplexMsg = do
  parseCdrHeader
  parseComplex

Now we just need to call these from our main script!

Parsing the Message Data

There’s not much to this last part. Recalling from last time, our main function currently looks something like this:

parseBareRecordsFromFile :: FilePath -> IO ()
parseBareRecordsFromFile fp = do
  input <- BS.readFile fp
  (result, st) <- runStateT (runParserT parseMcapFile' fp input) (TopicState HM.empty HM.empty HM.empty (HS.fromList ["/simple_topic", "/complex_topic"]) HS.empty)
  case result of
    Left e -> print e
    Right recs -> do
      forM_ recs $ \(_, rec) -> printRec rec
      let simpleMessages = fromMaybe [] $ HM.lookup ("/simple_topic", "my_package/msg/Simple") (tsMessages st)
      forM_ simpleMessages $ \(MessageData t1 t2 msg) -> do
        print (t1, t2, msg)
      let complexMessages = fromMaybe [] $ HM.lookup ("/complex_topic", "my_package/msg/Complex") (tsMessages st)
      forM_ complexMessages $ \(MessageData t1 t2 msg) -> do
        print (t1, t2, msg)

This will print the raw data for each message in the two topics /simple_topic and /complex_topic. Now, instead of just printing that data, we want to parse it! This involves invoking our CDRParser functions. We use runParserT on the raw data, while wrapping this in evalStateT with an initial alignment value of 0.

parseBareRecordsFromFile :: FilePath -> IO ()
parseBareRecordsFromFile fp = do
      ...
      let simpleMessages = fromMaybe [] $ HM.lookup ("/simple_topic", "my_package/msg/Simple") (tsMessages st)
      forM_ simpleMessages $ \(MessageData t1 t2 msg) -> do
        parseSimpleResult <- evalStateT (runParserT parseSimpleMsg "Simple" msg) 0
        case parseSimpleResult of
          Left e -> print e
          Right s -> putStrLn $ "Parsed Simple Message: " <> show t1 <> " " <> show t2 <> " " <> show s
      let complexMessages = fromMaybe [] $ HM.lookup ("/complex_topic", "my_package/msg/Complex") (tsMessages st)
      forM_ complexMessages $ \(MessageData t1 t2 msg) -> do
        parseComplexResult <- evalStateT (runParserT parseComplexMsg "Complex" msg) 0
        case parseComplexResult of
          Left e -> print e
          Right s -> putStrLn $ "Parsed Complex Message: " <> show t1 <> " " <> show t2 <> " " <> show s

And running this with our bag, we’ll see valid outputs!

Parsed Simple Message: 1759374126013925786 1759374126013925786 SimpleMsg {num = 6, value = 6.3, name = "Goodbye"}
Parsed Simple Message: 1759374125991715245 1759374125991715245 SimpleMsg {num = 5, value = 4.2, name = "Hello"}
Parsed Complex Message: 1759374126014015513 1759374126014015513 ComplexMsg {isGood = False, measurements = [4.0,3.0,1.5,6.7,9.2], complexName = "Jo", simple1 = SimpleMsg {num = 12, value = 14.2, name = "Seymour"}, simple2 = SimpleMsg {num = 13, value = 15.1, name = "Sylvester"}}
Parsed Complex Message: 1759374126013960101 1759374126013960101 ComplexMsg {isGood = True, measurements = [1.0,2.0,2.5,4.5,6.1], complexName = "Jacky", simple1 = SimpleMsg {num = 10, value = 12.4, name = "Timothy"}, simple2 = SimpleMsg {num = 11, value = 13.3, name = "Jack"}}

Conclusion

At long last, we have turned raw MCAP data into structured Haskell types! This is a great accomplishment, but there’s still more we can do. There’s a few different directions to go, but next time we’re going to try to optimize our parse so that we don’t have to scan the whole bag to find our topics. This will involve using indexes in the summary section, and we’ll have to re-think how we use Megaparsec.

If Megaparsec is still a bit confusing for you, you should sign up for Solve.hs! In addition to teaching you about data structures, algorithms, and problem solving tricks in Haskell, you’ll also learn how to parse in Haskell! You’ll learn Megaparsec from the ground up, AND you’ll learn how to use regular expressions!