Throw in C++ in case of mismatch between proto type and message

KangarooKoala · Jul 16, 2024 · e342773 · e342773
1 parent f14a3ae
commit e342773
Show file tree

Hide file tree

Showing 4 changed files with 35 additions and 4 deletions.
diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/SwerveDriveKinematicsProto.inc b/wpimath/src/main/native/include/frc/kinematics/proto/SwerveDriveKinematicsProto.inc
@@ -4,6 +4,9 @@
 
 #pragma once
 
+#include <stdexcept>
+
+#include <fmt/format.h>
 #include <wpi/ProtoHelper.h>
 
 #include "frc/kinematics/proto/SwerveDriveKinematicsProto.h"
@@ -21,6 +24,12 @@ frc::SwerveDriveKinematics<NumModules>
 wpi::Protobuf<frc::SwerveDriveKinematics<NumModules>>::Unpack(
     const google::protobuf::Message& msg) {
   auto m = static_cast<const wpi::proto::ProtobufSwerveDriveKinematics*>(&msg);
+  if (m->modules_size() != NumModules) {
+    throw std::invalid_argument(
+        fmt::format("Tried to unpack message with {} elements in modules into "
+                    "SwerveDriveKinematics with {} modules",
+                    m->modules_size(), NumModules));
+  }
   return frc::SwerveDriveKinematics<NumModules>{
       wpi::UnpackProtobufArray<wpi::proto::ProtobufTranslation2d,
                                frc::Translation2d, NumModules>(m->modules())};

diff --git a/wpimath/src/main/native/include/frc/proto/MatrixProto.inc b/wpimath/src/main/native/include/frc/proto/MatrixProto.inc
@@ -4,6 +4,9 @@
 
 #pragma once
 
+#include <stdexcept>
+
+#include <fmt/format.h>
 #include <wpi/ProtoHelper.h>
 
 #include "frc/proto/MatrixProto.h"
@@ -24,10 +27,16 @@ wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Unpack(
     const google::protobuf::Message& msg) {
   auto m = static_cast<const wpi::proto::ProtobufMatrix*>(&msg);
   if (m->num_rows() != Rows || m->num_cols() != Cols) {
-    // TODO Error
+    throw std::invalid_argument(
+        fmt::format("Tried to unpack message with {} rows and {} columns into "
+                    "Matrix with {} rows and {} columns",
+                    m->num_rows(), m->num_cols(), Rows, Cols));
   }
   if (m->data_size() != Rows * Cols) {
-    // TODO Error
+    throw std::invalid_argument(
+        fmt::format("Tried to unpack message with {} elements in data into "
+                    "Matrix with {} elements",
+                    m->data_size(), Rows * Cols));
   }
   frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> mat;
   for (int i = 0; i < Rows * Cols; i++) {

diff --git a/wpimath/src/main/native/include/frc/proto/VectorProto.inc b/wpimath/src/main/native/include/frc/proto/VectorProto.inc
@@ -4,6 +4,9 @@
 
 #pragma once
 
+#include <stdexcept>
+
+#include <fmt/format.h>
 #include <wpi/ProtoHelper.h>
 
 #include "frc/proto/VectorProto.h"
@@ -22,7 +25,10 @@ wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Unpack(
     const google::protobuf::Message& msg) {
   auto m = static_cast<const wpi::proto::ProtobufVector*>(&msg);
   if (m->rows_size() != Size) {
-    // TODO Error
+    throw std::invalid_argument(
+        fmt::format("Tried to unpack message with {} elements in rows into "
+                    "Vector with {} rows",
+                    m->rows_size(), Size));
   }
   frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> vec;
   for (int i = 0; i < Size; i++) {

diff --git a/wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.inc b/wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.inc
@@ -4,6 +4,9 @@
 
 #pragma once
 
+#include <stdexcept>
+
+#include <fmt/format.h>
 #include <wpi/ProtoHelper.h>
 
 #include "frc/system/proto/LinearSystemProto.h"
@@ -23,7 +26,11 @@ wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>>::Unpack(
   auto m = static_cast<const wpi::proto::ProtobufLinearSystem*>(&msg);
   if (m->num_states() != States || m->num_inputs() != Inputs ||
       m->num_outputs() != Outputs) {
-    // TODO Error
+    throw std::invalid_argument(fmt::format(
+        "Tried to unpack message with {} states and {} inputs and {} outputs "
+        "into LinearSystem with {} states and {} inputs and {} outputs",
+        m->num_states(), m->num_inputs(), m->num_outputs(), States, Inputs,
+        Outputs));
   }
   return frc::LinearSystem<States, Inputs, Outputs>{
       wpi::UnpackProtobuf<frc::Matrixd<States, States>>(m->a()),