WIP: Generic state estimator

[garimellagowtham]

This change is 

[msheckells]

Need a connector to own both front end and back end and expose a status function and state getter

Reviewed 5 of 5 files at r1.
Reviewable status: all files reviewed, 7 unresolved discussions (waiting on @garimellagowtham)

---

include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 10 at r1 (raw file):

#include <vector>

class GenericStateEstimatorBackend {

Just call it StateEstimatorBackend

---

include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 26 at r1 (raw file):

              const geometry_msgs::Vector3 acceleration);
  void addAltitude(time_point sensor_time, const double altitude);
  void addTargetPosition(time_point sensor_time,

Should just be one function called addTargetPose

---

include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 31 at r1 (raw file):

                            const geometry_msgs::Quaternion target_orientation);
  // Optimize based on available data
  void optimize();

should have an optimization status

---

include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 36 at r1 (raw file):

  tf::StampedTransform getTargetPose();
  tf::Stamped<tf::Vector3> getBodyVelocity();
  tf::Stamped<tf::Vector3> getSpatialVelocity();

we can implement our own Stamped for std::chrono

---

include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 41 at r1 (raw file):

protected:
  template <class T> using DequeVector = std::deque<std::vector<T>>;
  DequeVector<geometry_msgs::Vector3> position_deque;

we should use eigen instead geometry_msgs (easier to map to **)

---

include/aerial_autonomy/estimators/generic_state_estimator_front_end.h, line 17 at r1 (raw file):

                                GenericStateEstimatorBackend &back_end);
  void trackingVectorCallback(time_point sensor_time, tf::Transform pose);
  void poseCallback(const geometry_msgs::TransformStamped &pose_input);

some of these will need the proper ROS callback parametrization

---

include/aerial_autonomy/estimators/generic_state_estimator_front_end.h, line 22 at r1 (raw file):

  void accCallback(time_point sensor_time, const geometry_msgs::Vector3 &acc);
  void altitudeCallback(time_point sensor_time, const double &altitude);
  void guidanceCallback(const geometry_msgs::Vector3Stamped &velocity);

need a status function

[garimellagowtham]

Reviewable status: 2 of 7 files reviewed, 7 unresolved discussions (waiting on @msheckells)

---

include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 10 at r1 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

Just call it StateEstimatorBackend

Done.

---

include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 26 at r1 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

Should just be one function called addTargetPose

Done.

---

include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 31 at r1 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

should have an optimization status

Done.

---

include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 36 at r1 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

we can implement our own Stamped for std::chrono

Done.

---

include/aerial_autonomy/estimators/generic_state_estimator_back_end.h, line 41 at r1 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

we should use eigen instead geometry_msgs (easier to map to **)

Done.

---

include/aerial_autonomy/estimators/generic_state_estimator_front_end.h, line 17 at r1 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

some of these will need the proper ROS callback parametrization

Done.

---

include/aerial_autonomy/estimators/generic_state_estimator_front_end.h, line 22 at r1 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

need a status function

Done.

[msheckells]

Reviewed 17 of 17 files at r2.
Reviewable status: all files reviewed, 6 unresolved discussions (waiting on @garimellagowtham)

---

include/aerial_autonomy/common/chrono_stamped.h, line 9 at r2 (raw file):

  using time_point =
      std::chrono::time_point<std::chrono::high_resolution_clock>;
  time_point stamp;

should this be public? Do we ever expect to have to reset the stamp?

---

include/aerial_autonomy/common/chrono_stamped.h, line 15 at r2 (raw file):

  Stamped(const T &input)
      : Stamped(input, std::chrono::high_resolution_clock::now()) {}
  void setData(const T &input) { *static_cast<T *>(this) = input; };

This is interesting. So this resets the T part of the class without resetting stamp?

---

include/aerial_autonomy/estimators/state_estimator_backend.h, line 16 at r2 (raw file):

  using time_point =
      std::chrono::time_point<std::chrono::high_resolution_clock>;
  void addPositionData(time_point sensor_time, const Eigen::Vector3d position);

call it addPosition to be consistent

---

include/aerial_autonomy/estimators/state_estimator_backend.h, line 31 at r2 (raw file):

protected:
  template <class T> using DequeVector = std::deque<std::vector<T>>;
  DequeVector<Eigen::Vector3d> position_deque;

The DequeVector should contain a Stamped so that we keep the time stamps

---

include/aerial_autonomy/estimators/state_estimator_status.h, line 4 at r2 (raw file):

struct CumulativeSensorStatus {
  enum class SensorStatus { Active = 0, Inactive = 1, Critical = 2 };

is SensorStatus defined somewhere else?

---

include/aerial_autonomy/estimators/state_estimator_status.h, line 15 at r2 (raw file):

  CumulativeSensorStatus sensor_status;
  OptimizationStatus optimization_status = OptimizationStatus::SUCCESS;
  bool estimator_status = false;

should this be a SensorStatus?