Search code examples
pythongeometryrosrobotics

Trying to make a turtlebot circle another moving turtlebot in ROS

I am trying to make one turtlebot named turtle2 to circle around a stationary turtlebot and then have another turtlebot turtle3 circle around turtle 2. Turtle2 circles around turtle1 with a radius of 2 and an angular velocity of 1 and turtle3 should circle around turtle2 with a radius of 0.5 and an angular velocity of 2. However I cannot figure out how to make turtle3 circle around turtle2 while also closing the distance caused by turtle2's movement. Here's my ROS code and a screenshot of what I get with my results to help you understand my goal. Any help would be appreciated.

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

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

    listener = tf.TransformListener()

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)

    # wait for the world to spawn
    rospy.sleep(0.3)

    (trans, rot) = listener.lookupTransform('/world', '/turtle1', rospy.Time(0))
    r2 = 2
    w2 = 1
    # spawn second turtle along the orbit of the first still turtle
    spawner(trans[0], trans[1] - r2, 0, 'turtle2')
    turtle_vel2 = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)

    # wait for second turtle to spawn
    rospy.sleep(0.3)
    (trans, rot) = listener.lookupTransform('/world', '/turtle2', rospy.Time(0))
    r3 = 0.5
    w3 = 2
    # spawn third turtle along the orbit of the second turtle
    spawner(trans[0], trans[1] - r3, 0, 'turtle3')
    turtle_vel3 = rospy.Publisher('turtle3/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        flag2 = True
        flag3 = True
        try:
            (trans, rot) = listener.lookupTransform('/world', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            flag2 = False

        if(flag2):
            angular2 = w2
            linear2 = w2 * r2
            cmd2 = geometry_msgs.msg.Twist()
            cmd2.linear.x = linear2
            cmd2.angular.z = angular2
            turtle_vel2.publish(cmd2)
        
        try:
            (trans, rot) = listener.lookupTransform('/turtle3', '/turtle2', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            flag3 = False

        if(flag3):
            angular3 = w3
            linear3 = w3 * r3
            cmd3 = geometry_msgs.msg.Twist()
            cmd3.linear.x = linear3
            cmd3.angular.z = angular3
            turtle_vel3.publish(cmd3)

        rate.sleep()

enter image description here

Solution

  • That's how I would approach it:

    1. Get turtle2 position let's call it Pos2 which is vector (x2, y2)
    2. Make a function which generates point which is circling around (0, 0) so it's something like SpinnerPos = (cx, cy) = DistanceToCenter * (cos(angle), sin(angle)) where angle is incremented every frame so the point (aka vector) would spin. enter image description here
    3. Combine both: Target = Pos2 + SpinnerPos. Now if drawn Target point would perfectly spin around turtle2
    4. Now make turtle3 follow Target point by calculating distance to it (Pythagorean theorem) and adjusting it's orientation (trigonometry atan2()) and speed proportional to distance
    5. After tuning parameters turtle3 should circle turtle2