localizer.cpp

Go to the documentation of this file.

#include <youbot_overhead_localization/localizer.h>

localizer::localizer()
{
  std::string odomTopicName;

  odometryInitialized = false;
  pose.x = 0;
  pose.y = 0;
  pose.theta = 0;

  P[0] = .5;
  P[1] = .5;
  P[2] = .5;

  Q[0] = Q_X;
  Q[1] = Q_Y;
  Q[2] = Q_T;

  R[0] = R_X;
  R[1] = R_Y;
  R[2] = R_T;

  n.getParam("/base_localizer/odomTopic", odomTopicName);

  //subscriber, publisher, and service initialization
  baseOdometrySubscriber = n.subscribe(odomTopicName, 0, &localizer::baseOdometryCallback, this);
  positionFromCameraSubscriber = n.subscribe("/image_calibration/position_from_camera", 1,
                                             &localizer::positionFromCameraCallback, this);
  baseLocalizationPublisher = n.advertise<youbot_overhead_localization::Pose2d>("base_localizer/pose_2d", 1);
  coordConversionClient = n.serviceClient<youbot_overhead_vision::CoordConversion>("coord_conversion");
}

void localizer::baseOdometryCallback(const nav_msgs::Odometry& odometry)
{
  if (!odometryInitialized)
  {
    lastReceivedOdometry = odometry;
    odometryInitialized = true;
  }
  else
  {
    float u[3]; //change in odometry
    float B[3][3]; //transformation matrix
    double timePassed = (odometry.header.stamp - lastReceivedOdometry.header.stamp).toSec();

    u[0] = odometry.twist.twist.linear.x * timePassed;
    u[1] = odometry.twist.twist.linear.y * timePassed;
    u[2] = odometry.twist.twist.angular.z * timePassed;

    B[0][0] = cos(pose.theta);
    B[0][1] = sin(pose.theta);
    B[0][2] = 0.0;
    B[1][0] = -sin(pose.theta);
    B[1][1] = cos(pose.theta);
    B[1][2] = 0.0;
    B[2][0] = 0.0;
    B[2][1] = 0.0;
    B[2][2] = 1.0;

    //Kalman Filter Time Update Equations
    //Update position based on odometry
    pose.x += u[0] * B[0][0] + u[1] * B[1][0] + u[2] * B[2][0];
    pose.y += u[0] * B[0][1] + u[1] * B[1][1] + u[2] * B[2][1];
    pose.theta += u[0] * B[0][2] + u[1] * B[1][2] + u[2] * B[2][2];

    //Correct angle
    pose.theta = convertAngle(pose.theta);

    //Update error covariance
    P[0] += Q[0];
    P[1] += Q[1];
    P[2] += Q[2];

    baseLocalizationPublisher.publish(pose);

    lastReceivedOdometry = odometry;
  }
}

void localizer::positionFromCameraCallback(const youbot_overhead_vision::PositionFromCamera& newPos)
{
  float y[3]; //measurement vector
  float J[3]; //Kalman gain
  float heading = (float)(newPos.heading);
  int posx = newPos.midpointX;
  int posy = newPos.midpointY;

  youbot_overhead_vision::CoordConversion srv;
  srv.request.x = posx;
  srv.request.y = posy;
  srv.request.type = 1; //convert from pixels to meters
  coordConversionClient.call(srv);

  y[0] = srv.response.xConverted - pose.x;
  y[1] = srv.response.yConverted - pose.y;
  y[2] = convertAngle(heading - pose.theta);

  //Kalman Filter Measurement Update Equations
  //Calculate Kalman Gain
  J[0] = P[0] * (1.0 / (P[0] + R[0]));
  J[1] = P[1] * (1.0 / (P[1] + R[1]));
  J[2] = P[2] * (1.0 / (P[2] + R[2]));

  //Calculate new position using Kalman Gain
  pose.x = pose.x + J[0] * y[0];
  pose.y = pose.y + J[1] * y[1];
  pose.theta = convertAngle(pose.theta + J[2] * y[2]);

  //Calculate updated error covariance matrix
  P[0] = (1.0 - J[0]) * P[0];
  P[1] = (1.0 - J[1]) * P[1];
  P[2] = (1.0 - J[2]) * P[2];

  baseLocalizationPublisher.publish(pose);
}

float localizer::convertAngle(float theta)
{
  while (theta > PI)
  {
    theta -= 2 * PI;
  }
  while (theta < -PI)
  {
    theta += 2 * PI;
  }

  return theta;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "base_localizer");

  localizer baseLocalizer;

  ros::spin();

  return 0;
}