#!/usr/bin/env python
# -*- coding: utf-8 -*-
 
import rospy
import numpy as np
import cv2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, CompressedImage

class DetectSign():
    def __init__(self):

        # parameter for communication
        # rospy.set_param('/tb2/mission_start', False)
        # rospy.set_param('/tb3/mission_start', False)        

        # color parameter 
        self.hue_red_l = rospy.get_param("~detect/lane/red/hue_l", 0)
        self.hue_red_h = rospy.get_param("~detect/lane/red/hue_h", 9)
        self.saturation_red_l = rospy.get_param("~detect/lane/red/saturation_l", 255)
        self.saturation_red_h = rospy.get_param("~detect/lane/red/saturation_h", 255)
        self.lightness_red_l = rospy.get_param("~detect/lane/red/lightness_l", 179)
        self.lightness_red_h = rospy.get_param("~detect/lane/red/lightness_h", 255)

        self.hue_green_l = rospy.get_param("~detect/lane/green/hue_l", 38)
        self.hue_green_h = rospy.get_param("~detect/lane/green/hue_h", 72)
        self.saturation_green_l = rospy.get_param("~detect/lane/green/saturation_l", 200)
        self.saturation_green_h = rospy.get_param("~detect/lane/green/saturation_h", 255)
        self.lightness_green_l = rospy.get_param("~detect/lane/green/lightness_l", 34)
        self.lightness_green_h = rospy.get_param("~detect/lane/green/lightness_h", 255)

        self.hue_blue_l = rospy.get_param("~detect/lane/blue/hue_l", 78)
        self.hue_blue_h = rospy.get_param("~detect/lane/blue/hue_h", 117)
        self.saturation_blue_l = rospy.get_param("~detect/lane/blue/saturation_l", 222)
        self.saturation_blue_h = rospy.get_param("~detect/lane/blue/saturation_h", 255)
        self.lightness_blue_l = rospy.get_param("~detect/lane/blue/lightness_l", 165)
        self.lightness_blue_h = rospy.get_param("~detect/lane/blue/lightness_h", 255)

        self.sub_image_type = "raw"          # "compressed" / "raw"
        self.pub_image_type = "raw"          # "compressed" / "raw"

        self.counter = 1
        
        if self.sub_image_type == "compressed":
            # subscribes compressed image
            self.sub_image_original = rospy.Subscriber('/detect/image_input/compressed', CompressedImage, self.cbGetImage, queue_size = 1)
        elif self.sub_image_type == "raw":
            # subscribes raw image
            self.sub_image_original = rospy.Subscriber('/detect/image_input', Image, self.cbGetImage, queue_size = 1)
 
        if self.pub_image_type == "compressed":
            # publishes compensated image in compressed type 
            self.pub_image_sign = rospy.Publisher('/detect/image_output/compressed', CompressedImage, queue_size = 1)
        elif self.pub_image_type == "raw":
            # publishes compensated image in raw type
            self.pub_image_sign = rospy.Publisher('/detect/image_output', Image, queue_size = 1)

        self.cvBridge = CvBridge()
        self.cv_image = None

        self.is_image_available = False

        rospy.sleep(1)

        loop_rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            if self.is_image_available == True:
                self.fnFindSign()
            loop_rate.sleep()

    def cbGetImage(self, image_msg):
        # drop the frame to 1/5 (6fps) because of the processing speed. This is up to your computer's operating power.
        if self.counter % 3 != 0:
            self.counter += 1
            return
        else:
            self.counter = 1

        if self.sub_image_type == "compressed":
            np_arr = np.fromstring(image_msg.data, np.uint8)
            self.cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        else:
            self.cv_image = self.cvBridge.imgmsg_to_cv2(image_msg, "bgr8")

        self.is_image_available = True

    def fnFindSign(self):
        cv_image_mask_g = self.fnMaskGreenSign()
        cv_image_mask_g = cv2.GaussianBlur(cv_image_mask_g,(5,5),0)
        self.detected_green = np.count_nonzero(cv_image_mask_g == 0)

        cv_image_mask_b = self.fnMaskBlueSign()
        cv_image_mask_b = cv2.GaussianBlur(cv_image_mask_b,(5,5),0)
        self.detected_blue = np.count_nonzero(cv_image_mask_b == 0)

        cv_image_mask_r = self.fnMaskRedSign()
        cv_image_mask_r = cv2.GaussianBlur(cv_image_mask_r,(5,5),0)
        self.detected_red = np.count_nonzero(cv_image_mask_r == 0)

        if self.detected_green > 3000 and self.detected_blue < 100 and self.detected_red < 100:
            rospy.set_param('/tb3/mission_start', True)
            print("TURTLEBOT3 MISSION START")   
        if self.detected_blue > 3000 and self.detected_green < 100 and self.detected_red < 100:
            rospy.set_param('/tb2/mission_start', True)
            print("TURTLEBOT2 MISSION START")  

        if self.detected_red > 1000 and self.detected_green < 100 and self.detected_blue < 100:
            rospy.set_param('/tb1/move_forward', False)
            print("WAIT FOR TB2 ALIGNED")

        if self.detected_red > 500 and self.detected_green < 100 and self.detected_blue < 100 and rospy.get_param('/tb2/aligned') == True:
            self.detected_red = 0
            rospy.set_param('/tb1/move_forward', True)
            print("GO TO PARKING LOT")

        if self.detected_blue > 1000 and self.detected_green > 1000:
            rospy.set_param('/tb1/move_forward', False)
            rospy.set_param('/tb1/parking_start', True)
            print("PARKING")

    def fnMaskRedSign(self):
        image = np.copy(self.cv_image)

        # Convert BGR to HSV
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

        Hue_l = self.hue_red_l
        Hue_h = self.hue_red_h
        Saturation_l = self.saturation_red_l
        Saturation_h = self.saturation_red_h
        Lightness_l = self.lightness_red_l
        Lightness_h = self.lightness_red_h

        # define range of red color in HSV
        lower_red = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_red = np.array([Hue_h, Saturation_h, Lightness_h])

        # Threshold the HSV image to get only red colors
        mask = cv2.inRange(hsv, lower_red, upper_red)

        # Bitwise-AND mask and original image
        res = cv2.bitwise_and(image, image, mask = mask)

        mask = cv2.bitwise_not(mask)

        return mask

    def fnMaskBlueSign(self):
        image = np.copy(self.cv_image)

        # Convert BGR to HSV
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

        Hue_l = self.hue_blue_l
        Hue_h = self.hue_blue_h
        Saturation_l = self.saturation_blue_l
        Saturation_h = self.saturation_blue_h
        Lightness_l = self.lightness_blue_l
        Lightness_h = self.lightness_blue_h

        # define range of blue color in HSV
        lower_blue = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_blue = np.array([Hue_h, Saturation_h, Lightness_h])

        # Threshold the HSV image to get only blue colors
        mask = cv2.inRange(hsv, lower_blue, upper_blue)

        # Bitwise-AND mask and original image
        res = cv2.bitwise_and(image, image, mask = mask)

        mask = cv2.bitwise_not(mask)

        return mask

    def fnMaskGreenSign(self):
        image = np.copy(self.cv_image)

        # Convert BGR to HSV
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

        Hue_l = self.hue_green_l
        Hue_h = self.hue_green_h
        Saturation_l = self.saturation_green_l
        Saturation_h = self.saturation_green_h
        Lightness_l = self.lightness_green_l
        Lightness_h = self.lightness_green_h

        # define range of green color in HSV
        lower_green = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_green = np.array([Hue_h, Saturation_h, Lightness_h])

        # Threshold the HSV image to get only green colors
        mask = cv2.inRange(hsv, lower_green, upper_green)

        # Bitwise-AND mask and original image
        res = cv2.bitwise_and(image, image, mask = mask)

        mask = cv2.bitwise_not(mask)

        return mask

    def main(self):
        rospy.spin()

