def __init__(self, dynamics, params=strategyOBCAParams()): self.dynamics = dynamics self.dt = params.dt self.n_x = params.n self.n_u = params.d self.N = params.N self.n_obs = params.n_obs self.n_ineq = params.n_ineq # Number of constraints for each obstacle self.d_ineq = params.d_ineq # Dimension of constraints for all obstacles self.G = params.G self.g = params.g self.m_ineq = self.G.shape[0] # Number of constraints for controlled object self.Q = params.Q self.R = params.R self.d_min = params.d_min self.u_l = params.u_l self.u_u = params.u_u self.du_l = params.du_l self.du_u = params.du_u self.N_ineq = np.sum(self.n_ineq) self.M_ineq = self.n_obs * self.m_ineq self.optlevel = params.optlevel
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)