tf2 tutorial: follower turtle makes circles, doesn't follow leader
Hello all. I was following the TF2 tutorial and reached the listener part. I have implemented the code accordingly but I noticed my outcome is very different than the one shipped with :
sudo apt-get install ros-$ROS_DISTRO-turtle-tf2 ros-$ROS_DISTRO-tf2-tools ros-$ROS_DISTRO-tf
Before I continue more, here is what I have written so far:
#include <iostream>
#include <string>
#include <functional>
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TransformStamped.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "tf2_demo_listener_node");
ros::NodeHandle n;
ros::service::waitForService("spawn");
ros::ServiceClient spawner = n.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn turtle;
turtle.request.x = 2;
turtle.request.y = 2;
turtle.request.theta = 0;
turtle.request.name = "turtle2";
spawner.call(turtle);
ros::Publisher publisher = n.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate rate(10.0);
while (n.ok())
{
geometry_msgs::TransformStamped transform_stamped;
try
{
transform_stamped = buffer.lookupTransform("turtle2", "turtle1",
ros::Time(0),
ros::Duration(0.1));
}
catch (const std::exception &e)
{
ROS_WARN(e.what());
ros::Duration(0.2).sleep();
continue;
}
geometry_msgs::Twist vel_msg;
//Note
// here we are converting the cartesian coordinate (x,y) to polar coordinates(r,theta):
// for theta we have arctan(y,x) which would be atan2(y,x) (its in radian) and
// for r we have sqrt(x^2 + y^2)
auto theta_radian = atan2(transform_stamped.transform.translation.y,
transform_stamped.transform.translation.x);
auto r = sqrt(pow(transform_stamped.transform.translation.x, 2) +
pow(transform_stamped.transform.translation.y, 2));
vel_msg.angular.z = 4.0 * theta_radian; //4.0 1.0
vel_msg.linear.x = 0.5 * r;
publisher.publish(vel_msg);
rate.sleep();
}
return 0;
}
Now if I run this this is the pattern I get :
and ultimately it ends up spinning next to the turtle1 like this:
Now if I run the roslaunch turtle_tf2 turtle_tf2_demo.launch
it performs neatly and nicely follows the turtle1 everywhere until it reaches to it:
now my question is, what is it that I'm doing wrong that does not yield the very same result? seemingly the codes are the same(although implemented in different languages (c++ vs python), however the effect is very different.
What am I missing here?
I tried your code and it's running as the tutorial so your issue is probably somewhere else. Are you running everything as in the tutorial ? Have you changed anything else ?
Also, you tried
turtle_tf2_demo.launch
but it's using the python script, can you tryturtle_tf2_demo_cpp.launch
to see if it's working fine ?I tried running your code, it is working fine for me also.
https://github.com/ros/geometry_tutor...
Have a look at this package, I think you will find the solution to your problem. I think your code is perfect.
Thank you both very much. yes you were right the problem was somewhere else. I posted an answer below. but I'd also appreciate if someone could shed some light on why broadcaster needs to be only one?