# Transform concept - tf tutorial

I'm doing tf tutorial on writing broadcaster and listener .

The scenario is : the user control turtle1 using keyboard, turtle2 will follow turtle1.

In the original listener code:

-It lookuptransform of turtle2 relative to turtle 1

listener.lookupTransform("/turtle2", "/turtle1",
ros::Time(0), transform);


-and then use the relative position(position turtle1-turtle2) to determine velocity.

vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
pow(transform.getOrigin().y(), 2));
turtle_vel.publish(vel_msg);


MY QUESTION : Instead of getting the relative position of turtle2 relative to turtle1 directly.. I try to get the position of turtle2 relative to world(fix) , & position of turtle1 relative to world(fix). Then I calculate the relative position of turtle 1 & 2 by calculation the difference, and use it for the velocity calculation.

  while (node.ok()){
//tf::StampedTransform transform;
tf::StampedTransform transform1;//
tf::StampedTransform transform2;//
try{
listener.lookupTransform("/world","/turtle2",ros::Time(0),transform2);//get coordinate of turtle2 relative to world (fix)
listener.lookupTransform("/world","/turtle1",ros::Time(0),transform1);//get coordinate of turtle1 relative to world (fix)
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}

geometry_msgs::Twist vel_msg;

vel_msg.angular.z = 4.0 * atan2(transform1.getOrigin().y()-transform2.getOrigin().y(),
transform1.getOrigin().x()-transform2.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform1.getOrigin().x()-transform2.getOrigin().x(), 2) +
pow(transform1.getOrigin().y()-transform2.getOrigin().y(), 2));

turtle_vel.publish(vel_msg);

rate.sleep();
}


I tried and it didn't behave like it supposed to do (turtle2 follow turtle1). It moved rather in a strange motion. Why is that so? Please explain, what is wrong with my code, and how to fix?

PS: I understand the original code, and it works. I just want to know why my code doesn't work and maybe I misunderstand the concept.

What I understand is when I lookupTransform (turtle2, turtle1, ...) , I calculate: position of turtle 1 - position of turtle 2. So I think if I take the difference between lookuptransform(world, turtle2) and lookuptransform(world,turtle1), it will result to the same thing....

edit retag close merge delete

Sort by » oldest newest most voted

I think I've got it!!!

There's nothing wrong with getting the position of turtle2 and turtle1 separately.

Here is my final code (It reproduced the SAME behavior :)

geometry_msgs::Twist vel_msg;

float teta=atan2(transform1.getOrigin().y()-transform2.getOrigin().y(), transform1.getOrigin().x()-transform2.getOrigin().x()) -tf::getYaw(transform2.getRotation());

//to keep teta between -180 and 180
while (teta<-3.14159)
{teta+=2*3.14159;}
while (teta>3.14159)
{teta-=2*3.14159;}

vel_msg.angular.z = 4.0 * (teta);
vel_msg.linear.x = 0.5 * sqrt(pow(transform1.getOrigin().x()-transform2.getOrigin().x(), 2) +
pow(transform1.getOrigin().y()-transform2.getOrigin().y(), 2));

turtle_vel.publish(vel_msg);


IMPORTANT THINGS I need to be careful with when working in ground frame:

• atan (y1-y2/ x1-x2) only gives alfa. The angular velocity should be proportional to alfa-beta, not just alfa. That's why I need to reduce with beta: tf::getYaw(transform2.getRotation()) • I need to make sure that the teta is between -180 degree to 180 degree! Here's why:

Consider a case: e.g. alfa=30 degree .. beta=300 degree. In my code, the angular velocity will be proportional to 30-300=-270. So the turtle2 will rotate clockwise porportional to 270. HOWEVER It's more efficient if the angular velocity is just 90, just rotate counterclockwise

-----------------------------------------------------------a---------------------------------------------------------------------

PS: My previous answer I used

float teta=atan2(transform1.getOrigin().y()-transform2.getOrigin().y(),
transform1.getOrigin().x()-transform2.getOrigin().x()) - transform2.getRotation().z();


And It's WRONG, Cause transform2.getRotation().z() gives result in Quaternion, and I want angle in radian, thats why it behaved wrongly!

more Your code is calculating velocity commands in the world frame, whereas the tutorial is calculating velocity commands in the turtle2 frame. That's why you get different results.

more