def __init__(self):
self.yoloBoxes = BoundingBoxes()
self.trafficLightBoxes = BoundingBoxes()
self.ldw_inst = laneDepartureWarning()
self.image_pub = rospy.Publisher("adasresult", Image,queue_size = 1)
self.networkret_pub = rospy.Publisher("networkret", Image,queue_size = 1)
self.img = np.zeros([CFG.imgWidth, CFG.imgHeight, 3],np.uint8)
self.bridge = CvBridge()
def __init__(self):
self.image_pub = rospy.Publisher("lanedetframe", Image, queue_size=1)
self.maskimg_pub = rospy.Publisher("lanedetmask", Image, queue_size=1)
self.binimg_pub = rospy.Publisher("lanedetbin", Image, queue_size=1)
self.morphoimg_pub = rospy.Publisher("lanedetmorph",
Image,
queue_size=1)
# self.bridge = CvBridge()
self.yolo_sub = rospy.Subscriber("YOLO_detect_result_boxes",
BoundingBoxes, self.callbackyolo)
self.tlight_sub = rospy.Subscriber("tlight_detect_result_boxes",
BoundingBoxes,
self.callbackTrafficLight)
# self.image_sub = rospy.Subscriber("YOLO_detect_result", Image, self.callbackRos)
# self.image_sub = rospy.Subscriber("/camera/image_color", Image, self.callbackRos)
self.image_sub = rospy.Subscriber("/wideangle/image_color", Image,
self.callbackRos)
# self.image_sub = message_filters.Subscriber("/camera/rgb/image_raw", Image，queue_size=1, buff_size=110592*6)
self.weights_file = rospy.get_param("lanenet_weight")
self.CUDA = torch.cuda.is_available()
self.postprocessor = LaneNetPostProcessor()
self.warningModule = Detection()
self.band_width = 1.5
self.image_X = CFG.IMAGE_WIDTH
self.image_Y = CFG.IMAGE_HEIGHT
self.car_X = self.image_X / 2
self.car_Y = self.image_Y
self.model = LaneNet(pretrained=False,
embed_dim=4,
delta_v=.5,
delta_d=3.)
self.save_dict = torch.load(self.weights_file, map_location='cuda:0')
self.model.load_state_dict(self.save_dict['net'])
# self.model.load_state_dict(torch.load(self.weights_file, map_location='cuda:0'))
if self.CUDA: self.model.cuda()
self.model.set_test()
self.lastlane = np.ndarray(4, )
self.bridge = CvBridge()
self.leftlane = Lane('left')
self.rightlane = Lane('right')
self.tracker = LaneTracker()
# self.out = cv2.VideoWriter(str(time.time())+'testwrite.avi',cv2.VideoWriter_fourcc(*'MJPG'), 10.0, (CFG.IMAGE_WIDTH, CFG.IMAGE_HEIGHT),True)
self.img = np.zeros([CFG.IMAGE_WIDTH, CFG.IMAGE_HEIGHT, 3], np.uint8)
self.yoloBoxes = BoundingBoxes()
self.trafficLightBoxes = BoundingBoxes()
self.warning = 0
def __init__(self):
self.boxes = BoundingBoxes()
self.box = BoundingBox()
self.image_pub = rospy.Publisher("YOLO_detect_result",
Image,
queue_size=1)
self.boxes_pub = rospy.Publisher("YOLO_detect_result_boxes",
BoundingBoxes,
queue_size=1)
# self.result = rospy.Publisher('YOLO_detect_result', Float64MultiArray, queue_size=10)
self.bridge = CvBridge()
#self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.callback)
# self.image_sub = rospy.Subscriber("/wideangle/image_color", Image, self.callback)
self.image_sub = rospy.Subscriber('/cam_front/csi_cam/image_raw',
Image, self.callback)
self.batch_size = 1
self.reso = 416
self.confidence = 0.5
self.nms_thesh = 0.4
self.CUDA = torch.cuda.is_available()
self.num_classes = 80
# self.classes = load_classes("/home/iairiv/code/yolo/src/yolo_detection/src/data/coco.names")
# self.cfg_file = "/home/iairiv/code/yolo/src/yolo_detection/src/cfg/yolov3.cfg"
# self.weights_file = "/home/iairiv/code/yolo/src/yolo_detection/src/yolov3.weights"
self.colors = random_color()
# self.classes = load_classes("/space/code/rosadas/src/yolo_detection/src/data/coco.names")
# self.cfg_file = "/space/code/rosadas/src/yolo_detection/src/cfg/yolov3.cfg"
# self.weights_file = "/space/code/rosadas/src/yolo_detection/src/yolov3.weights"
self.classes = load_classes(
rospy.get_param(
"yolo_classname",
"/space/code/rosadas/src/yolo_detection/src/data/coco.names"))
self.cfg_file = rospy.get_param(
"yolo_cfg",
"/space/code/rosadas/src/yolo_detection/src/cfg/yolov3.cfg")
self.weights_file = rospy.get_param(
"yolo_weight",
"/space/code/rosadas/src/yolo_detection/src/yolov3.weights")
self.model = Darknet(self.cfg_file)
self.model.load_weights(self.weights_file)
self.model.net_info["height"] = self.reso
if self.CUDA: self.model.cuda()
self.model.eval()
self.send_by_UDP = False
self.draw_res = True
def callback(self, data):
print('callback!!!!')
startt = time.time()
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
cv_image = cv2.resize(cv_image, (0, 0),
fx=0.5,
fy=0.5,
interpolation=cv2.INTER_NEAREST)
input_image = self.transform_input(cv_image)
prediction = self.detection(input_image)
# self.UDP.UDP_data_transform_queue(prediction[0], self.reso, self.cls_conf)
if prediction[0] is None:
result = cv_image
else:
# print prediction
im_dim_list = [(cv_image.shape[1], cv_image.shape[0])]
im_dim_list = torch.FloatTensor(im_dim_list).repeat(1, 2)
if self.CUDA:
im_dim_list = im_dim_list.cuda()
scaling_factor = torch.min(self.reso / im_dim_list,
1)[0].view(-1, 1)
print(scaling_factor)
tlightRet = prediction[0]
tlightRet[:,
[0, 2]] -= (self.reso -
scaling_factor * im_dim_list[:, 0]) / 2
tlightRet[:,
[1, 3]] -= (self.reso -
scaling_factor * im_dim_list[:, 1]) / 2
tlightRet[:, 0:4] /= scaling_factor
result = self.write(prediction, cv_image)
tlightRet[:, [0, 2]] *= 2
tlightRet[:, [1, 3]] *= 2
except CvBridgeError as e:
print('CvBridgeError;', e)
# cv2.imshow("image windows", result)
# cv2.waitKey(3)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(result, "bgr8"))
except CvBridgeError as e:
print(e)
boxes = BoundingBoxes()
if prediction[0] is None:
detec_len = 0
else:
detec_len = len(prediction[0])
for i in range(detec_len):
box = BoundingBox()
# box.num = prediction[i][0]
box.xmin = tlightRet[i][0]
box.ymin = tlightRet[i][1]
box.xmax = tlightRet[i][2]
box.ymax = tlightRet[i][3]
# box.probability = prediction[i][6]
box.id = "{0}".format(self.classes[int(tlightRet[i][-1])])
#self.box_pub.publish(self.box)
boxes.bounding_boxes.append(box)
boxes.objNum = detec_len
boxes.header.stamp = rospy.Time.now()
self.boxes_pub.publish(boxes)
print('light det all use：', time.time() - startt)
def callback(self, data):
startt = time.time()
try:
start_time = rospy.Time.now()
start_time_second = start_time.to_sec()
timeArray = time.localtime(start_time_second)
timeArray_H_M_S = time.strftime("%H_%M_%S", timeArray)
nano_seconds = str(
int(start_time.to_nsec() -
int(start_time_second) * 1e9)).zfill(9)
timeArray_H_M_S_MS = timeArray_H_M_S + "_" + nano_seconds[:3]
print(timeArray_H_M_S_MS)
# YOLO detect
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
# cv_image = cv2.resize(cv_image, (0, 0), fx=0.5, fy=0.5, interpolation=cv2.INTER_NEAREST)
input_image = self.transform_input(cv_image)
prediction = self.yolo_detection(input_image)
# print(type(prediction))
# coordinate transformation
if type(prediction) == int:
if self.draw_res == True:
result = cv_image
# if self.send_by_UDP:
# self.UDP.send_message(timeArray_H_M_S_MS, None)
else:
# image size should be the same with the size when we calibrate
im_dim_list_list = [(cv_image.shape[1], cv_image.shape[0])]
# print im_dim_list_list
im_dim_list = torch.FloatTensor(im_dim_list_list).repeat(1, 2)
if self.CUDA:
im_dim_list = im_dim_list.cuda()
scaling_factor = torch.min(self.reso / im_dim_list,
1)[0].view(-1, 1)
prediction[:,
[1, 3]] -= (self.reso -
scaling_factor * im_dim_list[:, 0]) / 2
prediction[:,
[2, 4]] -= (self.reso -
scaling_factor * im_dim_list[:, 1]) / 2
prediction[:, 1:5] /= scaling_factor
prediction[:, [1, 3]] = torch.clamp(prediction[:, [1, 3]], 0.0,
im_dim_list_list[0][0])
prediction[:, [2, 4]] = torch.clamp(prediction[:, [2, 4]], 0.0,
im_dim_list_list[0][1])
# print prediction
# UDP send
# if self.send_by_UDP:
# self.UDP.send_message(timeArray_H_M_S_MS, prediction.cpu().numpy().tolist())
# draw Image
if self.draw_res:
result = self.write(prediction, cv_image)
# pub.publish(self.boxes)
except CvBridgeError as e:
print(e)
# return prediction
#
# # cv2.imshow("image windows", result)
# # cv2.waitKey(3)
#
try:
# prediction = prediction.cpu().numpy().tolist()
# boxes = self.boxes.bounding_boxes()
# print(type(prediction))
boxes = BoundingBoxes()
if type(prediction) == int:
detec_len = 0
else:
detec_len = len(prediction)
for i in range(detec_len):
box = BoundingBox()
box.num = prediction[i][0]
box.xmin = prediction[i][1]
box.ymin = prediction[i][2]
box.xmax = prediction[i][3]
box.ymax = prediction[i][4]
box.probability = prediction[i][6]
box.id = "{0}".format(self.classes[int(prediction[i][7])])
#self.box_pub.publish(self.box)
boxes.bounding_boxes.append(box)
boxes.objNum = detec_len
boxes.header.stamp = rospy.Time.now()
self.image_pub.publish(self.bridge.cv2_to_imgmsg(result, "bgr8"))
self.boxes_pub.publish(boxes)
except CvBridgeError as e:
print(e)
# time.sleep(0.05)
print('yolo use：', time.time() - startt)