[wpiutil] Faster nanopb submessage encode (#7374)

Due to how submessages are encoded (with a length prefix), nanopb currently does the encoding twice. It encodes once to get the length to write, then writes the length, then reencodes the entire message a 2nd time.

This results in a requirement that each encode always encodes the same. Generally, this is fine, but it'd be nice to not make this a requirement.

The double encode also requires going through the entire set of fields again, which has the possibility to be slow.

Instead of doing this, write to a temporary SmallVector. Then we can just encode the length of that buffer, and do a memcpy into primary stream. Theoretically in most cases, this should be much faster.

wpiutil/src/main/native/cpp/protobuf/Protobuf.cpp

@@ -48,3 +48,28 @@ bool detail::WriteFromStdVector(pb_ostream_t* stream, const pb_byte_t* buf,
   vec->insert(vec->end(), buf, buf + count);
   return true;
 }
+
+bool detail::WriteSubmessage(pb_ostream_t* stream, const pb_msgdesc_t* desc,
+                             const void* msg) {
+  // Write the submessage to a separate buffer
+  wpi::SmallVector<uint8_t, 64> buf;
+  pb_ostream_t subStream{
+      .callback = WriteFromSmallVector,
+      .state = &buf,
+      .max_size = SIZE_MAX,
+      .bytes_written = 0,
+      .errmsg = nullptr,
+  };
+  if (!pb_encode(&subStream, desc, msg)) {
+    return false;
+  }
+
+  uint64_t size = static_cast<uint64_t>(buf.size());
+
+  // Write the size to the original stream.
+  if (!pb_encode_varint(stream, size)) {
+    return false;
+  }
+
+  return pb_write(stream, reinterpret_cast<const pb_byte_t*>(buf.data()), size);
+}