Ejemplo n.º 1
0
def main():
	dynamics = bike_dynamics_rk4()
	controller = StrategyOBCAController(dynamics)
	controller.initialize(regen=False)

	# Generate fake obs and hyp to test the solver
	obs = []
	hyp = []
	for _ in range(21):
		obs_i = []
		for i in range(3):
			if i == 1:
				obs_i.append( {"A": np.zeros((4,2)), "b": np.zeros((4,1))} )
			else:
				obs_i.append( {"A": np.zeros(2), "b": np.zeros(1)} )

		obs.append(obs_i)
		hyp.append( {"w": np.ones(4), "b": 0} )

	z_ws = np.zeros((21, 4))
	u_ws = np.zeros((20, 2))

	controller.solve_ws(z_ws, u_ws, obs)

	z_s = np.zeros(4)
	u_prev = np.zeros(2)
	z_ref = np.zeros((21, 4))

	controller.solve(z_s, u_prev, z_ref, obs, hyp)
def main():
	dynamics = bike_dynamics_rk4()
	params = trackingParams()
	controller = trackingController(dynamics, params)
	controller.initialize()

	# Generate fake obs and hyp to test the solver
	z_ws = np.zeros((51, 4))
	u_ws = np.zeros((50, 2))

	z_s = np.zeros(4)
	u_prev = np.zeros(2)
	z_ref = np.zeros((51, 4))

	controller.solve(z_s, u_prev, z_ref, z_ws, u_ws)
