Move the robot base

edit

I am trying to move the robot base to either point A(1,0) or point B(0,1) at each step. The robot initially starts at the origin. For example, the robot is at O(0,0) then it moves to A and it can stay at A for a while. Then it moves to B and moves back to A.

Here is what I do. I keep sending a msg to move the robot with the speed as a function of distance between the current position of the robot and the goal (goal_x, goal_y). I will stop when the robot is very close to the goal. Here is the code I use.

tf::StampedTransform start_transform;
tf::StampedTransform current_transform;
geometry_msgs::Twist base_cmd;
ros::Rate rate(50.0);
bool done = false;
listener_.waitForTransform("base_footprint", "odom_combined", ros::Time(0), ros::Duration(1000.0));
listener_.lookupTransform("base_footprint", "odom_combined", ros::Time(0), start_transform);

double initial_x = -start_transform.getOrigin().x();
double initial_y = -start_transform.getOrigin().y();
base_cmd.linear.x = (goal_x-initial_x)/20;
base_cmd.linear.y = (goal_y-initial_y)/20;
base_cmd.angular.z = 0;

while (!done && n.ok())
{
  //send the drive command
  cmd_vel_pub_.publish(base_cmd);
  rate.sleep();
  //get the current transform
  try
  {
    listener_.lookupTransform("base_footprint", "odom_combined", ros::Time(0), current_transform);
  }
  catch (tf::TransformException ex)
  {
    ROS_ERROR("%s",ex.what());
    break;
  }    
  double current_x = -current_transform.getOrigin().x();
  double current_y = -current_transform.getOrigin().y();
  if(fabs(goal_x-current_x) < 0.001 && fabs(goal_y-current_y) < 0.001){
    done = true;
  } 

  base_cmd.linear.x = (goal_x-current_x)/6;
  base_cmd.linear.y = (goal_y-current_y)/6;
}

However, the result is a off by a lot. It is accurate and is what I expected at the beginning. However, later on, it seems like the position of the robot in the global frame is not correct. My concern is that I don't think it is right to get the global position of the robot by using the transformation from "base_footprint" to "odom_combined". Could anyone point me out how to correct this?