Move the robot base

asked 2015-01-14 15:22:57 -0500

newcastle gravatar image

updated 2015-01-14 15:25:35 -0500

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
  //get the current transform
    listener_.lookupTransform("base_footprint", "odom_combined", ros::Time(0), current_transform);
  catch (tf::TransformException ex)
  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?

edit retag flag offensive close merge delete