localizer.h

Go to the documentation of this file.

#include <nav_msgs/Odometry.h>

#include <ros/ros.h>

#include <youbot_overhead_localization/Pose2d.h>
#include <youbot_overhead_vision/CoordConversion.h>
#include <youbot_overhead_vision/PositionFromCamera.h>

//Noise covariance matrix
#define Q_X .05
#define Q_Y .05
#define Q_T .05

//Measurement covariance matrix
#define R_X .1
#define R_Y .1
#define R_T .1

#define PI 3.14159

class localizer
{
public:
  //ROS subscribers, publishers, and services
  ros::NodeHandle n;
  ros::Publisher baseLocalizationPublisher;
  ros::ServiceClient coordConversionClient;
  ros::Subscriber baseOdometrySubscriber;
  ros::Subscriber positionFromCameraSubscriber;

  //the localization object
  youbot_overhead_localization::Pose2d pose;

  //Kalman filter
  float P[3]; //error covariance
  float Q[3]; //noise covariance
  float R[3]; //measurement covariance

  //odometry
  bool odometryInitialized;
  nav_msgs::Odometry lastReceivedOdometry;

  void baseOdometryCallback(const nav_msgs::Odometry& odometry);

  void positionFromCameraCallback(const youbot_overhead_vision::PositionFromCamera& newPos);

  float convertAngle(float theta);

  localizer();
};