if __name__ == '__main__':
    rospy.init_node('sign_detector')
    node = DetectSign()

074f440002fbb57996f95c719b48c027f107b272
<mask token>


class DetectSign:
    <mask token>
    <mask token>
    <mask token>

    def fnMaskRedSign(self):
        image = np.copy(self.cv_image)
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        Hue_l = self.hue_red_l
        Hue_h = self.hue_red_h
        Saturation_l = self.saturation_red_l
        Saturation_h = self.saturation_red_h
        Lightness_l = self.lightness_red_l
        Lightness_h = self.lightness_red_h
        lower_red = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_red = np.array([Hue_h, Saturation_h, Lightness_h])
        mask = cv2.inRange(hsv, lower_red, upper_red)
        res = cv2.bitwise_and(image, image, mask=mask)
        mask = cv2.bitwise_not(mask)
        return mask

    def fnMaskBlueSign(self):
        image = np.copy(self.cv_image)
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        Hue_l = self.hue_blue_l
        Hue_h = self.hue_blue_h
        Saturation_l = self.saturation_blue_l
        Saturation_h = self.saturation_blue_h
        Lightness_l = self.lightness_blue_l
        Lightness_h = self.lightness_blue_h
        lower_blue = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_blue = np.array([Hue_h, Saturation_h, Lightness_h])
        mask = cv2.inRange(hsv, lower_blue, upper_blue)
        res = cv2.bitwise_and(image, image, mask=mask)
        mask = cv2.bitwise_not(mask)
        return mask

    def fnMaskGreenSign(self):
        image = np.copy(self.cv_image)
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        Hue_l = self.hue_green_l
        Hue_h = self.hue_green_h
        Saturation_l = self.saturation_green_l
        Saturation_h = self.saturation_green_h
        Lightness_l = self.lightness_green_l
        Lightness_h = self.lightness_green_h
        lower_green = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_green = np.array([Hue_h, Saturation_h, Lightness_h])
        mask = cv2.inRange(hsv, lower_green, upper_green)
        res = cv2.bitwise_and(image, image, mask=mask)
        mask = cv2.bitwise_not(mask)
        return mask
    <mask token>


