Cannot get turtle2 to move Ex2.3

Course Support TF ROS 101
1

I am working with the code provided as an example, no changes at all.

#!/usr/bin/env python
import sys
import rospy
import math
import tf
import geometry_msgs.msg

if __name__ == '__main__':
    rospy.init_node('tf_listener_turtle')

    listener = tf.TransformListener()

    if len(sys.argv) < 3:
        print("usage: turtle_tf_listener.py follower_model_name model_to_be_followed_name")
    else:
        follower_model_name = sys.argv[1]
        model_to_be_followed_name = sys.argv[2]
        
        turtle_vel = rospy.Publisher(follower_model_name+'/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
    
        rate = rospy.Rate(10.0)
        ctrl_c = False
        
        follower_model_frame = "/"+follower_model_name
        model_to_be_followed_frame = "/"+model_to_be_followed_name
        
        def shutdownhook():
            # works better than the rospy.is_shut_down()
            global ctrl_c
            print "shutdown time! Stop the robot"
            cmd = geometry_msgs.msg.Twist()
            cmd.linear.x = 0.0
            cmd.angular.z = 0.0
            turtle_vel.publish(cmd)
            ctrl_c = True

        rospy.on_shutdown(shutdownhook)
        
        while not ctrl_c:
            try:
                (trans,rot) = listener.lookupTransform(follower_model_frame, model_to_be_followed_frame, rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
    
            angular = 4 * math.atan2(trans[1], trans[0])
            linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
            cmd = geometry_msgs.msg.Twist()
            cmd.linear.x = linear
            cmd.angular.z = angular
            turtle_vel.publish(cmd)
    
            rate.sleep()

All the topics needed are being published (turtle 1 and 2/cmd_vel).

I found out that the problem I am having is that my code gets caught in the except part in the loop, so I never get any data to work with. Now, I am not exactly sure why this is happening, or what am I missing.

Thank you in advance.

2

Hi,

So the issue must have been that you werent launching the TF publisher. Remember that this code uses the TF published with the previous python script to then calcualte the cmd_vel t move the IRobot to the RealTurtle.

These should be the commands that you should launch and the code to make this work:

python multiple_broadcaster_ex2_1.py

Now launch the moving based on the TF published with the previous launch

python model1_to_model2_listener.py turtle2 turtle1

It should move the irobot to the realturtle

multiple_broadcaster_ex2_1.py

model1_to_model2_listener.py, I made a minor modification to detect when there is no transform between the two models, but it not necessary for this to work.