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()