def __init__(self, db):
self.title = ""
self.content = ""
self.nav = nav.Nav(db)
self.db = db
self.script = ""
self.data = ""
self.styles = []
self.d3 = False
def test_clamp_steer_angle(self):
new_bike = bikeState.Bike(0, 0, 0, 0, 0, 0, 0)
waypoints = []
new_map_model = mapModel.Map_Model(new_bike, waypoints, [], [])
new_nav = nav.Nav(new_map_model)
result = nav.Nav.clamp_steer_angle(new_nav, math.pi / 4)
expected = math.pi / 6
result2 = nav.Nav.clamp_steer_angle(new_nav, -math.pi / 4)
expected2 = -math.pi / 6
self.assertEquals(result, expected)
self.assertEquals(result2, expected2)
# Now do the same thing again, since everything shifted down by one
del sys.argv[video_index]
else:
print(USAGE)
sys.exit(1)
if len(sys.argv) < 2 or (sys.argv[1] not in PATHS
and not sys.argv[1].startswith("angle")):
print(USAGE)
sys.exit(1)
# waypoints = requestHandler.parse_json(True)
if sys.argv[1].startswith("angle"):
path_angle = math.radians(int(sys.argv[1][5:]))
waypoints = [(0, 0), (20, 0),
(20 * (1 + math.cos(path_angle)),
20 * math.sin(path_angle))]
else:
waypoints = PATHS[sys.argv[1]]
new_map_model = mapModel.Map_Model(new_bike, waypoints, [], [])
new_nav = nav.Nav(new_map_model)
if filename:
loop_matplotlib_blitting(new_nav,
new_bike,
new_map_model,
blitting=True,
filename=filename)
else:
get_loop_function()(new_nav, new_bike, new_map_model)
if __name__ == '__main__':
try:
old_psi = 0
old_v = 0
d_psi = 0
new_bike = bikeState.Bike(0, -10, 0.1, np.pi / 3, 0, 0, 3.00)
# right intersection to middle intersection
orig_waypoints = [(42.444253, -76.483271), (42.444238, -76.483661)]
#straight path bottom of stairs to middle intersection
#orig_waypoints = [(133.74892645377858, -432.5469806370678), (114.05523219700194, -395.85300512410294)]
#Straight forward path
#orig_waypoints = [(42.444260000000001, -76.48366), (42.44447, -76.48367), (42.44485, -76.48369000000001)]
#long path
#orig_waypoints = [(42.44422, -76.43866), (42,44447, -76.48367), (42.44458, -76.4835)]
#orig_waypoints = [(164.92918389320673, -412.53122538008699), (160.82636519097008, -408.08352424480717), (158.36456020192119, -400.29993577850547), (157.54391784874556, -395.85215731941992), (152.62045116162616, -385.84472352909091), (141.95309049412472, -369.16571007809335), (133.74760279568892, -363.6061261255179), (124.72163122543957, -360.27044577941609), (118.1572971243023, -358.04666168562972), (110.7724130084174, -354.71093523541589), (97.643830726023069, -354.7111316365864), (98.464555727252943, -368.05451128333181), (101.74674006642893, -370.2783626484474), (102.56727829387685, -370.27835061499496), (103.3878330236748, -371.39028775136137), (114.05523219700194, -395.85300512410294), (134.56949349422308, -433.6589141004269)]
#converting from lat long to local coordinate plane
waypoints = [0 for _ in range(len(orig_waypoints))]
for i in range(len(orig_waypoints)):
latitude, longitude = orig_waypoints[i][0], orig_waypoints[i][1]
waypoints[i] = global_to_local(float(latitude), float(longitude))
new_map = mapModel.Map_Model(new_bike, waypoints, [], [])
new_nav = nav.Nav(new_map)
talker()
except rospy.ROSInterruptException:
rospy.logerr('this should never print')
pass