<mask token>

<mask token>


class DetectSign:

    def __init__(self):
        self.hue_red_l = rospy.get_param('~detect/lane/red/hue_l', 0)
        self.hue_red_h = rospy.get_param('~detect/lane/red/hue_h', 9)
        self.saturation_red_l = rospy.get_param('~detect/lane/red/saturation_l'
            , 255)
        self.saturation_red_h = rospy.get_param('~detect/lane/red/saturation_h'
            , 255)
        self.lightness_red_l = rospy.get_param('~detect/lane/red/lightness_l',
            179)
        self.lightness_red_h = rospy.get_param('~detect/lane/red/lightness_h',
            255)
        self.hue_green_l = rospy.get_param('~detect/lane/green/hue_l', 38)
        self.hue_green_h = rospy.get_param('~detect/lane/green/hue_h', 72)
        self.saturation_green_l = rospy.get_param(
            '~detect/lane/green/saturation_l', 200)
        self.saturation_green_h = rospy.get_param(
            '~detect/lane/green/saturation_h', 255)
        self.lightness_green_l = rospy.get_param(
            '~detect/lane/green/lightness_l', 34)
        self.lightness_green_h = rospy.get_param(
            '~detect/lane/green/lightness_h', 255)
        self.hue_blue_l = rospy.get_param('~detect/lane/blue/hue_l', 78)
        self.hue_blue_h = rospy.get_param('~detect/lane/blue/hue_h', 117)
        self.saturation_blue_l = rospy.get_param(
            '~detect/lane/blue/saturation_l', 222)
        self.saturation_blue_h = rospy.get_param(
            '~detect/lane/blue/saturation_h', 255)
        self.lightness_blue_l = rospy.get_param('~detect/lane/blue/lightness_l'
            , 165)
        self.lightness_blue_h = rospy.get_param('~detect/lane/blue/lightness_h'
            , 255)
        self.sub_image_type = 'raw'
        self.pub_image_type = 'raw'
        self.counter = 1
        if self.sub_image_type == 'compressed':
            self.sub_image_original = rospy.Subscriber(
                '/detect/image_input/compressed', CompressedImage, self.
                cbGetImage, queue_size=1)
        elif self.sub_image_type == 'raw':
            self.sub_image_original = rospy.Subscriber('/detect/image_input',
                Image, self.cbGetImage, queue_size=1)
        if self.pub_image_type == 'compressed':
            self.pub_image_sign = rospy.Publisher(
                '/detect/image_output/compressed', CompressedImage,
                queue_size=1)
        elif self.pub_image_type == 'raw':
            self.pub_image_sign = rospy.Publisher('/detect/image_output',
                Image, queue_size=1)
        self.cvBridge = CvBridge()
        self.cv_image = None
        self.is_image_available = False
        rospy.sleep(1)
        loop_rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            if self.is_image_available == True:
                self.fnFindSign()
            loop_rate.sleep()
    <mask token>
    <mask token>

    def fnMaskRedSign(self):
        image = np.copy(self.cv_image)
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        Hue_l = self.hue_red_l
        Hue_h = self.hue_red_h
        Saturation_l = self.saturation_red_l
        Saturation_h = self.saturation_red_h
        Lightness_l = self.lightness_red_l
        Lightness_h = self.lightness_red_h
        lower_red = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_red = np.array([Hue_h, Saturation_h, Lightness_h])
        mask = cv2.inRange(hsv, lower_red, upper_red)
        res = cv2.bitwise_and(image, image, mask=mask)
        mask = cv2.bitwise_not(mask)
        return mask

    def fnMaskBlueSign(self):
        image = np.copy(self.cv_image)
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        Hue_l = self.hue_blue_l
        Hue_h = self.hue_blue_h
        Saturation_l = self.saturation_blue_l
        Saturation_h = self.saturation_blue_h
        Lightness_l = self.lightness_blue_l
        Lightness_h = self.lightness_blue_h
        lower_blue = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_blue = np.array([Hue_h, Saturation_h, Lightness_h])
        mask = cv2.inRange(hsv, lower_blue, upper_blue)
        res = cv2.bitwise_and(image, image, mask=mask)
        mask = cv2.bitwise_not(mask)
        return mask

    def fnMaskGreenSign(self):
        image = np.copy(self.cv_image)
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        Hue_l = self.hue_green_l
        Hue_h = self.hue_green_h
        Saturation_l = self.saturation_green_l
        Saturation_h = self.saturation_green_h
        Lightness_l = self.lightness_green_l
        Lightness_h = self.lightness_green_h
        lower_green = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_green = np.array([Hue_h, Saturation_h, Lightness_h])
        mask = cv2.inRange(hsv, lower_green, upper_green)
        res = cv2.bitwise_and(image, image, mask=mask)
        mask = cv2.bitwise_not(mask)
        return mask

    def main(self):
        rospy.spin()


