def __init__(self):
self.img_prep = ImagePreparator()
self.camera = None
self.horizon_y = None
self.transformation_matrix = None
self.image_resolution = DEFAULT_RESOLUTION
self.transformated_image_resolution = DEFAULT_RESOLUTION
def __init__(self, node_name, sub_topic, pub_topic):
self.bridge = CvBridge()
self.img_prep = ImagePreparator()
self.image_pub = rospy.Publisher(pub_topic,
Image,
queue_size=QUEUE_SIZE)
rospy.init_node(node_name, anonymous=True)
self.image_sub = rospy.Subscriber(sub_topic, Image, self.callback)
rospy.spin()
class RoiNode:
def __init__(self, node_name, sub_topic, pub_topic):
self.bridge = CvBridge()
self.img_prep = ImagePreparator()
self.image_pub = rospy.Publisher(pub_topic,
Image,
queue_size=QUEUE_SIZE)
rospy.init_node(node_name, anonymous=True)
self.image_sub = rospy.Subscriber(sub_topic, Image, self.callback)
rospy.spin()
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
rospy.logerr(e)
above_value = rospy.get_param("/autonomous_driving/roi_node/above", 0)
below_value = rospy.get_param("/autonomous_driving/roi_node/below", 0)
roi = self.img_prep.define_roi(cv_image, above_value, below_value)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(roi, "bgr8"))
except CvBridgeError as e:
rospy.logerr(e)
class SmoothingNode:
def __init__(self, node_name, sub_topic, pub_topic):
self.bridge = CvBridge()
self.img_prep = ImagePreparator()
self.image_pub = rospy.Publisher(pub_topic,
Image,
queue_size=QUEUE_SIZE)
rospy.init_node(node_name, anonymous=True)
self.image_sub = rospy.Subscriber(sub_topic, Image, self.callback)
rospy.spin()
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "mono8")
except CvBridgeError as e:
rospy.logerr(e)
deviation = rospy.get_param(
"/autonomous_driving/smoothing_node/deviation", 5)
border = rospy.get_param("/autonomous_driving/smoothing_node/border",
0)
blurred = self.img_prep.blur(cv_image, (deviation, deviation), border)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(blurred, "mono8"))
except CvBridgeError as e:
rospy.logerr(e)
class GrayscaleNode:
def __init__(self, node_name, sub_topic, pub_topic):
self.bridge = CvBridge()
self.img_prep = ImagePreparator()
self.image_pub = rospy.Publisher(pub_topic,
Image,
queue_size=QUEUE_SIZE)
rospy.init_node(node_name, anonymous=True)
self.image_sub = rospy.Subscriber(sub_topic, Image, self.callback)
rospy.spin()
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
rospy.logerr(e)
gray = self.img_prep.grayscale(cv_image)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(gray, "mono8"))
except CvBridgeError as e:
rospy.logerr(e)
def main():
print os.getcwd()
capture = cv2.VideoCapture('../data/road.avi')
ticks = 0
img_prep = ImagePreparator()
vis = Visualizer()
while capture.isOpened():
prevTick = ticks
ticks = cv2.getTickCount()
t = (ticks - prevTick) / cv2.getTickFrequency()
fps = int(1 / t)
retval, image = capture.read()
height, width, channels = image.shape
rect = np.array([[0, 200], [639, 200], [639, 359], [0, 359]],
dtype="float32")
dst = np.array([[0, 0], [639, 0], [350, 699], [298, 699]],
dtype="float32")
# Aufbereitung des Bilder
warped = img_prep.warp_perspective(image.copy(), rect, dst, (640, 700))
roi = img_prep.define_roi(warped, 0.6, 0, 0.40)
gray = img_prep.grayscale(roi)
blur = img_prep.blur(gray, (5, 5), 0)
canny = img_prep.edge_detection(blur, 50, 150, 3)
vis.draw_text(canny, 'FPS: ' + str(fps), 1, (255, 0, 0),
(int(width * 0.015), int(height * 0.15)))
vis.show(canny)
def __init__(self, node_name, sub_topic, pub_topic):
self.img_prep = ImagePreparator()
self.bridge = CvBridge()
self.camera = None
self.horizon_y = None
self.transformation_matrix = None
self.image_resolution = DEFAULT_RESOLUTION
self.transformated_image_resolution = DEFAULT_RESOLUTION
self.image_pub = rospy.Publisher(pub_topic,
Image,
queue_size=QUEUE_SIZE)
rospy.init_node(node_name, anonymous=True)
self.image_sub = rospy.Subscriber(sub_topic, Image, self.callback)
rospy.spin()
class InversePerspectiveMapping:
def __init__(self):
self.img_prep = ImagePreparator()
self.camera = None
self.horizon_y = None
self.transformation_matrix = None
self.image_resolution = DEFAULT_RESOLUTION
self.transformated_image_resolution = DEFAULT_RESOLUTION
def initializeTransformationMatrix(self, cv_image):
if self._camera_needs_to_be_initialized(cv_image):
self._initialize_camera_parameters(cv_image)
self._calculate_transformation_matrix()
if self.transformation_matrix is None:
self._calculate_transformation_matrix()
def warp(self, cv_image):
return self.img_prep.warp_perspective(
cv_image, self.transformation_matrix,
self.transformated_image_resolution)
def _calculate_world_coordinates(self):
p1 = self.camera.image_to_world_coordinates(self.horizon_y, 0)
p2 = self.camera.image_to_world_coordinates(
self.horizon_y, self.image_resolution[1] - 1)
p3 = self.camera.image_to_world_coordinates(
self.image_resolution[0] - 1, self.image_resolution[1] - 1)
p4 = self.camera.image_to_world_coordinates(
self.image_resolution[0] - 1, 0)
return p1, p2, p3, p4
def _calculate_destination_points(self, p1_w, p2_w, p3_w, p4_w):
max_y = max(p1_w[0], p2_w[0], p3_w[0], p4_w[0])
y_factor = self.image_resolution[1] / float(max_y)
min_x = min(p1_w[1], p2_w[1], p3_w[1], p4_w[1])
p1_new = ((p1_w[1] - min_x) * y_factor,
self.image_resolution[0] - 1 - p1_w[0] * y_factor)
p2_new = ((p2_w[1] - min_x) * y_factor,
self.image_resolution[0] - 1 - p2_w[0] * y_factor)
p3_new = ((p3_w[1] - min_x) * y_factor,
self.image_resolution[0] - 1 - p3_w[0] * y_factor)
p4_new = ((p4_w[1] - min_x) * y_factor,
self.image_resolution[0] - 1 - p4_w[0] * y_factor)
return p1_new, p2_new, p3_new, p4_new
def _camera_needs_to_be_initialized(self, image):
height, width, _ = image.shape
return self.camera is None or self.horizon_y is None or cmp(
self.image_resolution, (height, width)) is not 0
def _initialize_camera_parameters(self, image):
height, width, _ = image.shape
self.image_resolution = (height, width)
self.camera = Camera(h=20, aperture=140, m=width, n=height)
self.horizon_y = self.camera.get_horizon_y() + int(
height * DEFAULT_HORIZON_CORRECTION)
print("New Camera Parameters", self.image_resolution)
print("New horizon", self.horizon_y)
def _calculate_transformation_matrix(self):
p1_w, p2_w, p3_w, p4_w = self._calculate_world_coordinates()
rect = np.array(
[[0, self.horizon_y], [
self.image_resolution[1] - 1, self.horizon_y
], [self.image_resolution[1] - 1, self.image_resolution[0] - 1],
[0, self.image_resolution[0] - 1]],
dtype="float32")
p1_new, p2_new, p3_new, p4_new = self._calculate_destination_points(
p1_w, p2_w, p3_w, p4_w)
dst = np.array([[p1_new[0], p1_new[1]], [p2_new[0], p2_new[1]],
[p3_new[0], p3_new[1]], [p4_new[0], p4_new[1]]],
dtype="float32")
self.transformation_matrix = cv2.getPerspectiveTransform(rect, dst)
self.transformated_image_resolution = (
int(p2_new[0]), self.image_resolution[0]
) # width: most right point / height: height from orignal image
class InversePerspectiveMappingNode:
def __init__(self, node_name, sub_topic, pub_topic):
self.img_prep = ImagePreparator()
self.bridge = CvBridge()
self.camera = None
self.horizon_y = None
self.transformation_matrix = None
self.image_resolution = DEFAULT_RESOLUTION
self.transformated_image_resolution = DEFAULT_RESOLUTION
self.image_pub = rospy.Publisher(pub_topic,
Image,
queue_size=QUEUE_SIZE)
rospy.init_node(node_name, anonymous=True)
self.image_sub = rospy.Subscriber(sub_topic, Image, self.callback)
rospy.spin()
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
rospy.logerr(e)
if self._camera_needs_to_be_initialized(cv_image):
self._initialize_camera_parameters(cv_image)
self._calculate_transformation_matrix()
if self.transformation_matrix is None:
self._calculate_transformation_matrix()
warped = self.img_prep.warp_perspective(
cv_image, self.transformation_matrix,
self.transformated_image_resolution)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(warped, "bgr8"))
except CvBridgeError as e:
rospy.logerr(e)
def _calculate_world_coordinates(self):
p1 = self.camera.image_to_world_coordinates(self.horizon_y, 0)
p2 = self.camera.image_to_world_coordinates(
self.horizon_y, self.image_resolution[1] - 1)
p3 = self.camera.image_to_world_coordinates(
self.image_resolution[0] - 1, self.image_resolution[1] - 1)
p4 = self.camera.image_to_world_coordinates(
self.image_resolution[0] - 1, 0)
return p1, p2, p3, p4
def _calculate_destination_points(self, p1_w, p2_w, p3_w, p4_w):
max_y = max(p1_w[0], p2_w[0], p3_w[0], p4_w[0])
y_factor = self.image_resolution[1] / float(max_y)
min_x = min(p1_w[1], p2_w[1], p3_w[1], p4_w[1])
p1_new = ((p1_w[1] - min_x) * y_factor,
self.image_resolution[0] - 1 - p1_w[0] * y_factor)
p2_new = ((p2_w[1] - min_x) * y_factor,
self.image_resolution[0] - 1 - p2_w[0] * y_factor)
p3_new = ((p3_w[1] - min_x) * y_factor,
self.image_resolution[0] - 1 - p3_w[0] * y_factor)
p4_new = ((p4_w[1] - min_x) * y_factor,
self.image_resolution[0] - 1 - p4_w[0] * y_factor)
return p1_new, p2_new, p3_new, p4_new
def _camera_needs_to_be_initialized(self, image):
height, width, _ = image.shape
return self.camera is None or self.horizon_y is None or cmp(
self.image_resolution, (height, width)) is not 0
def _initialize_camera_parameters(self, image):
height, width, _ = image.shape
self.image_resolution = (height, width)
self.camera = Camera(h=20, aperture=140, m=width, n=height)
self.horizon_y = self.camera.get_horizon_y() + int(
height * DEFAULT_HORIZON_CORRECTION)
rospy.loginfo("New Camera Parameters (%s)", self.image_resolution)
rospy.loginfo("New horizon (%s)", self.horizon_y)
def _calculate_transformation_matrix(self):
p1_w, p2_w, p3_w, p4_w = self._calculate_world_coordinates()
rect = np.array(
[[0, self.horizon_y], [
self.image_resolution[1] - 1, self.horizon_y
], [self.image_resolution[1] - 1, self.image_resolution[0] - 1],
[0, self.image_resolution[0] - 1]],
dtype="float32")
p1_new, p2_new, p3_new, p4_new = self._calculate_destination_points(
p1_w, p2_w, p3_w, p4_w)
dst = np.array([[p1_new[0], p1_new[1]], [p2_new[0], p2_new[1]],
[p3_new[0], p3_new[1]], [p4_new[0], p4_new[1]]],
dtype="float32")
self.transformation_matrix = cv2.getPerspectiveTransform(rect, dst)
self.transformated_image_resolution = (
int(p2_new[0]), self.image_resolution[0]
) # width: most right point / height: height from orignal image
def __init__(self, node_name, sub_topic, sub_base_throttle_topic,
pub_topic, pub_setpoint_topic, pub_state_topic,
pub_throttle_topic, reset_service):
self.bridge = CvBridge()
self.img_prep = ImagePreparator()
self.ipm = InversePerspectiveMapping()
# Publisher
self.image_pub = rospy.Publisher(pub_topic,
Image,
queue_size=QUEUE_SIZE)
self.setpoint_pub = rospy.Publisher(pub_setpoint_topic,
Float64,
queue_size=QUEUE_SIZE)
self.state_pub = rospy.Publisher(pub_state_topic,
Float64,
queue_size=QUEUE_SIZE)
self.throttle_pub = rospy.Publisher(pub_throttle_topic,
Float64,
queue_size=QUEUE_SIZE)
self.reset_srv = rospy.Service(reset_service, Empty,
self.reset_callback)
self.reset_tracking = False
rospy.init_node(node_name, anonymous=True)
self.image_sub = rospy.Subscriber(sub_topic, Image, self.callback)
self.base_throttle_sub = rospy.Subscriber(sub_base_throttle_topic,
Float64,
self.callbackBaseThrottle)
# Base Throttle
self.base_throttle = rospy.get_param(
"/autonomous_driving/lane_detection_node/base_throttle", 0.6)
# Crop Parameters
self.above_value = rospy.get_param(
"/autonomous_driving/lane_detection_node/above", 0.58)
self.below_value = rospy.get_param(
"/autonomous_driving/lane_detection_node/below", 0.1)
self.side_value = rospy.get_param(
"/autonomous_driving/lane_detection_node/side", 0.3)
# Lane Tracking Parameters
self.deviation = rospy.get_param(
"/autonomous_driving/lane_detection_node/deviation", 5)
self.border = rospy.get_param(
"/autonomous_driving/lane_detection_node/border", 0)
# Canny Parameters
self.threshold_low = rospy.get_param(
"/autonomous_driving/lane_detection_node/threshold_low", 50)
self.threshold_high = rospy.get_param(
"/autonomous_driving/lane_detection_node/threshold_high", 150)
self.aperture = rospy.get_param(
"/autonomous_driving/lane_detection_node/aperture", 3)
# Lane Tracking
self.init_lanemodel()
rospy.spin()
class LaneDetectionNode:
def __init__(self, node_name, sub_topic, sub_base_throttle_topic,
pub_topic, pub_setpoint_topic, pub_state_topic,
pub_throttle_topic, reset_service):
self.bridge = CvBridge()
self.img_prep = ImagePreparator()
self.ipm = InversePerspectiveMapping()
# Publisher
self.image_pub = rospy.Publisher(pub_topic,
Image,
queue_size=QUEUE_SIZE)
self.setpoint_pub = rospy.Publisher(pub_setpoint_topic,
Float64,
queue_size=QUEUE_SIZE)
self.state_pub = rospy.Publisher(pub_state_topic,
Float64,
queue_size=QUEUE_SIZE)
self.throttle_pub = rospy.Publisher(pub_throttle_topic,
Float64,
queue_size=QUEUE_SIZE)
self.reset_srv = rospy.Service(reset_service, Empty,
self.reset_callback)
self.reset_tracking = False
rospy.init_node(node_name, anonymous=True)
self.image_sub = rospy.Subscriber(sub_topic, Image, self.callback)
self.base_throttle_sub = rospy.Subscriber(sub_base_throttle_topic,
Float64,
self.callbackBaseThrottle)
# Base Throttle
self.base_throttle = rospy.get_param(
"/autonomous_driving/lane_detection_node/base_throttle", 0.6)
# Crop Parameters
self.above_value = rospy.get_param(
"/autonomous_driving/lane_detection_node/above", 0.58)
self.below_value = rospy.get_param(
"/autonomous_driving/lane_detection_node/below", 0.1)
self.side_value = rospy.get_param(
"/autonomous_driving/lane_detection_node/side", 0.3)
# Lane Tracking Parameters
self.deviation = rospy.get_param(
"/autonomous_driving/lane_detection_node/deviation", 5)
self.border = rospy.get_param(
"/autonomous_driving/lane_detection_node/border", 0)
# Canny Parameters
self.threshold_low = rospy.get_param(
"/autonomous_driving/lane_detection_node/threshold_low", 50)
self.threshold_high = rospy.get_param(
"/autonomous_driving/lane_detection_node/threshold_high", 150)
self.aperture = rospy.get_param(
"/autonomous_driving/lane_detection_node/aperture", 3)
# Lane Tracking
self.init_lanemodel()
rospy.spin()
def callbackBaseThrottle(self, data):
self.base_throttle = data.data
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
rospy.logerr(e)
# Inverse Perspective Mapping
self.ipm.initializeTransformationMatrix(cv_image)
warped = self.ipm.warp(cv_image)
# crop
cropped = self.img_prep.crop(warped, self.above_value,
self.below_value, self.side_value)
# grayscale
gray = self.img_prep.grayscale(cropped)
# blur
blurred = self.img_prep.blur(gray, (self.deviation, self.deviation),
self.border)
# canny
canny = self.img_prep.edge_detection(blurred, self.threshold_low,
self.threshold_high,
self.aperture)
heigth, width = canny.shape
if width == 63: # TODO dirty hack
cv2.line(canny, (0, 4 / 2), (18 / 2, heigth), (0, 0, 0), 2)
cv2.line(canny, (width, 4 / 2), (width - 18 / 2, heigth),
(0, 0, 0), 2)
else:
cv2.line(canny, (0, 4), (18, heigth), (0, 0, 0), 2)
cv2.line(canny, (width, 4), (width - 18, heigth), (0, 0, 0), 2)
# Lane Detection
canny = cv2.cvtColor(canny, cv2.COLOR_GRAY2BGR)
self.lane_model.update_segments(canny.copy())
self.lane_model.draw_segments(canny)
state_point_x = self.lane_model.state_point_x()
if self.reset_tracking is True:
self.init_lanemodel()
self.reset_tracking = False
# publish to pid
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(canny, "bgr8"))
self.setpoint_pub.publish(0.0)
if state_point_x:
heigth, width, _ = canny.shape
deviation = state_point_x - int(width / 2)
self.state_pub.publish(deviation)
devThrottle = abs(deviation / 30.0)
if devThrottle < 0.1:
devThrottle = 0.0
elif devThrottle > 0.25:
devThrottle = 0.25
self.throttle_pub.publish(
(1.0 - devThrottle) * self.base_throttle)
except CvBridgeError as e:
rospy.logerr(e)
def init_lanemodel(self):
lane_width = rospy.get_param(
"/autonomous_driving/lane_detection_node/lane_width",
DEFAULT_LANE_WIDTH)
segment_start = rospy.get_param(
"/autonomous_driving/lane_detection_node/segment_start",
DEFAULT_SEGMENT_START)
segment_amount = rospy.get_param(
"/autonomous_driving/lane_detection_node/segment_amount",
DEFAULT_SEGMENT_AMOUNT)
self.lane_model = LaneModel(lane_width, segment_amount, segment_start)
def reset_callback(self, req):
rospy.loginfo("Reset Lanetracking")
self.reset_tracking = True
return EmptyResponse()