def main(dt, dt_ros):
# import pkl file
rospack = rospkg.RosPack()
file_name = os.path.join(rospack.get_path("path_follower"), "scripts",
"waypoint_loader", 'output_0_768.avi.pkl')
with open(file_name, 'rb') as f:
data = pickle.load(f)
rospy.init_node('toy_planner_waypoints', anonymous=True)
pub = rospy.Publisher('waypoints', Waypoints, queue_size=1)
rate = rospy.Rate(1 / dt_ros)
Index = 0
while (rospy.is_shutdown() != 1):
waypoints = Waypoints()
for i in range(8):
pt = Point2D()
pt.x = data[Index][i, 0]
pt.y = data[Index][i, 1]
waypoints.points.append(pt)
waypoints.dt = dt
pub.publish(waypoints)
Index += 1
rate.sleep()
def main(dt, dt_ros, horizon):
global X, Y, psi, stateEstimate_mark
# import track file
track = Tra('Tra_3', horizon)
rospy.init_node('toy_planner_waypoints', anonymous=True)
rospy.Subscriber('state_estimate', state_Dynamic, stateEstimateCallback)
pub = rospy.Publisher('waypoints', Waypoints, queue_size=1)
rate = rospy.Rate(1 / dt_ros)
# first set the horizon to be very large to get where the vehicle is
track.horizon = horizon
track.currentIndex = 0
num_points = 400
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 + int(dt / 0.01),
track.currentIndex + num_points, 1)
num_steps = int(
floor((track.t[track.currentIndex + num_points] -
track.t[track.currentIndex + 1]) / dt) + 1)
t = np.linspace(track.t[track.currentIndex],
track.t[track.currentIndex] + num_steps * dt,
num_steps)
t_sub = track.t[index_list]
x_sub = track.x[index_list]
y_sub = track.y[index_list]
spl_x = UnivariateSpline(t_sub, x_sub, k=3)
spl_y = UnivariateSpline(t_sub, y_sub, k=3)
spl_x_val = spl_x(t)
spl_y_val = spl_y(t)
waypoints = Waypoints()
for i in range(1, num_steps):
pt = Point2D()
pt.x = (spl_x_val[i] - X) * cos(psi) + (spl_y_val[i] -
Y) * sin(psi)
pt.y = -(spl_x_val[i] - X) * sin(psi) + (spl_y_val[i] -
Y) * cos(psi)
waypoints.points.append(pt)
waypoints.dt = dt
pub.publish(waypoints)
rate.sleep()
# define the initial states and timestep
ra_to_cg = 1.65
X = 0
Y = 0
vx = 0
vy = 0
psi = 0
X_ref = 0
Y_ref = 0
vx_ref = 0
vy_ref = 0
psi_ref = 0
stateEstimate_mark = True
Waypoints_mark = False
Waypoints_received = Waypoints()
state_received = state_Dynamic()
steering_ratio = 14.8
def stateEstimateCallback(data):
global X, Y, psi, vx, vy, stateEstimate_mark, state_received
X = data.X + cos(data.psi) * ra_to_cg
Y = data.Y + sin(psi) * ra_to_cg
vx = data.vx
vy = data.vy
psi = data.psi
stateEstimate_mark = True
state_received = data
def on_image_received(data):
# this would directly receive the raw image from the driver
if vehicle_real_speed_kmh is None:
return
global count_middle
count_middle += 1
if count_middle % IMAGE_DOWNSAMPLE_MAIN != 0:
return
time0 = time.time()
global bridge, driving_model, vis_pub_full, controller, waypoint_pub, parent_conn, dbw_enable, vis_pub_full_enabled
img = bridge.imgmsg_to_cv2(data, "bgr8")
# deliberately not resizing the image, since it might be zoomed later
#img = cv2.resize(img, (IM_WIDTH, IM_HEIGHT))
# flip because the camera is flipped
img = img[::-1, ::-1, :]
if use_left_right:
if left_cache is None or right_cache is None:
return
left_lock.acquire()
right_lock.acquire()
sensors = [left_cache, img, right_cache]
left_lock.release()
right_lock.release()
else:
sensors = [img]
t00 = time.time()
'''
print("before sending out the images")
parent_conn.send([sensors, vehicle_real_speed_kmh, direction])
print("before receving")
control_dict, vis = parent_conn.recv()
control = lambda : None
control.steer = control_dict["steer"]
control.throttle = control_dict["throttle"]
control.brake = control_dict["brake"]
print("after receiving")
'''
# TODO: fill the model name and current parameters
exp_id = sys.argv[1]
global message_from_controller
extra_extra = exp_id + "\n" + \
message_from_controller + \
"real_speed={:.2f} m/s \n".format(vehicle_real_speed_kmh/3.6)
control, vis = driving_model.compute_action(sensors, vehicle_real_speed_kmh, direction,
save_image_to_disk=False, return_vis=True, return_extra=False, extra_extra=extra_extra,
mapping_support={"town_id": "rfs", "pos": vehicle_pos, "ori": vehicle_yaw})
#print("time for compute action is ", time.time() - t00)
#control, vis = driving_model.compute_action(sensors, 0.0, direction, save_image_to_disk=False, return_vis=True)
global use_waypoint
if use_waypoint:
# shift the waypoint forward by the computation time
time_passed = time.time() - time0
time_list = np.array([0.2*(i+1) for i in range(control.shape[0])])
time_list -= time_passed # this typically less than 200ms
desired_time = np.array([0.2*(i+1) for i in range(control.shape[0]-1)])
wp0 = np.interp(desired_time, time_list, control[:, 0])
wp1 = np.interp(desired_time, time_list, control[:, 1])
control = np.stack([wp0, wp1], axis=1)
# TODO: have a manner to control the safety speed
waypoints = Waypoints()
for i in range(1, control.shape[0]):
pt = Point2D()
pt.x = control[i, 0]
pt.y = -control[i, 1]
waypoints.points.append(pt)
waypoints.dt = 0.2
waypoint_pub.publish(waypoints)
else:
v = Vector3()
v.x = control.steer
v.y = control.throttle
v.z = control.brake
global raw_control_pub
if raw_control_pub != None:
raw_control_pub.publish(v)
#cv2.imshow("Cameras", vis)
#cv2.waitKey(5)
msg = bridge.cv2_to_imgmsg(vis, "rgb8")
vis_pub_full.publish(msg)
if dbw_enable:
print("enabling, republishing the message to a second topic")
vis_pub_full_enabled.publish(msg)
time_now = time.time()
global last_computation
print ("total time for on image receive is ", time.time()-time0, ". Runs at ", 1.0 / (time_now - last_computation), "Hz")
last_computation = time.time()