Merge branch 'development' into main

.clang-format

@@ -8,7 +8,6 @@ AllowAllParametersOfDeclarationOnNextLine: false
 AllowShortIfStatementsOnASingleLine: false
 AllowShortLoopsOnASingleLine: false
 AllowShortFunctionsOnASingleLine: None
-AllowShortLoopsOnASingleLine: false
 AlwaysBreakTemplateDeclarations: true
 AlwaysBreakBeforeMultilineStrings: false
 BreakBeforeBinaryOperators: false

README.md

@@ -407,7 +407,7 @@ Leading to the following derivatives
 
 ```math
 \begin{align*}
-   \frac{\mathrm{d}h(x)}{\mathrm{d}\delta_{rm}} &= -\mathbf{R}_{WI}[\alpha]\left\|\mu\right\|-\mathbf{R}_{WI}\alpha\frac{\mu^\mathsf{T}}{\left\|\mu\right\|}\mathbf{R}_{WI}\left[\left[\omega_i\right]_{\times} \mathbf{P}_{IG}\right]_{\times} \\
+   \frac{\mathrm{d}h(x)}{\mathrm{d}\delta_{rm}} &= -\mathbf{R}_{WI}\left[\alpha\right]_{\times} \left\|\mu\right\|-\mathbf{R}_{WI}\alpha\frac{\mu^\mathsf{T}}{\left\|\mu\right\|}\mathbf{R}_{WI}\left[\left[\omega_i\right]_{\times} \mathbf{P}_{IG}\right]_{\times} \\
    \frac{\mathrm{d}h(x)}{\mathrm{d}\Delta\mathbf{V}}_{WI} &= \mathbf{R}_{WI}\alpha\frac{\mu^\mathsf{T}}{\left\|\mu\right\|} \\
    \frac{\mathrm{d}h(x)}{\mathrm{d}\Delta\mathbf{P}}_{IG} &= \mathbf{R}_{WI}\alpha\frac{\mu^\mathsf{T}}{\left\|\mu\right\|}\mathbf{R}_{WI}\left[\omega_i\right]_{\times}
 \end{align*}

source/mars/CMakeLists.txt

@@ -94,6 +94,9 @@ set(headers
     ${include_path}/sensors/empty/empty_measurement_type.h
     ${include_path}/sensors/empty/empty_sensor_class.h
     ${include_path}/sensors/empty/empty_sensor_state_type.h
+    ${include_path}/sensors/velocity/velocity_measurement_type.h
+    ${include_path}/sensors/velocity/velocity_sensor_class.h
+    ${include_path}/sensors/velocity/velocity_sensor_state_type.h
     ${include_path}/data_utils/read_csv.h
     ${include_path}/data_utils/write_csv.h
     ${include_path}/data_utils/read_sim_data.h
@@ -105,6 +108,7 @@ set(headers
     ${include_path}/data_utils/read_gps_data.h
     ${include_path}/data_utils/read_mag_data.h
     ${include_path}/data_utils/read_baro_data.h
+    ${include_path}/data_utils/read_velocity_data.h
     ${include_path}/data_utils/filesystem.h
 )
 

source/mars/include/mars/buffer.h

@@ -186,6 +186,13 @@ class Buffer
   ///
   bool get_entry_at_idx(const int& index, BufferEntryType* entry) const;
 
+  ///
+  /// \brief RemoveSensorFromBuffer Removes all entrys that are associated with the given sensor handle
+  /// \param sensor_handle Sensor handle to be removed
+  /// \return true if the operation was performed, false otherwise
+  ///
+  bool RemoveSensorFromBuffer(const std::shared_ptr<SensorAbsClass>& sensor_handle);
+
   ///
   /// \brief AddEntrySorted Adds a new entry to the buffer and ensures the buffer is sorted
   /// \param new_entry new buffer entry to be added

source/mars/include/mars/data_utils/read_velocity_data.h

@@ -0,0 +1,55 @@
+// Copyright (C) 2024 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
+//
+// All rights reserved.
+//
+// This software is licensed under the terms of the BSD-2-Clause-License with
+// no commercial use allowed, the full terms of which are made available
+// in the LICENSE file. No license in patents is granted.
+//
+// You can contact the author at <[email protected]>
+
+#ifndef READ_VELOCITY_DATA_H
+#define READ_VELOCITY_DATA_H
+
+#include <mars/data_utils/read_csv.h>
+#include <mars/sensors/velocity/velocity_measurement_type.h>
+#include <mars/time.h>
+#include <mars/type_definitions/buffer_data_type.h>
+#include <mars/type_definitions/buffer_entry_type.h>
+#include <Eigen/Dense>
+#include <vector>
+
+namespace mars
+{
+class ReadVelocityData
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  ReadVelocityData(std::vector<BufferEntryType>* data_out, std::shared_ptr<SensorAbsClass> sensor,
+                   const std::string& file_path, const double& time_offset = 0)
+  {
+    std::vector<std::string> expect_entry = { "t", "v_x", "v_y", "v_z" };
+
+    CsvDataType csv_data;
+    ReadCsv(&csv_data, file_path);
+
+    unsigned long number_of_datapoints = csv_data["t"].size();
+    data_out->resize(number_of_datapoints);
+
+    for (size_t k = 0; k < number_of_datapoints; k++)
+    {
+      Time time = csv_data["t"][k] + time_offset;
+      Eigen::Vector3d velocity(csv_data["v_x"][k], csv_data["v_y"][k], csv_data["v_z"][k]);
+
+      BufferDataType data;
+      data.set_sensor_data(std::make_shared<VelocityMeasurementType>(velocity));
+
+      BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
+      data_out->at(k) = current_entry;
+    }
+  }
+};
+}  // namespace mars
+
+#endif  // READ_VELOCITY_DATA_H

source/mars/include/mars/sensor_manager.h

@@ -1,4 +1,5 @@
-// Copyright (C) 2022 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
+// Copyright (C) 2024 Christian Brommer and Thomas Jantos, Control of Networked Systems, University of Klagenfurt,
+// Austria.
 //
 // All rights reserved.
 //
@@ -11,12 +12,133 @@
 #ifndef SENSORMANAGER_HPP
 #define SENSORMANAGER_HPP
 
+#include <mars/sensors/sensor_abs_class.h>
+#include <memory>
+#include <vector>
+
 namespace mars
 {
 class SensorManager
 {
 public:
+  std::vector<std::shared_ptr<SensorAbsClass>> sensor_list_;  ///< Vector containing all registered sensors
   SensorManager() = default;
+
+  ///
+  /// \brief register_sensor Register a sensor with the sensor manager
+  /// \param sensor Sensor to be registered
+  /// \return True if the sensor was registered, false if the sensor is already registered
+  ///
+  bool register_sensor(std::shared_ptr<SensorAbsClass> sensor)
+  {
+    // Check if sensor already exists
+    if (std::find(sensor_list_.begin(), sensor_list_.end(), sensor) != sensor_list_.end())
+    {
+      // Sensor is already registered
+      return false;
+    }
+
+    sensor_list_.push_back(sensor);
+    std::cout << "Registered sensor [" << sensor->name_ << "] with Sensor Manager" << std::endl;
+    return true;
+  }
+
+  ///
+  /// \brief remove_sensor Remove a sensor from the sensor manager
+  /// \param buffer Buffer to remove the sensor from
+  /// \param sensor Sensor to be removed
+  /// \return True if the sensor was removed, false if the sensor is not registered
+  ///
+  bool remove_sensor(Buffer& buffer, std::shared_ptr<SensorAbsClass> sensor)
+  {
+    if (!does_sensor_exist(sensor))
+    {
+      // Sensor is not registered
+      return false;
+    }
+    // Deactive the sensor
+    this->deactivate_sensor(buffer, sensor);
+    // Remove the sensor from the list
+    sensor_list_.erase(std::remove(sensor_list_.begin(), sensor_list_.end(), sensor), sensor_list_.end());
+    std::cout << "Removed sensor [" << sensor->name_ << "] from Sensor Manager" << std::endl;
+    return true;
+  }
+
+  ///
+  /// \brief list_sensors Print the information of all registered sensors
+  ///
+  void list_sensors()
+  {
+    std::cout << "Sensor Manager contains " << sensor_list_.size() << " sensors" << std::endl;
+    for (auto& sensor : sensor_list_)
+    {
+      std::cout << *sensor << std::endl;
+    }
+  }
+
+  ///
+  /// \brief does_sensor_exist Check if a sensor is registered
+  /// \param sensor Sensor to be checked
+  /// \return True if the sensor is registered, otherwise false
+  ///
+  bool does_sensor_exist(std::shared_ptr<SensorAbsClass> sensor)
+  {
+    if (std::find(sensor_list_.begin(), sensor_list_.end(), sensor) != sensor_list_.end())
+    {
+      return true;
+    }
+    return false;
+  }
+
+  ///
+  /// \brief deactivate_sensor Deactivate a sensor
+  /// \param buffer Buffer to remove the sensor from
+  /// \param sensor Sensor to be deactivated
+  /// \return False if the sensor is not registered, otherwise true
+  ///
+  bool deactivate_sensor(Buffer& buffer, std::shared_ptr<SensorAbsClass> sensor)
+  {
+    if (!does_sensor_exist(sensor))
+    {
+      // Sensor is not registered
+      return false;
+    }
+
+    // Reset the sensor
+    sensor->do_update_ = false;
+    sensor->is_initialized_ = false;
+    sensor->ref_to_nav_given_ = false;
+
+    // Call buffer to clear all entries of the sensor
+    if (buffer.RemoveSensorFromBuffer(sensor))
+    {
+      std::cout << "Removed sensor [" << sensor->name_ << "] from buffer" << std::endl;
+    }
+    else
+    {
+      std::cout << "Could not remove sensor [" << sensor->name_ << "] from buffer as buffer is empty" << std::endl;
+      return false;
+    }
+    return true;
+  }
+
+  ///
+  /// \brief activate_sensor Activate a sensor
+  /// \param sensor Sensor to be activated
+  /// \return False if the sensor is not registered, otherwise true
+  ///
+  bool activate_sensor(std::shared_ptr<SensorAbsClass> sensor)
+  {
+    if (!does_sensor_exist(sensor))
+    {
+      // Sensor is not registered
+      return false;
+    }
+
+    sensor->do_update_ = true;
+    std::cout << "Activated sensor [" << sensor->name_ << "]" << std::endl;
+    return true;
+  }
 };
 }  // namespace mars
 

source/mars/include/mars/sensors/sensor_abs_class.h

@@ -22,7 +22,24 @@ class SensorAbsClass : public SensorInterface
   int id_{ -1 };
   std::string name_;              ///< Name of the individual sensor instance
   bool is_initialized_{ false };  ///< True if the sensor has been initialized
+  bool do_update_{ true };        ///< True if updates should be performed with the sensor
   int type_{ -1 };  ///< Future feature, holds information such as position or orientation for highlevel decissions
+  bool const_ref_to_nav_{ true };   ///< True if the reference should not be estimated
+  bool ref_to_nav_given_{ false };  ///< True if the reference to the navigation frame is given by initial calibration
+  bool use_dynamic_meas_noise_{ false };  ///< True if dynamic noise values from measurements should be used
+
+  ///
+  /// \brief operator << Overload the << operator for easy printing of the sensor information
+  ///
+  friend std::ostream& operator<<(std::ostream& out, const SensorAbsClass& sensor)
+  {
+    out << "\tSensor: " << sensor.name_ << std::endl;
+    out << "\tInitialized: " << sensor.is_initialized_ << std::endl;
+    out << "\tDo update: " << sensor.do_update_ << std::endl;
+    out << "\tReference to nav: " << sensor.const_ref_to_nav_ << std::endl;
+    out << "\tUse dynamic noise: " << sensor.use_dynamic_meas_noise_ << std::endl;
+    return out;
+  }
 };
 }  // namespace mars
 #endif  // SENSORABSCLASS_H

source/mars/include/mars/sensors/update_sensor_abs_class.h

@@ -36,13 +36,11 @@ class UpdateSensorAbsClass : public SensorAbsClass
 
   std::shared_ptr<void> initial_calib_{ nullptr };
   bool initial_calib_provided_{ false };  ///< True if an initial calibration was provided
-  bool const_ref_to_nav_{ true };         ///< True if the reference should not be estimated
-  bool use_dynamic_meas_noise_{ false };  ///< True if dynamic noise values from measurements should be used
 
   Chi2 chi2_;
 
   std::shared_ptr<CoreState> core_states_;
 };
-}  // namespace mars
+};  // namespace mars
 
 #endif  // UPDATE_SENSOR_ABS_CLASS_H

source/mars/include/mars/sensors/velocity/velocity_measurement_type.h

@@ -0,0 +1,55 @@
+// Copyright (C) 2024 Christian Brommer, Control of Networked Systems, University of Klagenfurt,
+// Austria.
+//
+// All rights reserved.
+//
+// This software is licensed under the terms of the BSD-2-Clause-License with
+// no commercial use allowed, the full terms of which are made available
+// in the LICENSE file. No license in patents is granted.
+//
+// You can contact the author at <[email protected]>
+
+#ifndef VELOCITYMEASUREMENTTYPE_H
+#define VELOCITYMEASUREMENTTYPE_H
+
+#include <mars/sensors/measurement_base_class.h>
+#include <Eigen/Dense>
+#include <utility>
+
+namespace mars
+{
+class VelocityMeasurementType : public BaseMeas
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  Eigen::Vector3d velocity_;  ///< Velocity [x y z]
+
+  VelocityMeasurementType() = default;
+
+  VelocityMeasurementType(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  // VELOCITYMEASUREMENTTYPE_H