Ejemplo n.º 3
0
    def __init__(self):
        # Read parameter values from ROS parameter server
        rospy.init_node('strategy_obca_control')

        self.dt = rospy.get_param('controller/dt')
        self.init_time = rospy.get_param('controller/init_time')
        self.max_time = rospy.get_param('controller/max_time')
        self.accel_max = rospy.get_param('controller/accel_max')
        self.accel_min = rospy.get_param('controller/accel_min')
        self.steer_max = rospy.get_param('controller/steer_max')
        self.steer_min = rospy.get_param('controller/steer_min')
        self.daccel_max = rospy.get_param('controller/daccel_max')
        self.daccel_min = rospy.get_param('controller/daccel_min')
        self.dsteer_max = rospy.get_param('controller/dsteer_max')
        self.dsteer_min = rospy.get_param('controller/dsteer_min')
        self.lanewidth = rospy.get_param('controller/lanewidth')

        self.v_ref = rospy.get_param('controller/obca/v_ref')
        self.n_x = rospy.get_param('controller/obca/n')
        self.n_u = rospy.get_param('controller/obca/d')
        self.N = rospy.get_param('controller/obca/N')
        self.n_obs = rospy.get_param('controller/obca/n_obs')
        self.n_ineq = rospy.get_param('controller/obca/n_ineq')
        self.d_ineq = rospy.get_param('controller/obca/d_ineq')
        self.d_min = rospy.get_param('controller/obca/d_min')
        self.Q = np.diag(rospy.get_param('controller/obca/Q'))
        self.R = np.diag(rospy.get_param('controller/obca/R'))
        self.optlevel = rospy.get_param('controller/obca/optlevel')
        self.lock_steps = rospy.get_param('controller/obca/lock_steps')
        self.T_tv = rospy.get_param('controller/obca/T_tv')

        self.nn_model_file = rospy.get_param('controller/obca/nn_model_file')
        self.smooth_prediction = rospy.get_param(
            'controller/obca/smooth_prediction')
        self.W_kf = eval(rospy.get_param('controller/obca/W_kf'))
        self.V_kf = eval(rospy.get_param('controller/obca/V_kf'))
        self.Pm_kf = eval(rospy.get_param('controller/obca/Pm_kf'))
        self.A_kf = eval(rospy.get_param('controller/obca/A_kf'))
        self.H_kf = eval(rospy.get_param('controller/obca/H_kf'))

        self.P_accel = rospy.get_param('controller/safety/P_accel')
        self.I_accel = rospy.get_param('controller/safety/I_accel')
        self.D_accel = rospy.get_param('controller/safety/D_accel')
        self.P_steer = rospy.get_param('controller/safety/P_steer')
        self.I_steer = rospy.get_param('controller/safety/I_steer')
        self.D_steer = rospy.get_param('controller/safety/D_steer')

        self.L_r = rospy.get_param('controller/dynamics/L_r')
        self.L_f = rospy.get_param('controller/dynamics/L_f')
        self.M = rospy.get_param('controller/dynamics/M')

        self.EV_L = rospy.get_param('car/plot/L')
        self.EV_W = rospy.get_param('car/plot/W')
        self.TV_L = rospy.get_param('/target_vehicle/car/plot/L')
        self.TV_W = rospy.get_param('/target_vehicle/car/plot/W')

        dyn_params = dynamicsKinBikeParams(dt=self.dt,
                                           L_r=self.L_r,
                                           L_f=self.L_f,
                                           M=self.M)
        self.dynamics = bike_dynamics_rk4(dyn_params)

        G = np.array([[1, 0], [-1, 0], [0, 1], [0, -1]])
        g = np.array(
            [self.EV_L / 2, self.EV_L / 2, self.EV_W / 2, self.EV_W / 2])
        obca_params = strategyOBCAParams(
            dt=self.dt,
            N=self.N,
            n=self.n_x,
            d=self.n_u,
            n_obs=self.n_obs,
            n_ineq=self.n_ineq,
            d_ineq=self.d_ineq,
            G=G,
            g=g,
            Q=self.Q,
            R=self.R,
            d_min=self.d_min,
            u_l=np.array([self.steer_min, self.accel_min]),
            u_u=np.array([self.steer_max, self.accel_max]),
            du_l=np.array([self.dsteer_min, self.daccel_min]),
            du_u=np.array([self.dsteer_max, self.daccel_max]),
            optlevel=self.optlevel)
        self.obca_controller = StrategyOBCAController(self.dynamics,
                                                      obca_params)
        self.obca_controller.initialize(regen=False)

        safety_params = safetyParams(dt=self.dt,
                                     P_accel=self.P_accel,
                                     I_accel=self.I_accel,
                                     D_accel=self.D_accel,
                                     P_steer=self.P_steer,
                                     I_steer=self.I_steer,
                                     D_steer=self.D_steer,
                                     accel_max=self.accel_max,
                                     accel_min=self.accel_min,
                                     daccel_max=self.daccel_max,
                                     daccel_min=self.daccel_min,
                                     steer_max=self.steer_max,
                                     steer_min=self.steer_min,
                                     dsteer_max=self.dsteer_max,
                                     dsteer_min=self.dsteer_min)
        self.safety_controller = safetyController(safety_params)

        emergency_params = safetyParams(dt=self.dt,
                                        P_accel=self.P_accel,
                                        I_accel=self.I_accel,
                                        D_accel=self.D_accel,
                                        P_steer=self.P_steer,
                                        I_steer=self.I_steer,
                                        D_steer=self.D_steer,
                                        accel_max=self.accel_max,
                                        accel_min=self.accel_min,
                                        daccel_max=self.daccel_max,
                                        daccel_min=self.daccel_min,
                                        steer_max=self.steer_max,
                                        steer_min=self.steer_min,
                                        dsteer_max=self.dsteer_max,
                                        dsteer_min=self.dsteer_min)
        self.emergency_controller = emergencyController(emergency_params)

        strategy_params = strategyPredictorParams(
            nn_model_file=self.nn_model_file,
            smooth_prediction=self.smooth_prediction,
            W_kf=self.W_kf,
            V_kf=self.V_kf,
            Pm_kf=self.Pm_kf,
            A_kf=self.A_kf,
            H_kf=self.H_kf)
        self.strategy_predictor = strategyPredictor(strategy_params)

        collision_buffer_r = np.sqrt(self.EV_L**2 + self.EV_W**2)
        a_lim = np.array([self.accel_min, self.accel_max])
        exp_params = experimentParams(dt=self.dt,
                                      car_L=self.EV_L,
                                      car_W=self.EV_W,
                                      N=self.N,
                                      collision_buffer_r=collision_buffer_r,
                                      T=self.max_time,
                                      T_tv=self.T_tv,
                                      lock_steps=self.lock_steps,
                                      a_lim=a_lim)
        self.constraint_generator = hyperplaneConstraintGenerator(exp_params)
        self.state_machine = stateMachine(exp_params)

        self.obs = [[] for _ in range(self.n_obs)]
        for i in range(self.N + 1):
            # Add lane constraints to end of obstacle list
            self.obs[-2].append({
                'A': np.array([0, -1]).reshape((-1, 1)),
                'b': np.array([-self.lanewidth / 2])
            })
            self.obs[-1].append({
                'A': np.array([0, 1]).reshape((-1, 1)),
                'b': np.array([-self.lanewidth / 2])
            })

        self.state = np.zeros(self.n_x)
        self.last_state = np.zeros(self.n_x)
        self.tv_state_prediction = np.zeros((self.N + 1, self.n_x))
        self.input = np.zeros(self.n_u)
        self.last_input = np.zeros(self.n_u)
        self.ev_state_prediction = None
        self.ev_input_prediction = None

        rospy.Subscriber('est_states',
                         States,
                         self.estimator_callback,
                         queue_size=1)
        rospy.Subscriber('/target_vehicle/pred_states',
                         Prediction,
                         self.prediction_callback,
                         queue_size=1)

        # Publisher for steering and motor control
        self.ecu_pub = rospy.Publisher('ecu', ECU, queue_size=1)
        # Publisher for mpc prediction
        self.pred_pub = rospy.Publisher('pred_states',
                                        Prediction,
                                        queue_size=1)
        # Publisher for FSM state
        self.fsm_state_pub = rospy.Publisher('fsm_state',
                                             FSMState,
                                             queue_size=1)
        # Publisher for FSM state
        self.score_pub = rospy.Publisher('strategy_scores',
                                         Scores,
                                         queue_size=1)
        # Publisher for data logger
        # self.log_pub = rospy.Publisher('log_states', States, queue_size=1)

        # Create bond to shutdown data logger and arduino interface when controller stops
        bond_id = rospy.get_param('car/name')
        self.bond_log = bondpy.Bond('controller_logger', bond_id)
        self.bond_ard = bondpy.Bond('controller_arduino', bond_id)

        self.start_time = 0
        self.task_finished = False

        self.rate = rospy.Rate(1.0 / self.dt)
