def __init__(self, params=safetyParams()):
self.accel_pid_params = PIDParams(dt=params.dt,
Kp=params.P_accel, Ki=params.I_accel, Kd=params.D_accel,
int_e_max=params.accel_int_e_max, int_e_min=params.accel_int_e_min,
u_max=params.accel_max, u_min=params.accel_min,
du_max=params.daccel_max, du_min=params.daccel_min)
self.steer_pid_params = PIDParams(dt=params.dt,
Kp=params.P_steer, Ki=params.I_steer, Kd=params.D_steer,
int_e_max=params.steer_int_e_max, int_e_min=params.steer_int_e_min,
u_max=params.steer_max, u_min=params.steer_min,
du_max=params.dsteer_max, du_min=params.dsteer_min)
self.accel_ref = 0
self.steer_ref = 0
self.accel_pid = PID(self.accel_pid_params)
self.accel_pid.initialize(x_ref=self.accel_ref)
self.steer_pid = PID(self.steer_pid_params)
self.steer_pid.initialize(x_ref=self.steer_ref)
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)
accel, _ = self.accel_pid.solve(v, last_accel)
if v < 0:
steer, _ = self.steer_pid.solve(y-10*heading, last_steer)
else:
steer = 0
u = [steer, accel]
return u
def set_accel_ref(self, accel, accel_ref):
self.accel_ref = accel_ref
self.accel_pid.set_x_ref(accel, accel_ref)
def set_steer_ref(self, steer, steer_ref):
self.steer_ref = steer_ref
self.steer_pid.set_x_ref(steer, steer_ref)
if __name__ == '__main__':
import numpy as np
safety_params = safetyParams()
safety_control = safetyController(safetyParams)
z_ev = np.random.rand(4)
Z_tv = np.random.rand(20,4)
last_u = np.random.rand(2)
u = safety_control.solve(z_ev, Z_tv, last_u)
print(u)
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)