From 8b9f917f4327c0d36e73b06c80fb444bdf0d3739 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 14 Dec 2020 14:32:04 -0500
Subject: [PATCH] refactored code for ImuMeasurements

---
 tests/ImuMeasurement.h | 26 +++++++++++++++++++-------
 tests/Measurement.h    | 24 +++++++++++++-----------
 2 files changed, 32 insertions(+), 18 deletions(-)

diff --git a/tests/ImuMeasurement.h b/tests/ImuMeasurement.h
index 5ac3d54be..d49759545 100644
--- a/tests/ImuMeasurement.h
+++ b/tests/ImuMeasurement.h
@@ -2,11 +2,11 @@
 
 #include <tests/Measurement.h>
 
-namespace drs {
+namespace gtsam {
 
-///
-/// \brief Contains data from the IMU mesaurements.
-///
+/**
+ *\brief Contains data from the IMU mesaurements.
+ */
 class ImuMeasurement : public Measurement {
  public:
   enum Name { BODY = 0, RF_FOOT = 1, RH_FOOT = 2 };
@@ -15,9 +15,21 @@ class ImuMeasurement : public Measurement {
   Eigen::Vector3d I_a_WI;  ///< Raw acceleration from the IMU (m/s/s)
   Eigen::Vector3d I_w_WI;  ///< Raw angular velocity from the IMU (rad/s)
 
+  ImuMeasurement()
+      : Measurement("ImuMeasurement"), I_a_WI{0, 0, 0}, I_w_WI{0, 0, 0} {}
+
   virtual ~ImuMeasurement() override {}
-  ImuMeasurement();
-  friend std::ostream& operator<<(std::ostream& stream, const ImuMeasurement& meas);
+
+  friend std::ostream& operator<<(std::ostream& stream,
+                                  const ImuMeasurement& meas);
 };
 
-}  // namespace drs
+std::ostream& operator<<(std::ostream& stream, const ImuMeasurement& meas) {
+  stream << "IMU Measurement at time = " << meas.time << " : \n"
+         << "dt    : " << meas.dt << "\n"
+         << "I_a_WI: " << meas.I_a_WI.transpose() << "\n"
+         << "I_w_WI: " << meas.I_w_WI.transpose() << "\n";
+  return stream;
+}
+
+}  // namespace gtsam
diff --git a/tests/Measurement.h b/tests/Measurement.h
index e26c5d918..be38ac4f3 100644
--- a/tests/Measurement.h
+++ b/tests/Measurement.h
@@ -3,21 +3,23 @@
 #include <Eigen/Core>
 #include <string>
 
-namespace drs {
+namespace gtsam {
 
-///
-/// \brief This is the base class for all measurement types.
-///
+/**
+ * \brief This is the base class for all measurement types.
+ */
 class Measurement {
-public:
+ public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  uint64_t dt;      ///< Time since the last message of this type (nanoseconds).
-  uint64_t time;    ///< ROS time message recieved (nanoseconds).
-  std::string type; ///< The type of message (to enable dynamic/static casting).
+  size_t dt;    ///< Time since the last message of this type (nanoseconds).
+  size_t time;  ///< ROS time message recieved (nanoseconds).
+  ///< The type of message (to enable dynamic/static casting).
+  std::string type;
+
+  Measurement() : dt(0), time(0), type("UNDEFINED") {}
+  Measurement(std::string _type) : dt(0), time(0), type(_type) {}
 
   virtual ~Measurement() {}
-  Measurement();
-  Measurement(std::string _type);
 };
 
-} // namespace drs
+}  // namespace gtsam