def racing(args):
track_layout = args["track_layout"]
track_spec = np.genfromtxt("data/track_layout/" + track_layout + ".csv",
delimiter=",")
if args["simulation"]:
track = racing_env.ClosedTrack(track_spec, track_width=1.0)
# setup ego car
ego = offboard.DynamicBicycleModel(
name="ego",
param=base.CarParam(edgecolor="black"),
system_param=base.SystemParam())
mpc_cbf_param = base.MPCCBFRacingParam(vt=0.8)
ego.set_state_curvilinear(np.zeros((X_DIM, )))
ego.set_state_global(np.zeros((X_DIM, )))
ego.start_logging()
ego.set_ctrl_policy(
offboard.MPCCBFRacing(mpc_cbf_param, ego.system_param))
ego.ctrl_policy.set_timestep(0.1)
ego.set_track(track)
ego.ctrl_policy.set_track(track)
# setup surrounding cars
t_symbol = sp.symbols("t")
car1 = offboard.NoDynamicsModel(
name="car1", param=base.CarParam(edgecolor="orange"))
car1.set_track(track)
car1.set_state_curvilinear_func(t_symbol, 0.2 * t_symbol + 4.0,
0.1 + 0.0 * t_symbol)
car1.start_logging()
car2 = offboard.NoDynamicsModel(
name="car2", param=base.CarParam(edgecolor="orange"))
car2.set_track(track)
car2.set_state_curvilinear_func(t_symbol, 0.2 * t_symbol + 10.0,
-0.1 + 0.0 * t_symbol)
car2.start_logging()
# setup simulation
simulator = offboard.CarRacingSim()
simulator.set_timestep(0.1)
simulator.set_track(track)
simulator.add_vehicle(ego)
ego.ctrl_policy.set_racing_sim(simulator)
simulator.add_vehicle(car1)
simulator.add_vehicle(car2)
simulator.sim(sim_time=50.0)
with open("data/simulator/racing.obj", "wb") as handle:
pickle.dump(simulator, handle, protocol=pickle.HIGHEST_PROTOCOL)
else:
with open("data/simulator/racing.obj", "rb") as handle:
simulator = pickle.load(handle)
if args["plotting"]:
simulator.plot_simulation()
simulator.plot_state("ego")
if args["animation"]:
simulator.animate(filename="racing", mpc_cbf=True)
def set_vehicle(args):
veh_name = args["veh_name"]
initial_xcurv = (
args["vx"],
args["vy"],
args["wz"],
args["epsi"],
args["s"],
args["ey"],
)
veh_color = args["color"]
# initial a ros node for vehicle
rospy.init_node(veh_name, anonymous=True)
# call ros service to add vehicle in simulator and visualization node
add_vehicle_client_simulator(veh_name, veh_color)
add_vehicle_client_visualization(veh_name, veh_color)
veh = realtime.DynamicBicycleModel(name=veh_name, param=base.CarParam())
veh.set_subscriber_track()
veh.set_subscriber_input(veh_name)
veh_state_topic = veh_name + "/state"
veh.__pub_state = rospy.Publisher(veh_state_topic,
VehicleState,
queue_size=10)
# get initial state in Frenet and X-Y coordinates
veh.set_state_curvilinear(initial_xcurv)
s0 = initial_xcurv[4]
ey0 = initial_xcurv[5]
veh.realtime_flag = True
xglob0 = np.zeros((X_DIM, ))
xglob0[0:3] = initial_xcurv[0:3]
psi0 = racing_env.get_orientation(veh.lap_length, veh.lap_width,
veh.point_and_tangent, s0, ey0)
x0, y0 = racing_env.get_global_position(veh.lap_length, veh.lap_width,
veh.point_and_tangent, s0, ey0)
xglob0[3] = psi0
xglob0[4] = x0
xglob0[5] = y0
veh.set_state_global(xglob0)
loop_rate = 100
timestep = 1 / loop_rate
veh.set_timestep(timestep)
r = rospy.Rate(loop_rate) # 100Hz
start_timer = datetime.datetime.now()
tmp = 0
msg_state = VehicleState()
msg_state.name = veh_name
while not rospy.is_shutdown():
current_time = datetime.datetime.now()
# update the vehicle's state at 100 Hz
if (current_time -
start_timer).total_seconds() > (tmp + 1) * (1 / 100):
veh.forward_one_step(veh.realtime_flag)
msg_state.state_curv = get_msg_xcurv(veh.xcurv)
msg_state.state_glob = get_msg_xglob(veh.xglob)
veh.__pub_state.publish(msg_state)
tmp = tmp + 1
else:
pass
r.sleep()
def add_vehicle(self, req):
self.vehicles[req.name] = DynamicBicycleModel(name=req.name, param=base.CarParam())
self.vehicles[req.name].xglob = np.zeros((X_DIM,))
self.vehicles[req.name].xcurv = np.zeros((X_DIM,))
self.vehicles[req.name].msg_state.name = req.name
self.vehicles[req.name].set_subscriber_sim(req.name)
self.num_vehicle = self.num_vehicle + 1
return 1
def set_up_other_vehicles(track, num_veh):
vehicles = []
for index in range(0, num_veh):
veh_name = "car" + str(index + 1)
vehicles.append(
offboard.NoDynamicsModel(name=veh_name,
param=base.CarParam(edgecolor="orange")))
vehicles[index].set_track(track)
return vehicles
def __sub_veh_list_cb(self, msg):
if self.num_veh == None:
pass
else:
for index in range(self.num_veh):
name = msg.vehicle_list[index]
# ego vehicle
if name == self.agent_name:
self.vehicles[name] = DynamicBicycleModel(name=name, param=base.CarParam())
self.vehicles[name].name = self.agent_name
self.vehicles[name].xglob = np.zeros((X_DIM,))
self.vehicles[name].xcurv = np.zeros((X_DIM,))
# other vehicle
else:
self.vehicles[name] = DynamicBicycleModel(name=name, param=base.CarParam())
self.vehicles[name].xglob = np.zeros((X_DIM,))
self.vehicles[name].xcurv = np.zeros((X_DIM,))
self.vehicles[name].timestep = self.timestep
self.vehicles[name].lap_length = self.lap_length
self.vehicles[name].lap_width = self.lap_width
self.vehicles[name].point_and_tangent = self.point_and_tangent
self.vehicles[name].set_subscriber_ctrl(name)
def add_vehicle(self, req):
self.vehicles[req.name] = DynamicBicycleModel(name=req.name, param=base.CarParam())
self.vehicles[req.name].ax = self.ax
self.vehicles[req.name].xglob = np.zeros((X_DIM,))
self.vehicles[req.name].xcurv = np.zeros((X_DIM,))
self.num_vehicle = self.num_vehicle + 1
(x_car, y_car, width_car, height_car, angle_car,) = self.vehicles[
req.name
].get_vehicle_in_rectangle(self.vehicles[req.name].xglob)
self.vehicles[req.name].patch = patches.Rectangle(
(x_car, y_car), width_car, height_car, angle_car, color=req.color
)
self.vehicles[req.name].set_subscriber_visual(req.name)
self.vehicles[req.name].ani = animation.FuncAnimation(
self.fig,
self.vehicles[req.name].update,
init_func=self.vehicles[req.name].init,
)
return 1
def tracking(args):
track_layout = args["track_layout"]
track_spec = np.genfromtxt("data/track_layout/" + track_layout + ".csv",
delimiter=",")
if args["simulation"]:
track = racing_env.ClosedTrack(track_spec, track_width=0.8)
# setup ego car
ego = offboard.DynamicBicycleModel(
name="ego",
param=base.CarParam(edgecolor="black"),
system_param=base.SystemParam())
ego.set_state_curvilinear(np.zeros((X_DIM, )))
ego.set_state_global(np.zeros((X_DIM, )))
ego.start_logging()
if args["ctrl_policy"] == "pid":
ego.set_ctrl_policy(offboard.PIDTracking(vt=0.8))
elif args["ctrl_policy"] == "mpc-lti":
mpc_lti_param = base.MPCTrackingParam(vt=0.8)
ego.set_ctrl_policy(
offboard.MPCTracking(mpc_lti_param, ego.system_param))
else:
raise NotImplementedError
ego.ctrl_policy.set_timestep(0.1)
ego.ctrl_policy.set_track(track)
ego.set_track(track)
# setup simulation
simulator = offboard.CarRacingSim()
simulator.set_timestep(0.1)
simulator.set_track(track)
simulator.add_vehicle(ego)
ego.ctrl_policy.set_racing_sim(simulator)
simulator.sim(sim_time=90.0)
with open("data/simulator/tracking.obj", "wb") as handle:
pickle.dump(simulator, handle, protocol=pickle.HIGHEST_PROTOCOL)
else:
with open("data/simulator/tracking.obj", "rb") as handle:
simulator = pickle.load(handle)
if args["plotting"]:
simulator.plot_simulation()
simulator.plot_state("ego")
if args["animation"]:
simulator.animate(filename="tracking", ani_time=250)
def set_up_ego(timestep, track):
ego = offboard.DynamicBicycleModel(name="ego",
param=base.CarParam(edgecolor="black"),
system_param=base.SystemParam())
ego.set_timestep(timestep)
# run the pid controller for the first lap to collect data
pid_controller = offboard.PIDTracking(vt=0.7, eyt=0.0)
pid_controller.set_timestep(timestep)
ego.set_ctrl_policy(pid_controller)
pid_controller.set_track(track)
ego.set_state_curvilinear(np.zeros((X_DIM, )))
ego.set_state_global(np.zeros((X_DIM, )))
ego.start_logging()
ego.set_track(track)
# run mpc-lti controller for the second lap to collect data
mpc_lti_param = base.MPCTrackingParam(vt=0.7, eyt=0.0)
mpc_lti_controller = offboard.MPCTracking(mpc_lti_param, ego.system_param)
mpc_lti_controller.set_timestep(timestep)
mpc_lti_controller.set_track(track)
return ego, pid_controller, mpc_lti_controller
def linear_time_invariant():
# define closed_track
track_spec = np.array([
[3, 0],
[np.pi / 2 * 1.5, -1.5],
[2, 0],
[np.pi / 2 * 1.5, -1.5],
[6, 0],
[np.pi / 2 * 1.5, -1.5],
[2.0, 0],
[np.pi / 2 * 1.5, -1.5],
])
track = racing_env.ClosedTrack(track_spec, track_width=1.0)
# setup ego car
ego = offboard.DynamicBicycleModel(name="ego",
param=base.CarParam(edgecolor="black"))
ego.set_state_curvilinear(np.array([0.3, 0, 0, 0, 0, 0]))
ego.set_state_global(np.array([0.3, 0, 0, 0, 0, 0]))
ego.set_ctrl_policy(offboard.PIDTracking(vt=0.5))
ego.ctrl_policy.set_timestep(0.1)
ego.set_track(track)
# setup simulation
simulator = offboard.CarRacingSim()
simulator.set_timestep(0.1)
simulator.set_track(track)
simulator.add_vehicle(ego)
ego.ctrl_policy.set_racing_sim(simulator)
simulator.sim(sim_time=500.0)
# calculate linearized dynamics
xdata = np.stack(simulator.vehicles["ego"].xcurv_log, axis=0)
udata = system_identification.get_udata(simulator.vehicles["ego"])
lamb = 1e-9
matrix_A, matrix_B, error = system_identification.linear_regression(
xdata, udata, lamb)
np.savetxt("data/track_layout/ellipse.csv", track_spec, delimiter=",")
np.savetxt("data/sys/LTI/matrix_A.csv", matrix_A, delimiter=",")
np.savetxt("data/sys/LTI/matrix_B.csv", matrix_B, delimiter=",")
print(matrix_A)
print(matrix_B)