Move verify_subtype_sizes into its own file

RobotLocomotion · Oct 29, 2017 · 7566307 · 7566307
1 parent 4937261
commit 7566307
Showing 7 changed files with 73 additions and 56 deletions.
diff --git a/drake/systems/robotInterfaces/BUILD.bazel b/drake/systems/robotInterfaces/BUILD.bazel
@@ -20,8 +20,14 @@ drake_cc_library(
 
 drake_cc_library(
     name = "qp_locomotion_plan",
-    srcs = ["QPLocomotionPlan.cpp"],
-    hdrs = ["QPLocomotionPlan.h"],
+    srcs = [
+        "QPLocomotionPlan.cpp",
+        "verify_subtype_sizes.cc",
+        "verify_subtype_sizes.h",
+    ],
+    hdrs = [
+        "QPLocomotionPlan.h",
+    ],
     deps = [
         ":body_motion_data",
         ":convex_hull",

diff --git a/drake/systems/robotInterfaces/QPLocomotionPlan.cpp b/drake/systems/robotInterfaces/QPLocomotionPlan.cpp
@@ -19,6 +19,7 @@
 #include "drake/math/quaternion.h"
 #include "drake/math/rotation_matrix.h"
 #include "drake/systems/robotInterfaces/convex_hull.h"
+#include "drake/systems/robotInterfaces/verify_subtype_sizes.h"
 #include "drake/util/drakeGeometryUtil.h"
 #include "drake/util/drakeUtil.h"
 #include "drake/util/lcmUtil.h"

diff --git a/drake/systems/robotInterfaces/verify_subtype_sizes.cc b/drake/systems/robotInterfaces/verify_subtype_sizes.cc
@@ -0,0 +1,54 @@
+#include "drake/systems/robotInterfaces/verify_subtype_sizes.h"
+
+#include <iostream>
+#include <sstream>
+#include <stdexcept>
+
+// NOLINTNEXTLINE(runtime/references)
+void verifySubtypeSizes(drake::lcmt_support_data& support_data) {
+  // Check for errors in sizes of variable-length fields.
+  if (support_data.contact_pts.size() != 3) {
+    throw std::runtime_error("contact_pts must have 3 rows");
+  }
+  for (int j = 0; j < 3; ++j) {
+    if (static_cast<int32_t>(support_data.contact_pts[j].size()) !=
+        support_data.num_contact_pts) {
+      std::stringstream msg;
+      msg << "num_contact_pts must match the size of each row of contact_pts."
+          << std::endl;
+      msg << "num_contact_pts: " << support_data.num_contact_pts
+          << ", support_data.contact_pts[" << j
+          << "].size(): " << support_data.contact_pts[j].size() << std::endl;
+      throw std::runtime_error(msg.str().c_str());
+    }
+  }
+}
+
+// NOLINTNEXTLINE(runtime/references)
+void verifySubtypeSizes(drake::lcmt_qp_controller_input& qp_input) {
+  // Check (and try to fix) errors in the sizes of the variable-length fields in
+  // our message
+  if (static_cast<int32_t>(qp_input.support_data.size()) !=
+      qp_input.num_support_data) {
+    std::cerr << "WARNING: num support data doesn't match" << std::endl;
+    qp_input.num_support_data = qp_input.support_data.size();
+  }
+  if (static_cast<int32_t>(qp_input.body_motion_data.size()) !=
+      qp_input.num_tracked_bodies) {
+    std::cerr << "WARNING: num tracked bodies doesn't match" << std::endl;
+    qp_input.num_tracked_bodies = qp_input.body_motion_data.size();
+  }
+  if (static_cast<int32_t>(qp_input.body_wrench_data.size()) !=
+      qp_input.num_external_wrenches) {
+    std::cerr << "WARNING: num external wrenches doesn't match" << std::endl;
+    qp_input.num_external_wrenches = qp_input.body_wrench_data.size();
+  }
+  if (static_cast<int32_t>(qp_input.joint_pd_override.size()) !=
+      qp_input.num_joint_pd_overrides) {
+    std::cerr << "WARNING: num joint pd override doesn't match" << std::endl;
+    qp_input.num_joint_pd_overrides = qp_input.joint_pd_override.size();
+  }
+  for (int i = 0; i < qp_input.num_support_data; ++i) {
+    verifySubtypeSizes(qp_input.support_data[i]);
+  }
+}
diff --git a/drake/systems/robotInterfaces/verify_subtype_sizes.h b/drake/systems/robotInterfaces/verify_subtype_sizes.h
@@ -0,0 +1,10 @@
+#pragma once
+
+#include "drake/lcmt_qp_controller_input.hpp"
+
+void verifySubtypeSizes(
+    // NOLINTNEXTLINE(runtime/references)
+    drake::lcmt_support_data& support_data);
+void verifySubtypeSizes(
+    // NOLINTNEXTLINE(runtime/references)
+    drake::lcmt_qp_controller_input& qp_input);
diff --git a/drake/util/BUILD.bazel b/drake/util/BUILD.bazel
@@ -32,7 +32,6 @@ drake_cc_library(
         "//drake/lcmtypes:piecewise_polynomial",
         "//drake/lcmtypes:polynomial",
         "//drake/lcmtypes:polynomial_matrix",
-        "//drake/lcmtypes:qp_controller_input",
         "//drake/math:geometric_transform",
         "@eigen",
         "@lcmtypes_bot2_core",

diff --git a/drake/util/lcmUtil.cpp b/drake/util/lcmUtil.cpp
@@ -96,50 +96,3 @@ PiecewisePolynomial<double> decodePiecewisePolynomial(
   }
   return PiecewisePolynomial<double>(polynomial_matrices, msg.breaks);
 }
-
-void verifySubtypeSizes(drake::lcmt_support_data& support_data) {
-  // Check for errors in sizes of variable-length fields.
-  if (support_data.contact_pts.size() != 3) {
-    throw std::runtime_error("contact_pts must have 3 rows");
-  }
-  for (int j = 0; j < 3; ++j) {
-    if (static_cast<int32_t>(support_data.contact_pts[j].size()) !=
-        support_data.num_contact_pts) {
-      std::stringstream msg;
-      msg << "num_contact_pts must match the size of each row of contact_pts."
-          << std::endl;
-      msg << "num_contact_pts: " << support_data.num_contact_pts
-          << ", support_data.contact_pts[" << j
-          << "].size(): " << support_data.contact_pts[j].size() << std::endl;
-      throw std::runtime_error(msg.str().c_str());
-    }
-  }
-}
-
-void verifySubtypeSizes(drake::lcmt_qp_controller_input& qp_input) {
-  // Check (and try to fix) errors in the sizes of the variable-length fields in
-  // our message
-  if (static_cast<int32_t>(qp_input.support_data.size()) !=
-      qp_input.num_support_data) {
-    std::cerr << "WARNING: num support data doesn't match" << std::endl;
-    qp_input.num_support_data = qp_input.support_data.size();
-  }
-  if (static_cast<int32_t>(qp_input.body_motion_data.size()) !=
-      qp_input.num_tracked_bodies) {
-    std::cerr << "WARNING: num tracked bodies doesn't match" << std::endl;
-    qp_input.num_tracked_bodies = qp_input.body_motion_data.size();
-  }
-  if (static_cast<int32_t>(qp_input.body_wrench_data.size()) !=
-      qp_input.num_external_wrenches) {
-    std::cerr << "WARNING: num external wrenches doesn't match" << std::endl;
-    qp_input.num_external_wrenches = qp_input.body_wrench_data.size();
-  }
-  if (static_cast<int32_t>(qp_input.joint_pd_override.size()) !=
-      qp_input.num_joint_pd_overrides) {
-    std::cerr << "WARNING: num joint pd override doesn't match" << std::endl;
-    qp_input.num_joint_pd_overrides = qp_input.joint_pd_override.size();
-  }
-  for (int i = 0; i < qp_input.num_support_data; ++i) {
-    verifySubtypeSizes(qp_input.support_data[i]);
-  }
-}
diff --git a/drake/util/lcmUtil.h b/drake/util/lcmUtil.h
@@ -12,7 +12,6 @@
 #include "drake/lcmt_piecewise_polynomial.hpp"
 #include "drake/lcmt_polynomial.hpp"
 #include "drake/lcmt_polynomial_matrix.hpp"
-#include "drake/lcmt_qp_controller_input.hpp"
 
 void EncodeVector3d(
     const Eigen::Ref<const Eigen::Vector3d>& vec, bot_core::vector_3d_t& msg);
@@ -83,8 +82,3 @@ void encodePiecewisePolynomial(
 
 PiecewisePolynomial<double> decodePiecewisePolynomial(
     const drake::lcmt_piecewise_polynomial& msg);
-
-void verifySubtypeSizes(
-    drake::lcmt_support_data& support_data);
-void verifySubtypeSizes(
-    drake::lcmt_qp_controller_input& qp_input);