<mask token>

<mask token>


class DetectSign:

    def __init__(self):
        self.hue_red_l = rospy.get_param('~detect/lane/red/hue_l', 0)
        self.hue_red_h = rospy.get_param('~detect/lane/red/hue_h', 9)
        self.saturation_red_l = rospy.get_param('~detect/lane/red/saturation_l'
            , 255)
        self.saturation_red_h = rospy.get_param('~detect/lane/red/saturation_h'
            , 255)
        self.lightness_red_l = rospy.get_param('~detect/lane/red/lightness_l',
            179)
        self.lightness_red_h = rospy.get_param('~detect/lane/red/lightness_h',
            255)
        self.hue_green_l = rospy.get_param('~detect/lane/green/hue_l', 38)
        self.hue_green_h = rospy.get_param('~detect/lane/green/hue_h', 72)
        self.saturation_green_l = rospy.get_param(
            '~detect/lane/green/saturation_l', 200)
        self.saturation_green_h = rospy.get_param(
            '~detect/lane/green/saturation_h', 255)
        self.lightness_green_l = rospy.get_param(
            '~detect/lane/green/lightness_l', 34)
        self.lightness_green_h = rospy.get_param(
            '~detect/lane/green/lightness_h', 255)
        self.hue_blue_l = rospy.get_param('~detect/lane/blue/hue_l', 78)
        self.hue_blue_h = rospy.get_param('~detect/lane/blue/hue_h', 117)
        self.saturation_blue_l = rospy.get_param(
            '~detect/lane/blue/saturation_l', 222)
        self.saturation_blue_h = rospy.get_param(
            '~detect/lane/blue/saturation_h', 255)
        self.lightness_blue_l = rospy.get_param('~detect/lane/blue/lightness_l'
            , 165)
        self.lightness_blue_h = rospy.get_param('~detect/lane/blue/lightness_h'
            , 255)
        self.sub_image_type = 'raw'
        self.pub_image_type = 'raw'
        self.counter = 1
        if self.sub_image_type == 'compressed':
            self.sub_image_original = rospy.Subscriber(
                '/detect/image_input/compressed', CompressedImage, self.
                cbGetImage, queue_size=1)
        elif self.sub_image_type == 'raw':
            self.sub_image_original = rospy.Subscriber('/detect/image_input',
                Image, self.cbGetImage, queue_size=1)
        if self.pub_image_type == 'compressed':
            self.pub_image_sign = rospy.Publisher(
                '/detect/image_output/compressed', CompressedImage,
                queue_size=1)
        elif self.pub_image_type == 'raw':
            self.pub_image_sign = rospy.Publisher('/detect/image_output',
                Image, queue_size=1)
        self.cvBridge = CvBridge()
        self.cv_image = None
        self.is_image_available = False
        rospy.sleep(1)
        loop_rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            if self.is_image_available == True:
                self.fnFindSign()
            loop_rate.sleep()

    def cbGetImage(self, image_msg):
        if self.counter % 3 != 0:
            self.counter += 1
            return
        else:
            self.counter = 1
        if self.sub_image_type == 'compressed':
            np_arr = np.fromstring(image_msg.data, np.uint8)
            self.cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        else:
            self.cv_image = self.cvBridge.imgmsg_to_cv2(image_msg, 'bgr8')
        self.is_image_available = True

    def fnFindSign(self):
        cv_image_mask_g = self.fnMaskGreenSign()
        cv_image_mask_g = cv2.GaussianBlur(cv_image_mask_g, (5, 5), 0)
        self.detected_green = np.count_nonzero(cv_image_mask_g == 0)
        cv_image_mask_b = self.fnMaskBlueSign()
        cv_image_mask_b = cv2.GaussianBlur(cv_image_mask_b, (5, 5), 0)
        self.detected_blue = np.count_nonzero(cv_image_mask_b == 0)
        cv_image_mask_r = self.fnMaskRedSign()
        cv_image_mask_r = cv2.GaussianBlur(cv_image_mask_r, (5, 5), 0)
        self.detected_red = np.count_nonzero(cv_image_mask_r == 0)
        if (self.detected_green > 3000 and self.detected_blue < 100 and 
            self.detected_red < 100):
            rospy.set_param('/tb3/mission_start', True)
            print('TURTLEBOT3 MISSION START')
        if (self.detected_blue > 3000 and self.detected_green < 100 and 
            self.detected_red < 100):
            rospy.set_param('/tb2/mission_start', True)
            print('TURTLEBOT2 MISSION START')
        if (self.detected_red > 1000 and self.detected_green < 100 and self
            .detected_blue < 100):
            rospy.set_param('/tb1/move_forward', False)
            print('WAIT FOR TB2 ALIGNED')
        if (self.detected_red > 500 and self.detected_green < 100 and self.
            detected_blue < 100 and rospy.get_param('/tb2/aligned') == True):
            self.detected_red = 0
            rospy.set_param('/tb1/move_forward', True)
            print('GO TO PARKING LOT')
        if self.detected_blue > 1000 and self.detected_green > 1000:
            rospy.set_param('/tb1/move_forward', False)
            rospy.set_param('/tb1/parking_start', True)
            print('PARKING')

    def fnMaskRedSign(self):
        image = np.copy(self.cv_image)
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        Hue_l = self.hue_red_l
        Hue_h = self.hue_red_h
        Saturation_l = self.saturation_red_l
        Saturation_h = self.saturation_red_h
        Lightness_l = self.lightness_red_l
        Lightness_h = self.lightness_red_h
        lower_red = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_red = np.array([Hue_h, Saturation_h, Lightness_h])
        mask = cv2.inRange(hsv, lower_red, upper_red)
        res = cv2.bitwise_and(image, image, mask=mask)
        mask = cv2.bitwise_not(mask)
        return mask

    def fnMaskBlueSign(self):
        image = np.copy(self.cv_image)
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        Hue_l = self.hue_blue_l
        Hue_h = self.hue_blue_h
        Saturation_l = self.saturation_blue_l
        Saturation_h = self.saturation_blue_h
        Lightness_l = self.lightness_blue_l
        Lightness_h = self.lightness_blue_h
        lower_blue = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_blue = np.array([Hue_h, Saturation_h, Lightness_h])
        mask = cv2.inRange(hsv, lower_blue, upper_blue)
        res = cv2.bitwise_and(image, image, mask=mask)
        mask = cv2.bitwise_not(mask)
        return mask

    def fnMaskGreenSign(self):
        image = np.copy(self.cv_image)
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        Hue_l = self.hue_green_l
        Hue_h = self.hue_green_h
        Saturation_l = self.saturation_green_l
        Saturation_h = self.saturation_green_h
        Lightness_l = self.lightness_green_l
        Lightness_h = self.lightness_green_h
        lower_green = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_green = np.array([Hue_h, Saturation_h, Lightness_h])
        mask = cv2.inRange(hsv, lower_green, upper_green)
        res = cv2.bitwise_and(image, image, mask=mask)
        mask = cv2.bitwise_not(mask)
        return mask

    def main(self):
        rospy.spin()


