From d34d71cf9bf12df9f9d3d01b92ed3bac37a737a3 Mon Sep 17 00:00:00 2001 From: "Jantos, Thomas Georg" Date: Mon, 26 Feb 2024 17:20:01 +0100 Subject: [PATCH 1/2] add: Added sensor manager capabilities --- source/mars/include/mars/buffer.h | 7 + source/mars/include/mars/sensor_manager.h | 124 +++++++++++++++++- .../include/mars/sensors/sensor_abs_class.h | 17 +++ .../mars/sensors/update_sensor_abs_class.h | 4 +- source/mars/source/buffer.cpp | 18 +++ source/mars/source/core_logic.cpp | 13 +- 6 files changed, 178 insertions(+), 5 deletions(-) diff --git a/source/mars/include/mars/buffer.h b/source/mars/include/mars/buffer.h index 9fe0adb..c0fa3a6 100644 --- a/source/mars/include/mars/buffer.h +++ b/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& 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 diff --git a/source/mars/include/mars/sensor_manager.h b/source/mars/include/mars/sensor_manager.h index 063e4f1..b457a3e 100644 --- a/source/mars/include/mars/sensor_manager.h +++ b/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 +#include +#include + namespace mars { class SensorManager { public: + std::vector> 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 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 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 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 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 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 diff --git a/source/mars/include/mars/sensors/sensor_abs_class.h b/source/mars/include/mars/sensors/sensor_abs_class.h index ee1fd5c..e7e5d8f 100644 --- a/source/mars/include/mars/sensors/sensor_abs_class.h +++ b/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 diff --git a/source/mars/include/mars/sensors/update_sensor_abs_class.h b/source/mars/include/mars/sensors/update_sensor_abs_class.h index faba42e..215bef4 100644 --- a/source/mars/include/mars/sensors/update_sensor_abs_class.h +++ b/source/mars/include/mars/sensors/update_sensor_abs_class.h @@ -36,13 +36,11 @@ class UpdateSensorAbsClass : public SensorAbsClass std::shared_ptr 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 core_states_; }; -} // namespace mars +}; // namespace mars #endif // UPDATE_SENSOR_ABS_CLASS_H diff --git a/source/mars/source/buffer.cpp b/source/mars/source/buffer.cpp index 6882b6a..60e8fc5 100644 --- a/source/mars/source/buffer.cpp +++ b/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& 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); diff --git a/source/mars/source/core_logic.cpp b/source/mars/source/core_logic.cpp index 5cc1145..443991d 100644 --- a/source/mars/source/core_logic.cpp +++ b/source/mars/source/core_logic.cpp @@ -382,7 +382,18 @@ bool CoreLogic::ProcessMeasurement(std::shared_ptr 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 From e6077580ed21691c2a979dfe05439bab559411fe Mon Sep 17 00:00:00 2001 From: "Jantos, Thomas Georg" Date: Mon, 26 Feb 2024 17:49:24 +0100 Subject: [PATCH 2/2] feat: vision_sensor reference frame initialization --- .../mars/sensors/vision/vision_sensor_class.h | 48 ++++++++++++++++++- 1 file changed, 46 insertions(+), 2 deletions(-) diff --git a/source/mars/include/mars/sensors/vision/vision_sensor_class.h b/source/mars/include/mars/sensors/vision/vision_sensor_class.h index de588ae..4a840bb 100644 --- a/source/mars/include/mars/sensors/vision/vision_sensor_class.h +++ b/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 /*sensor_data*/, + BufferDataType Initialize(const Time& timestamp, std::shared_ptr sensor_data, std::shared_ptr latest_core_data) { - // VisionMeasurementType measurement = *static_cast(sensor_data.get()); + VisionMeasurementType measurement = *static_cast(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 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(measurement.get()); VisionSensorData* prior_sensor_data = static_cast(latest_sensor_data.get());