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()
