Merge branch 'development' into main

aau-cns · Sep 23, 2023 · 2474d06 · 2474d06
2 parents 1c08c90 + 64ace9a
commit 2474d06
Show file tree

Hide file tree

Showing 12 changed files with 389 additions and 79 deletions.
diff --git a/README.md b/README.md
diff --git a/source/examples/mars_thl/mars_thl_example.cpp b/source/examples/mars_thl/mars_thl_example.cpp
@@ -128,6 +128,10 @@ int main(int /*argc*/, char** /*argv[]*/)
   ofile_pose.open("/tmp/mars_pose_state.csv", std::ios::out);
   ofile_pose << std::setprecision(17);
 
+  std::ofstream ofile_meas;
+  ofile_meas.open("/tmp/mars_pose_meas.csv", std::ios::out);
+  ofile_meas << std::setprecision(17);
+
   // process data
   for (auto k : measurement_data)
   {
@@ -165,11 +169,18 @@ int main(int /*argc*/, char** /*argv[]*/)
       core_logic.buffer_.get_latest_sensor_handle_state(pose_sensor_sptr, &latest_result);
       mars::PoseSensorStateType last_state = pose_sensor_sptr->get_state(latest_result.data_.sensor_);
       ofile_pose << last_state.to_csv_string(latest_result.timestamp_.get_seconds()) << std::endl;
+
+      // Retreive the last measurement and write it to the measurement file
+      mars::BufferEntryType latest_meas;
+      core_logic.buffer_.get_latest_sensor_handle_measurement(pose_sensor_sptr, &latest_meas);
+      mars::PoseMeasurementType* last_meas = static_cast<mars::PoseMeasurementType*>(latest_meas.data_.sensor_.get());
+      ofile_meas << last_meas->to_csv_string(k.timestamp_.get_seconds()) << std::endl;
     }
   }
 
   ofile_core.close();
   ofile_pose.close();
+  ofile_meas.close();
 
   mars::BufferEntryType latest_result;
   core_logic.buffer_.get_latest_state(&latest_result);

diff --git a/source/mars/include/mars/sensors/attitude/attitude_measurement_type.h b/source/mars/include/mars/sensors/attitude/attitude_measurement_type.h
@@ -49,6 +49,27 @@ class AttitudeMeasurementType : public BaseMeas
   AttitudeMeasurementType(const Eigen::Matrix3d& rot_mat) : attitude_(rot_mat)
   {
   }
+
+  static std::string get_csv_state_header_string()
+  {
+    std::stringstream os;
+    os << "t, ";
+    os << "q_w, q_x, q_y, q_z";
+
+    return os.str();
+  }
+
+  std::string to_csv_string(const double& timestamp) const
+  {
+    std::stringstream os;
+    os.precision(17);
+    os << timestamp;
+
+    os << ", " << attitude_.quaternion_.w() << ", " << attitude_.quaternion_.x() << ", " << attitude_.quaternion_.y()
+       << ", " << attitude_.quaternion_.z();
+
+    return os.str();
+  }
 };
 }  // namespace mars
 

diff --git a/source/mars/include/mars/sensors/bodyvel/bodyvel_measurement_type.h b/source/mars/include/mars/sensors/bodyvel/bodyvel_measurement_type.h
@@ -31,6 +31,26 @@ class BodyvelMeasurementType : public BaseMeas
   BodyvelMeasurementType(Eigen::Vector3d velocity) : velocity_(std::move(velocity))
   {
   }
+
+  static std::string get_csv_state_header_string()
+  {
+    std::stringstream os;
+    os << "t, ";
+    os << "v_x, v_y, v_z";
+
+    return os.str();
+  }
+
+  std::string to_csv_string(const double& timestamp) const
+  {
+    std::stringstream os;
+    os.precision(17);
+    os << timestamp;
+
+    os << ", " << velocity_.x() << ", " << velocity_.y() << ", " << velocity_.z();
+
+    return os.str();
+  }
 };
 }  // namespace mars
 #endif  // BODYVELMEASUREMENTTYPE_H
