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);

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)

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?

edit retag close merge delete

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 try turtle_tf2_demo_cpp.launch to see if it's working fine ?

( 2021-07-06 05:35:15 -0500 )edit

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.

( 2021-07-06 06:07:54 -0500 )edit

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?

( 2021-07-07 00:22:11 -0500 )edit

Sort by » oldest newest most voted

OK I found where my problem had stemmed from. The code here is OK as others kindly pointed out in the comments. what was wrong, was the broadcaster that I had written for sending the trutle1/turtle2 transformations.

basically I was instantiating a new broadcaster in my callback and this was the culprit. I had to either make it static or use one instance for this to work(why is that an issue? I still don't know).

So this is my broadcaster I provided some explanation concerning the issue in the comments of callback_fn:

 #include <iostream>
#include <string>
#include <functional>

#include <ros/ros.h>
#include <geometry_msgs/TransformStamped.h>
#include <turtlesim/Pose.h>
#include <tf/LinearMath/Quaternion.h>

int main(int argc, char** argv)
{
ros::NodeHandle n;
ros::NodeHandle tmp_node("~");
std::string turtle_name = "";

if (!tmp_node.hasParam("turtle"))
{
if (argc != 2)
{
ROS_ERROR("Number of arguments do not match! must provide a turtle's name as the argument!");
return -1;
}
else
{
// Get the second argument which is the turtlename (the first argument is the applications exe path)
turtle_name = std::string(argv[1]);
std::cout<<"turtle_name is: "<<turtle_name<<std::endl;
}
}
else
{
// if we already have a turtlename in our parameter server, fetch it!
tmp_node.getParam("turtle", turtle_name);
std::cout<<"turtle_name: "<<turtle_name<<std::endl;
}

// see the comment below, either uncomment this or
// the static one below. see the explanation for more details
auto callback_fn = [&](turtlesim::PoseConstPtr pose)
{
// short story: it doesn't have to be defined as static or be shared!
// what matters is that they shouldn't be created/destroyed very fast
// that is every few milliseconds e.g.
// or as jvdhoorn says :
//  "What is important is to make sure it has a sane lifecycle
// (ie: it does not get created and destroyed every few milliseconds)."
// otherwise, you will end-up with all sorts of nonsensical weird happenings
// on this, the trutle2 was moving erratically, in circles, it would get close to
// turtle1, but not always and not as fast, it would take a lot of time and huge
// circles! remove static and see it yourself.
// (run tf2_transform_carrot_demo.launch and see the result)
geometry_msgs::TransformStamped transform;
tf::Quaternion qtr;

// since we want all the nodes to be linked together, here we are specifying
// one parent called "world" for this node. any other nodes that have this as
// parent will be able to communicate with this node. and because our turtles
// need to be linked in order to  communicate through tf2 this is necessary
//this means we are naming our frame after turtlename
// and our parent frame is world, this allows us to
// be with other frames that already exist and
// world is their parent (i.e. this way we can have multiple turtles
// roaming in the same world ...
more