def publish_mpc_path_message(self, sol_dict): mpc_path_msg = mpc_path() mpc_path_msg.header.stamp = rospy.get_rostime() mpc_path_msg.solve_status = 'optimal' if sol_dict['optimal'] else 'suboptimal' mpc_path_msg.solve_time = sol_dict['solve_time'] mpc_path_msg.xs = sol_dict['z_mpc'][:,0] # x_mpc mpc_path_msg.ys = sol_dict['z_mpc'][:,1] # y_mpc mpc_path_msg.psis = sol_dict['z_mpc'][:,2] # psi_mpc mpc_path_msg.vs = sol_dict['z_mpc'][:,3] # v_mpc if 'z_mpc_frenet' in sol_dict: mpc_path_msg.ss = sol_dict['z_mpc_frenet'][:,0] mpc_path_msg.eys = sol_dict['z_mpc_frenet'][:,1] mpc_path_msg.epsis = sol_dict['z_mpc_frenet'][:,2] mpc_path_msg.vrf = sol_dict['v_ref_frenet'] mpc_path_msg.crf = sol_dict['curv_ref_frenet'] mpc_path_msg.xr = sol_dict['z_ref'][:,0] # x_ref mpc_path_msg.yr = sol_dict['z_ref'][:,1] # y_ref mpc_path_msg.psir = sol_dict['z_ref'][:,2] # psi_ref mpc_path_msg.vr = sol_dict['z_ref'][:,3] # v_ref mpc_path_msg.acc = sol_dict['u_mpc'][:,0] # acc_mpc mpc_path_msg.df = sol_dict['u_mpc'][:,1] # df_mpc self.mpc_path_pub.publish(mpc_path_msg)
def pub_loop(acc_pub_obj, steer_pub_obj, mpc_path_pub_obj):
loop_rate = rospy.Rate(20.0)
while not rospy.is_shutdown():
if not received_reference:
# Reference not received so don't use MPC yet.
loop_rate.sleep()
continue
# Ref lock used to ensure that get/set of state doesn't happen simultaneously.
global ref_lock
ref_lock = True
global x_curr, y_curr, psi_curr, v_curr, des_speed, command_stop, stopped
if not track_with_time:
# fixed velocity-based path tracking
x_ref, y_ref, psi_ref, des_speed, stop_cmd = grt.get_waypoints(
x_curr, y_curr, psi_curr)
if stop_cmd == True:
command_stop = True
else:
# trajectory tracking
x_ref, y_ref, psi_ref, stop_cmd = grt.get_waypoints(
x_curr, y_curr, psi_curr)
if stop_cmd == True:
command_stop = True
#the car is stopped
if v_curr < 0.7 and stop_cmd:
stopped = True
# Update Model
kmpc.update_init_cond(x_curr, y_curr, psi_curr, v_curr)
kmpc.update_reference(x_ref, y_ref, psi_ref, des_speed)
ref_lock = False
if not stopped:
a_opt, df_opt, is_opt, solv_time = kmpc.solve_model()
rostm = rospy.get_rostime()
tm_secs = rostm.secs + 1e-9 * rostm.nsecs
log_str = "Solve Status: %s, Acc: %.3f, SA: %.3f, ST: %.3f, V: %.3f" % (
is_opt, a_opt, df_opt, solv_time, v_curr)
rospy.loginfo(log_str)
if is_opt == 'Optimal':
acc_pub_obj.publish(Float32Msg(a_opt))
steer_pub_obj.publish(Float32Msg(df_opt))
kmpc.update_current_input(df_opt, a_opt)
res = kmpc.get_solver_results()
mpc_path_msg = mpc_path()
mpc_path_msg.header.stamp = rostm
mpc_path_msg.solv_status = is_opt
mpc_path_msg.solv_time = solv_time
mpc_path_msg.xs = res[0] # x_mpc
mpc_path_msg.ys = res[1] # y_mpc
mpc_path_msg.vs = res[2] # v_mpc
mpc_path_msg.psis = res[3] # psi_mpc
mpc_path_msg.xr = res[4] # x_ref
mpc_path_msg.yr = res[5] # y_ref
mpc_path_msg.vr = res[6] # v_ref
mpc_path_msg.psir = res[7] # psi_ref
mpc_path_msg.df = res[8] # d_f
mpc_path_msg.acc = res[9] # acc
mpc_path_pub_obj.publish(mpc_path_msg)
else:
print "The car is stopped!"
acc_pub_obj.publish(Float32Msg(-1.0))
steer_pub_obj.publish(Float32Msg(0.0))
loop_rate.sleep()