1. #!/usr/bin/env python
2. # -*- coding: utf-8 -*-
3.  
4. import rospy
5. import tf
6. from nav_msgs.msg import Odometry
7. from geometry_msgs.msg import Twist
8. from turtlesim.msg  import Pose
9. from sensor_msgs.msg import LaserScan
10. from math import pi
11. from math import atan2
12.  
13. goal = (9.5, 1.5)
14. perpendicular_compare = 0
15. state = 0
16. previous_front = 0
17.  
18.  
19. # funkcja wywolywana przy przyjsciu danych ze skanera laserowego
20. move_x = 1
21. move_theta = 1
22. def choose_direction (directions):
23.         num_in_list = 0
24.         global angle
25.         global dir
26.         global front
27.         global back
28.         global turn_left_detector
29.         global turn_right_detector
30.         global perpendicular
31.         global head_min
32.         head_min = min(min(directions[0:10]), min(directions[350:359]))
33.         dir = min(directions)
34.         for direction in directions:
35.                 if (direction == dir):
36.                         print "INDEX", num_in_list
37.                         angle = num_in_list*2*pi/360
38.                         print "LOOP ANGLE", angle
39.                 else:
40.                         num_in_list += 1
41.         turn_left_detector = directions[45]
42.         #turn_right_detector = direstions[]
43.         front = directions[70]
44.         back = directions[110]
45.         perpendicular = directions[90]
46.         return (dir, angle, front, back, turn_left_detector)
47.  
48. def check_front():
49.         global previous_front
50.         if (previous_front == 0):
51.                 previous_front = front
52.                 return False
53.         else:
54.                 if(front > previous_front + 0.5):
55.                         return True
56.                 else:
57.                         previous_front = front
58.                         return False
59.        
60.  
61. def scan_callback(scan):
62.         choose_direction(scan.ranges)
63.         #print "ANGLE", angle
64.         #print "DIR", dir
65.         #print scan
66.        
67.                        
68. def count_dest_angle(c2y, c1y, c2x, c1x):
69.         global dest_angle
70.         dest_angle = atan2 (c2y-c1y, c2x-c1x)
71.         return dest_angle
72.  
73. angle_to_goal = float(atan2(0.5-0.5, 9.5-0.5))
74.  
75. def drive_to_finish (position_theta):
76.         global state
77.         if abs(position_theta - angle_to_goal) > 0.1 and dir > 0.4:
78.                 if position_theta <= angle_to_goal:
79.                         new_vel.angular.z = 0.3
80.                 elif position_theta > angle_to_goal:
81.                         new_vel.angular.z = -0.3
82.                 new_vel.linear.x = 0
83.                 state = 1
84.                 print ("Rotating to goal angle")
85.         else:
86.                 state = 1
87.                 #print ("Following goal angle")
88.                 print ("DISTOWALL", dir)
89.                 print ("FRONT", front)
90.                 print ("BACK", back)
91.                 if  dir >= 0.4:
92.                         new_vel.angular.z = 0
93.                         new_vel.linear.x = 0.3
94.                         print ("GO AHEAD")
95.                 elif dir < 0.4:
96.                         #jezeli najblizsza odleglosc od sciany jest pod katem 90 lub 270 stopni to jedz do przodu
97.                         # or (round(angle, 2) != (round((pi, 2)*1.5))))
98.                         #(round(angle, 2) != (round((pi*0.5, 2))))
99.                         if (round(front, 1) != round(back, 1) or perpendicular > 0.5):
100.                                 new_vel.angular.z = -0.3
101.                                 new_vel.linear.x = 0
102.                                 print ("ROTATE TO WALL FOLLOW")
103.                         else:
104.                                 new_vel.linear.x = 0
105.                                 state = 3
106.                                         #if (perpendicular > 0.4 or (round(front, 1) != round(back, 1))):
107.                                         #       new_vel.linear.x = 0.3
108.                                         #       new_vel.angular.z = 0.3
109.                                         #
110.                                         #       state = 8
111. def drive_next_wall():
112.         global state
113.         if check_front() == False:     
114.                 if (round(front, 2) == round(back, 2)):
115.                         new_vel.linear.x = 0.4
116.                         new_vel.angular.z = 0
117.                         state = 3
118.                         print ("LINE FOLLOW")
119.                 else:
120.                         if front >= back:
121.                                 new_vel.angular.z = 0.2
122.                                 new_vel.linear.x = 0.2
123.                                 state = 3
124.                                 print ("EQUALISE FROM FRONT TO BACK")
125.                         else:
126.                                 new_vel.angular.z = -0.2
127.                                 new_vel.linear.x = 0.2
128.                                 state = 3
129.                                 print ("EQUALISE FROM BACK TO FRONT")
130.                 return state
131.         elif check_front() == True:
132.                 new_vel.angular.z = 0
133.                 new_vel.linear.x = 0
134.                 state = 4
135.                 print ("LEFT TURN DETECTED")
136. def turn_left():
137.         global state
138.         #if (head_min < 1):
139.         if (perpendicular > 0.4 or (round(front, 1) == round(back, 1))):
140.                 new_vel.linear.x = 0.04
141.                 new_vel.angular.z = 0.04
142.                 print ("TURN LEFT")
143.         else:
144.                 state = 3
145.                 print ("NEXT TO WALL AGAIN")
146.  
147. # funkcja wywolywana przy przyjsciu danych o lokalizacji robota
148. def odom_callback(odom):
149.         global new_vel
150.         global distance_to_goal
151.         #global perpendicular_compare
152.         pose = Pose()
153.         pose.x = odom.pose.pose.position.x
154.         pose.y = odom.pose.pose.position.y
155.         pose.theta = tf.transformations.euler_from_quaternion([odom.pose.pose.orientation.x,odom.pose.pose.orientation.y,odom.pose.pose.orientation.z,odom.pose.pose.orientation.w])[2]
156.         #print "Pozycja x: ",odom.pose.pose.position.x
157.         #print "Pozycja y: ",odom.pose.pose.position.y
158.         print "Pozycja theta: ",pose.theta
159.         print "Kat do celu:", angle_to_goal
160.         curr_angle_to_goal = count_dest_angle(9.5, pose.y, 1.5, pose.x)
161.         #print "ANGLE ABS", abs(angle_to_goal - curr_angle_to_goal)
162.         #print "ANGLE", angle
163.         #print "FRONT", front
164.  
165.         if state == 0 or state == 1:
166.                 drive_to_finish(pose.theta)
167.         elif state == 3:
168.                 drive_next_wall()
169.         elif state == 4:
170.                 turn_left()
171.  
172.  
173. if __name__== "__main__":
174.         global new_vel
175.         new_vel = Twist()
176.         rospy.init_node('wr_zad', anonymous=True)
177.         print("ready")
178.         rospy.Subscriber( '/odom' , Odometry, odom_callback)
179.         pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
180.         rospy.Subscriber( '/scan' , LaserScan, scan_callback)
181.        
182.         rate=rospy.Rate(10) # 10Hz
183.         while not rospy.is_shutdown():
184.                 pub.publish(new_vel)#wyslanie predkosci zadanej
185.                 rate.sleep()
186.  
187.         print("END")
188.