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....