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)
def main(self):
rospy.init_node("plotter")
while not rospy.has_param('path'):
time.sleep(1)
self.path = rospy.get_param("path")
plt.ion() # enables interactive plotting
waypoints = map(lambda p: global_to_local(*p), self.path)
rospy.logerr(waypoints)
map_model = mapModel.Map_Model(None, waypoints, [], [])
paths = map_model.paths
fig = plt.figure()
#ax = plt.axes(**find_display_bounds(map_model.waypoints))
ax = plt.axes(xlim=(-100,100), ylim=(-100,100))
lc = mc.LineCollection(paths, linewidths=2, color = "blue")
ax.add_collection(lc)
# For plotting the bicycle
self.axes = plt.gca()
self.circle = Circle((0,0),1,ec='r', fc='none')
self.axes.add_patch(self.circle)
self.circle2 = Circle((0,0),1,ec='b', fc='none')
self.axes.add_patch(self.circle2)
# Holds past locations of the bike, for plotting
self.bike_trajectory = []
# We need to keep this around to clear it after path updates
#self.path_patch = None
self.prev_bike_patch = None
rospy.Subscriber("/android/gps/fused", Location, self.loc_listener) # RED
rospy.Subscriber("/android/gps/gps", Location, self.loc2_listener) # BLUE
plt.show(block=True)
# 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)