WIP: Generic state estimator

CMakeLists.txt

@@ -239,6 +239,7 @@ PROTOBUF_GENERATE_CPP(PROTO_SRC PROTO_HEADER
   proto/rpyt_reference_connector_config.proto
   proto/polynomial_reference_config.proto
   proto/odom_from_pose_sensor_config.proto
+  proto/state_estimator_config.proto
 )
 add_library(proto ${PROTO_HEADER} ${PROTO_SRC})
 

include/aerial_autonomy/common/chrono_stamped.h

@@ -0,0 +1,17 @@
+#pragma once
+#include <chrono>
+
+namespace common {
+template <typename T> class Stamped : public T {
+public:
+  using time_point =
+      std::chrono::time_point<std::chrono::high_resolution_clock>;
+  time_point stamp;
+  Stamped() = default;
+  Stamped(const T &input, const time_point &timestamp)
+      : T(input), stamp_(timestamp) {}
+  Stamped(const T &input)
+      : Stamped(input, std::chrono::high_resolution_clock::now()) {}
+  void setData(const T &input) { *static_cast<T *>(this) = input; };
+};
+}

include/aerial_autonomy/estimators/state_estimator.h

@@ -1,27 +1,23 @@
 #pragma once
+#include "state_estimator_backend.h"
+#include "state_estimator_frontend.h"
+#include <aerial_autonomy/common/chrono_stamped.h>
 
