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