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