def inputToPWM():
# initialize node
rospy.init_node('inputToPWM', anonymous=True)
global pubname , newECU , subname, move , still_moving
newECU = ECU()
newECU.motor = 1500
newECU.servo = 1550
move = False
still_moving = False
#print("1")
#print(move)
# topic subscriptions / publications
pubname = rospy.Publisher('ecu_pwm',ECU, queue_size = 2)
rospy.Subscriber('turtle1/cmd_vel', Twist, start_callback)
subname = rospy.Subscriber('uOpt', Input, callback_function)
rospy.Subscriber('moving', Moving, moving_callback_function)
# set node rate
loop_rate = 40
ts = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
t0 = time.time()
rospy.spin()
def inputToPWM():
# initialize node
rospy.init_node('inputToPWM', anonymous=True)
global pubname, newECU, subname, move, still_moving
newECU = ECU()
newECU.motor = 1500
newECU.servo = 1550
move = False
still_moving = False
#print("1")
#print(move)
# topic subscriptions / publications
pubname = rospy.Publisher('ecu_pwm', ECU, queue_size=2)
rospy.Subscriber('turtle1/cmd_vel', Twist, start_callback)
rospy.Subscriber('encoder', Encoder, enc_callback) # Line Added
subname = rospy.Subscriber('uOpt', Input, callback_function)
rospy.Subscriber('moving', Moving, moving_callback_function)
# set node rate
loop_rate = 40
ts = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
t0 = time.time()
while not rospy.is_shutdown():
# calculate acceleration from PID controller.
motor_pwm = PID_control.acc_calculate(v_ref, v_meas) + motor_pwm_offset
# publish control command
ecu_pub.publish(ECU(motor_pwm, servo_pwm))
# wait
rate.sleep()
def inputToPWM():
global motor_pwm, servo_pwm, motor_pwm_offset, servo_pwm_offset
global v_ref, v_meas
# initialize node
rospy.init_node('inputToPWM', anonymous=True)
global pubname, newECU, subname, move, still_moving
newECU = ECU()
newECU.motor = 1500
newECU.servo = 1550
move = False
still_moving = False
#print("1")
#print(move)
# topic subscriptions / publications
pubname = rospy.Publisher('ecu_pwm', ECU, queue_size=2)
rospy.Subscriber('turtle1/cmd_vel', Twist, start_callback)
subname = rospy.Subscriber('uOpt', Input, callback_function)
rospy.Subscriber('moving', Moving, moving_callback_function)
rospy.Subscriber('hold_previous_turn', Bool, hold_turn_function)
rospy.Subscriber('encoder', Encoder, enc_callback)
# set node rate
loop_rate = 40
ts = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
t0 = time.time()
# Initialize the PID controller
longitudinal_control = PID(kp=30, ki=3, kd=0)
maxspeed = 1620
minspeed = 1400
while not rospy.is_shutdown():
try:
# acceleration calculated from PID controller
motor_pwm = longitudinal_control.acc_calculate(
v_ref, v_meas) + motor_pwm_offset
if (motor_pwm < minspeed):
motor_pwm = minspeed
elif (motor_pwm > maxspeed):
motor_pwm = maxspeed
if (move == False):
motor_pwm = 1500
servo_pwm = 1550
# publish information
pubname.publish(ECU(motor_pwm, servo_pwm))
# wait
rate.sleep()
except:
pass
def move(current):
msg = ECU()
msg.motor = 1500
msg.servo = 1500
if 'a' in current:
msg.servo -= 300
elif 'd' in current:
msg.servo += 300
if 'w' in current:
msg.motor += 100
elif 's' in current:
msg.motor -= 200
return msg
def image_node():
global error
error = 0
param_names = [
'lowerY', 'upperY', 'display_image', 'display_processed_image',
'debug_info', 'publish_processed_image'
]
params = {}
for param in param_names:
params[param] = rospy.get_param(param)
print(params)
rospy.init_node('Test_Imager')
img_pub = rospy.Publisher('/processed_image', Image, queue_size=1)
#params['img_pub'] = img_pub
rospy.Subscriber('/image_raw/compressed', CompressedImage, image_process,
params)
pub = rospy.Publisher('ecu_pwm', ECU, queue_size=10)
rate = rospy.Rate(10)
K = 400
Ki = 20
integral = 0
while not rospy.is_shutdown():
msg = ECU()
# if params['debug_info']:
# print(error)
msg.motor = 1590
msg.servo = 1500 + K * error # Ki*integral
pub.publish(msg)
integral += (1 / 10) * error
if integral > 20:
integral = 20
if integral < -20:
integral = -20
rate.sleep()
def key_node():
pub = rospy.Publisher('ecu_pwm', ECU, queue_size=10)
rospy.init_node('key_node')
rate = rospy.Rate(10)
print('Use aswd to navigate the BARC, use q to stop')
while not rospy.is_shutdown():
char = getch.getch()
msg = ECU()
if char == 'a':
msg.motor = 1600
msg.servo = 1200
pub.publish(msg)
elif char == 'e':
msg.motor = 1300
msg.servo = 1800
pub.publish(msg)
elif char == 'q':
msg.motor = 1300
msg.servo = 1200
pub.publish(msg)
elif char == 's':
msg.motor = 1300
msg.servo = 1500
pub.publish(msg)
elif char == 'w':
msg.motor = 1600
msg.servo = 1500
pub.publish(msg)
elif char == 'd':
msg.motor = 1600
msg.servo = 1800
pub.publish(msg)
elif char == 'r':
msg.motor = 1600
msg.servo = 1800
pub.publish(msg)
elif char == 'f':
exit(0)
def spin(self):
rospy.sleep(self.init_time)
self.start_time = rospy.get_rostime().to_sec()
while not rospy.is_shutdown():
t = rospy.get_rostime().to_sec() - self.start_time
ecu_msg = ECU()
if t >= self.max_time:
end_msg = 'Max time of %g reached, controller shutting down...' % self.max_time
self.task_finished = True
elif self.state_machine.state == 'End':
end_msg = 'Task finished, controller shutting down...'
self.task_finished = True
if self.task_finished:
ecu_msg.servo = 0.0
ecu_msg.motor = 0.0
# Publish the final motor and steering commands
self.ecu_pub.publish(ecu_msg)
self.bond_log.break_bond()
self.bond_ard.break_bond()
rospy.loginfo(end_msg)
rospy.signal_shutdown(end_msg)
EV_state = self.state
TV_pred = self.tv_state_prediction[:self.N + 1]
EV_x, EV_y, EV_heading, EV_v = EV_state
TV_x, TV_y, TV_heading, TV_v = TV_pred[0]
# Generate target vehicle obstacle descriptions
self.obs[0] = get_car_poly(TV_pred, self.TV_W, self.TV_L)
# Get target vehicle trajectory relative to ego vehicle state
if self.state_machine.state in [
'Safe-Confidence', 'Safe-Yield', 'Safe-Infeasible'
] or EV_v * np.cos(EV_heading) <= 0:
rel_state = np.vstack((TV_pred[:,0]-EV_x, \
TV_pred[:,1]-EV_y, \
TV_pred[:,2]-EV_heading, \
np.multiply(TV_pred[:,3], np.cos(TV_pred[:,2]))-0, \
np.multiply(TV_pred[:,3], np.sin(TV_pred[:,2]))-EV_v*np.sin(EV_heading))).T
else:
rel_state = np.vstack((TV_pred[:,0]-EV_x, \
TV_pred[:,1]-EV_y, \
TV_pred[:,2]-EV_heading, \
np.multiply(TV_pred[:,3], np.cos(TV_pred[:,2]))-EV_v*np.cos(EV_heading), \
np.multiply(TV_pred[:,3], np.sin(TV_pred[:,2]))-EV_v*np.sin(EV_heading))).T
# Scale the relative state
# rel_state = scale_state(rel_state, rel_range, scale, bias)
# Predict strategy to use
scores = self.strategy_predictor.predict(
rel_state.flatten(order='C'))
exp_state = experimentStates(
t=t,
EV_curr=EV_state,
TV_pred=TV_pred,
score=scores,
ref_col=[False for _ in range(self.N + 1)])
self.state_machine.state_transition(exp_state)
if self.state_machine.state == 'End':
continue
# Generate reference trajectory
X_ref = EV_x + np.arange(self.N + 1) * self.dt * self.v_ref
Y_ref = np.zeros(self.N + 1)
Heading_ref = np.zeros(self.N + 1)
V_ref = self.v_ref * np.ones(self.N + 1)
Z_ref = np.vstack((X_ref, Y_ref, Heading_ref, V_ref)).T
# Check for collisions along reference trajecotry
Z_check = Z_ref
scale_mult = 1.0
scalings = np.maximum(np.ones(self.N + 1),
scale_mult * np.abs(TV_v) / self.v_ref)
collisions = self.constraint_generator.check_collision_points(
Z_check[:, :2], TV_pred, scalings)
exp_state.ref_col = collisions
self.state_machine.state_transition(exp_state)
# Generate hyperplane constraints
hyp = [{
'w':
np.array(
[np.sign(Z_check[i, 0]),
np.sign(Z_check[i, 1]), 0, 0]),
'b':
0,
'pos':
None
} for i in range(self.N + 1)]
for i in range(self.N + 1):
if collisions[i]:
if self.state_machine.strategy == 'Left':
d = np.array([0, 1])
elif self.state_machine.strategy == 'Right':
d = np.array([0, -1])
else:
d = np.array(
[EV_x - TV_pred[i, 0], EV_y - TV_pred[i, 1]])
d = d / la.norm(d)
hyp_xy, hyp_w, hyp_b, _ = self.constraint_generator.generate_constraint(
Z_check[i], TV_pred[i], d, scalings[i])
hyp[i] = {
'w': np.concatenate((hyp_w, np.zeros(2))),
'b': hyp_b,
'pos': hyp_xy
}
if self.ev_state_prediction is None:
Z_ws = Z_ref
U_ws = np.zeros((self.N, self.n_u))
else:
Z_ws = np.vstack(
(self.ev_state_prediction[1:],
self.dynamics.f_dt(self.ev_state_prediction[-1],
self.ev_input_prediction[-1],
type='numpy')))
U_ws = np.vstack((self.ev_input_prediction[1:],
self.ev_input_prediction[-1]))
status_ws = self.obca_controller.solve_ws(Z_ws, U_ws, self.obs)
if status_ws['success']:
rospy.loginfo('Warm start solved in %g s' %
status_ws['solve_time'])
Z_obca, U_obca, status_sol = self.obca_controller.solve(
EV_state, self.last_input, Z_ref, self.obs, hyp)
if status_ws['success'] and status_sol['success']:
feasible = True
collision = False
else:
feasible = False
rospy.loginfo(
'OBCA MPC not feasible, activating safety controller')
exp_state.feas = feasible
self.state_machine.state_transition(exp_state)
if self.state_machine.state in [
'Safe-Confidence', 'Safe-Yield', 'Safe-Infeasible'
]:
self.safety_controller.set_accel_ref(EV_v * np.cos(EV_heading),
TV_v * np.cos(TV_heading))
u_safe = self.safety_controller.solve(EV_state, TV_pred,
self.last_input)
z_next = self.dynamics.f_dt(EV_state, u_safe, type='numpy')
collision = check_collision_poly(z_next,
(self.EV_W, self.EV_L),
TV_pred[1],
(self.TV_W, self.TV_L))
exp_state.actual_collision = collision
self.state_machine.state_transition(exp_state)
if self.state_machine.state in [
'Free-Driving', 'HOBCA-Unlocked', 'HOBCA-Locked'
]:
Z_pred, U_pred = Z_obca, U_obca
rospy.loginfo('Applying HOBCA control')
elif self.state_machine.state in [
'Safe-Confidence', 'Safe-Yield', 'Safe-Infeasible'
]:
U_pred = np.vstack((u_safe, np.zeros((self.N - 1, self.n_u))))
Z_pred = np.vstack((EV_state, np.zeros((self.N, self.n_x))))
for i in range(self.N):
Z_pred[i + 1] = self.dynamics.f_dt(Z_pred[i],
U_pred[i],
type='numpy')
rospy.loginfo('Applying safety control')
elif self.state_machine.state in ['Emergency-Break']:
u_ebrake = self.emergency_controller.solve(
EV_state, TV_pred, self.last_input)
U_pred = np.vstack((u_ebrake, np.zeros(
(self.N - 1, self.n_u))))
Z_pred = np.vstack((EV_state, np.zeros((self.N, self.n_x))))
for i in range(self.N):
Z_pred[i + 1] = self.dynamics.f_dt(Z_pred[i],
U_pred[i],
type='numpy')
rospy.loginfo('Applying ebrake control')
else:
raise RuntimeError('State unrecognized')
self.ev_state_prediction = Z_pred
self.ev_input_prediction = U_pred
self.last_input = U_pred[0]
ecu_msg = ECU()
ecu_msg.servo = U_pred[0, 0]
ecu_msg.motor = U_pred[0, 1]
self.ecu_pub.publish(ecu_msg)
pred_msg = Prediction()
pred_msg.x = Z_pred[:, 0]
pred_msg.y = Z_pred[:, 1]
pred_msg.psi = Z_pred[:, 2]
pred_msg.v = Z_pred[:, 3]
pred_msg.df = U_pred[:, 0]
pred_msg.a = U_pred[:, 1]
self.pred_pub.publish(pred_msg)
score_msg = Scores()
score_msg.scores = scores
self.score_pub.publish(score_msg)
fsm_state_msg = FSMState()
fsm_state_msg.fsm_state = self.state_machine.state
self.fsm_state_pub.publish(fsm_state_msg)
self.rate.sleep()
def spin(self):
start_time_msg = rospy.wait_for_message('/start_time',
Float64,
timeout=5.0)
self.start_time = start_time_msg.data
rospy.sleep(0.5)
rospy.loginfo(
'============ STRATEGY: Node START, time %g ============' %
self.start_time)
while not rospy.is_shutdown():
t = rospy.get_rostime().to_sec() - self.start_time
time_stats_msg = TimeStats()
# Get EV state and TV prediction at current time
EV_state = self.state
TV_pred = self.tv_state_prediction[:self.N + 1]
EV_x, EV_y, EV_heading, EV_v = EV_state
TV_x, TV_y, TV_heading, TV_v = TV_pred[0]
ecu_msg = ECU()
ecu_msg.servo = 0.0
ecu_msg.motor = 0.0
if t >= self.init_time and not self.task_start:
self.task_start = True
rospy.loginfo(
'============ STRATEGY: Controler START ============')
if t >= self.max_time and not self.task_finish:
shutdown_msg = '============ STRATEGY: Max time of %g reached. Controler SHUTTING DOWN ============' % self.max_time
self.task_finish = True
elif self.state_machine.state == 'End' and not self.task_finish:
shutdown_msg = '============ STRATEGY: Controller reached END state. Controler SHUTTING DOWN ============'
self.task_finish = True
elif (EV_x < self.x_boundary[0] or EV_x > self.x_boundary[1]
) or (EV_y < self.y_boundary[0]
or EV_y > self.y_boundary[1]) and not self.task_finish:
# Check if car has left the experiment area
self.task_finish = True
shutdown_msg = '============ STRATEGY: Track bounds exceeded reached. Controler SHUTTING DOWN ============'
elif np.abs(EV_x - self.x_goal) <= 0.10 and not self.task_finish:
self.task_finish = True
shutdown_msg = '============ STRATEGY: Goal position reached. Controler SHUTTING DOWN ============'
if self.task_finish:
self.ecu_pub.publish(ecu_msg)
self.bond_log.break_bond()
self.bond_ard.break_bond()
rospy.loginfo(shutdown_msg)
rospy.signal_shutdown(shutdown_msg)
# Generate target vehicle obstacle descriptions
t_s = rospy.get_rostime().to_sec()
self.obs[0] = get_car_poly(TV_pred, self.TV_W, self.TV_L)
time_stats_msg.tv_obs_gen = rospy.get_rostime().to_sec() - t_s
# Get target vehicle trajectory relative to ego vehicle state
if self.state_machine.state in [
'Safe-Confidence', 'Safe-Yield', 'Safe-Infeasible'
] or EV_v * np.cos(EV_heading) <= 0:
rel_state = np.vstack((TV_pred[:,0]-EV_x, \
TV_pred[:,1]-EV_y, \
TV_pred[:,2]-EV_heading, \
np.multiply(TV_pred[:,3], np.cos(TV_pred[:,2]))-0, \
np.multiply(TV_pred[:,3], np.sin(TV_pred[:,2]))-EV_v*np.sin(EV_heading))).T
else:
rel_state = np.vstack((TV_pred[:,0]-EV_x, \
TV_pred[:,1]-EV_y, \
TV_pred[:,2]-EV_heading, \
np.multiply(TV_pred[:,3], np.cos(TV_pred[:,2]))-EV_v*np.cos(EV_heading), \
np.multiply(TV_pred[:,3], np.sin(TV_pred[:,2]))-EV_v*np.sin(EV_heading))).T
# Scale the relative state
# rel_state = scale_state(rel_state, rel_range, scale, bias)
# Predict strategy to use
t_s = rospy.get_rostime().to_sec()
scores = self.strategy_predictor.predict(
rel_state.flatten(order='C'))
time_stats_msg.strategy_pred = rospy.get_rostime().to_sec() - t_s
exp_state = experimentStates(
t=t,
EV_curr=EV_state,
TV_pred=TV_pred,
score=scores,
ref_col=[False for _ in range(self.N + 1)])
self.state_machine.state_transition(exp_state)
if self.state_machine.state == 'End':
continue
# Generate reference trajectory
X_ref = EV_x + np.arange(self.N + 1) * self.dt * self.v_ref
Y_ref = np.zeros(self.N + 1)
Heading_ref = np.zeros(self.N + 1)
V_ref = self.v_ref * np.ones(self.N + 1)
Z_ref = np.vstack((X_ref, Y_ref, Heading_ref, V_ref)).T
# Check for collisions along reference trajecotry
Z_check = Z_ref
scale_mult = 1.0
scalings = np.maximum(np.ones(self.N + 1),
scale_mult * np.abs(TV_v) / self.v_ref)
t_s = rospy.get_rostime().to_sec()
collisions = self.constraint_generator.check_collision_points(
Z_check[:, :2], TV_pred, scalings)
time_stats_msg.ref_collision_check = rospy.get_rostime().to_sec(
) - t_s
exp_state.ref_col = collisions
self.state_machine.state_transition(exp_state)
# Generate hyperplane constraints
t_s = rospy.get_rostime().to_sec()
hyp = [{
'w':
np.array(
[np.sign(Z_check[i, 0]),
np.sign(Z_check[i, 1]), 0, 0]),
'b':
0,
'pos':
None
} for i in range(self.N + 1)]
for i in range(self.N + 1):
if collisions[i]:
if self.state_machine.strategy == 'Left':
d = np.array([0, 1])
elif self.state_machine.strategy == 'Right':
d = np.array([0, -1])
else:
d = np.array(
[EV_x - TV_pred[i, 0], EV_y - TV_pred[i, 1]])
d = d / la.norm(d)
hyp_xy, hyp_w, hyp_b, _ = self.constraint_generator.generate_constraint(
Z_check[i], TV_pred[i], d, scalings[i])
hyp[i] = {
'w': np.concatenate((hyp_w, np.zeros(2))),
'b': hyp_b,
'pos': hyp_xy
}
time_stats_msg.hyperplane_gen = rospy.get_rostime().to_sec() - t_s
if self.ev_state_prediction is None:
Z_ws = Z_ref
U_ws = np.zeros((self.N, self.n_u))
else:
Z_ws = np.vstack(
(self.ev_state_prediction[1:],
self.dynamics.f_dt(self.ev_state_prediction[-1],
self.ev_input_prediction[-1],
type='numpy')))
U_ws = np.vstack((self.ev_input_prediction[1:],
self.ev_input_prediction[-1]))
t_s = rospy.get_rostime().to_sec()
status_ws = self.obca_controller.solve_ws(Z_ws, U_ws, self.obs)
time_stats_msg.warm_start_solve = rospy.get_rostime().to_sec(
) - t_s
if status_ws['success']:
# rospy.loginfo('Warm start solved in %g s' % status_ws['solve_time'])
t_s = rospy.get_rostime().to_sec()
Z_obca, U_obca, status_sol = self.obca_controller.solve(
EV_state, self.last_input, Z_ref, self.obs, hyp)
time_stats_msg.obca_solve = rospy.get_rostime().to_sec() - t_s
else:
time_stats_msg.obca_solve = -1
if status_ws['success'] and status_sol['success']:
feasible = True
collision = False
else:
feasible = False
rospy.loginfo(
'============ STRATEGY: OBCA MPC not feasible, activating safety controller ============'
)
exp_state.feas = feasible
self.state_machine.state_transition(exp_state)
if self.state_machine.state in [
'Safe-Confidence', 'Safe-Yield', 'Safe-Infeasible'
]:
t_s = rospy.get_rostime().to_sec()
self.safety_controller.set_accel_ref(EV_v * np.cos(EV_heading),
TV_v * np.cos(TV_heading))
u_safe = self.safety_controller.solve(EV_state, TV_pred,
self.last_input)
z_next = self.dynamics.f_dt(EV_state, u_safe, type='numpy')
collision = check_collision_poly(z_next,
(self.EV_W, self.EV_L),
TV_pred[1],
(self.TV_W, self.TV_L))
exp_state.actual_collision = collision
time_stats_msg.safety_solve = rospy.get_rostime().to_sec(
) - t_s
else:
time_stats_msg.safety_solve = -1
self.state_machine.state_transition(exp_state)
if self.state_machine.state in [
'Free-Driving', 'HOBCA-Unlocked', 'HOBCA-Locked'
]:
Z_pred, U_pred = Z_obca, U_obca
time_stats_msg.ebrake_solve = -1
rospy.loginfo(
'============ STRATEGY: Applying HOBCA control ============'
)
elif self.state_machine.state in [
'Safe-Confidence', 'Safe-Yield', 'Safe-Infeasible'
]:
U_pred = np.vstack((u_safe, np.zeros((self.N - 1, self.n_u))))
Z_pred = np.vstack((EV_state, np.zeros((self.N, self.n_x))))
for i in range(self.N):
Z_pred[i + 1] = self.dynamics.f_dt(Z_pred[i],
U_pred[i],
type='numpy')
time_stats_msg.ebrake_solve = -1
rospy.loginfo(
'============ STRATEGY: Applying safety control ============'
)
elif self.state_machine.state in ['Emergency-Break']:
t_s = rospy.get_rostime().to_sec()
u_ebrake = self.emergency_controller.solve(
EV_state, TV_pred, self.last_input)
U_pred = np.vstack((u_ebrake, np.zeros(
(self.N - 1, self.n_u))))
Z_pred = np.vstack((EV_state, np.zeros((self.N, self.n_x))))
for i in range(self.N):
Z_pred[i + 1] = self.dynamics.f_dt(Z_pred[i],
U_pred[i],
type='numpy')
time_stats_msg.ebrake_solve = rospy.get_rostime().to_sec(
) - t_s
rospy.loginfo(
'============ STRATEGY: Applying ebrake control ============'
)
else:
ecu_msg.servo = 0.0
ecu_msg.motor = 0.0
self.ecu_pub.publish(ecu_msg)
time_stats_msg.ebrake_solve = -1
rospy.loginfo(
'============ STRATEGY: State unrecognized shutting down... ============'
)
if not self.task_start:
rospy.loginfo(
'============ STRATEGY: Node up time: %g s. Controller not yet started ============'
% t)
self.last_input = np.zeros(self.n_u)
else:
ecu_msg.servo = U_pred[0, 0]
ecu_msg.motor = U_pred[0, 1]
self.last_input = U_pred[0]
# deadband = 0.01
# if np.abs(ecu_msg.motor) <= deadband:
# ecu_msg.motor = 0.0
# elif ecu_msg.motor > deadband:
# ecu_msg.motor = ecu_msg.motor - deadband
# else:
# ecu_msg.motor = ecu_msg.motor + deadband
self.ecu_pub.publish(ecu_msg)
self.ev_state_prediction = Z_pred
self.ev_input_prediction = U_pred
pred_msg = Prediction()
pred_msg.x = Z_pred[:, 0]
pred_msg.y = Z_pred[:, 1]
pred_msg.psi = Z_pred[:, 2]
pred_msg.v = Z_pred[:, 3]
pred_msg.df = U_pred[:, 0]
pred_msg.a = U_pred[:, 1]
self.pred_pub.publish(pred_msg)
score_msg = Scores()
score_msg.scores = scores
self.score_pub.publish(score_msg)
fsm_state_msg = FSMState()
fsm_state_msg.fsm_state = self.state_machine.state
self.fsm_state_pub.publish(fsm_state_msg)
time_stats_msg.total = (rospy.get_rostime().to_sec() -
self.start_time) - t
self.time_stats_pub.publish(time_stats_msg)
self.rate.sleep()
def main():
rospy.init_node("LPV-MPC")
input_commands = rospy.Publisher('ecu', ECU, queue_size=1)
pred_treajecto = rospy.Publisher('OL_predictions',
prediction,
queue_size=1)
racing_info = rospy.Publisher('Racing_Info', Racing_Info, queue_size=1)
mode = rospy.get_param("/control/mode")
N = rospy.get_param("/control/N")
loop_rate = 30.0
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
## TODO: change this in order to be taken from the launch file
Steering_Delay = 0 #3
Velocity_Delay = 0
#Steering_Delay = int(rospy.get_param("/simulator/delay_df")/dt)
NN_LPV_MPC = False
#########################################################
#########################################################
# OFFLINE planning from LPV-MPP racing:
planning_path = '/home/euge/GitHub/barc/workspace/src/barc/src/data/Planner_Refs'
## TRACK (OVAL, Racing Planner) : (30 July 19)
CURV_Planner = sio.loadmat(planning_path +
'/LPV_MPC_PLANNING_1_.mat')['pCURV']
VEL_Planner = sio.loadmat(planning_path +
'/LPV_MPC_PLANNING_1_.mat')['pnew_Vx']
X_Planner = sio.loadmat(planning_path + '/LPV_MPC_PLANNING_1_.mat')['pxp']
Y_Planner = sio.loadmat(planning_path + '/LPV_MPC_PLANNING_1_.mat')['pyp']
PSI_Planner = sio.loadmat(planning_path +
'/LPV_MPC_PLANNING_1_.mat')['pyaw']
#########################################################
#########################################################
# Objects initializations
OL_predictions = prediction()
cmd = ECU() # Command message
rac_info = Racing_Info()
cmd.servo = 0.0
cmd.motor = 0.0
ClosedLoopData = ClosedLoopDataObj(dt, 6000, 0) # Closed-Loop Data
estimatorData = EstimatorData()
map = Map() # Map
planning_data = PlanningData()
first_it = 1
NumberOfLaps = 10
# Initialize variables for main loop
GlobalState = np.zeros(6)
LocalState = np.zeros(6)
HalfTrack = 0
LapNumber = 0
RunController = 1
Counter = 0
CounterRacing = 0
uApplied = np.array([0.0, 0.0])
oldU = np.array([0.0, 0.0])
RMSE_ve = np.zeros(NumberOfLaps)
RMSE_ye = np.zeros(NumberOfLaps)
RMSE_thetae = np.zeros(NumberOfLaps)
RMSE_acc_y = np.zeros(NumberOfLaps)
RMSE_matrix = np.zeros(NumberOfLaps)
Norm_matrix = np.zeros((NumberOfLaps, 3))
ALL_LOCAL_DATA = np.zeros(
(2000, 8)) # [vx vy psidot thetae s ye vxaccel vyaccel udelta uaccel]
GLOBAL_DATA = np.zeros((2000, 3)) # [x y psi]
PREDICTED_DATA = np.zeros(
(2000, 120)) # [vx vy psidot thetae s ye] presicted to N steps
TLAPTIME = np.zeros((30, 1))
ELAPSD_TIME = np.zeros((2000, 1))
IDENT_DATA = np.zeros((2000, 5))
Data_for_RMSE = np.zeros((2000, 4))
References = np.array([0.0, 0.0, 0.0, 1.0])
# Loop running at loop rate
TimeCounter = 0
PlannerCounter = 0
count = True
index = 0
start_LapTimer = datetime.datetime.now()
insideTrack = 1
rospy.sleep(
1
) # Soluciona los problemas de inicializacion esperando a que el estimador se inicialice bien
vel_ref = 1
Cf_new = 60
SS = 0
###----------------------------------------------------------------###
### PATH TRACKING TUNING:
### 33 ms - 20 Hp
Q = np.diag([100.0, 1.0, 1.0, 20.0, 0.0, 900.0])
R = 0.5 * 0.5 * np.diag([1.0, 1.0]) # delta, a
dR = 1.5 * 25 * np.array([1.3, 1.0]) # Input rate cost u
Controller = PathFollowingLPV_MPC(Q, R, dR, N, vel_ref, dt, map, "OSQP",
Steering_Delay, Velocity_Delay)
# ### RACING TAJECTORY TRACKING TUNING:
# ### 33 ms - 20 Hp
Q = np.diag([400.0, 1.0, 1.0, 20.0, 0.0, 1100.0])
R = 0.0 * np.diag([1.0, 1.0]) # delta, a
dR = np.array([100.0, 45.0]) # Input rate cost u
# dR = np.array([ 10.0, 10.0 ]) # Input rate cost u
Controller_TT = PathFollowingLPV_MPC(Q, R, dR, N, vel_ref, dt, map, "OSQP",
Steering_Delay, Velocity_Delay)
### RACING TAJECTORY TRACKING TUNING:
### 33 ms - 20 Hp
# Q = np.diag([800.0, 1.0, 2.0, 10.0, 0.0, 1200.0])
# R = np.diag([0.0, 0.0]) # delta, a
# # R = np.array([146.0, 37.0]) # Input rate cost u
# dR = np.array([20, 30.0]) # Input rate cost u
# Controller_TT = PathFollowingLPV_MPC(Q, R, dR, N, vel_ref, dt, map, "OSQP", Steering_Delay, Velocity_Delay)
###----------------------------------------------------------------###
###----------------------------------------------------------------###
###----------------------------------------------------------------###
###----------------------------------------------------------------###
L_LPV_States_Prediction = np.zeros((N, 6))
LPV_States_Prediction = np.zeros((N, 6))
while (not rospy.is_shutdown()) and RunController == 1:
# Read Measurements
GlobalState[:] = estimatorData.CurrentState # The current estimated state vector [vx vy w x y psi]
LocalState[:] = estimatorData.CurrentState # [vx vy w x y psi]
if LocalState[0] < 0.01:
LocalState[0] = 0.01
if LapNumber == 0: # Path tracking:
# OUT: s, epsi, ey IN: x, y, psi
LocalState[4], LocalState[3], LocalState[
5], insideTrack = map.getLocalPosition(GlobalState[3],
GlobalState[4],
GlobalState[5])
# Check if the lap has finished
if LocalState[4] >= 3 * map.TrackLength / 4:
HalfTrack = 1
new_Ref = np.array([0.0, 0.0, 0.0, 1.0])
References = np.vstack(
(References,
new_Ref)) # Simplemente se usa para guardar datos en fichero
else: # Trajectory tracking:
GlobalState[5] = GlobalState[
5] - 2 * np.pi * LapNumber ## BODY FRAME ERROR COMPUTATION:
GlobalState[5] = wrap(GlobalState[5])
PSI_Planner[0, PlannerCounter] = wrap(PSI_Planner[0,
PlannerCounter])
# Offline planning data:
# OUT: s, ex, ey, epsi
# LocalState[4], Xerror, LocalState[3], LocalState[5] = Body_Frame_Errors(GlobalState[3],
# GlobalState[4], GlobalState[5], X_Planner[0,PlannerCounter], Y_Planner[0,PlannerCounter],
# PSI_Planner[0,PlannerCounter], SS, LocalState[0], LocalState[1], CURV_Planner[0, PlannerCounter], dt )
# print "Desired yaw = ", PSI_Planner[0,PlannerCounter], " and Real yaw = ", GlobalState[5]
# Online planning data:
new_Ref = np.array([
planning_data.x_d[0], planning_data.y_d[0],
planning_data.psi_d[0], planning_data.vx_d[0]
])
References = np.vstack((References, np.transpose(new_Ref)))
max_window = 0
if index <= max_window:
if index == 0:
X_REF_vector = planning_data.x_d[0:N + max_window]
Y_REF_vector = planning_data.y_d[0:N + max_window]
YAW_REF_vector = planning_data.psi_d[0:N + max_window]
VEL_REF_vector = planning_data.vx_d[0:N + max_window]
CURV_REF_vector = planning_data.curv_d[0:N + max_window]
x_ref = X_REF_vector[index:index + N]
y_ref = Y_REF_vector[index:index + N]
yaw_ref = YAW_REF_vector[index:index + N]
vel_ref = VEL_REF_vector[index:index + N]
curv_ref = CURV_REF_vector[index:index + N]
index += 1
else:
index = 0
# OUT: s, ex, ey, epsi
LocalState[4], Xerror, LocalState[5], LocalState[
3] = Body_Frame_Errors(GlobalState[3], GlobalState[4],
GlobalState[5], x_ref[0], y_ref[0],
yaw_ref[0], SS, LocalState[0],
LocalState[1], curv_ref[0], dt)
SS = LocalState[4]
### END OF THE LAP.
# PATH TRACKING:
if (HalfTrack == 1 and (LocalState[4] <= map.TrackLength / 4)):
HalfTrack = 0
LapNumber += 1
SS = 0
PlannerCounter = 0
TimeCounter = 0
TotalLapTime = datetime.datetime.now() - start_LapTimer
print 'Lap completed in', TotalLapTime, 'seconds. Starting lap:', LapNumber, '\n'
TLAPTIME[LapNumber - 1] = TotalLapTime.total_seconds()
start_LapTimer = datetime.datetime.now()
# RACING:
elif ((LapNumber >= 1) and (np.abs(GlobalState[3]) < 0.1)
and (LocalState[4] >= (map.TrackLength - map.TrackLength / 10))):
LapNumber += 1
SS = 0
# PlannerCounter = TrackCounter
TotalLapTime = datetime.datetime.now() - start_LapTimer
print 'Lap completed in', TotalLapTime, 'seconds. Starting lap:', LapNumber, '. PlannerCounter = ', PlannerCounter, '\n'
TLAPTIME[LapNumber - 1] = TotalLapTime.total_seconds()
start_LapTimer = datetime.datetime.now()
if LapNumber > NumberOfLaps:
print('RunController = 0')
RunController = 0
startTimer = datetime.datetime.now()
oldU = uApplied
uApplied = np.array([cmd.servo, cmd.motor])
if LapNumber == 0:
Controller.OldSteering.append(
cmd.servo) # meto al final del vector
Controller.OldAccelera.append(cmd.motor)
Controller.OldSteering.pop(0)
Controller.OldAccelera.pop(0)
else:
Controller_TT.OldSteering.append(
cmd.servo) # meto al final del vector
Controller_TT.OldAccelera.append(cmd.motor)
Controller_TT.OldSteering.pop(0)
Controller_TT.OldAccelera.pop(0)
### Publish input ###
input_commands.publish(cmd)
###################################################################################################
###################################################################################################
if first_it < 10:
# print('Iteration = ', first_it)
vel_ref = np.ones(N)
xx, uu = predicted_vectors_generation(N, LocalState, uApplied, dt)
Controller.solve(LocalState[0:6], xx, uu, NN_LPV_MPC, vel_ref, 0,
0, 0, first_it)
first_it = first_it + 1
# print('---> Controller.uPred = ', Controller.uPred)
Controller.OldPredicted = np.hstack(
(Controller.OldSteering[0:len(Controller.OldSteering) - 1],
Controller.uPred[Controller.steeringDelay:Controller.N, 0]))
Controller.OldPredicted = np.concatenate(
(np.matrix(Controller.OldPredicted).T,
np.matrix(Controller.uPred[:, 1]).T),
axis=1)
else:
NN_LPV_MPC = False
if LapNumber == 0: # PATH TRACKING : First lap at 1 m/s
vel_ref = np.ones(N + 1)
curv_ref = np.zeros(N)
LPV_States_Prediction, A_L, B_L, C_L = Controller.LPVPrediction(
LocalState[0:6], Controller.uPred, vel_ref, curv_ref,
Cf_new, LapNumber)
Controller.solve(LPV_States_Prediction[0, :],
LPV_States_Prediction, Controller.uPred,
NN_LPV_MPC, vel_ref, A_L, B_L, C_L, first_it)
# IDENT_DATA[Counter,:] = [LocalState[0], LocalState[1], LocalState[2], uApplied[0], uApplied[1]]
Controller_TT.uPred = Controller.uPred
else: # TRAJECTORY TRACKING
# Offline planning data:
# vel_ref = VEL_Planner[0, PlannerCounter:PlannerCounter+N+1]
# curv_ref = CURV_Planner[0, PlannerCounter:PlannerCounter+N]
# VEL_REF_vector = planning_data.vx_d[0:N+10]
# CURV_REF_vector = planning_data.curv_d[0:N+10]
# if index < 10:
# vel_ref = VEL_REF_vector[index:index+N]
# curv_ref = CURV_REF_vector[index:index+N]
# index += 1
# else:
# index = 0
# Online planning data:curv_ref
# vel_ref = planning_data.vx_d[0:N]
# curv_ref = planning_data.curv_d[0:N]
LPV_States_Prediction, A_L, B_L, C_L = Controller_TT.LPVPrediction(
LocalState[0:6], Controller_TT.uPred, vel_ref, curv_ref,
Cf_new, LapNumber)
Controller_TT.solve(LocalState[0:6], 0.0, Controller_TT.uPred,
NN_LPV_MPC, vel_ref, A_L, B_L, C_L,
first_it)
###################################################################################################
###################################################################################################
if first_it > 19:
new_LPV_States_Prediction = LPV_States_Prediction[0, :]
for i in range(1, N):
new_LPV_States_Prediction = np.hstack(
(new_LPV_States_Prediction, LPV_States_Prediction[i, :]))
PREDICTED_DATA[Counter, :] = new_LPV_States_Prediction
if LapNumber == 0:
cmd.servo = Controller.uPred[0 + Controller.steeringDelay, 0]
cmd.motor = Controller.uPred[0 + Controller.velocityDelay, 1]
else:
cmd.servo = Controller_TT.uPred[0 + Controller.steeringDelay, 0]
cmd.motor = Controller_TT.uPred[0 + Controller.velocityDelay, 1]
#print Controller.uPred[0, :]
endTimer = datetime.datetime.now()
deltaTimer = endTimer - startTimer
# ESTO HABRA QUE RESCATARLO PARA PRUEBAS FISICAS...
# else: # If car out of the track
# cmd.servo = 0
# cmd.motor = 0
# input_commands.publish(cmd)
## Record Prediction
if LapNumber < 1:
OL_predictions.s = Controller.xPred[:, 4]
OL_predictions.ex = []
OL_predictions.ey = Controller.xPred[:, 5]
OL_predictions.epsi = Controller.xPred[:, 3]
else:
OL_predictions.s = Controller_TT.xPred[:, 4]
OL_predictions.ex = []
OL_predictions.ey = Controller_TT.xPred[:, 5]
OL_predictions.epsi = Controller_TT.xPred[:, 3]
ALL_LOCAL_DATA[CounterRacing, :] = np.hstack(
(LocalState, uApplied))
GLOBAL_DATA[CounterRacing, :] = [
GlobalState[3], GlobalState[4], GlobalState[5]
]
CounterRacing += 1
pred_treajecto.publish(OL_predictions)
# ClosedLoopData.addMeasurement(GlobalState, LocalState, uApplied, Counter, deltaTimer.total_seconds())
# Data_for_RMSE[TimeCounter,:] = [ LocalState[0], LocalState[5], LocalState[3], LocalState[7]]
ELAPSD_TIME[Counter, :] = deltaTimer.total_seconds()
# Publishing important info about the racing:
rac_info.LapNumber = LapNumber
# rac_info.PlannerCounter = PlannerCounter
racing_info.publish(rac_info)
# Increase time counter and ROS sleep()
# print PlannerCounter
TimeCounter += 1
PlannerCounter += 1
# if PlannerCounter > 580: #offline racing planning
if PlannerCounter > 999:
cmd.servo = 0
cmd.motor = 0
input_commands.publish(cmd)
day = '31_7_19'
num_test = 'Test_1'
newpath = '/home/euge/GitHub/barc/results_simu_test/' + day + '/' + num_test + '/'
if not os.path.exists(newpath):
os.makedirs(newpath)
np.savetxt(newpath + '/ALL_LOCAL_DATA.dat',
ALL_LOCAL_DATA,
fmt='%.5e')
np.savetxt(newpath + '/PREDICTED_DATA.dat',
PREDICTED_DATA,
fmt='%.5e')
np.savetxt(newpath + '/GLOBAL_DATA.dat', GLOBAL_DATA, fmt='%.5e')
np.savetxt(newpath + '/References.dat', References, fmt='%.5e')
np.savetxt(newpath + '/TLAPTIME.dat', TLAPTIME, fmt='%.5e')
np.savetxt(newpath + '/ELAPSD_TIME.dat', ELAPSD_TIME, fmt='%.5e')
quit()
Counter += 1
rate.sleep()
# END WHILE
# Save Data
# file_data = open(sys.path[0]+'/data/'+mode+'/ClosedLoopData'+"LPV_MPC"+'.obj', 'wb')
# pickle.dump(ClosedLoopData, file_data)
# pickle.dump(Controller, file_data)
#############################################################
# file_data = open(newpath+'/ClosedLoopDataLPV_MPC.obj', 'wb')
# pickle.dump(ClosedLoopData, file_data)
# pickle.dump(Controller, file_data)
# file_data.close()
# INFO_data = open(newpath+'/INFO.txt', 'wb')
# INFO_data.write('Running in Track number ')
# INFO_data.write('%d \n' % Planning_Track)
# #INFO_data.write(' ')
# INFO_data.close()
#############################################################
# plotTrajectory(map, ClosedLoopData, Complete_Vel_Vect)
quit()
def publish_throttle_steering(self, action):
msg = ECU()
msg.motor = self.throttle
msg.servo = action * (500 / 0.366519) + 1520
self.ecu.publish(msg)
def spin(self):
rospy.sleep(self.init_time)
self.start_time = rospy.get_rostime().to_sec()
while not rospy.is_shutdown():
t = rospy.get_rostime().to_sec()
ecu_msg = ECU()
if t - self.start_time >= self.max_time:
ecu_msg.servo = 0.0
ecu_msg.motor = 0.0
# Publish the final motor and steering commands
self.ecu_pub.publish(ecu_msg)
self.bond_log.break_bond()
self.bond_ard.break_bond()
rospy.signal_shutdown(
'Max time of %g reached, controller shutting down...' %
self.max_time)
EV_state = self.state
TV_pred = self.tv_state_prediction[:self.N + 1]
EV_x, EV_y, EV_heading, EV_v = EV_state
TV_x, TV_y, TV_heading, TV_v = TV_pred[0]
X_ref = EV_x + np.arange(self.N + 1) * self.dt * self.v_ref
Z_ref = np.zeros((self.N + 1, self.n_x))
Z_ref[:, 0] = X_ref
self.obs[0] = get_car_poly(TV_pred, self.TV_W, self.TV_L)
if self.ev_state_prediction is None:
Z_ws = Z_ref
U_ws = np.zeros((self.N, self.n_u))
else:
Z_ws = np.vstack(
(self.ev_state_prediction[1:],
self.dynamics.f_dt(self.ev_state_prediction[-1],
self.ev_input_prediction[-1],
type='numpy')))
U_ws = np.vstack((self.ev_input_prediction[1:],
self.ev_input_prediction[-1]))
obca_mpc_ebrake = False
obca_mpc_safety = False
status_ws = self.obca_controller.solve_ws(Z_ws, U_ws, self.obs)
if status_ws['success']:
rospy.loginfo('Warm start solved in %g s' %
status_ws['solve_time'])
Z_obca, U_obca, status_sol = self.obca_controller.solve(
EV_state, self.last_input, Z_ref, self.obs)
if not status_ws['success'] or not status_sol['success']:
obca_mpc_safety = True
rospy.loginfo(
'OBCA MPC not feasible, activating safety controller')
if obca_mpc_safety:
self.safety_controller.set_accel_ref(EV_v * np.cos(EV_heading),
TV_v * np.cos(TV_heading))
u_safe = self.safety_controller.solve(EV_state, TV_pred,
self.last_input)
z_next = self.dynamics.f_dt(EV_state, u_safe, type='numpy')
collision = check_collision_poly(z_next,
(self.EV_W, self.EV_L),
TV_pred[1],
(self.TV_W, self.TV_L))
if collision:
obca_mpc_ebrake = True
u_safe = self.emergency_controller.solve(
EV_state, TV_pred, self.last_input)
U_pred = np.vstack((u_safe, np.zeros((self.N - 1, self.n_u))))
Z_pred = np.vstack((EV_state, np.zeros((self.N, self.n_x))))
for i in range(self.N):
Z_pred[i + 1] = self.dynamics.f_dt(Z_pred[i],
U_pred[i],
type='numpy')
else:
Z_pred, U_pred = Z_obca, U_obca
if obca_mpc_ebrake:
fsm_state = 'Emergency-Break'
elif obca_mpc_safety:
fsm_state = 'Safe-Infeasible'
else:
fsm_state = 'HOBCA-Unlocked'
self.ev_state_prediction = Z_pred
self.ev_input_prediction = U_pred
self.last_input = U_pred[0]
ecu_msg = ECU()
ecu_msg.servo = U_pred[0, 0]
ecu_msg.motor = U_pred[0, 1]
self.ecu_pub.publish(ecu_msg)
pred_msg = Prediction()
pred_msg.x = Z_pred[:, 0]
pred_msg.y = Z_pred[:, 1]
pred_msg.psi = Z_pred[:, 2]
pred_msg.v = Z_pred[:, 3]
pred_msg.df = U_pred[:, 0]
pred_msg.a = U_pred[:, 1]
self.pred_pub.publish(pred_msg)
fsm_state_msg = FSMState()
fsm_state_msg.fsm_state = fsm_state
self.fsm_state_pub.publish(fsm_state_msg)
self.rate.sleep()
def main():
# Initializa ROS node
rospy.init_node("LMPC")
input_commands = rospy.Publisher('ecu', ECU, queue_size=1)
pred_treajecto = rospy.Publisher('OL_predictions',
prediction,
queue_size=1)
sel_safe_set = rospy.Publisher('SS', SafeSetGlob, queue_size=1)
mode = rospy.get_param("/control/mode")
loop_rate = 30.0
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
Steering_Delay = 1
### BUILDING THE GRID FOR TUNNING:
Data_for_RMSE = np.zeros((2000, 3))
VQ_ve = np.array([100.0, 150.0, 200.0, 250.0])
VQ_ye = np.array([50.0, 100.0, 150.0, 200.0])
VQ_thetae = np.array([
150.0, 200.0, 250.0, 300.0, 350.0, 400.0, 450.0, 500.0, 550.0, 600.0,
650.0, 700.0, 750.0, 800.0
])
grid_length = len(VQ_ve) * len(VQ_ye) * len(VQ_thetae)
Q_matrix_grid = np.zeros((grid_length, 3))
counter = 0
for i in range(0, len(VQ_ve)):
Q_ve = VQ_ve[i]
for j in range(0, len(VQ_ye)):
Q_ye = VQ_ye[j]
for k in range(0, len(VQ_thetae)):
Q_thetae = VQ_thetae[k]
Q_matrix_grid[counter, :] = [Q_ve, Q_ye, Q_thetae]
counter += 1
#print Q_matrix_grid
# Objects initializations
SS_glob_sel = SafeSetGlob()
OL_predictions = prediction()
cmd = ECU() # Command message
cmd.servo = 0.0
cmd.motor = 0.0
ClosedLoopData = ClosedLoopDataObj(dt, 6000, 0) # Closed-Loop Data
estimatorData = EstimatorData()
map = Map() # Map
PickController = "LPV_MPC"
first_it = 1
NumberOfLaps = grid_length
vt = 1.0
PathFollowingLaps = 0 #EA: With this at 0 skips the first PID lap
# Initialize variables for main loop
GlobalState = np.zeros(6)
LocalState = np.zeros(6)
HalfTrack = 0
LapNumber = 0
RunController = 1
PlotingCounter = 0
uApplied = np.array([0.0, 0.0])
oldU = np.array([0.0, 0.0])
RMSE_ve = np.zeros(grid_length)
RMSE_ye = np.zeros(grid_length)
RMSE_thetae = np.zeros(grid_length)
RMSE_matrix = np.zeros(grid_length)
# Loop running at loop rate
TimeCounter = 0
KeyInput = raw_input("Press enter to start the controller... \n")
oneStepPrediction = np.zeros(6)
### CONTROLLER INITIALIZATION:
### 33 ms - 10 Hp
Q = np.diag([100.0, 1.0, 1.0, 50.0, 0.0, 1000.0])
R = 1 * np.diag([1.0, 0.5]) # delta, a
dR = 30 * np.array([2.0, 1.5]) # Input rate cost u
N = 14
Controller = PathFollowingLPV_MPC(Q, R, dR, N, vt, dt, map, "OSQP",
Steering_Delay)
print "Q matrix: \n", Q, "\n"
while (not rospy.is_shutdown()) and RunController == 1:
# Read Measurements
GlobalState[:] = estimatorData.CurrentState
LocalState[:] = estimatorData.CurrentState #The current estimated state vector
# EA: s, ey, epsi
LocalState[4], LocalState[5], LocalState[
3], insideTrack = map.getLocalPosition(GlobalState[4],
GlobalState[5],
GlobalState[3])
if LocalState[0] < 0.01:
LocalState[0] = 0.01
# Check if the lap has finished
if LocalState[4] >= 3 * map.TrackLength / 4:
HalfTrack = 1
### END OF THE LAP:
if HalfTrack == 1 and (LocalState[4] <= map.TrackLength / 4):
HalfTrack = 0
LapNumber += 1
print "Lap completed starting lap:", LapNumber, ". Lap time: ", float(
TimeCounter) / loop_rate
if LapNumber > 1:
print "Recording RMSE... \n"
PointAndTangent = map.PointAndTangent
cur, vel = Curvature(LocalState[4], PointAndTangent)
RMSE_ve[LapNumber - 2] = np.sqrt(
((vel - Data_for_RMSE[0:TimeCounter, 0])**2).mean())
RMSE_ye[LapNumber - 2] = np.sqrt((Data_for_RMSE[0:TimeCounter,
1]**2).mean())
RMSE_thetae[LapNumber - 2] = np.sqrt(
(Data_for_RMSE[0:TimeCounter, 2]**2).mean())
Data_for_RMSE = np.zeros((2000, 3))
TimeCounter = 0
first_it = 1
Q = np.diag([
Q_matrix_grid[LapNumber - 1,
0], 1.0, 1.0, Q_matrix_grid[LapNumber - 1, 1],
0.0, Q_matrix_grid[LapNumber - 1, 2]
])
R = 1 * np.diag([1.0, 0.5]) # delta, a
dR = 30 * np.array([2.0, 1.5]) # Input rate cost u
N = 14
Controller = PathFollowingLPV_MPC(Q, R, dR, N, vt, dt, map, "OSQP",
Steering_Delay)
print "Q matrix: \n", Q, "\n"
if LapNumber >= NumberOfLaps:
RunController = 0
# If inside the track publish input
if (insideTrack == 1):
startTimer = datetime.datetime.now()
oldU = uApplied
uApplied = np.array([cmd.servo, cmd.motor])
#Controller.OldInput = uApplied
#print "Insert steering in the last space of the vector...", "\n"
Controller.OldSteering.append(
cmd.servo) # meto al final del vector
Controller.OldAccelera.append(cmd.motor)
#print "1o-Controller.OldSteering: ",Controller.OldSteering, "\n"
#print "Remove the first element of the vector...", "\n"
Controller.OldSteering.pop(0)
Controller.OldAccelera.pop(0)
#print "2o-Controller.OldSteering: ",Controller.OldSteering, "\n"
### Publish input ###
input_commands.publish(cmd)
#print "Publishing: ", cmd.servo, "\n"
oneStepPredictionError = LocalState - oneStepPrediction # Subtract the local measurement to the previously predicted one step
uAppliedDelay = [
Controller.OldSteering[-1 - Controller.steeringDelay],
Controller.OldAccelera[-1]
]
#print "uAppliedDelay: ",Controller.OldSteering[-1 - Controller.steeringDelay], "\n"
oneStepPrediction, oneStepPredictionTime = Controller.oneStepPrediction(
LocalState, uAppliedDelay, 1)
if first_it < 10: # EA: Starting mode:
xx = np.array([[
LocalState[0] + 0.05, LocalState[1], 0., 0.000001,
LocalState[4], 0.0001
],
[
LocalState[0] + 0.2, LocalState[1], 0.,
0.000001, LocalState[4] + 0.01, 0.0001
],
[
LocalState[0] + 0.4, LocalState[1], 0.,
0.000001, LocalState[4] + 0.02, 0.0001
],
[
LocalState[0] + 0.6, LocalState[1], 0.,
0.000001, LocalState[4] + 0.04, 0.0001
],
[
LocalState[0] + 0.7, LocalState[1], 0.,
0.000001, LocalState[4] + 0.07, 0.0001
],
[
LocalState[0] + 0.8, LocalState[1], 0.,
0.000001, LocalState[4] + 0.1, 0.0001
],
[
LocalState[0] + 0.8, LocalState[1], 0.,
0.000001, LocalState[4] + 0.14, 0.0001
],
[
LocalState[0] + 1.0, LocalState[1], 0.,
0.000001, LocalState[4] + 0.18, 0.0001
],
[
LocalState[0] + 1.05, LocalState[1], 0.,
0.000001, LocalState[4] + 0.23, 0.0001
],
[
LocalState[0] + 1.05, LocalState[1], 0.,
0.000001, LocalState[4] + 0.55, 0.0001
],
[
LocalState[0] + 1.05, LocalState[1], 0.,
0.000001, LocalState[4] + 0.66, 0.0001
],
[
LocalState[0] + 1.05, LocalState[1], 0.,
0.000001, LocalState[4] + 0.77, 0.0001
],
[
LocalState[0] + 1.05, LocalState[1], 0.,
0.000001, LocalState[4] + 0.89, 0.0001
],
[
LocalState[0] + 1.0, LocalState[1], 0.,
0.000001, LocalState[4] + 0.999, 0.0001
]])
uu = np.array([[0., 0.], [0., 0.3], [0., 0.5], [0., 0.7],
[0., 0.9], [0., 1.0], [0., 1.0], [0., 1.0],
[0., 1.0], [0., 0.8], [0., 0.8], [0., 0.8],
[0., 0.8], [0., 0.8]])
Controller.solve(LocalState, xx, uu)
first_it = first_it + 1
#print "Resolving MPC...", "\n "
#print "Steering predicted: ",Controller.uPred[:,0] , "\n "
#Controller.OldPredicted = np.hstack((Controller.OldSteering, Controller.uPred[Controller.steeringDelay:Controller.N-1,0]))
#Controller.OldPredicted = np.concatenate((np.matrix(Controller.OldPredicted).T, np.matrix(Controller.uPred[:,1]).T), axis=1)
Controller.OldPredicted = np.hstack(
(Controller.OldSteering[0:len(Controller.OldSteering) - 1],
Controller.uPred[Controller.steeringDelay:Controller.N,
0]))
Controller.OldPredicted = np.concatenate(
(np.matrix(Controller.OldPredicted).T,
np.matrix(Controller.uPred[:, 1]).T),
axis=1)
#print "OldPredicted: ",Controller.OldPredicted[:,0], "\n"
# We store the predictions since we are going to rebuild the controller object:
U_pred = Controller.uPred
last_Hp = len(uu)
else:
# # Recomputamos el objeto controller con nuevo Hp:
# PointAndTangent = map.PointAndTangent
# cur, vel = Curvature(LocalState[4], PointAndTangent)
# print "Curvature: ",cur, "\n"
# if cur > 0.1:
# Q = np.diag([100.0, 1.0, 1.0, 20.0, 0.0, 700.0])
# R = 0 * np.diag([1.0, 1.0]) # delta, a
# dR = 40 * np.array([2.0, 1.5]) # Input rate cost u
# N = 10
# Controller = PathFollowingLPV_MPC(Q, R, dR, N, vt, dt, map, "OSQP", Steering_Delay)
# print "Hp = ", N, "\n"
# if last_Hp == 4:
# last_input = U_pred[3,:]
# for i in range(last_Hp, N):
# U_pred = np.vstack((U_pred, last_input))
# LPV_States_Prediction = Controller.LPVPrediction(LocalState, U_pred)
# Controller.solve(LPV_States_Prediction[0,:], LPV_States_Prediction, U_pred)
# U_pred = Controller.uPred
# #print U_pred
# last_Hp = N
# else:
# LPV_States_Prediction = Controller.LPVPrediction(LocalState, U_pred)
# Controller.solve(LPV_States_Prediction[0,:], LPV_States_Prediction, U_pred)
# U_pred = Controller.uPred
# #print U_pred
# last_Hp = N
# else:
# Q = np.diag([100.0, 1.0, 1.0, 20.0, 0.0, 700.0])
# R = 0 * np.diag([1.0, 1.0]) # delta, a
# dR = 40 * np.array([2.0, 1.5]) # Input rate cost u
# N = 4
# Controller = PathFollowingLPV_MPC(Q, R, dR, N, vt, dt, map, "OSQP", Steering_Delay)
# print "Hp = ", N, "\n"
# LPV_States_Prediction = Controller.LPVPrediction(LocalState, U_pred)
# Controller.solve(LPV_States_Prediction[0,:], LPV_States_Prediction, U_pred)
# U_pred = Controller.uPred
# #print "U predicted:",U_pred, "\n"
# last_Hp = N
# Old version:
#LPV_States_Prediction = Controller.LPVPrediction(LocalState, Controller.uPred)
#Controller.solve(LPV_States_Prediction[0,:], LPV_States_Prediction, Controller.uPred)
#print "Resolving MPC...", "\n "
LPV_States_Prediction = Controller.LPVPrediction(
LocalState, Controller.OldPredicted)
Controller.solve(LPV_States_Prediction[0, :],
LPV_States_Prediction,
Controller.OldPredicted)
###################################################################################################
###################################################################################################
cmd.servo = Controller.uPred[0 + Controller.steeringDelay, 0]
cmd.motor = Controller.uPred[0, 1]
#print "Steering que SE PUBLICARA teniendo en cuenta el delay: ","\n", "------>", cmd.servo, "\n \n \n \n \n"
##################################################################
# GETTING DATA FOR IDENTIFICATION:
# cmd.servo = 0.5*np.cos(50*TimeCounter)
# if LocalState[0]>1.6:
# cmd.motor = 0.0
# else:
# cmd.motor = 1.0
##################################################################
#print Controller.uPred[steps_Delay,0], Controller.uPred[0,0]
# if (Controller.solverTime.total_seconds() + Controller.linearizationTime.total_seconds() + oneStepPredictionTime.total_seconds() > dt):
# print "NOT REAL-TIME FEASIBLE!!"
# print "Solver time: ", Controller.solverTime.total_seconds(), " Linearization Time: ", Controller.linearizationTime.total_seconds() + oneStepPredictionTime.total_seconds()
endTimer = datetime.datetime.now()
deltaTimer = endTimer - startTimer
#print "Tot Solver Time: ", deltaTimer.total_seconds()
else: # If car out of the track
cmd.servo = 0
cmd.motor = 0
input_commands.publish(cmd)
print " Current Input: ", cmd.servo, cmd.motor
print " X, Y State: ", GlobalState
print " Current State: ", LocalState
# Record Prediction
OL_predictions.s = Controller.xPred[:, 4]
OL_predictions.ey = Controller.xPred[:, 5]
OL_predictions.epsi = Controller.xPred[:, 3]
pred_treajecto.publish(OL_predictions)
ClosedLoopData.addMeasurement(GlobalState, LocalState,
uApplied, PlotingCounter,
deltaTimer.total_seconds())
Data_for_RMSE[TimeCounter, :] = [
LocalState[0], LocalState[5], LocalState[3]
]
# Increase time counter and ROS sleep()
TimeCounter = TimeCounter + 1
PlotingCounter += 1
rate.sleep()
# Save Data
file_data = open(
sys.path[0] + '/data/' + mode + '/ClosedLoopData' + "LPV_MPC" + '.obj',
'wb')
pickle.dump(ClosedLoopData, file_data)
pickle.dump(Controller, file_data)
#pickle.dump(OpenLoopData, file_data)
print " \n \n \n Root Mean Squared Vel Error Normalised: \n", np.divide(
RMSE_ve[0:LapNumber - 1], np.amax(RMSE_ve[0:LapNumber - 1])), "\n"
print "Root Mean Squared Lateral Error Normalised: \n", np.divide(
RMSE_ye[0:LapNumber - 1], np.amax(RMSE_ye[0:LapNumber - 1])), "\n"
print "Root Mean Squared Theta Error Normalised: \n", np.divide(
RMSE_thetae[0:LapNumber - 1],
np.amax(RMSE_thetae[0:LapNumber - 1])), "\n"
print len(RMSE_thetae[0:LapNumber - 1])
for k in range(0, len(RMSE_thetae[0:LapNumber - 1]) - 1):
RMSE_matrix[k] = RMSE_ve[k] + RMSE_ye[k] + RMSE_thetae[k]
print "Best tunning: ", Q_matrix_grid[np.argmin(RMSE_matrix)], "\n \n \n"
#plotTrajectory(map, ClosedLoopData)
file_data.close()
def spin(self):
start_time_msg = rospy.wait_for_message('/start_time',
Float64,
timeout=10.0)
self.start_time = start_time_msg.data
rospy.loginfo('============ DIRECT: Node START, time %g ============' %
self.start_time)
while not rospy.is_shutdown():
t = rospy.get_rostime().to_sec() - self.start_time
# Get EV state and TV prediction at current time
EV_state = self.state
TV_pred = self.tv_state_prediction[:self.N + 1]
EV_x, EV_y, EV_heading, EV_v = EV_state
TV_x, TV_y, TV_heading, TV_v = TV_pred[0]
ecu_msg = ECU()
ecu_msg.servo = 0.0
ecu_msg.motor = 0.0
if t >= self.init_time and not self.task_start:
self.task_start = True
rospy.loginfo(
'============ DIRECT: Controler START ============')
if t >= self.max_time and not self.task_finished:
shutdown_msg = '============ DIRECT: Max time of %g reached. Controler SHUTTING DOWN ============' % self.max_time
self.task_finished = True
elif (EV_x < self.x_boundary[0] or EV_x > self.x_boundary[1]) or (
EV_y < self.y_boundary[0]
or EV_y > self.y_boundary[1]) and not self.task_finished:
# Check if car has left the experiment area
self.task_finished = True
shutdown_msg = '============ DIRECT: Track bounds exceeded reached. Controler SHUTTING DOWN ============'
elif np.abs(EV_x - self.x_goal) <= 0.10 and not self.task_finished:
self.task_finished = True
shutdown_msg = '============ DIRECT: Goal position reached. Controler SHUTTING DOWN ============'
if self.task_finished:
self.ecu_pub.publish(ecu_msg)
self.bond_log.break_bond()
self.bond_ard.break_bond()
rospy.loginfo(shutdown_msg)
rospy.signal_shutdown(shutdown_msg)
self.obs[0] = get_car_poly(TV_pred, self.TV_W, self.TV_L)
X_ref = EV_x + np.arange(self.N + 1) * self.dt * self.v_ref
Z_ref = np.zeros((self.N + 1, self.n_x))
Z_ref[:, 0] = X_ref
if self.ev_state_prediction is None:
Z_ws = Z_ref
U_ws = np.zeros((self.N, self.n_u))
else:
Z_ws = np.vstack(
(self.ev_state_prediction[1:],
self.dynamics.f_dt(self.ev_state_prediction[-1],
self.ev_input_prediction[-1],
type='numpy')))
U_ws = np.vstack((self.ev_input_prediction[1:],
self.ev_input_prediction[-1]))
obca_mpc_ebrake = False
obca_mpc_safety = False
status_ws = self.obca_controller.solve_ws(Z_ws, U_ws, self.obs)
if status_ws['success']:
# rospy.loginfo('Warm start solved in %g s' % status_ws['solve_time'])
Z_obca, U_obca, status_sol = self.obca_controller.solve(
EV_state, self.last_input, Z_ref, self.obs)
if not status_ws['success'] or not status_sol['success']:
obca_mpc_safety = True
rospy.loginfo(
'============ DIRECT: OBCA MPC not feasible, activating safety controller ============'
)
if obca_mpc_safety:
self.safety_controller.set_accel_ref(EV_v * np.cos(EV_heading),
TV_v * np.cos(TV_heading))
u_safe = self.safety_controller.solve(EV_state, TV_pred,
self.last_input)
z_next = self.dynamics.f_dt(EV_state, u_safe, type='numpy')
collision = check_collision_poly(z_next,
(self.EV_W, self.EV_L),
TV_pred[1],
(self.TV_W, self.TV_L))
if collision:
obca_mpc_ebrake = True
u_safe = self.emergency_controller.solve(
EV_state, TV_pred, self.last_input)
rospy.loginfo(
'============ DIRECT: Applying ebrake control ============'
)
else:
rospy.loginfo(
'============ DIRECT: Applying safety control ============'
)
U_pred = np.vstack((u_safe, np.zeros((self.N - 1, self.n_u))))
Z_pred = np.vstack((EV_state, np.zeros((self.N, self.n_x))))
for i in range(self.N):
Z_pred[i + 1] = self.dynamics.f_dt(Z_pred[i],
U_pred[i],
type='numpy')
else:
Z_pred, U_pred = Z_obca, U_obca
rospy.loginfo(
'============ DIRECT: Applying HOBCA control ============')
if obca_mpc_ebrake:
fsm_state = 'Emergency-Break'
elif obca_mpc_safety:
fsm_state = 'Safe-Infeasible'
else:
fsm_state = 'HOBCA-Unlocked'
if not self.task_start:
rospy.loginfo(
'============ DIRECT: Node up time: %g s. Controller not yet started ============'
% t)
self.last_input = np.zeros(self.n_u)
else:
ecu_msg.servo = U_pred[0, 0]
ecu_msg.motor = U_pred[0, 1]
self.last_input = U_pred[0]
# deadband = 0.01
# if np.abs(ecu_msg.motor) <= deadband:
# ecu_msg.motor = 0.0
# elif ecu_msg.motor > deadband:
# ecu_msg.motor = ecu_msg.motor - deadband
# else:
# ecu_msg.motor = ecu_msg.motor + deadband
self.ecu_pub.publish(ecu_msg)
self.ev_state_prediction = Z_pred
self.ev_input_prediction = U_pred
pred_msg = Prediction()
pred_msg.x = Z_pred[:, 0]
pred_msg.y = Z_pred[:, 1]
pred_msg.psi = Z_pred[:, 2]
pred_msg.v = Z_pred[:, 3]
pred_msg.df = U_pred[:, 0]
pred_msg.a = U_pred[:, 1]
self.pred_pub.publish(pred_msg)
fsm_state_msg = FSMState()
fsm_state_msg.fsm_state = fsm_state
self.fsm_state_pub.publish(fsm_state_msg)
self.rate.sleep()
if key in moveBindings.keys():
motor = moveBindings[key][0]
servo = moveBindings[key][1]
elif key in speedBindings.keys():
speed = speed * speedBindings[key][0]
turn = turn * speedBindings[key][1]
if (status == 14):
print(msg)
status = (status + 1) % 15
else:
motor = 0
servo = 0
if (key == '\x03'):
break
ecu = ECU()
ecu.motor = motor * speed
ecu.servo = servo * turn
pub.publish(ecu)
except Exception as e:
print(e)
finally:
ecu = ECU()
ecu.motor = 0
ecu.servo = 0
pub.publish(ecu)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
def spin(self):
start_time_msg = rospy.wait_for_message('/start_time',
Float64,
timeout=5.0)
self.start_time = start_time_msg.data
rospy.sleep(0.5)
counter = 0
rospy.loginfo('============ SAFETY: Node START, time %g ============' %
self.start_time)
while not rospy.is_shutdown():
t = rospy.get_rostime().to_sec() - self.start_time
# Get EV state and TV prediction at current time
EV_state = self.state
TV_pred = self.tv_state_prediction[:self.N + 1]
EV_x, EV_y, EV_heading, EV_v = EV_state
TV_x, TV_y, TV_heading, TV_v = TV_pred[0]
ecu_msg = ECU()
ecu_msg.servo = 0.0
ecu_msg.motor = 0.0
if t >= self.init_time and not self.task_start:
self.task_start = True
rospy.loginfo(
'============ SAFETY: Controler START ============')
if t >= self.max_time and not self.task_finish:
self.task_finish = True
shutdown_msg = '============ SAFETY: Max time of %g reached. Controler SHUTTING DOWN ============' % self.max_time
elif (EV_x < self.x_boundary[0] or EV_x > self.x_boundary[1]
) or (EV_y < self.y_boundary[0]
or EV_y > self.y_boundary[1]) and not self.task_finish:
# Check if car has left the experiment area
self.task_finish = True
shutdown_msg = '============ SAFETY: Track bounds exceeded. Controler SHUTTING DOWN ============'
# elif np.abs(y) > 0.7 and np.abs(heading - np.pi/2) <= 10*np.pi/180:
elif np.abs(EV_y) > 0.7:
self.task_finish = True
shutdown_msg = '============ SAFETY: Goal position reached. Controler SHUTTING DOWN ============'
if self.task_finish:
self.ecu_pub.publish(ecu_msg)
self.bond_log.break_bond()
self.bond_ard.break_bond()
rospy.loginfo(shutdown_msg)
rospy.signal_shutdown(shutdown_msg)
# Compute the distance threshold for applying braking assuming max
# decceleration is applied
rel_vx = TV_v * np.cos(TV_heading) - EV_v * np.cos(EV_heading)
min_ts = np.ceil(
-rel_vx / np.abs(self.a_lim[0]) / self.dt
) # Number of timesteps requred for relative velocity to be zero
v_brake = np.abs(
rel_vx) + np.arange(min_ts + 1) * self.dt * self.a_lim[
0] # Velocity when applying max decceleration
brake_thresh = np.sum(
np.abs(v_brake) * self.dt
) + 5 * self.collision_buffer_r # Distance threshold for safety controller to be applied
d = la.norm(np.array(
[EV_x - TV_x,
EV_y - TV_y])) # Distance between ego and target vehicles
if d <= brake_thresh:
self.safety_controller.set_accel_ref(EV_v * np.cos(EV_heading),
TV_v * np.cos(TV_heading))
else:
self.safety_controller.set_accel_ref(EV_v * np.cos(EV_heading),
self.v_ref)
u_safe = self.safety_controller.solve(EV_state, TV_pred,
self.last_input)
if not self.task_start:
rospy.loginfo(
'============ SAFETY: Node up time: %g s. Controller not yet started ============'
% t)
self.last_input = np.zeros(self.n_u)
else:
rospy.loginfo(
'============ SAFETY: Applying safety PID control ============'
)
ecu_msg.servo = u_safe[0]
ecu_msg.motor = u_safe[1]
self.last_input = u_safe
self.ecu_pub.publish(ecu_msg)
if self.task_start:
counter += 1
self.rate.sleep()
def spin(self):
self.start_time = rospy.get_rostime().to_sec()
counter = 0
waypt_counter = 0
self.start_pub.publish(Float64(self.start_time))
rospy.loginfo('============ TRACKING: Node START, time %g ============' % self.start_time)
while not rospy.is_shutdown():
# Get state at current time
state = self.state
x, y, heading, v = state
t = rospy.get_rostime().to_sec() - self.start_time
ecu_msg = ECU()
ecu_msg.servo = 0.0
ecu_msg.motor = 0.0
if t >= self.init_time and not self.task_start:
self.task_start = True
rospy.loginfo('============ TRACKING: Controler START ============')
if t >= self.max_time and not self.task_finish:
self.task_finish = True
shutdown_msg = '============ TRACKING: Max time of %g reached. Controler SHUTTING DOWN ============' % self.max_time
elif (x < self.x_boundary[0] or x > self.x_boundary[1]) or (y < self.y_boundary[0] or y > self.y_boundary[1]) and not self.task_finish:
# Check if car has left the experiment area
self.task_finish = True
shutdown_msg = '============ TRACKING: Track bounds exceeded. Controler SHUTTING DOWN ============'
# elif np.abs(y) > 0.7 and np.abs(heading - np.pi/2) <= 10*np.pi/180:
elif np.abs(y) > 0.7:
self.task_finish = True
shutdown_msg = '============ TRACKING: Goal position reached. Controler SHUTTING DOWN ============'
if self.task_finish:
self.ecu_pub.publish(ecu_msg)
self.bond_log.break_bond()
self.bond_ard.break_bond()
rospy.loginfo(shutdown_msg)
rospy.signal_shutdown(shutdown_msg)
if la.norm(np.array([x,y])-self.waypoints[waypt_counter]) <= 0.2 and waypt_counter < self.waypoints.shape[0]-1 and not self.wait:
rospy.loginfo('============ TRACKING: Waypoint (%g,%g) reached ============' \
% (self.waypoints[waypt_counter,0], self.waypoints[waypt_counter,1]))
self.wait = True
self.tracking_controller.Q = np.array([0,0,0,10])
if self.wait and counter >= self.next_ref_start[waypt_counter]:
rospy.loginfo('============ TRACKING: Setting next waypoint (%g,%g) ============' \
% (self.waypoints[waypt_counter,0], self.waypoints[waypt_counter,1]))
self.wait = False
self.tracking_controller.Q = self.Q
waypt_counter += 1
if self.wait:
Z_ref = np.zeros((self.N+1, self.n_x))
elif counter >= self.traj_len - 1:
Z_ref = np.tile(self.trajectory[-1], (self.N+1,1))
Z_ref[:,3] = 0
rospy.loginfo('============ TRACKING: End of trajectory reached ============')
elif counter >= self.traj_len - (self.N+1):
Z_ref = np.vstack((self.trajectory[counter:], np.tile(self.trajectory[-1], ((self.N+1)-(self.traj_len-counter),1))))
Z_ref[:,3] = 0
else:
Z_ref = self.trajectory[counter:counter+self.N+1]
if self.state_prediction is None:
Z_ws = Z_ref
U_ws = np.zeros((self.N, self.n_u))
else:
Z_ws = np.vstack((self.state_prediction[1:],
self.dynamics.f_dt(self.state_prediction[-1], self.input_prediction[-1], type='numpy')))
U_ws = np.vstack((self.input_prediction[1:],
self.input_prediction[-1]))
Z_pred, U_pred, status_sol = self.tracking_controller.solve(state, self.last_input, Z_ref, Z_ws, U_ws)
if not self.task_start:
rospy.loginfo('============ TRACKING: Node up time: %g s. Controller not yet started ============' % t)
self.last_input = np.zeros(self.n_u)
elif self.wait:
rospy.loginfo('============ TRACKING: Stopping, waiting for reference to move past transition ============')
ecu_msg.servo = U_pred[0,0]
ecu_msg.motor = U_pred[0,1]
self.last_input = U_pred[0]
elif not status_sol['success']:
rospy.loginfo('============ TRACKING: MPC not feasible ============')
self.last_input = np.zeros(self.n_u)
Z_pred = np.zeros((self.N+1, self.n_x))
U_pred = np.zeros((self.N, self.n_u))
else:
rospy.loginfo('============ TRACKING: Applying MPC control ============')
ecu_msg.servo = U_pred[0,0]
ecu_msg.motor = U_pred[0,1]
self.last_input = U_pred[0]
self.ecu_pub.publish(ecu_msg)
self.state_prediction = Z_pred
self.input_prediction = U_pred
pred_msg = Prediction()
pred_msg.x = Z_pred[:,0]
pred_msg.y = Z_pred[:,1]
pred_msg.psi = Z_pred[:,2]
pred_msg.v = Z_pred[:,3]
pred_msg.df = U_pred[:,0]
pred_msg.a = U_pred[:,1]
self.pred_pub.publish(pred_msg)
if self.task_start:
counter += 1
self.rate.sleep()