-
Notifications
You must be signed in to change notification settings - Fork 0
/
Turtlebot.py
113 lines (101 loc) · 4.11 KB
/
Turtlebot.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
import rospy
from math import radians, degrees, sqrt
from std_msgs.msg import String
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from tf.transformations import euler_from_quaternion
from sensor_msgs.msg import LaserScan
class Turtlebot():
def __init__(self, namespace=""):
self.namespace = namespace
self.x = 0
self.y = 0
self.angle = 0
self.LIDAR_ERR = 0.05
self.rate = rospy.Rate(10)
# Register publishers and subscribers
self.publisher = rospy.Publisher(self.namespace + '/cmd_vel', Twist, queue_size=1)
self.sub_odom = rospy.Subscriber(self.namespace + '/odom', Odometry, self.odomCallback)
rospy.on_shutdown(self.shutdown)
# Get the position of the robot from the onboard sensors
def odomCallback(self, odom):
roll, pitch, yaw = euler_from_quaternion([odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w])
self.angle = (round(degrees(yaw)) + 180) % 360
self.x = round(odom.pose.pose.position.x, 3)
self.y = round(odom.pose.pose.position.y, 3)
# Move the bot forward at speed desired
def moveForward(self, speed):
move_cmd = Twist()
move_cmd.linear.x = speed
self.publisher.publish(move_cmd)
# Turn to the left of a specified angle (degrees)
def turnLeft(self, angle):
move_cmd = Twist()
move_cmd.angular.z = radians(angle / 4)
start_angle = self.angle
end_angle = (start_angle + angle) % 360
if self.angle < end_angle:
while end_angle > self.angle and not rospy.is_shutdown():
self.publisher.publish(move_cmd)
self.rate.sleep()
elif self.angle > end_angle:
while self.angle > 0 and self.angle > end_angle and not rospy.is_shutdown():
self.publisher.publish(move_cmd)
self.rate.sleep()
while self.angle < end_angle and not rospy.is_shutdown():
self.publisher.publish(move_cmd)
self.rate.sleep()
move_cmd = Twist()
self.publisher.publish(move_cmd)
# Turn to the right of a specified angle (degrees)
def turnRight(self, angle):
move_cmd = Twist()
move_cmd.angular.z = -radians(angle / 4)
start_angle = self.angle
end_angle = ((start_angle - angle) + 360) % 360
if self.angle > end_angle:
while end_angle < self.angle and not rospy.is_shutdown():
self.publisher.publish(move_cmd)
self.rate.sleep()
elif self.angle < end_angle:
while self.angle > 0 and self.angle < end_angle and not rospy.is_shutdown():
self.publisher.publish(move_cmd)
self.rate.sleep()
while self.angle > end_angle and not rospy.is_shutdown():
self.publisher.publish(move_cmd)
self.rate.sleep()
move_cmd = Twist()
self.publisher.publish(move_cmd)
# Detect a hole in the maze to the right
def detectHoleRight(self):
msg = rospy.wait_for_message(self.namespace + "/scan", LaserScan)
self.scan_filter = []
for i in range(360):
if i >= 255 and i < 260:
if msg.ranges[i] >= self.LIDAR_ERR:
self.scan_filter.append(msg.ranges[i])
if len(self.scan_filter) == 0:
return False
else:
return max(self.scan_filter) >= 0.6
# Detect a hole in the maze to the left
def detectHoleLeft(self):
msg = rospy.wait_for_message(self.namespace + "/scan", LaserScan)
self.scan_filter = []
for i in range(360):
if i >= 100 and i < 105:
if msg.ranges[i] >= self.LIDAR_ERR:
self.scan_filter.append(msg.ranges[i])
if len(self.scan_filter) == 0:
return False
else:
return max(self.scan_filter) >= 0.6
# Stop the bot and shutdown the program
def shutdown(self):
self.stop()
rospy.loginfo("Stopping Robot")
exit(0)
# Stop the bot
def stop(self):
move_cmd = Twist()
self.publisher.publish(move_cmd)