Revision history [back]

I think I understand now.

There's nothing wrong with getting the position of turtle2 and turtle1 separately. But then I should be careful, cause now the angular velocity should be:

vel_msg.angular.z = 4.0 * (atan2(transform1.getOrigin().y()-transform2.getOrigin().y(),
                                transform1.getOrigin().x()-transform2.getOrigin().x()) - transform2.getRotation().z());

because it should depend on alfa-beta, not just alfa...

After I edited the angular velocity, It does the work (turtle2 follows turtle1), BUT sometimes it goes in a rather inefficient way, like moving in helix to reach turtle1. Shortly: It doesn't reproduce the behaviour of original code.

And I think the reason for that is because: Consider a case:

e.g. alfa=300 beta=30. In my code, the angular velocity will be proportional to 300-30=270. So the turtle2 will rotate countercw. HOWEVER It's more efficient to just rotate clockwise instead of counterclockwise (30-300=-270) for this case. That is why It does not reproduce the original code (that is more efficient)

So I need to also consider cases in my code too.

If I use lookuptransform (turtle2, turtle1, ...), I don't need to care about all those stuffs above , because it indirectly already does so.

• the alfa minus beta thing.
• and the case too

I think I understand now.

There's nothing wrong with getting the position of turtle2 and turtle1 separately. But then I should be careful, cause now the angular velocity should be:

vel_msg.angular.z = 4.0 * (atan2(transform1.getOrigin().y()-transform2.getOrigin().y(),
                                transform1.getOrigin().x()-transform2.getOrigin().x()) - transform2.getRotation().z());

because it should depend on alfa-beta, not just alfa...

After I edited the angular velocity, It does the work (turtle2 follows turtle1), BUT sometimes it goes in a rather inefficient way, like moving in helix to reach turtle1. Shortly: It doesn't reproduce the behaviour of original code.

And I think the reason for that is because: Consider a case:

e.g. alfa=300 beta=30. alfa=30 degree .. beta=300 degree. In my code, the angular velocity will be proportional to 300-30=270. 30-300=-270. So the turtle2 will rotate countercw. clockwise porportional to 270. HOWEVER It's more efficient to if the angular velocity is just 90, just rotate clockwise instead of counterclockwise (30-300=-270) for this case. counterclockwise That is why It does not reproduce the original code (that is more efficient)

So I need to also consider cases in my code too.

If I use lookuptransform (turtle2, turtle1, ...), I don't need to care about all those stuffs above , because it indirectly already does so.

• the alfa minus beta thing.
• and the case too

I think I understand now.

There's nothing wrong with getting the position of turtle2 and turtle1 separately. But then I should be careful, cause now the angular velocity should be:

vel_msg.angular.z = 4.0 * (atan2(transform1.getOrigin().y()-transform2.getOrigin().y(),
                                transform1.getOrigin().x()-transform2.getOrigin().x()) - transform2.getRotation().z());

because it should depend on alfa-beta, not just alfa...

After I edited the angular velocity, It does the work (turtle2 follows turtle1), BUT sometimes it goes in a rather inefficient way, like moving in helix to reach turtle1. Shortly: It doesn't reproduce the behaviour of original code.

And I think the reason for that is because: 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 That is why It does not reproduce the original code (that is more efficient)

So I need to also consider cases edited again become:

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;
float teta=atan2(transform1.getOrigin().y()-transform2.getOrigin().y(),

                                transform1.getOrigin().x()-transform2.getOrigin().x()) - transform2.getRotation().z();
//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);

rate.sleep();
  }

BUT I still haven't got the same efficient result such in my code too.

the original code. :((( It follows turtle1 but rather inefficient (suck as moves in helix, rotates a lot of times) -----------------------------------------------------------a--------------------------------------------------------------------- If I use lookuptransform (turtle2, turtle1, ...), I don't need to care about all those stuffs above , because it indirectly already does so.

• the alfa minus beta thing.
• and the case too

I think I understand now.

There's nothing wrong with getting the position of turtle2 and turtle1 separately. But then I should be careful, cause now the angular velocity should be:

vel_msg.angular.z = 4.0 * (atan2(transform1.getOrigin().y()-transform2.getOrigin().y(),
                                transform1.getOrigin().x()-transform2.getOrigin().x()) - transform2.getRotation().z());

because it should depend on alfa-beta, not just alfa...

After I edited the angular velocity, It does the work (turtle2 follows turtle1), BUT sometimes it goes in a rather inefficient way, like moving in helix to reach turtle1. Shortly: It doesn't reproduce the behaviour of original code.

And I think the reason for that is because: 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 That is why It does not reproduce the original code (that is more efficient)

So I edited again become:

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;
float teta=atan2(transform1.getOrigin().y()-transform2.getOrigin().y(),

                                transform1.getOrigin().x()-transform2.getOrigin().x()) - transform2.getRotation().z();
//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);

rate.sleep();
  }

BUT I still haven't got the same efficient result such in the original code. :((( It follows turtle1 but rather inefficient (suck as moves in helix, rotates a lot of times) -----------------------------------------------------------a--------------------------------------------------------------------- times)

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

PS:I know that If I use lookuptransform (turtle2, turtle1, ...), I don't need to care about all those stuffs above , because it indirectly already does so.

• the alfa minus beta thing.
• and the case too

I think I understand now.I've got it!!!

There's nothing wrong with getting the position of turtle2 and turtle1 separately. But then I should be careful, cause now the angular velocity should be:separately.

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

vel_msg.angular.z = 4.0 * (atan2(transform1.getOrigin().y()-transform2.getOrigin().y(),
                                transform1.getOrigin().x()-transform2.getOrigin().x()) - transform2.getRotation().z());

because it should depend on alfa-beta, not just alfa...

After I edited the angular velocity, It does the work (turtle2 follows turtle1), BUT sometimes it goes in a rather inefficient way, like moving in helix to reach turtle1. Shortly: It doesn't reproduce the behaviour of original code.

And I think the reason for that is because: 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 That is why It does not reproduce the original code (that is more efficient)

So I edited again become:

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;
 float teta=atan2(transform1.getOrigin().y()-transform2.getOrigin().y(),

                                teta=atan2(transform1.getOrigin().y()-transform2.getOrigin().y(), transform1.getOrigin().x()-transform2.getOrigin().x()) - transform2.getRotation().z();
-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);

rate.sleep();
  }

BUT I still haven't got the same 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 such in the original code. :((( It follows turtle1 but rather inefficient (suck as moves Quaternion, and I want angle in helix, rotates a lot of times)

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

PS:I know that If I use lookuptransform (turtle2, turtle1, ...), I don't need to care about all those stuffs above , because radian, thats why it indirectly already does so.

• the alfa minus beta thing.
• and the case too