How to add three turtle and follow the main turtle

asked 2021-06-26 10:46:03 -0600

Abdel_Meguid gravatar image

updated 2021-06-26 11:28:35 -0600

gvdhoorn gravatar image

In below three scripts there are the codes of project (turtle follow another turtle) all is run by launch file ...please I have two questions 1- I want to initiate another three turtle, not one only 2- i want turtle not follow the main turtle exactly, but keep a distance in both x and y directional (not completely identical) and move with it with keep a certain distance

launch file:

 <launch>
    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle1" />
    </node>
    <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle2" />
    </node>
    <node pkg="learning_tf" type="turtle_tf_listener.py" name="listener" />
  </launch>

Python script:

#!/usr/bin/env python3
import roslib 
import roslib
roslib.load_manifest('learning_tf')
import rospy
import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
       br = tf.TransformBroadcaster()
       br.sendTransform((msg.x, msg.y, 0),
                        tf.transformations.quaternion_from_euler(0, 0, msg.theta),rospy.Time.now(),turtlename,"world") 
if __name__ == '__main__':
       rospy.init_node('turtle_tf_broadcaster')
       turtlename = rospy.get_param('~turtle')
       rospy.Subscriber('/%s/pose' % turtlename,
                        turtlesim.msg.Pose,
                        handle_turtle_pose,
                        turtlename)
       rospy.spin()

Other python script:

#!/usr/bin/env python3
import roslib
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)
    spawner(4, 2, 0, 'turtle2')
    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
           try:
               (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', 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()
edit retag flag offensive close merge delete