Untitled

<launch>
	<node pkg="turtlesim" name="turtlesim" type="turtlesim_node"/>
	<node pkg="rosservice" type="rosservice" name="deleteturtle1" args="call --wait /kill turtle1"/> 	

	<node pkg="rosservice" type="rosservice" name="createturtle2" args="call --wait /spawn 2 5 1.570796327 turtle2"/> 	
	<node pkg="rosservice" type="rosservice" name="createturtle3" args="call --wait /spawn 8 5 4.712388980 turtle3"/> 
	<node pkg="rosservice" type="rosservice" name="createturtle4" args="call --wait /spawn 5 2 3.141592654 turtle4"/> 	
	<node pkg="rosservice" type="rosservice" name="createturtle5" args="call --wait /spawn 5 8 0.0 turtle5"/> 

	
	<node pkg="beginner_tutorials" name="rotatetask2" type="rotate.py" args="2"/>
	<node pkg="beginner_tutorials" name="rotatetask3" type="rotate.py" args="3"/>
	<node pkg="beginner_tutorials" name="rotatetask4" type="rotate.py" args="4"/>
	<node pkg="beginner_tutorials" name="rotatetask5" type="rotate.py" args="5"/>
</launch>
#! /usr/bin/env python3



import rospy
import sys
import random
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from std_msgs.msg import Int64
from math import pow, atan2, sqrt, pi, radians
PI = pi

nodeid=str(sys.argv[1])
nodename='turtle'+nodeid


class Turtlebot: 
	
	def recieve_event(self, msg):
		self.event = msg.data
		
	def update_event_pose(self, data):
		self.eventpose = data
	
	def _init_(self):
		rospy.init_node('turtletogoal', anonymous=True)
		self.pub = rospy.Publisher(nodename+'/cmd_vel', Twist, queue_size=10)
		self.sub = rospy.Subscriber(nodename+'/pose', Pose, self.update_pose)
		self.pose = Pose()
		self.rate = rospy.Rate(50)
		self.event = 0
		self.eventpose = Pose()
		self.eventpose.x = 5
		self.eventpose.y = 5
		
		#publish event and location, turtle2 is turtle1 because turtle1 gets killed at start
		if nodename == "turtle2": 
			self.eventPub = rospy.Publisher("/eventPub", Int64, queue_size = 10)
			self.eventLocation = rospy.Publisher("/eventLocation", Pose, queue_size = 10)
		#Subscribe to event and event location

		self.subEvent = rospy.Subscriber('/eventPub', Int64, self.recieve_event)
		self.subEventLocation = rospy.Subscriber('/eventLocation', Pose, self.update_event_pose)
		
	#######################################################################################

	
	def moveSquare(self,times = 5):
		for i in range (0,times):
			if nodename == "turtle2":
				self.event = random.randint(1, 2)
			for i in range (0,4):
				self.movetask2(8)
				rospy.sleep(0.2)
				self.rotate()
				rospy.sleep(0.2)
			if nodename == "turtle2" and self.event == 1:
				while not rospy.is_shutdown():
					self.eventPub.publish(self.event)
					self.eventLocation.publish(self.pose)
				timeout = rospy.Time.now() + rospy.Duration(5.0)
				while rospy.Time.now() < timeout:
					if self.event == 1:
						break
					self.rate.sleep()
			if self.event == 1:
				break
		self.move2goal()



	def normalize_angle(self, angle):
		while angle > pi:
			angle -= 2 * pi
		while angle < -pi:
			angle += 2 * pi
		return angle
		
		
	def update_pose(self, data):
		self.pose = data
		self.pose.x = round(self.pose.x, 4)
		self.pose.y = round(self.pose.y, 4)

	def euclidean_distance(self, goal_pose):
		return sqrt(pow((goal_pose.x - self.pose.x), 2)+pow((goal_pose.y - self.pose.y), 2))

	def linear_vel(self, goal_pose, constant=1.5):
		return constant * self.euclidean_distance(goal_pose)
		

	def steering_angle(self, goal_pose):
		return atan2(goal_pose.y-self.pose.y,goal_pose.x-self.pose.x)

	def angular_vel(self, goal_pose, constant=6):
		return constant * self.normalize_angle((self.steering_angle(goal_pose) - self.pose.theta))
		
		
				
	#######################################################################################

	def move2goal(self):
		
			
		goal_pose = self.eventpose		
			
		tolerance = 0.1

		vel_msg = Twist()
		while self.euclidean_distance(goal_pose) >= tolerance:
			vel_msg.linear.x = self.linear_vel(goal_pose)
			vel_msg.angular.z = self.angular_vel(goal_pose)
			self.pub.publish(vel_msg)
			self.rate.sleep()
			
			
		vel_msg.linear.x = 0
		vel_msg.angular.z = 0
		self.pub.publish(vel_msg)
	

		
	#######################################################################################	
	
	def rotate(self):
		vel_msg = Twist()
		angular_speed = 45*PI/180
		relative_angle = 90*PI/180
		t0 = rospy.Time.now().to_sec()
		current_angle = 0
		vel_msg.angular.z = -abs(angular_speed)
		while(current_angle < relative_angle):
			self.pub.publish(vel_msg)
			t1 = rospy.Time.now().to_sec()
			current_angle = angular_speed*(t1-t0)
			#self.rate.sleep() ################SİLLLLL
		    
		vel_msg.angular.z = 0
		self.pub.publish(vel_msg)
	
	
	
	#######################################################################################
		
	def movetask2(self,distance):
		vel_msg = Twist()
		vel_msg.linear.x = 5.0 
		vel_msg.angular.z = 0.0
		while self.pose.x == 0.0 and self.pose.y == 0.0 and not rospy.is_shutdown():
			rospy.sleep(0.01)
			self.rate.sleep()
		start_x = self.pose.x
		start_y = self.pose.y

		distance_moved = 0.0
		while not rospy.is_shutdown():
			self.pub.publish(vel_msg)
			distance_moved = sqrt(pow((self.pose.x - start_x), 2) + pow((self.pose.y - start_y), 2))
			if distance_moved >= distance:
				break
			self.rate.sleep()
		vel_msg.linear.x = 0.0
		self.pub.publish(vel_msg)

	#######################################################################################
    #####NOT USED RIGHT NOW
	def movetask(self,distance):
		vel_msg = Twist()
		speed = 0.8
		vel_msg.linear.x = speed
						
		currDist = 0
		t0 = rospy.Time.now().to_sec()
					
		while (currDist < distance):
			self.pub.publish(vel_msg)
			t1 = rospy.Time.now().to_sec()
			currDist = speed * (t1-t0)
			self.rate.sleep()
		#stop robot 
		vel_msg.linear.x = 0
		vel_msg.angular.z = 0
		self.pub.publish(vel_msg)
	
	#######################################################################################
		
		
if _name_ == "_main_":
	try:
		rospy.sleep(5)
		
		x = Turtlebot()
		#event = 2
			#x.movetask2(8)
			#rospy.sleep(0.2)
			#x.rotate()
			#rospy.sleep(0.2)
			
			
		x.moveSquare()
			

	except rospy.ROSInterruptException:
		pass