def balance_score(c1, c2, c3): bike = bikeState.Bike(0, -10, 0.1, numpy.pi / 3, 0, 0, 3.57) score = 0 for _ in range(300): new_bike, phidot = new_state(bike, 0, c1, c2, c3) bike.update(new_bike) score += phidot**2 val = (numpy.sqrt(score)) return (val)
def new_state(bike, steerD, c1, c2, c3): rhs_result = bike.rhs(steerD, c1, c2, c3) u = rhs_result[0] values = rhs_result[1] new_xB = bike.xB + values[0] * TIMESTEP new_yB = bike.yB + values[1] * TIMESTEP new_phi = bike.phi + values[2] * TIMESTEP new_psi = bike.psi + values[3] * TIMESTEP new_delta = bike.delta + values[4] * TIMESTEP new_w_r = bike.w_r + values[5] * TIMESTEP new_v = bike.v + values[6] * TIMESTEP new_state = bikeState.Bike(new_xB, new_yB, new_phi, new_psi, new_delta, new_w_r, new_v) return new_state, new_w_r
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 new_state(bike, steerD): """ Returns new bike state object after it passes through bike dynamics """ # Get navigation command # steerD = nav.command(bike.xB, bike.yB, bike.psi, bike.v, waypoints) rhs_result = bike.rhs(steerD) u = rhs_result[0] values = rhs_result[1] new_xB = bike.xB + values[0]*TIMESTEP new_yB = bike.yB + values[1]*TIMESTEP new_phi = bike.phi + values[2]*TIMESTEP new_psi = bike.psi+ values[3]*TIMESTEP new_delta = bike.delta + values[4]*TIMESTEP new_w_r = bike.w_r + values[5]*TIMESTEP new_v = bike.v + values[6]*TIMESTEP new_state = bikeState.Bike(new_xB, new_yB, new_phi, new_psi, new_delta, new_w_r, new_v) return new_state
def loop_pyqtgraph(nav, bike, map_model): traces = [dict()] # Set the background color of all plots to white for legibility pg.setConfigOption('background', 'w') qt_win = pg.GraphicsWindow(title="Bike Simulator 2017") # Stores every item in the "trajectory" plot plot_items = [dict()] # This ViewBox will hold the bike and trajectory viewbox = qt_win.addPlot(col=0, row=0, lockAspect=1.0) viewbox.setAspectLocked() viewbox.sigResized = viewbox.sigRangeChanged # Axes need this method viewbox.showAxis("bottom", show=True) # Make an item for the bike bike_polygon = QtGui.QPolygonF() for _ in xrange(3): bike_polygon.append(QtCore.QPointF(0, 0)) bikeItem = QtGui.QGraphicsPolygonItem(bike_polygon) plot_items[0]["bikeitem"] = bikeItem plot_items[0]["bike"] = bike_polygon bikeItem.setPen(pg.mkPen(None)) bikeItem.setBrush(pg.mkBrush('r')) viewbox.addItem(bikeItem) # Graphics helper def setPenWidth(pen, width): pen.setWidth(width) return pen # Make an item for the static (given) path paths = map_model.paths static_path = QtGui.QPainterPath() static_path.moveTo(paths[0][0][0], paths[0][0][1]) for each_segment in paths: static_path.lineTo(*each_segment[1]) static_path_item = QtGui.QGraphicsPathItem(static_path) static_path_item.setPen(setPenWidth(pg.mkPen('g'), 2)) static_path_item.setBrush(pg.mkBrush(None)) viewbox.addItem(static_path_item) # Make an item for the trajectory traj_path = QtGui.QPainterPath() traj_path.moveTo(bike.xB, bike.yB) plot_items[0]["traj"] = traj_path traj_path_item = QtGui.QGraphicsPathItem(traj_path) traj_path_item.setPen(setPenWidth(pg.mkPen('b'), 2)) traj_path_item.setBrush(pg.mkBrush(None)) plot_items[0]["trajitem"] = traj_path_item viewbox.addItem(traj_path_item) def traj_update(): plot_items[0]["traj"].lineTo(bike.xB, bike.yB) plot_items[0]["trajitem"].setPath(plot_items[0]["traj"]) traj_timer = QtCore.QTimer() traj_timer.timeout.connect(traj_update) traj_timer.start(100) # This bit simulates the algo receiving delayed information delayed_bike = bikeState.Bike(0, 0, 0, 0, 0, 0, 0) nav.map_model.bike = delayed_bike def delayed_update(): delayed_bike.update(bike) delayed_timer = QtCore.QTimer() delayed_timer.timeout.connect(delayed_update) delayed_timer.start(1) get_steering_angle = nav.get_steering_angle simulation_step = lambda angle: bike.update(bikeSim.new_state(bike, angle)) def update(): simulation_step(get_steering_angle()) x1, y1 = bike.xB, bike.yB x2, y2 = bike.xB + 2.0 * math.cos(bike.psi + 13 * math.pi / 12), bike.yB + 2.0 * math.sin( bike.psi + 13 * math.pi / 12) x3, y3 = bike.xB + 2.0 * math.cos(bike.psi + 11 * math.pi / 12), bike.yB + 2.0 * math.sin( bike.psi + 11 * math.pi / 12) #x1, y1, x2, y2, x3, y3 = tuple(int(num) for num in (x1, y1, x2, y2, x3, y3)) new_polygon = QtGui.QPolygonF() for each_point in ((x1, y1), (x2, y2), (x3, y3)): new_polygon.append(QtCore.QPointF(*each_point)) plot_items[0]["bikeitem"].setPolygon(new_polygon) anim_timer = QtCore.QTimer() anim_timer.timeout.connect(update) anim_timer.start(ANIM_INTERVAL) QtGui.QApplication.instance().exec_()
if waypoint[0] < xlim[0]: xlim[0] = waypoint[0] elif waypoint[0] > xlim[1]: xlim[1] = waypoint[0] if waypoint[1] < ylim[0]: ylim[0] = waypoint[1] elif waypoint[1] > ylim[1]: ylim[1] = waypoint[1] xlim, ylim = (xlim[0] - padding, xlim[1] + padding), (ylim[0] - padding, ylim[1] + padding) return {"xlim": xlim, "ylim": ylim} if __name__ == '__main__': new_bike = bikeState.Bike(0, 0, 0.1, 0, 0, 0, 3.57) # Define some preset paths PATHS = { "0": [(0, 0), (20, 5), (40, 5)], "line": [(0, 0), (50, 0)], "2": [(0, 0), (20, 5), (40, -5), (60, 10), (100, -20), (40, -50), (0, -10), (0, 0)], "3": [(40, 0), (20, -10), (0, 0)], "square": [(0, 0), (50, 0), (50, 50), (0, 50), (0, 0)], "5hard": [(0, 0), (10, 0), (20, 5), (25, 15), (12, 20), (0, 15), (0, 0)], "5easy": [(0, 0), (20, 0), (40, 10), (50, 30), (24, 40), (0, 30), (0, 0)] }
def setup_dimension(): dim = [] s = np.array( map_model.paths).shape[0] # .shape gives tuple of array dimensions dim.append(MultiArrayDimension('paths', len(map_model.paths), s)) dim.append(MultiArrayDimension('path', 2, 4)) dim.append(MultiArrayDimension('point', 2, 2)) return dim def map_server(): pub = rospy.Publisher('paths', Float32MultiArray, queue_size=10) rospy.init_node('map', anonymous=True) # Initialize ROS node 'map' rate = rospy.Rate(10) while not rospy.is_shutdown(): #rospy.loginfo(map_model.paths) dim = setup_dimension() layout = MultiArrayLayout(dim, 0) #rospy.loginfo(map_model.paths) pub.publish(layout, list(np.array(map_model.paths).ravel())) rate.sleep() if __name__ == "__main__": new_bike = bikeState.Bike(-5, -5, 0.1, 0, 0, 0, 3.57) waypoints = requestHandler.parse_json(presets=True) map_model = mapModel.Map_Model(new_bike, waypoints, [], []) map_server()
def listener(): pub = rospy.Publisher('bike_state', Float32MultiArray, queue_size=10) rospy.init_node('simulator', anonymous=True) rospy.Subscriber("nav_instr", Float32, update_graph) rospy.Subscriber("paths", Float32MultiArray, path_parse) rospy.Subscriber("gps", Float32MultiArray, update_bike_from_gps) rate = rospy.Rate(100) while not rospy.is_shutdown(): dim = [MultiArrayDimension('data', 8, 8)] layout = MultiArrayLayout(dim, 0) # This needs to publish the bike state from the Arduino l = [ new_bike.xB, new_bike.yB, new_bike.phi, new_bike.psi, new_bike.delta, new_bike.w_r, new_bike.v, new_bike.turning_r ] rospy.loginfo( "({:.4f}, {:.4f}), heading {:.4f}, velocity {:.4f}, lean angle/rate {:.4f}/{:.4f}, steering {:.4f} [gps_assisted_simulator_node]" .format(new_bike.xB, new_bike.yB, new_bike.psi, new_bike.v, new_bike.phi, new_bike.w_r, new_bike.delta)) rospy.loginfo(l) pub.publish(layout, l) rate.sleep() if __name__ == '__main__': new_bike = bikeState.Bike(0, -10, 0.1, np.pi / 3, 0, 0, 3.57) waypoints = [(0.1, 0.1), (30.1, 0.1), (31.1, 0.1)] map_model = mapModel.Map_Model(new_bike, waypoints, [], []) listener()