def save_image_and_steering_angle(video_file, output_path):
lane_follower = HandCodedLaneFollower()
cap = cv2.VideoCapture(video_file + '.avi')
try:
i = 4646
while cap.isOpened():
_, frame = cap.read()
final_frame , control_ang, ang_deg = lane_follower.follow_lane(frame)
cv2.imwrite(output_path + "%s_%03d_%03d.png" % ("video", i, ang_deg), frame)
i += 1
if cv2.waitKey(1) & 0xFF == ord('q'):
break
finally:
cap.release()
cv2.destroyAllWindows()
def __init__(self, settings, pkg_dir):
self.settings = settings
print("Creating Directory Structure")
self.base_dir = pkg_dir
self.out_dir = self.base_dir + "/output/" + self.settings['subdir'] + "/"
self.out_data_dir = self.out_dir + "data/"
self.out_info_dir = self.out_dir + "info/"
self.out_media_roads = self.out_dir + "source_media/roads/"
self.out_media_backgrounds = self.out_dir + "source_media/backgrounds/"
self.lane_follower = HandCodedLaneFollower()
print("Directory generation may produce error if dir exists, or for true errors")
for directory in [self.out_dir, self.out_data_dir, self.out_info_dir, self.out_media_backgrounds, self.out_media_roads]:
try:
os.makedirs(directory)
except OSError as e:
print(e)
pass
if settings['clear'] and raw_input('Confirm Delete Existing files in ' + settings['subdir'] +' (y/n): ') == 'y':
print('Clearing Existing Files')
def clear_dir(directory):
for filename in os.listdir(directory):
os.remove(directory + filename)
clear_dir(self.out_data_dir)
clear_dir(self.out_info_dir)
clear_dir(self.out_media_backgrounds)
clear_dir(self.out_media_roads)
else:
print('Not Clearing')
self.media_dir = self.base_dir + "/source_media/"
self.roads_dir = self.media_dir + "roads/"
self.backgrounds_dir = self.media_dir + "backgrounds/"
self.settings['road_images'] = []
for image_file in self.settings['road_image_files']:
img = cv2.imread(self.roads_dir + image_file, cv2.IMREAD_COLOR)
self.settings['road_images'].append(img)
self.settings['background_images'] = []
for image_file in self.settings['background_image_files']:
img = cv2.imread(self.backgrounds_dir + image_file, cv2.IMREAD_COLOR)
img = cv2.resize(img, (WIDTH, HEIGHT), interpolation=cv2.INTER_AREA)
self.settings['background_images'].append(img)
FOURCC = cv2.VideoWriter_fourcc(*'XVID')
RECORD = False
def create_video_recorder(path, width, height):
return cv2.VideoWriter(path, FOURCC, 20.0, (width, height))
if __name__ == "__main__":
car = ClientGenerator.CreateRCCarClient(
rospy.get_param("client_context_type"),
rospy.get_param("client_comm_type"))
basic_cam_calc = BasicCameraCalc()
lidar_calc = LidarCalc()
lane_follower = HandCodedLaneFollower()
status_msg = SimpleLaneFollowStatus()
status_pub = rospy.Publisher("/status",
SimpleLaneFollowStatus,
queue_size=3)
rospack = rospkg.RosPack()
package_dir = rospack.get_path("auto_rc_car_demos")
video_filepath = package_dir + "/training_data/video"
img, dt = car.get_latest_camera_rgb()
img = car.camera_to_cv_image(img)
height, width, _ = img.shape
datestr = datetime.datetime.now().strftime("%y%m%d_%H%M%S")
from auto_rc_car_demos.test_training import img_preprocess
if __name__ == "__main__":
rospack = rospkg.RosPack()
car = ClientGenerator.CreateRCCarClient(
rospy.get_param("client_context_type"),
rospy.get_param("client_comm_type"))
model = keras.models.load_model(
rospack.get_path("auto_rc_car_demos") +
"/output_model/lane_navigation_final.h5")
basic_cam_calc = BasicCameraCalc()
lidar_calc = LidarCalc()
lane_follower = HandCodedLaneFollower()
status_msg = SimpleLaneFollowStatus()
status_pub = rospy.Publisher("/status",
SimpleLaneFollowStatus,
queue_size=3)
print("Starting")
dist = 0
threshold = 15
base_speed = 8.0 #8.0
steer_speed_scale = 26.0 #20.0
t_start = rospy.Time.now()
timeout = rospy.get_param("timeout", -1)
class Generator:
def __init__(self, settings, pkg_dir):
self.settings = settings
print("Creating Directory Structure")
self.base_dir = pkg_dir
self.out_dir = self.base_dir + "/output/" + self.settings['subdir'] + "/"
self.out_data_dir = self.out_dir + "data/"
self.out_info_dir = self.out_dir + "info/"
self.out_media_roads = self.out_dir + "source_media/roads/"
self.out_media_backgrounds = self.out_dir + "source_media/backgrounds/"
self.lane_follower = HandCodedLaneFollower()
print("Directory generation may produce error if dir exists, or for true errors")
for directory in [self.out_dir, self.out_data_dir, self.out_info_dir, self.out_media_backgrounds, self.out_media_roads]:
try:
os.makedirs(directory)
except OSError as e:
print(e)
pass
if settings['clear'] and raw_input('Confirm Delete Existing files in ' + settings['subdir'] +' (y/n): ') == 'y':
print('Clearing Existing Files')
def clear_dir(directory):
for filename in os.listdir(directory):
os.remove(directory + filename)
clear_dir(self.out_data_dir)
clear_dir(self.out_info_dir)
clear_dir(self.out_media_backgrounds)
clear_dir(self.out_media_roads)
else:
print('Not Clearing')
self.media_dir = self.base_dir + "/source_media/"
self.roads_dir = self.media_dir + "roads/"
self.backgrounds_dir = self.media_dir + "backgrounds/"
self.settings['road_images'] = []
for image_file in self.settings['road_image_files']:
img = cv2.imread(self.roads_dir + image_file, cv2.IMREAD_COLOR)
self.settings['road_images'].append(img)
self.settings['background_images'] = []
for image_file in self.settings['background_image_files']:
img = cv2.imread(self.backgrounds_dir + image_file, cv2.IMREAD_COLOR)
img = cv2.resize(img, (WIDTH, HEIGHT), interpolation=cv2.INTER_AREA)
self.settings['background_images'].append(img)
def run(self):
for i in range(0, self.settings['N']):
print("Generating sample %d of %d" % (i+1, self.settings['N']))
config = None
while config is None or not config.is_valid():
config = GeneratorConfigSample(self.settings)
self.generate_image(config)
print("Saving Source Media")
for name, image in zip(self.settings['road_image_files'], self.settings['road_images']):
cv2.imwrite(self.out_media_roads + name, image)
for name, image in zip(self.settings['background_image_files'], self.settings['background_images']):
cv2.imwrite(self.out_media_roads + name, image)
with open(self.out_dir + "/gen_settings.yaml", 'w') as sett_file:
yaml.dump(self.settings, sett_file)
def generate_image(self, config):
segment = config.road_image
seg_width, seg_height, _ = segment.shape
road = config.background_image
centerpoints = []
left_corners = []
right_corners = []
centerpoints.append(RoadPoint(config.X0, config.Y0, -config.CAM_HEIGHT, -config.YAW0, config.ROAD_WIDTH))
left_corners.append(centerpoints[0].getLeftCorner())
right_corners.append(centerpoints[0].getRightCorner())
dTheta = 0.0
if not config.straight:
dTheta = config.DY / (1.0 / config.inv_rad)
else:
config.inv_rad = 0.0
for i in range(0, config.Segments):
pt = centerpoints[-1].nextPoint(config.DY, dTheta=dTheta)
centerpoints.append(pt)
left_corners.append(pt.getLeftCorner())
right_corners.append(pt.getRightCorner())
for i in range(1, len(centerpoints)):
bl = left_corners[i-1]
br = right_corners[i-1]
ul = left_corners[i]
ur = right_corners[i]
source = np.float32([[0,0], [seg_width, 0], [0, seg_height], [seg_width, seg_height]])
dest = np.float32([self.pointToPixels(ul, config),
self.pointToPixels(ur, config),
self.pointToPixels(bl, config),
self.pointToPixels(br, config)])
transformation = cv2.getPerspectiveTransform(source, dest)
new = cv2.warpPerspective(segment, transformation, (WIDTH,HEIGHT))
road[np.where((new != [0,0,0]).all(axis=2))] = [0,0,0]
road = cv2.add(road, new)
sample_uuid = str(uuid.uuid4())
final_frame, final_steering_angle, ang_deg = self.lane_follower.follow_lane(road)
cv2.imwrite(self.out_data_dir + '/' + sample_uuid + '.png', road)
with open(self.out_info_dir + "/" + sample_uuid + ".yaml", 'w') as file:
dict_to_write = config.gen_info_dict()
dict_to_write['steering_angle'] = ang_deg
dict_to_write['steering_command'] = final_steering_angle
yaml.dump(dict_to_write, file)
def pointToPixels(self, pos, config):
# http://www.cse.psu.edu/~rtc12/CSE486/lecture12.pdf
# http://www.cse.psu.edu/~rtc12/CSE486/lecture13.pdf
'''World Coordinates
X+ is left
Y+ is up
Z+ is out/forward
'''
X = -pos[0]
Y = pos[2]
Z = pos[1]
f = 1
'''Image Coordinates
Same as before
'''
x = config.FOCAL_LENGTH * X/Z
y = config.FOCAL_LENGTH * Y/Z
'''Pixel Coordinates
From top-left corner
u+ is right
v+ is down
'''
u = x/config.PIXEL_SIZE + WIDTH/2
v = y/config.PIXEL_SIZE + HEIGHT/2
v = HEIGHT - v
return [u,v]
from auto_rc_car_msgs.msg import SimpleLaneFollowStatus
if __name__ == "__main__":
rospack = rospkg.RosPack()
car = ClientGenerator.CreateRCCarClient(
rospy.get_param("client_context_type"),
rospy.get_param("client_comm_type"))
model = keras.models.load_model(
rospack.get_path("road_dataset_generation") +
"/test_model/test_model_blank.model")
basic_cam_calc = BasicCameraCalc()
lidar_calc = LidarCalc()
lane_follower = HandCodedLaneFollower()
status_msg = SimpleLaneFollowStatus()
status_pub = rospy.Publisher("/status",
SimpleLaneFollowStatus,
queue_size=3)
print("Starting")
dist = 0
threshold = 15
base_speed = 8.0 #8.0
steer_speed_scale = 26.0 #20.0
t_start = rospy.Time.now()
timeout = rospy.get_param("timeout", -1)
#!/usr/bin/env python
import cv2
import numpy as np
import logging
import math
import datetime
import sys
import rospkg
from auto_rc_car_demos.basic_camera import BasicCameraCalc, HandCodedLaneFollower
if __name__ == '__main__':
lane_follower = HandCodedLaneFollower()
rospack = rospkg.RosPack()
package_dir = rospack.get_path("auto_rc_car_demos")
lane_follower.test_photo(package_dir + '/test_images/rviz_lane_image.png')