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)