-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathrun_red_rover.py
174 lines (138 loc) · 5.48 KB
/
run_red_rover.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
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
"""
The controller for the different red rover models.
This module shares all the static content for running
a model scenario, and executes the model using a terminal
argument, e.g., "python run_red_rover.py dubins" runs the
dubins model for a given scenario hardcoded in this module.
"""
import sys
import math
import utm
import rospy
import math
import matplotlib.pyplot as plt
import red_rover_dubins
import red_rover_model
import red_rover_analysis
class RedRoverController(object):
def __init__(self):
self.model_names = ['simple', 'interp1d', 'dubins', 'combined', 'test_path']
# NOTE: Test with just first two points, then try full path:
self.eng_annex_goals_1 = [[259742.1089532437, 3485056.983515083], # Set of goals from 4-5-18 eng. annex test row
[259739.9971399921, 3485057.0322162253]]
# [259737.0933967415, 3485057.0991809973],
# [259733.65459685936, 3485056.8703144793],
# [259729.15988999663, 3485056.665800806],
# [259726.22773249005, 3485055.5007375698],
# [259721.68329927392, 3485053.1401727293],
# [259717.40284074377, 3485050.7735215155],
# [259713.1223801818, 3485048.406871946]]
def create_straight_path(self, spacing, num_points, row=1):
"""
Creates a straight line of num_points of given
spacing between them.
"""
x_array, y_array = [], []
for i in range(1, num_points, spacing):
x_array.append(row) # NOTE: straight line at x=1m
y_array.append(i)
return x_array, y_array
def create_straight_rows(self, spacing, num_points, row_spacing, num_rows):
"""
Similar to the create_straight_path function,
but with parallel rows.
Inputs:
+ spacing - spacing of points in a row.
+ num_points - num of points per row.
+ row_spacing - spacing between rows.
+ num_rows - number of rows.
"""
x_array, y_array = [], []
for i in range(0, num_rows):
xrow, yrow = self.create_straight_path(spacing, num_points, row=i*row_spacing )
x_array += xrow
y_array += yrow
return x_array, y_array
def test_path(self, x_path, y_path):
"""
Plots path to check how it looks before using
"""
plt.plot(x_path, y_path, 'bo')
plt.plot(x_path, y_path, 'b-')
plt.show()
def transform_angle_by_quadrant(self, initial_angle, x_diff, y_diff):
"""
Takes the change in X and Y to determine how the Jackal
should turn.
"""
if x_diff > 0 and y_diff > 0:
print("p1 in quadrant: {}".format(1))
# Point B in quadrant 1..
return math.degrees(initial_angle)
elif x_diff < 0 and y_diff > 0:
print("p1 in quadrant: {}".format(2))
# Point B in quadrant 2..
return 180 - math.degrees(initial_angle)
elif x_diff < 0 and y_diff < 0:
print("p1 in quadrant: {}".format(3))
# Point B in quadrant 3..
return 180 + math.degrees(initial_angle)
elif x_diff > 0 and y_diff < 0:
print("p1 in quadrant: {}".format(4))
# Point B in quadrant 4..
return 360 - math.degrees(initial_angle)
elif x_diff == 0 and y_diff == 0:
# No change in angle..
return 0.0
else:
raise "!Error occurred in basic_drive_3/transform_angle_by_quadrant func.."
# def call_jackal_pos_service(self, distance):
# """
# Get current GPS fix from Jackal's position
# """
# rospy.wait_for_service('get_jackal_pos')
# get_jackal_pos = rospy.ServiceProxy('get_jackal_pos', JackalPos)
# return get_jackal_pos(distance)
if __name__ == '__main__':
_model_name = sys.argv[1] # get model name from terminal
_rrc_obj = RedRoverController() # create instance of RRC
# Raise error if no model name specified:
if _model_name not in _rrc_obj.model_names:
_err_msg = "Enter a model name: {}".format(_rrc_obj.model_names)
raise KeyError(_err_msg)
# Creating a path:
# x_path, y_path = _rrc_obj.create_straight_rows(1, 2, 6, 1) # path to follow
# initial_pos = (2, -5, math.pi/2.0) # initial rover position
# final_pos = (x_path[-1], y_path[-1] + 1, (3*math.pi)/2.0) # final rover position
# current_position = _rrc_obj.call_jackal_pos_service(0)
# lat = curr_pose.jackal_fix.latitude
# _lon = curr_pose.jackal_fix.longitude
# print("Jackal's current lat, lon: {}, {}".format(_lat, _lon))
# curr_pose_utm = utm.from_latlon(curr_pose.jackal_fix.latitude, curr_pose.jackal_fix.longitude)
# Engineering annex goals 1:
goals_list = _rrc_obj.eng_annex_goals_1
x_path, y_path = [], []
for xygoal in goals_list:
x_path.append(xygoal[0])
y_path.append(xygoal[1])
initial_angle = math.radians(150)
initial_xy = (259746.13804448023, 3485055.429863461, None)
final_pos = (x_path[-1], y_path[-1], math.pi)
x_diff = x_path[0] - initial_xy[0]
y_diff = y_path[0] - initial_xy[1]
# _trans_angle = self.transform_imu_frame(degrees(A[2]))
AB_theta0 = math.atan2(abs(y_diff), abs(x_diff)) # get intitial angle, pre transform
AB_angle = _rrc_obj.transform_angle_by_quadrant(AB_theta0, x_diff, y_diff) # determine angle between vector A and B
turn_angle = AB_angle - initial_angle # angle to turn (signage should denote direction to turn)
initial_pos = (initial_xy[0], initial_xy[1], turn_angle)
if _model_name == 'simple':
# red_rover_model.run_red_rover_model(2, 2) # inputs: look-ahead, gps row step size
red_rover_model.run_red_rover_model(initial_pos, final_pos, x_path, y_path)
elif _model_name == 'interp1d':
red_rover_dubins.interp1d_example_1()
elif _model_name == 'dubins':
red_rover_dubins.dubins_example_1(initial_pos, final_pos, x_path, y_path)
elif _model_name == 'combined':
red_rover_dubins.combined_savitzky_dubins_example()
elif _model_name == 'test_path':
_rrc_obj.test_path(x_path, y_path)