def __init__(self):
# Node and Publisher/Subscriber Setup.
rospy.init_node("dbw_mpc_path_follower")
# Acceleration/Steering Command (for drive-by-wire) + MPC solution publisher (for viz/analysis).
self.acc_prev = 0. # last published acceleration
self.steer_prev = 0. # last published steering angle
self.acc_pub = rospy.Publisher("/control/accel", Float32Msg, queue_size=2)
self.steer_pub = rospy.Publisher("/control/steer_angle", Float32Msg, queue_size=2)
self.mpc_path_pub = rospy.Publisher("mpc_path", mpc_path, queue_size=2)
# State Estimation Subscriber.
self.current_state = {'t': -1., 'x0': 0., 'y0': 0., 'psi0': 0., 'v0': 0}
self.state_lock = Lock()
self.sub_state = rospy.Subscriber("state_est", state_est, self.state_est_callback, queue_size=2)
# TODO: update description and make yaw/psi consistent.
self.mpc = MPC(**MPC_PARAMS)
self.ref_traj = RGT.GPSRefTrajectory(mat_filename=mat_fname, LAT0=lat0, LON0=lon0, traj_horizon=self.mpc.N, traj_dt=self.mpc.DT)
# One time drive-by-wire enable message published. We assume the driver will disable drive-by-wire
# using brake/steering overrides, although it could be done by this class too.
acc_enable_pub = rospy.Publisher("/control/enable_accel", UInt8Msg, queue_size=2, latch=True)
steer_enable_pub = rospy.Publisher("/control/enable_spas", UInt8Msg, queue_size=2, latch=True)
acc_enable_pub.publish(UInt8Msg(1))
steer_enable_pub.publish(UInt8Msg(1))
self.pub_loop()
def __init__(self):
# Load Global Trajectory
if rospy.has_param('mat_waypoints'):
mat_name = rospy.get_param('mat_waypoints')
else:
raise ValueError("No Matfile of waypoints were provided!")
if not (rospy.has_param('lat0') and rospy.has_param('lon0')
and rospy.has_param('yaw0')):
raise ValueError('Invalid rosparam global origin provided!')
lat0 = rospy.get_param('lat0')
lon0 = rospy.get_param('lon0')
yaw0 = rospy.get_param('yaw0')
grt = r.GPSRefTrajectory(mat_filename=mat_name,
LAT0=lat0,
LON0=lon0,
YAW0=yaw0) # only 1 should be valid.
# Set up Data
self.x_global_traj = grt.get_Xs()
self.y_global_traj = grt.get_Ys()
self.x_ref_traj = self.x_global_traj[0]
self.y_ref_traj = self.y_global_traj[0]
self.x_mpc_traj = self.x_global_traj[0]
self.y_mpc_traj = self.y_global_traj[0]
self.x_vehicle = self.x_global_traj[0]
self.y_vehicle = self.y_global_traj[0]
self.psi_vehicle = 0.0
# Set up Plot: includes full ("global") trajectory, target trajectory, MPC prediction trajectory, and vehicle position.
self.f = plt.figure()
plt.ion()
l1, = plt.plot(self.x_global_traj, self.y_global_traj, 'k')
l2, = plt.plot(self.x_ref_traj, self.y_ref_traj, 'rx')
l3, = plt.plot(self.x_vehicle, self.y_vehicle, 'bo')
l4, = plt.plot(self.x_mpc_traj, self.y_mpc_traj, 'g*')
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
self.l_arr = [l1, l2, l3, l4]
plt.axis('equal')
rospy.init_node('vehicle_plotter', anonymous=True)
rospy.Subscriber('state_est',
state_est,
self.update_state,
queue_size=1)
rospy.Subscriber('mpc_path',
mpc_path,
self.update_mpc_trajectory,
queue_size=1)
self.loop()
def __init__(self):
# Load Global Trajectory
if rospy.has_param('mat_waypoints'):
mat_name = rospy.get_param('mat_waypoints')
else:
raise ValueError("No Matfile of waypoints were provided!")
if not (rospy.has_param('lat0') and rospy.has_param('lon0')
and rospy.has_param('yaw0')):
raise ValueError('Invalid rosparam global origin provided!')
lat0 = rospy.get_param('lat0')
lon0 = rospy.get_param('lon0')
yaw0 = rospy.get_param('yaw0')
grt = r.GPSRefTrajectory(mat_filename=mat_name,
LAT0=lat0,
LON0=lon0,
YAW0=yaw0) # only 1 should be valid.
# Set up Data
self.all_traj = grt.get_all_traj()
self.x_global_traj = grt.get_Xs()
self.y_global_traj = grt.get_Ys()
self.x_ref_traj = self.x_global_traj[0]
self.y_ref_traj = self.y_global_traj[0]
self.x_mpc_traj = self.x_global_traj[0]
self.y_mpc_traj = self.y_global_traj[0]
self.x_vehicle = self.x_global_traj[0]
self.y_vehicle = self.y_global_traj[0]
self.psi_vehicle = yaw0
self.df_vehicle = 0.0 # rad (steering angle)
# This should be include in a launch file/yaml for the future.
self.a = 1.5213 # m (CoG to front axle)
self.b = 1.4987 # m (CoG to rear axle)
self.vW = 1.89 # m (vehicle width)
self.rW = 0.3 # m (wheel radius, estimate based on Shelley's value of 0.34 m).
# Set up Plot: includes full ("global") trajectory, target trajectory, MPC prediction trajectory, and vehicle.
self.f = plt.figure()
self.ax = plt.gca()
plt.ion()
# Trajectory
traj1 = self.all_traj[0]
self.l1, = self.ax.plot(traj1[:, 4], traj1[:, 5], 'k')
self.l2, = self.ax.plot(self.x_ref_traj, self.y_ref_traj, 'rx')
self.l3, = self.ax.plot(self.x_mpc_traj, self.y_mpc_traj, 'g*')
traj2 = self.all_traj[1]
self.l4, = self.ax.plot(traj2[:, 4], traj2[:, 5], 'k')
traj3 = self.all_traj[2]
self.l5, = self.ax.plot(traj3[:, 4], traj3[:, 5], 'k')
traj4 = self.all_traj[3]
self.l6, = self.ax.plot(traj4[:, 4], traj4[:, 5], 'k')
traj5 = self.all_traj[4]
self.l7, = self.ax.plot(traj5[:, 4], traj5[:, 5], 'k')
# Vehicle coordinates
FrontBody, RearBody, FrontAxle, RearAxle, RightFrontTire, RightRearTire, LeftFrontTire, LeftRearTire = \
plotVehicle(self.x_vehicle, self.y_vehicle, self.psi_vehicle, self.df_vehicle, self.a, self.b, self.vW, self.rW)
self.vl0, = self.ax.plot(self.x_vehicle,
self.y_vehicle,
'bo',
MarkerSize=8)
self.vl1, = self.ax.plot(FrontBody[0, :],
FrontBody[1, :],
'gray',
LineWidth=2.5)
self.vl2, = self.ax.plot(RearBody[0, :],
RearBody[1, :],
'gray',
LineWidth=2.5)
self.vl3, = self.ax.plot(FrontAxle[0, :],
FrontAxle[1, :],
'gray',
LineWidth=2.5)
self.vl4, = self.ax.plot(RearAxle[0, :],
RearAxle[1, :],
'gray',
LineWidth=2.5)
self.vl5, = self.ax.plot(RightFrontTire[0, :],
RightFrontTire[1, :],
'r',
LineWidth=3)
self.vl6, = self.ax.plot(RightRearTire[0, :],
RightRearTire[1, :],
'k',
LineWidth=3)
self.vl7, = self.ax.plot(LeftFrontTire[0, :],
LeftFrontTire[1, :],
'r',
LineWidth=3)
self.vl8, = self.ax.plot(LeftRearTire[0, :],
LeftRearTire[1, :],
'k',
LineWidth=3)
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.axis('equal')
# Zoomed Inset Plot: Based off tutorial/code here: http://akuederle.com/matplotlib-zoomed-up-inset
self.ax_zoom = zoomed_inset_axes(
self.ax, 2, loc=2) # axis, zoom_factor, location (2 = upper left)
self.window = 25 # m
self.zl1, = self.ax_zoom.plot(traj1[:, 4], traj1[:, 5], 'k')
self.zl2, = self.ax_zoom.plot(self.x_ref_traj, self.y_ref_traj, 'rx')
self.zl3, = self.ax_zoom.plot(self.x_mpc_traj, self.y_mpc_traj, 'g*')
self.zl4, = self.ax_zoom.plot(traj2[:, 4], traj2[:, 5], 'k')
self.zl5, = self.ax_zoom.plot(traj3[:, 4], traj3[:, 5], 'k')
self.zl6, = self.ax_zoom.plot(traj4[:, 4], traj4[:, 5], 'k')
self.zl7, = self.ax_zoom.plot(traj5[:, 4], traj5[:, 5], 'k')
self.zvl0, = self.ax_zoom.plot(self.x_vehicle,
self.y_vehicle,
'bo',
MarkerSize=8)
self.zvl1, = self.ax_zoom.plot(FrontBody[0, :],
FrontBody[1, :],
'gray',
LineWidth=2.5)
self.zvl2, = self.ax_zoom.plot(RearBody[0, :],
RearBody[1, :],
'gray',
LineWidth=2.5)
self.zvl3, = self.ax_zoom.plot(FrontAxle[0, :],
FrontAxle[1, :],
'gray',
LineWidth=2.5)
self.zvl4, = self.ax_zoom.plot(RearAxle[0, :],
RearAxle[1, :],
'gray',
LineWidth=2.5)
self.zvl5, = self.ax_zoom.plot(RightFrontTire[0, :],
RightFrontTire[1, :],
'r',
LineWidth=3)
self.zvl6, = self.ax_zoom.plot(RightRearTire[0, :],
RightRearTire[1, :],
'k',
LineWidth=3)
self.zvl7, = self.ax_zoom.plot(LeftFrontTire[0, :],
LeftFrontTire[1, :],
'r',
LineWidth=3)
self.zvl8, = self.ax_zoom.plot(LeftRearTire[0, :],
LeftRearTire[1, :],
'k',
LineWidth=3)
self.ax_zoom.set_xlim(self.x_vehicle - self.window,
self.x_vehicle + self.window)
self.ax_zoom.set_ylim(self.y_vehicle - self.window,
self.y_vehicle + self.window)
plt.yticks(visible=False)
plt.xticks(visible=False)
rospy.init_node('vehicle_plotter', anonymous=True)
rospy.Subscriber('state_est',
state_est,
self.update_state,
queue_size=1)
rospy.Subscriber('mpc_path',
mpc_path,
self.update_mpc_trajectory,
queue_size=1)
self.loop()