-/**
-* @brief Estimate the state of the robot
-*
-* @tparam StateT The state type estimated by the estimator
-* @tparam ControlT  The control type used to propagate the estimator
-*/
-template <class StateT, class ControlT> struct AbstractStateEstimator {
-  /**
-  * @brief Run estimator loop. Propagates the estimator state using controls
-  *
-  * @param control Current control
-  */
-  virtual void propagate(const ControlT &control) = 0;
-  /**
-  * @brief Get the current estimate of state
-  *
-  * @return The initial state estimate
-  */
-  virtual StateT getState() = 0;
-  /**
-   * @brief check if estimator is fine
-   * @return true if estimator is ok
-   */
-  virtual bool getStatus() { return true; }
+class StateEstimator {
+public:
+  StateEstimator(StateEstimatorConfig config,
+                 parsernode::Parser &drone_hardware);
+  void process();
+  // Getters for accessing latest data
+  common::Stamped<Transform> getPose();
+  common::Stamped<Transform> getTargetPose();
+  common::Stamped<Eigen::Vector3d> getBodyVelocity();
+  common::Stamped<Eigen::Vector3d> getSpatialVelocity();
+  common::Stamped<Eigen::Vector3d> getBodyAngularVelocity();
+  StateEstimatorStatus getStatus();
+
+private:
+  StateEstimatorBackend backend_;
+  StateEstimatorFrontend frontend_;
+  StateEstimatorStatus status_;
 };

include/aerial_autonomy/estimators/state_estimator_backend.h

@@ -0,0 +1,41 @@
+#pragma once
+#include "state_estimator_config.pb.h"
+#include "state_estimator_status.h"
+#include <Eigen/Dense>
+#include <aerial_autonomy/common/chrono_stamped.h>
+#include <deque>
+#include <tf/tf.h>
+#include <vector>
+
+class StateEstimatorBackend {
+public:
+  StateEstimatorBackend(StateEstimatorConfig config);
+  // Methods to fill sensor data from front end
+  using time_point =
+      std::chrono::time_point<std::chrono::high_resolution_clock>;
+  void addPositionData(time_point sensor_time, const Eigen::Vector3d position);
+  void addOrientation(time_point sensor_time, const tf::Quaternion quat);
+  void addBodyVelocity(time_point sensor_time,
+                       const Eigen::Vector3d body_velocity);
+  void addOmega(time_point sensor_time, const Eigen::Vector3d omega);
+  void addAcceleration(time_point sensor_time,
+                       const Eigen::Vector3d acceleration);
+  void addAltitude(time_point sensor_time, const double altitude);
+  void addTargetPose(time_point sensor_time, const tf::Tranform target_pose);
+  // Optimize based on available data
+  StateEstimatorStatus::OptimizationStatus optimize();
+  Eigen::VectorXd getCurrentState();
+
+protected:
+  template <class T> using DequeVector = std::deque<std::vector<T>>;
+  DequeVector<Eigen::Vector3d> position_deque;
+  DequeVector<tf::Quaternion> orientation_deque;
+  DequeVector<Eigen::Vector3d> velocity_deque;
+  DequeVector<Eigen::Vector3d> angular_velocity_deque;
+  DequeVector<Eigen::Vector3d> body_accelection_deque;
+  DequeVector<double> altitudde_deque;
+  DequeVector<Eigen::Vector3d> target_position_deque;
+  DequeVector<tf::Quaternion> target_orientation_deque;
+  DequeVector<Eigen::MatrixXd> prior_covariance;
+  Eigen::VectorXd state_knots;
+};

include/aerial_autonomy/estimators/state_estimator_frontend.h

@@ -0,0 +1,28 @@
+#pragma once
+#include "ros/ros.h"
+#include "state_estimator_backend.h"
+#include "state_estimator_config.pb.h"
+#include <geometry_msgs/Vector3.h>
+#include <parsernode/parser.h>
+#include <tf/tf.h>
+
+class StateEstimatorFrontend {
+public:
+  StateEstimatorFrontend(StateEstimatorConfig config,
+                         parsernode::Parser &drone_hardware,
+                         StateEstimatorBackend &backend);
+  void poseCallback(const geometry_msgs::TransformStamped &pose_input);
+  void guidanceCallback(const geometry_msgs::Vector3Stamped &velocity);
+  void gyroCallback(const geometry_msgs::Vector3 &omega);
+  void accCallback(const geometry_msgs::Vector3 &acc);
+  void altitudeCallback(const double &altitude);
+  void trackingVectorCallback(tf::Transform pose);
+  CumulativeSensorStatus getStatus() { return sensor_status_; }
+
+private:
+  StateEstimatorConfig config_;
+  ros::NodeHandle nh_;
+  ros::Subscriber pose_sub_;
+  StateEstimatorBackend &backend_;
+  CumulativeSensorStatus sensor_status_;
+};

include/aerial_autonomy/estimators/state_estimator_status.h

@@ -0,0 +1,16 @@
+#pragma once
+
+struct CumulativeSensorStatus {
+  enum class SensorStatus { Active = 0, Inactive = 1, Critical = 2 };
+  SensorStatus external_pose_status = SensorStatus::Inactive;
+  SensorStatus flight_status = SensorStatus::Inactive;
+  SensorStatus guidance_status = SensorStatus::Inactive;
+  SensorStatus tracker_status = SensorStatus::Inactive;
+};
+
+struct StateEstimatorStatus {
+  enum class OptimizationStatus { SUCCESS = 0, FAILURE = 1 };
+  CumulativeSensorStatus sensor_status;
+  OptimizationStatus optimization_status = OptimizationStatus::SUCCESS;
+  bool estimator_status = false;
+};

include/aerial_autonomy/trackers/alvar_tracker.h

@@ -26,13 +26,6 @@ class AlvarTracker : public BaseTracker {
         alvar_sub_(nh_.subscribe("ar_pose_marker", 1,
                                  &AlvarTracker::markerCallback, this)),
         timeout_(timeout) {}
-  /**
-   * @brief Get the tracking vectors
-   * @param pos Returned tracking vectors
-   * @return True if successful, false otherwise
-   */
-  virtual bool
-  getTrackingVectors(std::unordered_map<uint32_t, tf::Transform> &pos);
   /**
   * @brief Check whether tracking is valid
   * @return True if the tracking is valid, false otherwise
@@ -76,10 +69,6 @@ class AlvarTracker : public BaseTracker {
   Atomic<std::chrono::time_point<std::chrono::high_resolution_clock>>
       last_tracking_time_;
   /**
-  * @brief Stored tracking transforms
-  */
-  Atomic<std::unordered_map<uint32_t, tf::Transform>> object_poses_;
-  /**
   * @brief Timeout for valid update
   */
   const std::chrono::duration<double> timeout_;

include/aerial_autonomy/trackers/base_tracker.h

@@ -1,4 +1,5 @@
 #pragma once
+#include "aerial_autonomy/common/atomic.h"
 #include "aerial_autonomy/trackers/tracking_strategy.h"
 
 #include <tf/tf.h>
@@ -37,20 +38,19 @@ class BaseTracker {
    * @param pos Returned tracking vector
    * @return True if successful, false otherwise
    */
-  virtual bool getTrackingVector(tf::Transform &pos);
+  bool getTrackingVector(tf::Transform &pos);
   /**
    * @brief Get the tracking vector
    * @param pos Returned tracking vector
    * @return True if successful, false otherwise
    */
-  virtual bool getTrackingVector(std::tuple<uint32_t, tf::Transform> &pos);
+  bool getTrackingVector(std::tuple<uint32_t, tf::Transform> &pos);
   /**
    * @brief Get the tracking vectors
    * @param pos Returned map of tracking vectors
    * @return True if successful, false otherwise
    */
-  virtual bool
-  getTrackingVectors(std::unordered_map<uint32_t, tf::Transform> &pos) = 0;
+  bool getTrackingVectors(std::unordered_map<uint32_t, tf::Transform> &pos);
   /**
   * @brief Check whether tracking is valid
   * @return True if the tracking is valid, false otherwise
@@ -67,9 +67,37 @@ class BaseTracker {
     return std::chrono::high_resolution_clock::now();
   }
 
+  /**
+   * @brief Set a callback function that will be called whenever a tracker
+   * matching tracking strategy is available
+   *
+   * @param tracker_callback Callback function
+   */
+  void setTrackerCallback(
+      std::function<void(uint32_t, tf::Transform)> tracker_callback);
+
+protected:
+  /**
+   * @brief Update internal tracking vectors. Subclasses should call this
+   * function to
+   * update the tracking vectors
+   *
+   * @param tracking_poses Map of tracking transforms and their ids
+   */
+  void updateTrackingPoses(
+      const std::unordered_map<uint32_t, tf::Transform> &tracking_poses);
+
 private:
   /**
   * @brief Strategy used to choose which object to track among multiple objects
   */
   std::unique_ptr<TrackingStrategy> tracking_strategy_;
+  /**
+   * @brief Callback function when tracked object is available
+   */
+  std::function<void(uint32_t, tf::Transform)> tracker_callback_;
+  /**
+   * @brief Map of tracker ids and poses
+   */
+  Atomic<std::unordered_map<uint32_t, tf::Transform>> tracking_poses_;
 };

include/aerial_autonomy/trackers/roi_to_position_converter.h

@@ -41,13 +41,6 @@ class RoiToPositionConverter : public BaseTracker {
             "depth", 1, &RoiToPositionConverter::depthCallback, this)),
         image_subscriber_(it_.subscribe(
             "image", 1, &RoiToPositionConverter::imageCallback, this)) {}
-
-  /**
-   * @brief Get the stored tracking vector
-   * @param pos Returned tracking vectors
-   * @return True if successful, false otherwise
-   */
-  bool getTrackingVectors(std::unordered_map<uint32_t, tf::Transform> &pos);
   /**
    * @brief Get the 3D position of the ROI (in the frame of the
    * camera)
@@ -158,10 +151,6 @@ class RoiToPositionConverter : public BaseTracker {
   */
   Atomic<sensor_msgs::RegionOfInterest> roi_rect_;
   /**
-  * @brief Transform of object in camera frame (meters)
-  */
-  Atomic<tf::Transform> object_pose_;
-  /**
   * @brief Max distance of object from camera (meters)
   * \todo Make this a configurable param
   */
@@ -171,7 +160,6 @@ class RoiToPositionConverter : public BaseTracker {
   * \todo Make this a configurable param
   */
   double foreground_percent_ = 0.25;
-
   /**
   * @brief last time ROI was updated
   */

include/aerial_autonomy/trackers/simple_multi_tracker.h

@@ -13,13 +13,6 @@ class SimpleMultiTracker : public BaseTracker {
   */
   SimpleMultiTracker(std::unique_ptr<TrackingStrategy> tracking_strategy)
       : BaseTracker(std::move(tracking_strategy)) {}
-  /**
-   * @brief Get the tracking vectors
-   * @param pos Returned list of tracking vectors
-   * @return True if successful, false otherwise
-   */
-  virtual bool
-  getTrackingVectors(std::unordered_map<uint32_t, tf::Transform> &pos);
   /**
   * @brief Check whether tracking is valid
   * @return True if the tracking is valid, false otherwise
@@ -32,10 +25,4 @@ class SimpleMultiTracker : public BaseTracker {
   */
   void
   setTrackingVectors(const std::unordered_map<uint32_t, tf::Transform> &pos);
-
-private:
-  /**
-  * @brief Tracking vectors to return
-  */
-  std::unordered_map<uint32_t, tf::Transform> tracking_vectors_;
 };

include/aerial_autonomy/trackers/simple_tracker.h

@@ -1,7 +1,9 @@
 #pragma once
 #include "uav_vision_system_config.pb.h"
+#include <aerial_autonomy/common/async_timer.h>
 #include <aerial_autonomy/trackers/base_tracker.h>
 #include <aerial_autonomy/types/position.h>
+#include <mutex>
 #include <parsernode/parser.h>
 #include <tf/tf.h>
 
@@ -19,13 +21,6 @@ class SimpleTracker : public BaseTracker {
   */
   SimpleTracker(parsernode::Parser &drone_hardware,
                 tf::Transform camera_transform);
-  /**
-   * @brief Get the tracking vector
-   * @param pos Returned tracking vector
-   * @return True if successful, false otherwise
-   */
-  virtual bool
-  getTrackingVectors(std::unordered_map<uint32_t, tf::Transform> &pos);
   /**
   * @brief Check whether tracking is valid
   * @return True if the tracking is valid, false otherwise
@@ -69,4 +64,8 @@ class SimpleTracker : public BaseTracker {
   bool tracking_valid_;                ///< Flag to specify if tracking is valid
   std::unordered_map<uint32_t, tf::Transform> target_poses_; ///< Tracked poses
   tf::Transform camera_transform_; ///< Transform of camera in uav frame
+  AsyncTimer
+      update_tracker_pose_timer_; ///< Update tracker poses based on quad pose
+  std::recursive_mutex timer_mutex_; ///< Lock access to target poses
+  void updateRelativePoses();
 };