-
Notifications
You must be signed in to change notification settings - Fork 17
/
autodrive.py
121 lines (97 loc) · 3.36 KB
/
autodrive.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
114
115
116
117
118
119
120
121
#! /usr/bin/env python
# DRIFT Implement
###### FUNCTIONS #################
## 1. listen odom of SLAM ########
## 2. transform it to grid unit ##
##################################
import rospy
from matplotlib import pyplot as plt
from geometry_msgs.msg import PoseWithCovarianceStamped
from pathplanning import Astar
import numpy as np
from time import sleep
from tf.transformations import euler_from_quaternion
import math
import threading
from easygo import dap
DRIFT_handling_authority = True
flagLocal=False
verbose = 0
class RobotPose:
def __init__(self):
self.x = 0.0
self.y = 0.0
self.rot = 0.0
def setPose(self, x, y, rot):
self.x = x
self.y = y
self.rot = rot
return (x, y, rot)
def convert2grid(self, scale=0.2):
x, y = round(self.x/scale), - round(self.y/scale) # give a minus to 'y', because y-axis towars left side of robot
return (x, y)
def rotate_origin_only(xy, radians):
"""Only rotate a point around the origin (0, 0)."""
x, y = xy
xx = x * math.cos(radians) + y * math.sin(radians)
yy = -x * math.sin(radians) + y * math.cos(radians)
return float(xx), float(yy)
def callback(data):
global pose
_position = data.pose.pose.position
pose.x = _position.x
pose.y = _position.y
quaternion = (
data.pose.pose.orientation.x,
data.pose.pose.orientation.y,
data.pose.pose.orientation.z,
data.pose.pose.orientation.w)
(_roll, _pitch, pose.rot) = euler_from_quaternion(quaternion) # radians
if verbose:
plt.scatter(pose.x, pose.y)
plt.hold(True)
plt.pause(0.05)
def listener():
#rospy.init_node('listener', anonymous=True)
rospy.Subscriber("/rtabmap/localization_pose", PoseWithCovarianceStamped, callback)
sleep(2)
def autodrive():
Astar.pathplanning(start, end, image_path="pathplanning/E5_223.jpg", verbose=0)
def d_main():
verbose = 0 # if true, show matplot of odometry
global pose
global flagLocal
pose = RobotPose()
ORIGIN = (83, 10) # mansually calculated. It means index of SLAN map origin on pathplanning binary maze
# E5-223, (32,25) lobby (83,10)
#print(pose.convert2grid())
listener()
while True:
if pose.x != 0.0 and pose.y != 0.0:
flagLocal=False
break
flagLocal = True
print("listener up")
position_idx = pose.convert2grid()
print("position in meters: " + str(pose.x) + ", " + str(pose.y))
print("current_position_idx: " + str(position_idx))
start = ((int)(position_idx[1] + ORIGIN[0]), (int)(position_idx[0] + ORIGIN[1]))
print("idx on the map: " + str(start))
end = (100, 144) # lobby (55,23) E5_223 (10,37)
rospy.set_param('/start_x', start[0])
rospy.set_param('/start_y', start[1])
final_path = Astar.pathplanning(start, end, image_path="pathplanning/lobby3.jpg", verbose=0) #press Q to quit
for i,node in enumerate(final_path):
final_path[i] = [node[0] - start[0], node[1] - start[1]] # change coordinates and calculate relative path. On the path image, downward(x) and right(y). Because the final coordinates of all module have [robotRight(x), robotStraight(y)].
#print(final_path)
final_path_meter = Astar.convert2meter(final_path)
print("before rotate")
print(final_path_meter)
for idx, node in enumerate(final_path_meter):
final_path_meter[idx] = rotate_origin_only(node, pose.rot)
print("after rotate")
print(final_path_meter)
dap.dap(final_path_meter, velRobot = 0.5, verbose=0)
if __name__ == '__main__':
rospy.init_node('robot_mvs', anonymous=False)
d_main()