/
wakeup.py
90 lines (73 loc) · 2.74 KB
/
wakeup.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
#!/usr/bin/env python
import rospy
import serial
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from transform_utils import quat_to_angle, normalize_angle
from math import radians
#from std_msgs.msg import Float32
class Test():
def __init__(self):
rospy.init_node('Test', anonymous=True)
#rospy.on_shutdown(self.shutdown)
self.cmd_vel = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=10)
rate = 20
r = rospy.Rate(rate)
angular_speed = radians(90)
angular_tolerance = radians(1.0)
#goal_angle = radians(270)
self.tf_listener = tf.TransformListener()
rospy.sleep(2)
self.odom_frame = '/odom'
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
position = Point()
move_cmd = Twist()
move_cmd.linear.x = 0
#move_cmd.angular.z = angular_speed
ser = serial.Serial( port = "/dev/ttyUSB0",baudrate = 115200,bytesize = serial.EIGHTBITS, parity = serial.PARITY_NONE,stopbits = serial.STOPBITS_ONE);
#while 1:
while not rospy.is_shutdown():
SERIAL = ser.readline()
if SERIAL.find("angle") != -1:
ANGLE = SERIAL
goal_angle = float(filter(str.isdigit,ANGLE.encode('gbk')))
print goal_angle
if goal_angle <= 180:
move_cmd.angular.z = -angular_speed
else:
move_cmd.angular.z = angular_speed
goal_angle = 360 - goal_angle
(position, rotation) = self.get_odom()
last_angle = rotation
turn_angle = 0
while abs(turn_angle + angular_tolerance) < abs(radians(goal_angle)) and not rospy.is_shutdown():
#while abs(turn_angle + angular_tolerance) < abs(radians(goal_angle)):
self.cmd_vel.publish(move_cmd)
r.sleep()
(position, rotation) = self.get_odom()
delta_angle = normalize_angle(rotation - last_angle)
turn_angle += delta_angle
last_angle = rotation
self.cmd_vel.publish(Twist())
def get_odom(self):
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return (Point(*trans),quat_to_angle(Quaternion(*rot)))
def shutdown(self):
rospy.loginfo("STOP")
self.cmd_vel.publish(Twist())
rospy.sleep(1)
if __name__ == "__main__":
Test()