diff --git a/source/mars/include/mars/sensors/empty/empty_measurement_type.h b/source/mars/include/mars/sensors/empty/empty_measurement_type.h
@@ -24,6 +24,26 @@ class EmptyMeasurementType : public BaseMeas
   EmptyMeasurementType(const double& value) : value_(value)
   {
   }
+
+  static std::string get_csv_state_header_string()
+  {
+    std::stringstream os;
+    os << "t, ";
+    os << "value";
+
+    return os.str();
+  }
+
+  std::string to_csv_string(const double& timestamp) const
+  {
+    std::stringstream os;
+    os.precision(17);
+    os << timestamp;
+
+    os << ", " << value_;
+
+    return os.str();
+  }
 };
 }  // namespace mars
 #endif  // EMPTYMEASUREMENTTYPE_H
diff --git a/source/mars/include/mars/sensors/gps/gps_measurement_type.h b/source/mars/include/mars/sensors/gps/gps_measurement_type.h
@@ -26,6 +26,26 @@ class GpsMeasurementType : public BaseMeas
     : coordinates_(std::move(latitude), std::move(longitude), std::move(altitude))
   {
   }
+
+  static std::string get_csv_state_header_string()
+  {
+    std::stringstream os;
+    os << "t, ";
+    os << "lat, lon, alt";
+
+    return os.str();
+  }
+
+  std::string to_csv_string(const double& timestamp) const
+  {
+    std::stringstream os;
+    os.precision(17);
+    os << timestamp;
+
+    os << ", " << coordinates_.latitude_ << ", " << coordinates_.longitude_ << ", " << coordinates_.altitude_;
+
+    return os.str();
+  }
 };
 }  // namespace mars
 #endif  // GPS_MEASUREMENTTYPE_H
diff --git a/source/mars/include/mars/sensors/gps_w_vel/gps_w_vel_measurement_type.h b/source/mars/include/mars/sensors/gps_w_vel/gps_w_vel_measurement_type.h
@@ -28,6 +28,28 @@ class GpsVelMeasurementType : public BaseMeas
     , velocity_(std::move(vel_x), std::move(vel_y), std::move(vel_z))
   {
   }
+
+  static std::string get_csv_state_header_string()
+  {
+    std::stringstream os;
+    os << "t, ";
+    os << "lat, lon, alt, ";
+    os << "vel_x, vel_y, vel_z";
+
+    return os.str();
+  }
+
+  std::string to_csv_string(const double& timestamp) const
+  {
+    std::stringstream os;
+    os.precision(17);
+    os << timestamp;
+
+    os << ", " << coordinates_.latitude_ << ", " << coordinates_.longitude_ << ", " << coordinates_.altitude_;
+    os << ", " << velocity_.x() << ", " << velocity_.y() << ", " << velocity_.z();
+
+    return os.str();
+  }
 };
 }  // namespace mars
 #endif  // GPSVELMEASUREMENTTYPE_H
diff --git a/source/mars/include/mars/sensors/mag/mag_measurement_type.h b/source/mars/include/mars/sensors/mag/mag_measurement_type.h
@@ -29,6 +29,26 @@ class MagMeasurementType : public BaseMeas
   MagMeasurementType(Eigen::Vector3d mag_vector) : mag_vector_(std::move(mag_vector))
   {
   }
+
+  static std::string get_csv_state_header_string()
+  {
+    std::stringstream os;
+    os << "t, ";
+    os << "m_x, m_y, m_z";
+
+    return os.str();
+  }
+
+  std::string to_csv_string(const double& timestamp) const
+  {
+    std::stringstream os;
+    os.precision(17);
+    os << timestamp;
+
+    os << ", " << mag_vector_.x() << ", " << mag_vector_.y() << ", " << mag_vector_.z();
+
+    return os.str();
+  }
 };
 }  // namespace mars
 