Ejemplo n.º 4
0
    def __init__(self):
        # Read parameter values from ROS parameter server
        rospy.init_node('naive_obca_param_control')

        self.dt = rospy.get_param('controller/dt')
        self.init_time = rospy.get_param('controller/init_time')
        self.max_time = rospy.get_param('controller/max_time')
        self.accel_max = rospy.get_param('controller/accel_max')
        self.accel_min = rospy.get_param('controller/accel_min')
        self.steer_max = rospy.get_param('controller/steer_max')
        self.steer_min = rospy.get_param('controller/steer_min')
        self.daccel_max = rospy.get_param('controller/daccel_max')
        self.daccel_min = rospy.get_param('controller/daccel_min')
        self.dsteer_max = rospy.get_param('controller/dsteer_max')
        self.dsteer_min = rospy.get_param('controller/dsteer_min')
        self.lanewidth = rospy.get_param('controller/lanewidth')

        self.v_ref = rospy.get_param('controller/obca/v_ref')
        self.n_x = rospy.get_param('controller/obca/n')
        self.n_u = rospy.get_param('controller/obca/d')
        self.N = rospy.get_param('controller/obca/N')
        self.n_obs = rospy.get_param('controller/obca/n_obs')
        self.n_ineq = rospy.get_param('controller/obca/n_ineq')
        self.d_ineq = rospy.get_param('controller/obca/d_ineq')
        self.d_min = rospy.get_param('controller/obca/d_min')
        self.Q = rospy.get_param('controller/obca/Q')
        self.R = rospy.get_param('controller/obca/R')
        self.R_d = rospy.get_param('controller/obca/R_d')
        self.optlevel = rospy.get_param('controller/obca/optlevel')

        self.P_accel = rospy.get_param('controller/safety/P_accel')
        self.I_accel = rospy.get_param('controller/safety/I_accel')
        self.D_accel = rospy.get_param('controller/safety/D_accel')
        self.P_steer = rospy.get_param('controller/safety/P_steer')
        self.I_steer = rospy.get_param('controller/safety/I_steer')
        self.D_steer = rospy.get_param('controller/safety/D_steer')
        self.deadband_accel = rospy.get_param(
            'controller/safety/deadband_accel')
        self.deadband_steer = rospy.get_param(
            'controller/safety/deadband_steer')

        self.L_r = rospy.get_param('controller/dynamics/L_r')
        self.L_f = rospy.get_param('controller/dynamics/L_f')
        self.M = rospy.get_param('controller/dynamics/M')

        self.x_goal = rospy.get_param('controller/x_goal')
        self.y_goal = rospy.get_param('controller/y_goal')

        self.EV_L = rospy.get_param('car/plot/L')
        self.EV_W = rospy.get_param('car/plot/W')
        self.TV_L = rospy.get_param('/target_vehicle/car/plot/L')
        self.TV_W = rospy.get_param('/target_vehicle/car/plot/W')
        self.x_boundary = rospy.get_param('/track/x_boundary')
        self.y_boundary = rospy.get_param('/track/y_boundary')

        dyn_params = dynamicsKinBikeParams(dt=self.dt,
                                           L_r=self.L_r,
                                           L_f=self.L_f,
                                           M=self.M)
        self.dynamics = bike_dynamics_rk4(dyn_params)

        G = np.array([[1, 0], [-1, 0], [0, 1], [0, -1]])
        g = np.array(
            [self.EV_L / 2, self.EV_L / 2, self.EV_W / 2, self.EV_W / 2])
        obca_params = strategyOBCAParams(
            dt=self.dt,
            N=self.N,
            n=self.n_x,
            d=self.n_u,
            n_obs=self.n_obs,
            n_ineq=self.n_ineq,
            d_ineq=self.d_ineq,
            G=G,
            g=g,
            Q=self.Q,
            R=self.R,
            R_d=self.R_d,
            d_min=self.d_min,
            u_l=np.array([self.steer_min, self.accel_min]),
            u_u=np.array([self.steer_max, self.accel_max]),
            du_l=np.array([self.dsteer_min, self.daccel_min]),
            du_u=np.array([self.dsteer_max, self.daccel_max]),
            optlevel=self.optlevel)
        self.obca_controller = NaiveOBCAParameterizedController(
            self.dynamics, obca_params)
        self.obca_controller.initialize(regen=False)

        safety_params = safetyParams(dt=self.dt,
                                     P_accel=self.P_accel,
                                     I_accel=self.I_accel,
                                     D_accel=self.D_accel,
                                     P_steer=self.P_steer,
                                     I_steer=self.I_steer,
                                     D_steer=self.D_steer,
                                     accel_max=self.accel_max,
                                     accel_min=self.accel_min,
                                     daccel_max=self.daccel_max,
                                     daccel_min=self.daccel_min,
                                     steer_max=self.steer_max,
                                     steer_min=self.steer_min,
                                     dsteer_max=self.dsteer_max,
                                     dsteer_min=self.dsteer_min,
                                     deadband_accel=self.deadband_accel,
                                     deadband_steer=self.deadband_steer)
        self.safety_controller = safetyController(safety_params)

        emergency_params = safetyParams(dt=self.dt,
                                        P_accel=self.P_accel,
                                        I_accel=self.I_accel,
                                        D_accel=self.D_accel,
                                        P_steer=self.P_steer,
                                        I_steer=self.I_steer,
                                        D_steer=self.D_steer,
                                        accel_max=self.accel_max,
                                        accel_min=self.accel_min,
                                        daccel_max=self.daccel_max,
                                        daccel_min=self.daccel_min,
                                        steer_max=self.steer_max,
                                        steer_min=self.steer_min,
                                        dsteer_max=self.dsteer_max,
                                        dsteer_min=self.dsteer_min,
                                        deadband_accel=self.deadband_accel,
                                        deadband_steer=self.deadband_steer)
        self.emergency_controller = emergencyController(emergency_params)

        self.obs = [[] for _ in range(self.n_obs)]
        for i in range(self.N + 1):
            # Add lane constraints to end of obstacle list
            self.obs[-2].append({
                'A': np.array([0, -1]).reshape((-1, 1)),
                'b': np.array([-self.lanewidth / 2])
            })
            self.obs[-1].append({
                'A': np.array([0, 1]).reshape((-1, 1)),
                'b': np.array([-self.lanewidth / 2])
            })

        self.state = np.zeros(self.n_x)
        self.last_state = np.zeros(self.n_x)
        self.tv_state_prediction = np.zeros((self.N + 1, self.n_x))
        self.input = np.zeros(self.n_u)
        self.last_input = np.zeros(self.n_u)
        self.ev_state_prediction = None
        self.ev_input_prediction = None

        rospy.Subscriber('est_states',
                         States,
                         self.estimator_callback,
                         queue_size=1)
        rospy.Subscriber('/target_vehicle/pred_states',
                         Prediction,
                         self.prediction_callback,
                         queue_size=1)

        # Publisher for steering and motor control
        self.ecu_pub = rospy.Publisher('ecu', ECU, queue_size=1)
        # Publisher for mpc prediction
        self.pred_pub = rospy.Publisher('pred_states',
                                        Prediction,
                                        queue_size=1)
        # Publisher for FSM state
        self.fsm_state_pub = rospy.Publisher('fsm_state',
                                             FSMState,
                                             queue_size=1)
        # Publisher for data logger
        # self.log_pub = rospy.Publisher('log_states', States, queue_size=1)

        # Create bond to shutdown data logger and arduino interface when controller stops
        bond_id = rospy.get_param('car/name')
        self.bond_log = bondpy.Bond('controller_logger', bond_id)
        self.bond_ard = bondpy.Bond('controller_arduino', bond_id)

        self.start_time = None
        self.task_finished = False
        self.task_start = False

        self.rate = rospy.Rate(1.0 / self.dt)
    def __init__(self):
        # Read parameter values from ROS parameter server
        rospy.init_node('safety_control')

        ns = rospy.get_namespace()
        vehicle_ns = '/'.join(ns.split('/')[:-1])

        self.dt = rospy.get_param('controller/dt')
        self.init_time = rospy.get_param('controller/init_time')
        self.max_time = rospy.get_param('controller/max_time')
        self.x_max = rospy.get_param('controller/x_max')
        self.x_min = rospy.get_param('controller/x_min')
        self.y_max = rospy.get_param('controller/y_max')
        self.y_min = rospy.get_param('controller/y_min')
        self.heading_max = rospy.get_param('controller/heading_max')
        self.heading_min = rospy.get_param('controller/heading_min')
        self.v_max = rospy.get_param('controller/v_max')
        self.v_min = rospy.get_param('controller/v_min')
        self.accel_max = rospy.get_param('controller/accel_max')
        self.accel_min = rospy.get_param('controller/accel_min')
        self.steer_max = rospy.get_param('controller/steer_max')
        self.steer_min = rospy.get_param('controller/steer_min')
        self.daccel_max = rospy.get_param('controller/daccel_max')
        self.daccel_min = rospy.get_param('controller/daccel_min')
        self.dsteer_max = rospy.get_param('controller/dsteer_max')
        self.dsteer_min = rospy.get_param('controller/dsteer_min')
        self.lanewidth = rospy.get_param('controller/lanewidth')
        self.a_lim = np.array([self.accel_min, self.accel_max])

        self.v_ref = rospy.get_param('controller/safety/v_ref')
        self.n_x = rospy.get_param('controller/safety/n')
        self.n_u = rospy.get_param('controller/safety/d')
        self.N = rospy.get_param('controller/safety/N')

        self.P_accel = rospy.get_param('controller/safety/P_accel')
        self.I_accel = rospy.get_param('controller/safety/I_accel')
        self.D_accel = rospy.get_param('controller/safety/D_accel')
        self.P_steer = rospy.get_param('controller/safety/P_steer')
        self.I_steer = rospy.get_param('controller/safety/I_steer')
        self.D_steer = rospy.get_param('controller/safety/D_steer')
        self.deadband_accel = rospy.get_param(
            'controller/safety/deadband_accel')
        self.deadband_steer = rospy.get_param(
            'controller/safety/deadband_steer')

        self.L_r = rospy.get_param('controller/dynamics/L_r')
        self.L_f = rospy.get_param('controller/dynamics/L_f')
        self.M = rospy.get_param('controller/dynamics/M')

        self.EV_L = rospy.get_param('car/plot/L')
        self.EV_W = rospy.get_param('car/plot/W')
        self.collision_buffer_r = np.sqrt(self.EV_L**2 + self.EV_W**2)

        x_init = rospy.get_param('car/car_init/x', 0.0)
        y_init = rospy.get_param('car/car_init/y', 0.0)
        heading_init = rospy.get_param('car/car_init/heading', 0.0)
        if type(heading_init) is str:
            heading_init = eval(heading_init)
        v_init = rospy.get_param('car/car_init/v', 0.0)

        self.x_boundary = rospy.get_param('/track/x_boundary')
        self.y_boundary = rospy.get_param('/track/y_boundary')

        dyn_params = dynamicsKinBikeParams(dt=self.dt,
                                           L_r=self.L_r,
                                           L_f=self.L_f,
                                           M=self.M)
        self.dynamics = bike_dynamics_rk4(dyn_params)

        safety_params = safetyParams(dt=self.dt,
                                     P_accel=self.P_accel,
                                     I_accel=self.I_accel,
                                     D_accel=self.D_accel,
                                     P_steer=self.P_steer,
                                     I_steer=self.I_steer,
                                     D_steer=self.D_steer,
                                     accel_max=self.accel_max,
                                     accel_min=self.accel_min,
                                     daccel_max=self.daccel_max,
                                     daccel_min=self.daccel_min,
                                     steer_max=self.steer_max,
                                     steer_min=self.steer_min,
                                     dsteer_max=self.dsteer_max,
                                     dsteer_min=self.dsteer_min,
                                     deadband_accel=self.deadband_accel,
                                     deadband_steer=self.deadband_steer)
        self.safety_controller = safetyController(safety_params)

        self.state = np.zeros(self.n_x)
        self.last_state = np.zeros(self.n_x)
        self.input = np.zeros(self.n_u)
        self.last_input = np.zeros(self.n_u)
        self.state_prediction = None
        self.input_prediction = None

        rospy.Subscriber('est_states',
                         States,
                         self.estimator_callback,
                         queue_size=1)
        rospy.Subscriber('/target_vehicle/pred_states',
                         Prediction,
                         self.prediction_callback,
                         queue_size=1)

        # Publisher for steering and motor control
        self.ecu_pub = rospy.Publisher('ecu', ECU, queue_size=1)
        # Publisher for tracking controller node start time (used as reference start time)
        self.start_pub = rospy.Publisher('/start_time',
                                         Float64,
                                         queue_size=1,
                                         latch=True)
        # Publisher for data logger
        # self.log_pub = rospy.Publisher('log_states', States, queue_size=1)

        # Create bond to shutdown data logger and arduino interface when controller stops
        bond_id = rospy.get_param('car/name')
        self.bond_log = bondpy.Bond('controller_logger', bond_id)
        self.bond_ard = bondpy.Bond('controller_arduino', bond_id)

        self.start_time = None
        self.task_finish = False
        self.task_start = False

        self.rate = rospy.Rate(1.0 / self.dt)
