Ask Your Question
0

tf2 tutorial: follower turtle makes circles, doesn't follow leader

asked 2021-07-06 04:12:20 -0500

Rika gravatar image

updated 2021-07-07 04:24:45 -0500

gvdhoorn gravatar image

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 :

image description

and ultimately it ends up spinning next to the turtle1 like this:

image description

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: image description

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 flag offensive close merge delete

Comments

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 ?

Delb gravatar image Delb  ( 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.

Ranjit Kathiriya gravatar image Ranjit Kathiriya  ( 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?

Rika gravatar image Rika  ( 2021-07-07 00:22:11 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-07-07 00:17:51 -0500

Rika gravatar image

updated 2021-07-07 04:19:53 -0500

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 <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <turtlesim/Pose.h>
#include <tf/LinearMath/Quaternion.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "tf2_broadcaster_node");
    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
    //tf2_ros::TransformBroadcaster broadcaster;
    auto callback_fn = [&](turtlesim::PoseConstPtr pose)
    {
        // read https://answers.ros.org/question/381937/
        // 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
        // previously I forgot about this and when I ran the listener app which relies 
        // 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)
        static tf2_ros::TransformBroadcaster broadcaster;
        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
        transform.header.frame_id = "world";
        transform.header.stamp = ros::Time::now();
        //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)
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-07-06 04:12:20 -0500

Seen: 171 times

Last updated: Jul 07 '21