if __name__ == '__main__':
    rospy.init_node('sign_detector')
    node = DetectSign()

import rospy
import numpy as np
import cv2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, CompressedImage


class DetectSign:

    def __init__(self):
        self.hue_red_l = rospy.get_param('~detect/lane/red/hue_l', 0)
        self.hue_red_h = rospy.get_param('~detect/lane/red/hue_h', 9)
        self.saturation_red_l = rospy.get_param('~detect/lane/red/saturation_l'
            , 255)
        self.saturation_red_h = rospy.get_param('~detect/lane/red/saturation_h'
            , 255)
        self.lightness_red_l = rospy.get_param('~detect/lane/red/lightness_l',
            179)
        self.lightness_red_h = rospy.get_param('~detect/lane/red/lightness_h',
            255)
        self.hue_green_l = rospy.get_param('~detect/lane/green/hue_l', 38)
        self.hue_green_h = rospy.get_param('~detect/lane/green/hue_h', 72)
        self.saturation_green_l = rospy.get_param(
            '~detect/lane/green/saturation_l', 200)
        self.saturation_green_h = rospy.get_param(
            '~detect/lane/green/saturation_h', 255)
        self.lightness_green_l = rospy.get_param(
            '~detect/lane/green/lightness_l', 34)
        self.lightness_green_h = rospy.get_param(
            '~detect/lane/green/lightness_h', 255)
        self.hue_blue_l = rospy.get_param('~detect/lane/blue/hue_l', 78)
        self.hue_blue_h = rospy.get_param('~detect/lane/blue/hue_h', 117)
        self.saturation_blue_l = rospy.get_param(
            '~detect/lane/blue/saturation_l', 222)
        self.saturation_blue_h = rospy.get_param(
            '~detect/lane/blue/saturation_h', 255)
        self.lightness_blue_l = rospy.get_param('~detect/lane/blue/lightness_l'
            , 165)
        self.lightness_blue_h = rospy.get_param('~detect/lane/blue/lightness_h'
            , 255)
        self.sub_image_type = 'raw'
        self.pub_image_type = 'raw'
        self.counter = 1
        if self.sub_image_type == 'compressed':
            self.sub_image_original = rospy.Subscriber(
                '/detect/image_input/compressed', CompressedImage, self.
                cbGetImage, queue_size=1)
        elif self.sub_image_type == 'raw':
            self.sub_image_original = rospy.Subscriber('/detect/image_input',
                Image, self.cbGetImage, queue_size=1)
        if self.pub_image_type == 'compressed':
            self.pub_image_sign = rospy.Publisher(
                '/detect/image_output/compressed', CompressedImage,
                queue_size=1)
        elif self.pub_image_type == 'raw':
            self.pub_image_sign = rospy.Publisher('/detect/image_output',
                Image, queue_size=1)
        self.cvBridge = CvBridge()
        self.cv_image = None
        self.is_image_available = False
        rospy.sleep(1)
        loop_rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            if self.is_image_available == True:
                self.fnFindSign()
            loop_rate.sleep()

    def cbGetImage(self, image_msg):
        if self.counter % 3 != 0:
            self.counter += 1
            return
        else:
            self.counter = 1
        if self.sub_image_type == 'compressed':
            np_arr = np.fromstring(image_msg.data, np.uint8)
            self.cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        else:
            self.cv_image = self.cvBridge.imgmsg_to_cv2(image_msg, 'bgr8')
        self.is_image_available = True

    def fnFindSign(self):
        cv_image_mask_g = self.fnMaskGreenSign()
        cv_image_mask_g = cv2.GaussianBlur(cv_image_mask_g, (5, 5), 0)
        self.detected_green = np.count_nonzero(cv_image_mask_g == 0)
        cv_image_mask_b = self.fnMaskBlueSign()
        cv_image_mask_b = cv2.GaussianBlur(cv_image_mask_b, (5, 5), 0)
        self.detected_blue = np.count_nonzero(cv_image_mask_b == 0)
        cv_image_mask_r = self.fnMaskRedSign()
        cv_image_mask_r = cv2.GaussianBlur(cv_image_mask_r, (5, 5), 0)
        self.detected_red = np.count_nonzero(cv_image_mask_r == 0)
        if (self.detected_green > 3000 and self.detected_blue < 100 and 
            self.detected_red < 100):
            rospy.set_param('/tb3/mission_start', True)
            print('TURTLEBOT3 MISSION START')
        if (self.detected_blue > 3000 and self.detected_green < 100 and 
            self.detected_red < 100):
            rospy.set_param('/tb2/mission_start', True)
            print('TURTLEBOT2 MISSION START')
        if (self.detected_red > 1000 and self.detected_green < 100 and self
            .detected_blue < 100):
            rospy.set_param('/tb1/move_forward', False)
            print('WAIT FOR TB2 ALIGNED')
        if (self.detected_red > 500 and self.detected_green < 100 and self.
            detected_blue < 100 and rospy.get_param('/tb2/aligned') == True):
            self.detected_red = 0
            rospy.set_param('/tb1/move_forward', True)
            print('GO TO PARKING LOT')
        if self.detected_blue > 1000 and self.detected_green > 1000:
            rospy.set_param('/tb1/move_forward', False)
            rospy.set_param('/tb1/parking_start', True)
            print('PARKING')

    def fnMaskRedSign(self):
        image = np.copy(self.cv_image)
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        Hue_l = self.hue_red_l
        Hue_h = self.hue_red_h
        Saturation_l = self.saturation_red_l
        Saturation_h = self.saturation_red_h
        Lightness_l = self.lightness_red_l
        Lightness_h = self.lightness_red_h
        lower_red = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_red = np.array([Hue_h, Saturation_h, Lightness_h])
        mask = cv2.inRange(hsv, lower_red, upper_red)
        res = cv2.bitwise_and(image, image, mask=mask)
        mask = cv2.bitwise_not(mask)
        return mask

    def fnMaskBlueSign(self):
        image = np.copy(self.cv_image)
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        Hue_l = self.hue_blue_l
        Hue_h = self.hue_blue_h
        Saturation_l = self.saturation_blue_l
        Saturation_h = self.saturation_blue_h
        Lightness_l = self.lightness_blue_l
        Lightness_h = self.lightness_blue_h
        lower_blue = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_blue = np.array([Hue_h, Saturation_h, Lightness_h])
        mask = cv2.inRange(hsv, lower_blue, upper_blue)
        res = cv2.bitwise_and(image, image, mask=mask)
        mask = cv2.bitwise_not(mask)
        return mask

    def fnMaskGreenSign(self):
        image = np.copy(self.cv_image)
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        Hue_l = self.hue_green_l
        Hue_h = self.hue_green_h
        Saturation_l = self.saturation_green_l
        Saturation_h = self.saturation_green_h
        Lightness_l = self.lightness_green_l
        Lightness_h = self.lightness_green_h
        lower_green = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_green = np.array([Hue_h, Saturation_h, Lightness_h])
        mask = cv2.inRange(hsv, lower_green, upper_green)
        res = cv2.bitwise_and(image, image, mask=mask)
        mask = cv2.bitwise_not(mask)
        return mask

    def main(self):
        rospy.spin()


