state_estimator.h

#ifndef STATE_ESTIMATOR_H
#define STATE_ESTIMATOR_H
 
#include <quad_msgs/ContactMode.h>
#include <quad_msgs/RobotState.h>
#include <quad_utils/quad_kd.h>
#include <quad_utils/ros_utils.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/String.h>
 
 
class StateEstimator {
 public:
  StateEstimator();
 
  virtual void init(ros::NodeHandle& nh) = 0;
 
  virtual bool updateOnce(quad_msgs::RobotState& last_robot_state_msg) = 0;
 
  void readIMU(const sensor_msgs::Imu::ConstPtr& last_imu_msg,
               Eigen::Vector3d& fk, Eigen::Vector3d& wk,
               Eigen::Quaterniond& qk);
 
  void readJointEncoder(
      const sensor_msgs::JointState::ConstPtr& last_joint_state_msg,
      Eigen::VectorXd& jk);
 
  void loadMocapMsg(geometry_msgs::PoseStamped::ConstPtr last_mocap_msg);
 
  void loadSensorMsg(sensor_msgs::Imu last_imu_msg,
                     sensor_msgs::JointState last_joint_state_msg);
 
 protected:
  quad_msgs::RobotState state_est_;
 
  std::shared_ptr<quad_utils::QuadKD> quadKD_;
 
  geometry_msgs::PoseStamped::ConstPtr last_mocap_msg_;
 
  sensor_msgs::Imu last_imu_msg_;
 
  sensor_msgs::JointState last_joint_state_msg_;
};
 
#endif  // STATE_ESTIMATOR_H