def main(dt):
global X_ref, Y_ref, psi_ref, vx_ref, vy_ref, Waypoints_received, stateEstimate_mark
rospy.init_node('waypoints_interface', anonymous=True)
rospy.Subscriber('waypoints', Waypoints, WaypointsCallback)
rospy.Subscriber('state_estimate', state_Dynamic, stateEstimateCallback)
pub = rospy.Publisher('final_trajectory', Trajectory2D, queue_size=1)
rate = rospy.Rate(1 / dt)
while (rospy.is_shutdown() != 1):
if stateEstimate_mark and Waypoints_mark:
num_steps_received = len(Waypoints_received.points) - 1
dt_received = Waypoints_received.dt
horizon = dt_received * num_steps_received
points = np.zeros((2, num_steps_received + 2))
points[0, 0] -= vx_ref * dt_received
#points[1, 0] -= vy_ref * dt_received
for i in range(num_steps_received):
points[0, i + 2] = Waypoints_received.points[i].x
points[1, i + 2] = Waypoints_received.points[i].y
t_received = np.linspace(-dt_received, horizon,
num_steps_received + 2)
num_points = int(floor(horizon / dt + 1)) + 40
t = np.linspace(0, (num_points - 1) * dt, num_points)
w = np.ones(num_steps_received + 2)
w[0:2] *= 10
w[-1] *= 5
spl_x = UnivariateSpline(t_received, points[0, :], k=3, w=w)
spl_y = UnivariateSpline(t_received, points[1, :], k=3, w=w)
spl_x_dot = spl_x.derivative()
spl_y_dot = spl_y.derivative()
spl_x_val = spl_x(t)
spl_y_val = spl_y(t)
spl_x_dot_val = spl_x_dot(t)
spl_y_dot_val = spl_y_dot(t)
spl_v_val = np.sqrt(spl_x_dot_val**2 + spl_y_dot_val**2)
spl_theta_val = np.arctan2(spl_y_dot_val, spl_x_dot_val)
spl_theta_val[0] = 0
w_theta = np.ones(spl_theta_val.shape[0])
w_theta[0] = w_theta[0] * 10
spl_theta_fn = UnivariateSpline(t, spl_theta_val, k=3, w=w_theta)
spl_theta_val = spl_theta_fn(t)
spl_yr_fn = spl_theta_fn.derivative()
spl_yr_val = spl_yr_fn(t)
traj = Trajectory2D()
for i in range(num_points):
pt = TrajectoryPoint2D()
pt.t = t[i]
pt.x = spl_x_val[i] * cos(psi_ref) - spl_y_val[i] * sin(
psi_ref) + X_ref
pt.y = spl_x_val[i] * sin(psi_ref) + spl_y_val[i] * cos(
psi_ref) + Y_ref
pt.theta = spl_theta_val[i] + psi_ref
pt.v = spl_v_val[i]
pt.kappa = spl_yr_val[i]
traj.point.append(pt)
traj.header = Header()
traj.header.stamp = rospy.get_rostime()
pub.publish(traj)
rate.sleep()
def main(dt, horizon):
global vx, vy, X, Y, psi, wz, d_f,stateEstimate_mark
# import track file
track = Tra('Tra_1', horizon)
rospy.init_node('toy_planner', anonymous=True)
rospy.Subscriber('state_estimate', state_Dynamic, stateEstimateCallback)
pub = rospy.Publisher('final_trajectory', Trajectory2D, queue_size=1)
rate = rospy.Rate(1/dt)
# first set the horizon to be very large to get where the vehicle is
track.horizon = horizon
track.currentIndex = 0
num_points = 400
w = np.ones(num_points) * 0.1
w[0] = w[0] * 30
w[-1] = w[-1] * 30
while (rospy.is_shutdown() != 1):
if stateEstimate_mark == True:
track.currentIndex, _ = track.searchClosestPt(X, Y, track.currentIndex)
if track.currentIndex + num_points < track.size:
index_list = np.arange(track.currentIndex, track.currentIndex+num_points, 1)
t = track.t[track.currentIndex:track.currentIndex+num_points]
t_sub = track.t[index_list]
x_sub = track.x[index_list] #- 0.16
y_sub = track.y[index_list] #+ 0.44
spl_x = UnivariateSpline(t_sub, x_sub, k=2)
spl_y = UnivariateSpline(t_sub, y_sub, k=2)
spl_x_dot = spl_x.derivative()
spl_y_dot = spl_y.derivative()
spl_x_val = spl_x(t)
spl_y_val = spl_y(t)
spl_x_dot_val = spl_x_dot(t)
spl_y_dot_val = spl_y_dot(t)
spl_v_val = np.sqrt(spl_x_dot_val**2 + spl_y_dot_val**2)
spl_theta_val = np.arctan2(spl_y_dot_val, spl_x_dot_val)
spl_yr_fn = UnivariateSpline(t, spl_theta_val, k=2).derivative()
spl_yr_val = spl_yr_fn(t)
traj = Trajectory2D()
for i in range(num_points):
pt = TrajectoryPoint2D()
pt.t = t[i]
pt.x = spl_x_val[i]
pt.y = spl_y_val[i]
pt.theta = spl_theta_val[i]
pt.v = spl_v_val[i]
pt.kappa = spl_yr_val[i]
traj.point.append(pt)
traj.header = Header()
traj.header.stamp = rospy.get_rostime()
pub.publish(traj)
rate.sleep()
def smooth(traj):
x = []
y = []
t = []
w = np.ones(len(traj.point))
#w[0] = w[0] * 100
#w[-1] = w[-1] * 100
for k in range(len(traj.point)):
x.append(traj.point[k].x)
y.append(traj.point[k].y)
t.append(traj.point[k].t)
spl_x = UnivariateSpline(t, x, k=3, w=w)
spl_y = UnivariateSpline(t, y, k=3, w=w)
spl_x_dot = spl_x.derivative()
spl_y_dot = spl_y.derivative()
spl_x_ddot = spl_x.derivative(n=2)
spl_y_ddot = spl_y.derivative(n=2)
t_resample = np.linspace(t[0], t[-1], int((t[-1] - t[0]) / 0.02) + 1)
spl_x_val = spl_x(t_resample)
spl_y_val = spl_y(t_resample)
spl_x_dot_val = spl_x_dot(t_resample)
spl_y_dot_val = spl_y_dot(t_resample)
spl_x_ddot_val = spl_x_ddot(t_resample)
spl_y_ddot_val = spl_y_ddot(t_resample)
spl_v_val = np.sqrt(spl_x_dot_val**2 + spl_y_dot_val**2)
spl_theta_val = np.arctan2(spl_y_dot_val, spl_x_dot_val)
spl_k_val = (spl_x_dot_val * spl_y_ddot_val -
spl_y_dot_val * spl_x_ddot_val) / spl_v_val**3
traj_resample = Trajectory2D()
for k in range(len(t_resample)):
pt = TrajectoryPoint2D()
pt.t = t_resample[k]
pt.x = spl_x_val[k]
pt.y = spl_y_val[k]
pt.v = spl_v_val[k]
pt.theta = spl_theta_val[k]
pt.kappa = spl_k_val[k]
traj_resample.point.append(pt)
return traj_resample
def talker():
pub = rospy.Publisher('final_trajectory', Trajectory2D, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
waypoints_mat = scipy.io.loadmat(
'/home/bdd/Desktop/PathFollower_ws/src/path_follower/nodes/waypoint_loader/New_tra_2.mat'
)['New_tra_2']
x = waypoints_mat[0][:]
y = waypoints_mat[1][:]
theta = waypoints_mat[2][:]
v = waypoints_mat[3][:]
t = waypoints_mat[4][:]
kappa = waypoints_mat[5][:]
now = rospy.get_rostime()
trajectory_record = Trajectory2D()
trajectory_record.header.stamp = now
trajectory_record.point = []
for i in range(waypoints_mat.shape[1]):
# define pose
temp_trajectory = TrajectoryPoint2D()
temp_trajectory.x = x[i]
temp_trajectory.y = y[i]
temp_trajectory.theta = theta[i]
temp_trajectory.v = v[i]
temp_trajectory.t = t[i]
temp_trajectory.kappa = kappa[i]
trajectory_record.point.append(temp_trajectory)
while not rospy.is_shutdown():
trajectory_record.header = Header()
trajectory_record.header.stamp = rospy.get_rostime()
pub.publish(trajectory_record)
rate.sleep()
def main(sim_steps, sim_steps2):
global vx, vy, X, Y, psi, wz, stateEstimate_mark, laneChange
dt = 0.1
# env, base policy, attribute policy and PAL-Net related
rospack = rospkg.RosPack()
model_path = os.path.join(rospack.get_path("planning_policy"),
"trained_model")
config = dict()
config['mode'] = 'Imitation'
config['run_type'] = 'train'
config['continue'] = True
# construction configuration:
# driver problem
config['env_type'] = 'driver'
config['update_name'] = 'driver'
config['e_update_type'] = 'regular'
# network config:
network_config(config, model_path)
network = Palnet(config)
network.restore()
# start expert structure.
g1 = tf.Graph()
with g1.as_default():
expert = Expert(model_path)
expert.restore()
env = Driving(story_index=100,
track_data='long_straight',
lane_deviation=9.1,
dt=dt)
P = np.array([[100, 0], [0, 1]])
solvers.options[
'show_progress'] = False # don't let cvxopt print iterations
# define the initial states and timestep
stateEstimate_mark = False
# import track file
rospy.init_node('RL_planner', anonymous=True)
rospy.Subscriber('state_estimate', state_Dynamic, stateEstimateCallback)
rospy.Subscriber('lane_signal', Int8, laneChangeCallback)
ref_traj_pub = rospy.Publisher('final_trajectory_origin',
Trajectory2D,
queue_size=1)
ref_traj_pub2 = rospy.Publisher('final_trajectory_origin2',
Trajectory2D,
queue_size=1)
obstacle_pub = rospy.Publisher('obstacle_pos',
TrajectoryPoint2D,
queue_size=1)
dt_planner = 0.5
rate = rospy.Rate(1 / dt_planner)
# get the sim_env ready
env.reset()
while (rospy.is_shutdown() != 1):
if stateEstimate_mark:
env.ego.state = np.array([X, Y, vx, psi])
# deal with keyboard lane change command input
if laneChange != 0:
env.hand_lanechange(laneChange)
laneChange = 0
# a dirty trick, if lane change then disable future lane change
if env.ego.track_select == 1:
env.hand_lanechange(0)
# get the initial observation and obstacle ref
env.get_all_ref()
ob = np.append(env.ego.state[2],
env.ego_track_ref_list[env.ego.track_select])
obstacle_ref_list = env.ego_obstacle_ref_list
# define traj for later data fulfillment
traj = Trajectory2D()
# get obstacle position and publish it
# TODO: currently static obstacle, future deal with moving obstacle, and
# TODO: since there is imaginary steps, need to reset obstacle state back
obstacle = TrajectoryPoint2D()
obstacle.t = 0
obstacle.x = env.obstacles[0].state[0]
obstacle.y = env.obstacles[0].state[1]
obstacle.v = env.obstacles[0].state[2]
obstacle.theta = env.obstacles[0].state[3]
obstacle_pub.publish(obstacle)
for i in range(sim_steps):
pt = TrajectoryPoint2D()
pt.t = i * dt
pt.x = env.ego.state[0]
pt.y = env.ego.state[1]
pt.v = env.ego.state[2]
pt.theta = env.ego.state[3]
traj.point.append(pt)
ac0 = expert.choose_action(ob)
dudt0 = np.multiply(ac0[:, np.newaxis], np.array([[5], [0.5]]))
if len(obstacle_ref_list):
# extract parameters from obstacle_ref_list
data = {'state0': np.vstack(obstacle_ref_list)[:, :10]}
feed_data = network.get_feed_dict(data)
ob_param = network.sess.run(network.means[network.index],
feed_data)
# None, 3.
M = ob_param[:, :2]
b = -ob_param[:, -1:]
if env.ego.track_select == 1:
M[0, 0] = 0
M[0, 1] = 0
b[0, 0] = 1
try:
sol = solvers.qp(P=matrix(0.5 * P),
q=matrix(-np.matmul(P, dudt0)),
G=matrix(M),
h=matrix(b))
except:
# if dAger is not useful, transfer back.
print("RL_planner:Something wrong with dAger run.")
num = len(obstacle_ref_list)
for i in range(num):
obstacle_ref = obstacle_ref_list[i]
A = obstacle_ref[10]
B = obstacle_ref[11]
C = obstacle_ref[12]
M[i, 0] = A
M[i, 1] = B
b[i, 0] = -C
sol = solvers.qp(P=matrix(0.5 * P),
q=matrix(-np.matmul(P, dudt0)),
G=matrix(M),
h=matrix(b))
dudt = sol['x']
else:
dudt = dudt0
ac = np.zeros(2)
ac[0] = dudt[0] / 5
ac[1] = dudt[1] / 0.5
np.clip(ac, -1, 1, out=ac)
ob, r, done, obstacle_ref_list = env.step(ac)
smooth_traj = smooth(traj)
ref_traj_pub.publish(smooth_traj)
env.ego.state = np.array([X, Y, vx, psi])
# deal with keyboard lane change command input
if laneChange != 0:
env.hand_lanechange(laneChange)
laneChange = 0
# a dirty trick, if lane change then disable future lane change
if env.ego.track_select == 1:
env.hand_lanechange(0)
# get the initial observation and obstacle ref
env.get_all_ref()
ob = np.append(env.ego.state[2],
env.ego_track_ref_list[env.ego.track_select])
obstacle_ref_list = env.ego_obstacle_ref_list
# define traj for later data fulfillment
traj = Trajectory2D()
# get obstacle position and publish it
# TODO: currently static obstacle, future deal with moving obstacle, and
# TODO: since there is imaginary steps, need to reset obstacle state back
obstacle = TrajectoryPoint2D()
obstacle.t = 0
obstacle.x = env.obstacles[0].state[0]
obstacle.y = env.obstacles[0].state[1]
obstacle.v = env.obstacles[0].state[2]
obstacle.theta = env.obstacles[0].state[3]
obstacle_pub.publish(obstacle)
'''
for i in range(sim_steps2):
pt = TrajectoryPoint2D()
pt.t = i * dt
pt.x = env.ego.state[0]
pt.y = env.ego.state[1]
pt.v = env.ego.state[2]
pt.theta = env.ego.state[3]
traj.point.append(pt)
ac0 = expert.choose_action(ob)
dudt0 = np.multiply(ac0[:, np.newaxis], np.array([[5], [0.5]]))
if len(obstacle_ref_list):
# extract parameters from obstacle_ref_list
data = {'state0': np.vstack(obstacle_ref_list)[:, :10]}
feed_data = network.get_feed_dict(data)
ob_param = network.sess.run(network.means[network.index], feed_data)
# None, 3.
M = ob_param[:, :2]
b = -ob_param[:, -1:]
if env.ego.track_select == 1:
M[0,0] = 0
M[0,1] = 0
b[0,0] = 1
try:
sol = solvers.qp(P=matrix(0.5 * P), q=matrix(- np.matmul(P, dudt0)), G=matrix(M), h=matrix(b))
except:
# if dAger is not useful, transfer back.
print("RL_planner:Something wrong with dAger run.")
num = len(obstacle_ref_list)
for i in range(num):
obstacle_ref = obstacle_ref_list[i]
A = obstacle_ref[10]
B = obstacle_ref[11]
C = obstacle_ref[12]
M[i, 0] = A
M[i, 1] = B
b[i, 0] = -C
sol = solvers.qp(P=matrix(0.5 * P), q=matrix(- np.matmul(P, dudt0)), G=matrix(M), h=matrix(b))
dudt = sol['x']
else:
dudt = dudt0
ac = np.zeros(2)
ac[0] = dudt[0] / 5
ac[1] = dudt[1] / 0.5
np.clip(ac, -1, 1, out=ac)
ob, r, done, obstacle_ref_list = env.step(ac)
smooth_traj = smooth(traj)
ref_traj_pub2.publish(smooth_traj)
'''
rate.sleep()
def main(dt):
global t_prev, t_now, xy_prev, X_ref, Y_ref, psi_ref, vx_ref, vy_ref, Waypoints_received, stateEstimate_mark, state_received
rospy.init_node('waypoints_interface', anonymous=True)
rospy.Subscriber('waypoints', Waypoints, WaypointsCallback)
rospy.Subscriber('state_estimate', state_Dynamic, stateEstimateCallback)
pub = rospy.Publisher('final_trajectory', Trajectory2D, queue_size=1)
pub2 = rospy.Publisher('waypoints_state_received',
Waypoints_state,
queue_size=1)
rate = rospy.Rate(1 / dt)
while (rospy.is_shutdown() != 1):
if stateEstimate_mark and Waypoints_mark >= 3:
num_steps_received = len(Waypoints_received.points) - 1
dt_received = Waypoints_received.dt
horizon = dt_received * num_steps_received
points = np.zeros((2, num_steps_received + 3))
points[:, :3] = xy_prev
for i in range(num_steps_received):
points[0, i + 3] = Waypoints_received.points[i].x * cos(
psi_ref) - Waypoints_received.points[i].y * sin(
psi_ref) + X_ref
points[1, i + 3] = Waypoints_received.points[i].x * sin(
psi_ref) + Waypoints_received.points[i].y * cos(
psi_ref) + Y_ref
t_received = np.linspace(dt_received, horizon, num_steps_received)
t_received = np.append(t_prev - t_now, t_received)
num_points = int(floor(horizon / dt + 1)) + 5
t = np.linspace(0, (num_points - 1) * dt, num_points)
w = np.ones(num_steps_received + 3) * 0.1
w[0:2] *= 20
w[-1] *= 1.2
w[int(num_steps_received / 2) + 2] *= 1.2
spl_x = UnivariateSpline(t_received, points[0, :], k=3, w=w)
spl_y = UnivariateSpline(t_received, points[1, :], k=3, w=w)
spl_x_dot = spl_x.derivative()
spl_y_dot = spl_y.derivative()
spl_x_val = spl_x(t)
spl_y_val = spl_y(t)
spl_x_dot_val = spl_x_dot(t)
spl_y_dot_val = spl_y_dot(t)
spl_v_val = np.sqrt(spl_x_dot_val**2 + spl_y_dot_val**2)
spl_theta_val = np.arctan2(spl_y_dot_val, spl_x_dot_val)
spl_theta_val[0] = psi_ref
w_theta = np.ones(spl_theta_val.shape[0])
w_theta[0] = w_theta[0] * 10
spl_theta_fn = UnivariateSpline(t, spl_theta_val, k=3, w=w_theta)
spl_theta_val = spl_theta_fn(t)
spl_yr_fn = spl_theta_fn.derivative()
spl_yr_val = spl_yr_fn(t)
traj = Trajectory2D()
for i in range(num_points):
pt = TrajectoryPoint2D()
pt.t = t[i]
pt.x = spl_x_val[i]
pt.y = spl_y_val[i]
pt.theta = spl_theta_val[i]
pt.v = spl_v_val[i]
pt.kappa = spl_yr_val[i]
traj.point.append(pt)
traj.header = Header()
traj.header.stamp = rospy.get_rostime()
pub.publish(traj)
waypoints_state_received = Waypoints_state()
waypoints_state_received.waypoints = Waypoints_received
waypoints_state_received.state = state_received
waypoints_state_received.traj = traj
pub2.publish(waypoints_state_received)
rate.sleep()