if __name__ == '__main__':
    rospy.init_node('sign_detector')
    node = DetectSign()

#!/usr/bin/env python
# -*- coding: utf-8 -*-
 
import rospy
import numpy as np
import cv2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, CompressedImage

class DetectSign():
    def __init__(self):

        # parameter for communication
        # rospy.set_param('/tb2/mission_start', False)
        # rospy.set_param('/tb3/mission_start', False)        

        # color parameter 
        self.hue_red_l = rospy.get_param("~detect/lane/red/hue_l", 0)
        self.hue_red_h = rospy.get_param("~detect/lane/red/hue_h", 9)
        self.saturation_red_l = rospy.get_param("~detect/lane/red/saturation_l", 255)
        self.saturation_red_h = rospy.get_param("~detect/lane/red/saturation_h", 255)
        self.lightness_red_l = rospy.get_param("~detect/lane/red/lightness_l", 179)
        self.lightness_red_h = rospy.get_param("~detect/lane/red/lightness_h", 255)

        self.hue_green_l = rospy.get_param("~detect/lane/green/hue_l", 38)
        self.hue_green_h = rospy.get_param("~detect/lane/green/hue_h", 72)
        self.saturation_green_l = rospy.get_param("~detect/lane/green/saturation_l", 200)
        self.saturation_green_h = rospy.get_param("~detect/lane/green/saturation_h", 255)
        self.lightness_green_l = rospy.get_param("~detect/lane/green/lightness_l", 34)
        self.lightness_green_h = rospy.get_param("~detect/lane/green/lightness_h", 255)

        self.hue_blue_l = rospy.get_param("~detect/lane/blue/hue_l", 78)
        self.hue_blue_h = rospy.get_param("~detect/lane/blue/hue_h", 117)
        self.saturation_blue_l = rospy.get_param("~detect/lane/blue/saturation_l", 222)
        self.saturation_blue_h = rospy.get_param("~detect/lane/blue/saturation_h", 255)
        self.lightness_blue_l = rospy.get_param("~detect/lane/blue/lightness_l", 165)
        self.lightness_blue_h = rospy.get_param("~detect/lane/blue/lightness_h", 255)

        self.sub_image_type = "raw"          # "compressed" / "raw"
        self.pub_image_type = "raw"          # "compressed" / "raw"

        self.counter = 1
        
        if self.sub_image_type == "compressed":
            # subscribes compressed image
            self.sub_image_original = rospy.Subscriber('/detect/image_input/compressed', CompressedImage, self.cbGetImage, queue_size = 1)
        elif self.sub_image_type == "raw":
            # subscribes raw image
            self.sub_image_original = rospy.Subscriber('/detect/image_input', Image, self.cbGetImage, queue_size = 1)
 
        if self.pub_image_type == "compressed":
            # publishes compensated image in compressed type 
            self.pub_image_sign = rospy.Publisher('/detect/image_output/compressed', CompressedImage, queue_size = 1)
        elif self.pub_image_type == "raw":
            # publishes compensated image in raw type
            self.pub_image_sign = rospy.Publisher('/detect/image_output', Image, queue_size = 1)

        self.cvBridge = CvBridge()
        self.cv_image = None

        self.is_image_available = False

        rospy.sleep(1)

        loop_rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            if self.is_image_available == True:
                self.fnFindSign()
            loop_rate.sleep()

    def cbGetImage(self, image_msg):
        # drop the frame to 1/5 (6fps) because of the processing speed. This is up to your computer's operating power.
        if self.counter % 3 != 0:
            self.counter += 1
            return
        else:
            self.counter = 1

        if self.sub_image_type == "compressed":
            np_arr = np.fromstring(image_msg.data, np.uint8)
            self.cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        else:
            self.cv_image = self.cvBridge.imgmsg_to_cv2(image_msg, "bgr8")

        self.is_image_available = True

    def fnFindSign(self):
        cv_image_mask_g = self.fnMaskGreenSign()
        cv_image_mask_g = cv2.GaussianBlur(cv_image_mask_g,(5,5),0)
        self.detected_green = np.count_nonzero(cv_image_mask_g == 0)

        cv_image_mask_b = self.fnMaskBlueSign()
        cv_image_mask_b = cv2.GaussianBlur(cv_image_mask_b,(5,5),0)
        self.detected_blue = np.count_nonzero(cv_image_mask_b == 0)

        cv_image_mask_r = self.fnMaskRedSign()
        cv_image_mask_r = cv2.GaussianBlur(cv_image_mask_r,(5,5),0)
        self.detected_red = np.count_nonzero(cv_image_mask_r == 0)

        if self.detected_green > 3000 and self.detected_blue < 100 and self.detected_red < 100:
            rospy.set_param('/tb3/mission_start', True)
            print("TURTLEBOT3 MISSION START")   
        if self.detected_blue > 3000 and self.detected_green < 100 and self.detected_red < 100:
            rospy.set_param('/tb2/mission_start', True)
            print("TURTLEBOT2 MISSION START")  

        if self.detected_red > 1000 and self.detected_green < 100 and self.detected_blue < 100:
            rospy.set_param('/tb1/move_forward', False)
            print("WAIT FOR TB2 ALIGNED")

        if self.detected_red > 500 and self.detected_green < 100 and self.detected_blue < 100 and rospy.get_param('/tb2/aligned') == True:
            self.detected_red = 0
            rospy.set_param('/tb1/move_forward', True)
            print("GO TO PARKING LOT")

        if self.detected_blue > 1000 and self.detected_green > 1000:
            rospy.set_param('/tb1/move_forward', False)
            rospy.set_param('/tb1/parking_start', True)
            print("PARKING")

    def fnMaskRedSign(self):
        image = np.copy(self.cv_image)

        # Convert BGR to HSV
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

        Hue_l = self.hue_red_l
        Hue_h = self.hue_red_h
        Saturation_l = self.saturation_red_l
        Saturation_h = self.saturation_red_h
        Lightness_l = self.lightness_red_l
        Lightness_h = self.lightness_red_h

        # define range of red color in HSV
        lower_red = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_red = np.array([Hue_h, Saturation_h, Lightness_h])

        # Threshold the HSV image to get only red colors
        mask = cv2.inRange(hsv, lower_red, upper_red)

        # Bitwise-AND mask and original image
        res = cv2.bitwise_and(image, image, mask = mask)

        mask = cv2.bitwise_not(mask)

        return mask

    def fnMaskBlueSign(self):
        image = np.copy(self.cv_image)

        # Convert BGR to HSV
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

        Hue_l = self.hue_blue_l
        Hue_h = self.hue_blue_h
        Saturation_l = self.saturation_blue_l
        Saturation_h = self.saturation_blue_h
        Lightness_l = self.lightness_blue_l
        Lightness_h = self.lightness_blue_h

        # define range of blue color in HSV
        lower_blue = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_blue = np.array([Hue_h, Saturation_h, Lightness_h])

        # Threshold the HSV image to get only blue colors
        mask = cv2.inRange(hsv, lower_blue, upper_blue)

        # Bitwise-AND mask and original image
        res = cv2.bitwise_and(image, image, mask = mask)

        mask = cv2.bitwise_not(mask)

        return mask

    def fnMaskGreenSign(self):
        image = np.copy(self.cv_image)

        # Convert BGR to HSV
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

        Hue_l = self.hue_green_l
        Hue_h = self.hue_green_h
        Saturation_l = self.saturation_green_l
        Saturation_h = self.saturation_green_h
        Lightness_l = self.lightness_green_l
        Lightness_h = self.lightness_green_h

        # define range of green color in HSV
        lower_green = np.array([Hue_l, Saturation_l, Lightness_l])
        upper_green = np.array([Hue_h, Saturation_h, Lightness_h])

        # Threshold the HSV image to get only green colors
        mask = cv2.inRange(hsv, lower_green, upper_green)

        # Bitwise-AND mask and original image
        res = cv2.bitwise_and(image, image, mask = mask)

        mask = cv2.bitwise_not(mask)

        return mask

    def main(self):
        rospy.spin()

if __name__ == '__main__':
    rospy.init_node('sign_detector')
    node = DetectSign()