Ejemplo n.º 6
0
    def __init__(self):
        # Read parameter values from ROS parameter server
        rospy.init_node('tracking_control')

        ns = rospy.get_namespace()
        vehicle_ns = '/'.join(ns.split('/')[:-1])

        self.dt = rospy.get_param('controller/dt')
        self.init_time = rospy.get_param('controller/init_time')
        self.max_time = rospy.get_param('controller/max_time')
        self.x_max = rospy.get_param('controller/x_max')
        self.x_min = rospy.get_param('controller/x_min')
        self.y_max = rospy.get_param('controller/y_max')
        self.y_min = rospy.get_param('controller/y_min')
        self.heading_max = rospy.get_param('controller/heading_max')
        self.heading_min = rospy.get_param('controller/heading_min')
        self.v_max = rospy.get_param('controller/v_max')
        self.v_min = rospy.get_param('controller/v_min')
        self.accel_max = rospy.get_param('controller/accel_max')
        self.accel_min = rospy.get_param('controller/accel_min')
        self.steer_max = rospy.get_param('controller/steer_max')
        self.steer_min = rospy.get_param('controller/steer_min')
        self.daccel_max = rospy.get_param('controller/daccel_max')
        self.daccel_min = rospy.get_param('controller/daccel_min')
        self.dsteer_max = rospy.get_param('controller/dsteer_max')
        self.dsteer_min = rospy.get_param('controller/dsteer_min')
        self.lanewidth = rospy.get_param('controller/lanewidth')
        trajectory_file = rospy.get_param('controller/trajectory_file', None)
        x_scaling = rospy.get_param('controller/x_scaling', 1.0)
        y_scaling = rospy.get_param('controller/y_scaling', 1.0)
        v_scaling = rospy.get_param('controller/v_scaling', 1.0)

        self.n_x = rospy.get_param('controller/tracking/n')
        self.n_u = rospy.get_param('controller/tracking/d')
        self.N = rospy.get_param('controller/tracking/N')
        self.Q = rospy.get_param('controller/tracking/Q')
        self.R = rospy.get_param('controller/tracking/R')
        self.R_d = rospy.get_param('controller/tracking/R_d')
        self.optlevel = rospy.get_param('controller/tracking/optlevel')

        self.L_r = rospy.get_param('controller/dynamics/L_r')
        self.L_f = rospy.get_param('controller/dynamics/L_f')
        self.M = rospy.get_param('controller/dynamics/M')

        self.EV_L = rospy.get_param('car/plot/L')
        self.EV_W = rospy.get_param('car/plot/W')

        x_init = rospy.get_param('car/car_init/x', 0.0)
        y_init = rospy.get_param('car/car_init/y', 0.0)
        heading_init = rospy.get_param('car/car_init/heading', 0.0)
        if type(heading_init) is str:
            heading_init = eval(heading_init)
        v_init = rospy.get_param('car/car_init/v', 0.0)

        self.x_boundary = rospy.get_param('/track/x_boundary')
        self.y_boundary = rospy.get_param('/track/y_boundary')

        if trajectory_file is not None:
            # Load reference trajectory from file and set initial conditions
            traj = load_vehicle_trajectory(trajectory_file)
            traj -= np.array([traj[0,0], traj[0,1], 0, 0])
            traj = np.multiply(traj, np.array([x_scaling, y_scaling, 1, v_scaling]))
            traj += np.array([x_init, y_init, 0, 0])
            self.trajectory = traj

            self.traj_len = self.trajectory.shape[0]
            rospy.set_param('/'.join((vehicle_ns,'car/car_init/x')), float(self.trajectory[0,0]))
            rospy.set_param('/'.join((vehicle_ns,'car/car_init/y')), float(self.trajectory[0,1]))
            rospy.set_param('/'.join((vehicle_ns,'car/car_init/z')), 0.0)
            rospy.set_param('/'.join((vehicle_ns,'car/car_init/heading')), float(self.trajectory[0,2]))
            rospy.set_param('/'.join((vehicle_ns,'car/car_init/v')), 0.0)

            rospy.loginfo('Initial state has been set to - X: %g, Y: %g, heading: %g, v: %g' %
                (float(self.trajectory[0,0]), float(self.trajectory[0,1]), float(self.trajectory[0,2]), 0.0))
        else:
            # Generate simple reference trajectory
            self.traj_len = rospy.get_param('controller/tracking/T')
            v_ref = rospy.get_param('controller/tracking/v_ref')
            y_ref = rospy.get_param('controller/tracking/y_ref')
            x_init = rospy.get_param('car/car_init/x')
            y_init = rospy.get_param('car/car_init/y')
            heading_init = rospy.get_param('car/car_init/heading')
            if type(heading_init) is str:
                heading_init = eval(heading_init)

            # x_ref = x_init + v_ref*np.cos(heading_init)*np.arange(0, self.traj_len*self.dt, self.dt)
            x_ref = np.zeros(self.traj_len)
            y_ref = y_ref*np.ones(self.traj_len)
            heading_ref = heading_init*np.ones(self.traj_len)
            v_ref = v_ref*np.ones(self.traj_len)

            self.trajectory = np.vstack((x_ref, y_ref, heading_ref, v_ref)).T

        # Find trajectory key points (i.e. times where the reference velocity is close to zero)
        # self.waypoints, self.next_ref_start = get_trajectory_waypoints(self.trajectory, 20, 0.1)
        self.waypoints = self.trajectory[:2,-1].reshape((1,-1))
        self.next_ref_start = [np.inf]

        dyn_params = dynamicsKinBikeParams(dt=self.dt, L_r=self.L_r, L_f=self.L_f, M=self.M)
        self.dynamics = bike_dynamics_rk4(dyn_params)

        tracking_params = trackingParams(dt=self.dt, N=self.N, n=self.n_x, d=self.n_u,
            Q=self.Q, R=self.R, R_d=self.R_d,
            z_l=np.array([self.x_min, self.y_min, self.heading_min, self.v_min]), z_u=np.array([self.x_max, self.y_max, self.heading_max, self.v_max]),
            u_l=np.array([self.steer_min,self.accel_min]), u_u=np.array([self.steer_max,self.accel_max]),
            du_l=np.array([self.dsteer_min,self.daccel_min]), du_u=np.array([self.dsteer_max,self.daccel_max]),
            optlevel=self.optlevel)
        self.tracking_controller = trackingController(self.dynamics, tracking_params)
        self.tracking_controller.initialize()

        self.state = np.zeros(self.n_x)
        self.last_state = np.zeros(self.n_x)
        self.input = np.zeros(self.n_u)
        self.last_input = np.zeros(self.n_u)
        self.state_prediction = None
        self.input_prediction = None

        rospy.Subscriber('est_states', States, self.estimator_callback, queue_size=1)

        # Publisher for steering and motor control
        self.ecu_pub = rospy.Publisher('ecu', ECU, queue_size=1)
        # Publisher for state predictions
        self.pred_pub = rospy.Publisher('pred_states', Prediction, queue_size=1)
        # Publisher for tracking controller node start time (used as reference start time)
        self.start_pub = rospy.Publisher('/start_time', Float64, queue_size=1, latch=True)
        # Publisher for data logger
        # self.log_pub = rospy.Publisher('log_states', States, queue_size=1)

        # Create bond to shutdown data logger and arduino interface when controller stops
        bond_id = rospy.get_param('car/name')
        self.bond_log = bondpy.Bond('controller_logger', bond_id)
        self.bond_ard = bondpy.Bond('controller_arduino', bond_id)

        self.start_time = None
        self.task_finish = False
        self.task_start = False
        self.wait = False

        self.rate = rospy.Rate(1.0/self.dt)