pose_estimation.h

Go to the documentation of this file.

#ifndef UCL_POSE_ESTIMATION_H
#define UCL_POSE_ESTIMATION_H

#include <ucl_drone/ucl_drone.h>

// #define USE_PROFILING  // Uncomment this line to display timing print in the standard output
#include <ucl_drone/profiling.h>

// Header files
#include <ardrone_autonomy/Navdata.h>
#include <ros/ros.h>
#include <signal.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <queue>

// messages
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Empty.h>
#include <std_srvs/Empty.h>
#include <ucl_drone/Pose3D.h>

class PoseEstimator
{
private:
  ros::NodeHandle nh;

  // Subscribers:
  ros::Subscriber navdata_sub;      
  ros::Subscriber odometry_sub;     
  ros::Subscriber pose_visual_sub;  
  ros::Subscriber reset_sub;        

  // Publishers:
  ros::Publisher pose_pub;            
  ros::Publisher end_reset_pose_pub;  

  // Paths to topics:
  std::string navdata_channel;
  std::string odometry_channel;
  std::string pose_channel;
  std::string end_reset_pose_channel;
  std::string pose_visual_channel;
  std::string pose_visual_correction_channel;
  std::string imu_channel;
  std::string reset_channel;

  ardrone_autonomy::Navdata lastNavdataReceived;

  nav_msgs::Odometry lastOdometryReceived;

  ucl_drone::Pose3D lastposeVisualReceived;

  ros::Time odom_time;
  double odometry_x;  
  std::queue< std::vector< double > > queue_dx;
  double odometry_y;  
  std::queue< std::vector< double > > queue_dy;
  double odometry_rotX;  
  double odometry_rotY;
  double odometry_rotZ;
  std::queue< std::vector< double > > queue_drotZ;
  double rot_Z_offset;   
  double lastRotX;       
  double lastRotY;       
  double lastRotZ;       
  double previous_rotX;  
  double previous_rotY;  
  double previous_rotZ;  

  void navdataCb(const ardrone_autonomy::Navdata::ConstPtr navdataPtr);

  void odometryCb(const nav_msgs::Odometry::ConstPtr odometryPtr);

  void poseVisualCb(const ucl_drone::Pose3D::ConstPtr poseVisualPtr);

  bool poseFusion(ucl_drone::Pose3D& pose_msg);

  bool queuePoseFusion(ucl_drone::Pose3D& pose_msg);

  void processQueue(std::queue< std::vector< double > >& myqueue, double& result);

  void pushQueue(std::queue< std::vector< double > >& myqueue, double item);

  bool poseCopy(ucl_drone::Pose3D& pose_msg);

  void resetCb(const std_msgs::Empty msg);

  bool use_visual_pose;  

public:
  PoseEstimator();

  ~PoseEstimator();

  bool odometry_publishing;       
  bool visual_pose_available;     
  bool pending_reset;             
  void publish_pose();            
  void publish_end_reset_pose();  
  void doReset();                 
  void doFlatTrim();              
};

#endif /* UCL_POSE_ESTIMATION_H */