diff --git a/source/mars/include/mars/sensors/pose/pose_measurement_type.h b/source/mars/include/mars/sensors/pose/pose_measurement_type.h
@@ -31,6 +31,28 @@ class PoseMeasurementType : public BaseMeas
     : position_(std::move(position)), orientation_(orientation)
   {
   }
+
+  static std::string get_csv_state_header_string()
+  {
+    std::stringstream os;
+    os << "t, ";
+    os << "p_x, p_y, p_z, ";
+    os << "q_w, q_x, q_y, q_z";
+
+    return os.str();
+  }
+
+  std::string to_csv_string(const double& timestamp) const
+  {
+    std::stringstream os;
+    os.precision(17);
+    os << timestamp;
+
+    os << ", " << position_.x() << ", " << position_.y() << ", " << position_.z();
+    os << ", " << orientation_.w() << ", " << orientation_.x() << ", " << orientation_.y() << ", " << orientation_.z();
+
+    return os.str();
+  }
 };
 }  // namespace mars
 #endif  // POSEMEASUREMENTTYPE_H
diff --git a/source/mars/include/mars/sensors/position/position_measurement_type.h b/source/mars/include/mars/sensors/position/position_measurement_type.h
@@ -27,6 +27,26 @@ class PositionMeasurementType : public BaseMeas
   PositionMeasurementType(Eigen::Vector3d position) : position_(std::move(position))
   {
   }
+
+  static std::string get_csv_state_header_string()
+  {
+    std::stringstream os;
+    os << "t, ";
+    os << "p_x, p_y, p_z";
+
+    return os.str();
+  }
+
+  std::string to_csv_string(const double& timestamp) const
+  {
+    std::stringstream os;
+    os.precision(17);
+    os << timestamp;
+
+    os << ", " << position_.x() << ", " << position_.y() << ", " << position_.z();
+
+    return os.str();
+  }
 };
 }  // namespace mars
 #endif  // POSITIONMEASUREMENTTYPE_H
diff --git a/source/mars/include/mars/sensors/pressure/pressure_measurement_type.h b/source/mars/include/mars/sensors/pressure/pressure_measurement_type.h
@@ -44,6 +44,30 @@ class PressureMeasurementType : public BaseMeas
     pressure_.data_ = pressure;
     pressure_.temperature_K_ = temperature;
   }
+
+  static std::string get_csv_header_string()
+  {
+    std::stringstream os;
+    os << "t, ";
+    os << "pressure, ";
+    os << "temperature, ";
+    os << "type";
+
+    return os.str();
+  }
+
+  std::string to_csv_string(const double& timestamp) const
+  {
+    std::stringstream os;
+    os.precision(17);
+    os << timestamp;
+
+    os << ", " << pressure_.data_;
+    os << ", " << pressure_.temperature_K_;
+    os << ", " << pressure_.type_;
+
+    return os.str();
+  }
 };
 }  // namespace mars
 

diff --git a/source/mars/include/mars/sensors/vision/vision_measurement_type.h b/source/mars/include/mars/sensors/vision/vision_measurement_type.h
@@ -31,6 +31,28 @@ class VisionMeasurementType : public BaseMeas
     : position_(std::move(position)), orientation_(orientation)
   {
   }
+
+  static std::string get_csv_state_header_string()
+  {
+    std::stringstream os;
+    os << "t, ";
+    os << "p_x, p_y, p_z, ";
+    os << "q_w, q_x, q_y, q_z";
+
+    return os.str();
+  }
+
+  std::string to_csv_string(const double& timestamp) const
+  {
+    std::stringstream os;
+    os.precision(17);
+    os << timestamp;
+
+    os << ", " << position_.x() << ", " << position_.y() << ", " << position_.z();
+    os << ", " << orientation_.w() << ", " << orientation_.x() << ", " << orientation_.y() << ", " << orientation_.z();
+
+    return os.str();
+  }
 };
 }  // namespace mars
 #endif  // VISIONMEASUREMENTTYPE_H