Merge branch 'feat/sensor_manager' into development

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/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/vision/vision_sensor_class.h

@@ -72,10 +72,10 @@ class VisionSensorClass : public UpdateSensorAbsClass
     initial_calib_provided_ = true;
   }
 
-  BufferDataType Initialize(const Time& timestamp, std::shared_ptr<void> /*sensor_data*/,
+  BufferDataType Initialize(const Time& timestamp, std::shared_ptr<void> sensor_data,
                             std::shared_ptr<CoreType> latest_core_data)
   {
-    // VisionMeasurementType measurement = *static_cast<VisionMeasurementType*>(sensor_data.get());
+    VisionMeasurementType measurement = *static_cast<VisionMeasurementType*>(sensor_data.get());
 
     VisionSensorData sensor_state;
     std::string calibration_type;
@@ -88,6 +88,44 @@ class VisionSensorClass : public UpdateSensorAbsClass
 
       sensor_state.state_ = calib.state_;
       sensor_state.sensor_cov_ = calib.sensor_cov_;
+
+      // Overwrite the calibration between the reference world and navigation world in given sensor_state
+      if (!this->ref_to_nav_given_)
+      {
+        // The calibration between reference world and navigation world is not given.
+        // Calculate it given the current estimate and measurement
+
+        // Orientation Vision World R_vw
+
+        Eigen::Quaterniond q_wi(latest_core_data->state_.q_wi_);
+        Eigen::Quaterniond q_ic(calib.state_.q_ic_);
+        Eigen::Quaterniond q_vc(measurement.orientation_);
+        Eigen::Quaterniond q_vw = (q_wi * q_ic * q_vc.inverse()).inverse();
+
+        Eigen::Matrix3d R_wi(q_wi.toRotationMatrix());
+        Eigen::Matrix3d R_ic(q_ic.toRotationMatrix());
+
+        Eigen::Matrix3d R_vw(q_vw.toRotationMatrix());
+
+        Eigen::Vector3d p_wi(latest_core_data->state_.p_wi_);
+        Eigen::Vector3d p_ic(calib.state_.p_ic_);
+        Eigen::Vector3d p_vc(measurement.position_);
+
+        Eigen::Vector3d p_vw = -(R_vw * (p_wi + (R_wi * p_ic)) - p_vc);
+
+        sensor_state.state_.q_vw_ = q_vw;
+        sensor_state.state_.p_vw_ = p_vw;
+      }
+      std::cout << "Info: [" << name_ << "] Reference Frame initialized to:" << std::endl;
+      std::cout << "\tP_vw[m]: [" << sensor_state.state_.p_vw_.transpose() << " ]" << std::endl;
+
+      Eigen::Vector4d q_vw_out(sensor_state.state_.q_vw_.w(), sensor_state.state_.q_vw_.x(),
+                               sensor_state.state_.q_vw_.y(), sensor_state.state_.q_vw_.z());
+
+      std::cout << "\tq_vw: [" << q_vw_out.transpose() << " ]" << std::endl;
+      std::cout << "\tR_vw[deg]: ["
+                << sensor_state.state_.q_vw_.toRotationMatrix().eulerAngles(0, 1, 2).transpose() * (180 / M_PI) << " ]"
+                << std::endl;
     }
     else
     {
@@ -148,6 +186,12 @@ class VisionSensorClass : public UpdateSensorAbsClass
                   std::shared_ptr<void> latest_sensor_data, const Eigen::MatrixXd& prior_cov,
                   BufferDataType* new_state_data)
   {
+    // Check if updates should be performed with the sensor
+    if (!do_update_)
+    {
+      return false;
+    }
+
     // Cast the sensor measurement and prior state information
     VisionMeasurementType* meas = static_cast<VisionMeasurementType*>(measurement.get());
     VisionSensorData* prior_sensor_data = static_cast<VisionSensorData*>(latest_sensor_data.get());

source/mars/source/buffer.cpp

@@ -339,6 +339,24 @@ bool Buffer::get_entry_at_idx(const int& index, BufferEntryType* entry) const
   return false;
 }
 
+bool Buffer::RemoveSensorFromBuffer(const std::shared_ptr<SensorAbsClass>& sensor_handle)
+{
+  if (this->IsEmpty())
+  {
+    return false;
+  }
+
+  for (int k = 0; k < this->get_length(); k++)
+  {
+    if (data_[k].sensor_ == sensor_handle)
+    {
+      *data_.erase(data_.begin() + k);
+    }
+  }
+
+  return true;
+}
+
 int Buffer::AddEntrySorted(const BufferEntryType& new_entry)
 {
   int index = InsertDataAtTimestamp(new_entry);

source/mars/source/core_logic.cpp

@@ -382,7 +382,18 @@ bool CoreLogic::ProcessMeasurement(std::shared_ptr<SensorAbsClass> sensor, const
 {
   if (verbose_)
   {
-    std::cout << "[CoreLogic]: Process Measurement (" << sensor->name_ << ")" << std::endl;
+    std::cout << "[CoreLogic]: Process Measurement (" << sensor->name_ << ")";
+    if (!sensor->do_update_)
+    {
+      std::cout << ". Sensor is deactivated.";
+    }
+    std::cout << std::endl;
+  }
+
+  if (!sensor->do_update_)
+  {
+    // Do not perform update for this sensor
+    return false;
   }
 
   // Generate buffer entry element for the measurement