<reponame>BhargavRE25/Rover-Machine-Learning
#!/usr/bin/env python
# Converts laser scan to planar distance
# and segments into obstalces and just hills

import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import LaserScan, Imu, Image
from tf.transformations import euler_from_quaternion
from cv_bridge import CvBridge, CvBridgeError
import cv2
from copy import deepcopy
import math
import scipy.stats
import numpy as np


def to_hsv(img):
    return cv2.cvtColor(img, cv2.COLOR_RGB2HSV)


class ObjectDetector:
    """Handles the logic of detecting the cubesat, the base, and the base front.
    """
    # color that ALWAYS occurs in a base connected component and 
    # NEVER occurs in a cubesat or rover connected component
    def __init__(self):
        pass

    base_key = ((5, 100, 100), (15, 255, 255))

    # color range of base, cubesat and rover
    object_ranges = [
        ((10, 25, 70), (60, 255, 255))
    ]

    # Base Front Color Range
    front_ranges = [
        ((75, 50, 50), (150, 255, 255))
    ]

    def segment(self, hsv_img):

        base_mask = self.detect_base(hsv_img)
        base_mask[base_mask > 0] = 100
        base_front_mask = self.detect_base_front(hsv_img)
        base_front_mask[base_front_mask > 0] = 150
        cubesat_mask = self.detect_cubesat(hsv_img)
        cubesat_mask[cubesat_mask > 0] = 50
        rover_mask = self.detect_rover(hsv_img)
        rover_mask[rover_mask > 0] = 00

        return base_mask + base_front_mask + cubesat_mask + rover_mask

    def detect_base(self, hsv_img):
        mask = self.comp_mask(hsv_img, self.object_ranges, self.base_key, inverse_key=False)
        return mask

    def detect_base_front(self, hsv_img):
        mask = self.comp_mask(hsv_img, self.front_ranges, max_width_height_ratio=2)
        return mask

    def detect_cubesat(self, hsv_img):
        mask = self.comp_mask(hsv_img, self.object_ranges, self.base_key, inverse_key=True)
        return mask

    def detect_rover(self, hsv_img):
        mask = self.comp_mask(hsv_img, self.object_ranges, self.base_key, inverse_key=True)
        return mask

    def comp_mask(self, hsv_img, ranges, key=None, inverse_key=False, max_width_height_ratio=None):
        mask = None
        for r in ranges:
            new_mask = cv2.inRange(hsv_img, r[0], r[1])
            if mask is None:
                mask = new_mask
            else:
                mask = cv2.bitwise_or(mask, new_mask)
        # now grow mask to encompass some background and connect things up
        kernel = np.ones((3, 3))
        mask = cv2.dilate(mask, kernel, iterations=4)
        # now remove mask elements of outlier low pixels
        connected_comps = cv2.connectedComponentsWithStats(mask, 8)
        if connected_comps[0] == 1:  # only background detected (i.e. no component)
            return mask  # return early
        labels = connected_comps[1]
        pixel_counts = [i[4] for i in connected_comps[2][1:]]
        avg_pixel_count = np.mean([max(pixel_counts), np.mean(pixel_counts)])
        min_count = min(pixel_counts)
        if avg_pixel_count / min_count < 4:  # no indication of "too small bits"
            threshold = 0
        else:
            threshold = min_count + (5 if (avg_pixel_count - min_count) / 4 < 5 else (avg_pixel_count - min_count) / 4)
        for idx, stats in enumerate(connected_comps[2]):
            if stats[4] < threshold:
                out_mask = np.array(labels, dtype=np.uint8)
                out_mask[labels == idx] = 0.0
                out_mask[labels != idx] = 1.0
                mask = cv2.bitwise_and(mask, out_mask)
        # Now strip any connected components that don't have key color if key is specified
        if key is not None or max_width_height_ratio is not None:
            conn_comps = cv2.connectedComponentsWithStats(mask, 8)
            labels = conn_comps[1]
            # print('Pre-cut # comps:', conn_comps[0] - 1)
            for lab in range(1, conn_comps[0]):
                do_cut = False
                out_mask = np.array(labels, dtype=np.uint8)
                out_mask[labels == lab] = 1
                out_mask[labels != lab] = 0
                if key is not None:
                    cropped = cv2.bitwise_and(hsv_img, hsv_img, mask=out_mask)
                    new_mask = cv2.inRange(cropped, key[0], key[1])
                    no_match = np.all(new_mask == 0)
                    do_cut = do_cut or ((no_match and not inverse_key) or (not no_match and inverse_key))
                if max_width_height_ratio is not None:
                    _, _, width, height, _ = conn_comps[2][lab]
                    if (width / height) > max_width_height_ratio:
                        do_cut = True
                if do_cut:  # colors that should or shouldn't be there
                    cutout = np.array(out_mask)
                    cutout[out_mask == 0] = 1
                    cutout[out_mask > 0] = 0
                    mask = cv2.bitwise_and(mask, mask, mask=cutout)  # cut out this component
        comps = cv2.connectedComponentsWithStats(mask, 8)
        # print('Final # connected components:', comps[0] - 1)
        return mask


class ImageSegmenter:

    """ Takes in stereo camera depth image and produces estimates of detected
    obstacles
    """

    def __init__(self):

        rospy.init_node("image_segmenter")

        # TODO: remove hard coded constant
        self.NUM_BUCKETS = 4
        self.bridge = CvBridge()
        self.detector = ObjectDetector()

        img_topic = rospy.get_param('~image_topic', '/stereo/image_proc_right/image_rect_color')
        depth_img_topic = rospy.get_param('~image_topic', '/stereo/image_proc_right/image_rect_color')
        pub_topic = rospy.get_param('~tagged_image_topic', '/segmented_image')

        # subscriber and publishers
        self.image_mask_pub = rospy.Publisher(pub_topic, Image, queue_size=4)
        rospy.Subscriber(img_topic, Image, callback=self.segment_callback)
        # rospy.Subscriber(depth_img_topic, Image, callback=self.depth_callback)

        rospy.spin()

    def segment_callback(self, data):

        img = self.bridge.imgmsg_to_cv2(data, "passthrough")
        hsv_img = to_hsv(img)
        segmented_frame = self.detector.segment(hsv_img)

        try:
            self.image_mask_pub.publish(self.bridge.cv2_to_imgmsg(segmented_frame, "mono8"))
        except CvBridgeError as e:
            print(e)

    def depth_callback(self, data):
        img = self.bridge.imgmsg_to_cv2(data, "32FC1")
        depths = np.array(img, dtype=np.float32)
        width = len(depths[0])
        buckets = [[] for i in range(self.NUM_BUCKETS)]
        for row in depths:
            for i, val in enumerate(row):
                if not np.isnan(val):
                    deg = self._idx_to_degree(i, len(depths[0]))
                    idx = self._deg_to_bucket(deg, self.NUM_BUCKETS)
                    buckets[idx].append(val)


if __name__ == "__main__":
    ImageSegmenter()

#!/usr/bin/env python
# Converts laser scan to planar distance
# and segments into obstalces and just hills

import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import LaserScan, Imu, Image
from tf.transformations import euler_from_quaternion
from cv_bridge import CvBridge, CvBridgeError
import cv2
from copy import deepcopy
import math
import scipy.stats
import numpy as np


def to_hsv(img):
    return cv2.cvtColor(img, cv2.COLOR_RGB2HSV)


class ObjectDetector:
    """Handles the logic of detecting the cubesat, the base, and the base front.
    """
    # color that ALWAYS occurs in a base connected component and 
    # NEVER occurs in a cubesat or rover connected component
    def __init__(self):
        pass

    base_key = ((5, 100, 100), (15, 255, 255))

    # color range of base, cubesat and rover
    object_ranges = [
        ((10, 25, 70), (60, 255, 255))
    ]

    # Base Front Color Range
    front_ranges = [
        ((75, 50, 50), (150, 255, 255))
    ]

    def segment(self, hsv_img):

        base_mask = self.detect_base(hsv_img)
        base_mask[base_mask > 0] = 100
        base_front_mask = self.detect_base_front(hsv_img)
        base_front_mask[base_front_mask > 0] = 150
        cubesat_mask = self.detect_cubesat(hsv_img)
        cubesat_mask[cubesat_mask > 0] = 50
        rover_mask = self.detect_rover(hsv_img)
        rover_mask[rover_mask > 0] = 00

        return base_mask + base_front_mask + cubesat_mask + rover_mask

    def detect_base(self, hsv_img):
        mask = self.comp_mask(hsv_img, self.object_ranges, self.base_key, inverse_key=False)
        return mask

    def detect_base_front(self, hsv_img):
        mask = self.comp_mask(hsv_img, self.front_ranges, max_width_height_ratio=2)
        return mask

    def detect_cubesat(self, hsv_img):
        mask = self.comp_mask(hsv_img, self.object_ranges, self.base_key, inverse_key=True)
        return mask

    def detect_rover(self, hsv_img):
        mask = self.comp_mask(hsv_img, self.object_ranges, self.base_key, inverse_key=True)
        return mask

    def comp_mask(self, hsv_img, ranges, key=None, inverse_key=False, max_width_height_ratio=None):
        mask = None
        for r in ranges:
            new_mask = cv2.inRange(hsv_img, r[0], r[1])
            if mask is None:
                mask = new_mask
            else:
                mask = cv2.bitwise_or(mask, new_mask)
        # now grow mask to encompass some background and connect things up
        kernel = np.ones((3, 3))
        mask = cv2.dilate(mask, kernel, iterations=4)
        # now remove mask elements of outlier low pixels
        connected_comps = cv2.connectedComponentsWithStats(mask, 8)
        if connected_comps[0] == 1:  # only background detected (i.e. no component)
            return mask  # return early
        labels = connected_comps[1]
        pixel_counts = [i[4] for i in connected_comps[2][1:]]
        avg_pixel_count = np.mean([max(pixel_counts), np.mean(pixel_counts)])
        min_count = min(pixel_counts)
        if avg_pixel_count / min_count < 4:  # no indication of "too small bits"
            threshold = 0
        else:
            threshold = min_count + (5 if (avg_pixel_count - min_count) / 4 < 5 else (avg_pixel_count - min_count) / 4)
        for idx, stats in enumerate(connected_comps[2]):
            if stats[4] < threshold:
                out_mask = np.array(labels, dtype=np.uint8)
                out_mask[labels == idx] = 0.0
                out_mask[labels != idx] = 1.0
                mask = cv2.bitwise_and(mask, out_mask)
        # Now strip any connected components that don't have key color if key is specified
        if key is not None or max_width_height_ratio is not None:
            conn_comps = cv2.connectedComponentsWithStats(mask, 8)
            labels = conn_comps[1]
            # print('Pre-cut # comps:', conn_comps[0] - 1)
            for lab in range(1, conn_comps[0]):
                do_cut = False
                out_mask = np.array(labels, dtype=np.uint8)
                out_mask[labels == lab] = 1
                out_mask[labels != lab] = 0
                if key is not None:
                    cropped = cv2.bitwise_and(hsv_img, hsv_img, mask=out_mask)
                    new_mask = cv2.inRange(cropped, key[0], key[1])
                    no_match = np.all(new_mask == 0)
                    do_cut = do_cut or ((no_match and not inverse_key) or (not no_match and inverse_key))
                if max_width_height_ratio is not None:
                    _, _, width, height, _ = conn_comps[2][lab]
                    if (width / height) > max_width_height_ratio:
                        do_cut = True
                if do_cut:  # colors that should or shouldn't be there
                    cutout = np.array(out_mask)
                    cutout[out_mask == 0] = 1
                    cutout[out_mask > 0] = 0
                    mask = cv2.bitwise_and(mask, mask, mask=cutout)  # cut out this component
        comps = cv2.connectedComponentsWithStats(mask, 8)
        # print('Final # connected components:', comps[0] - 1)
        return mask


class ImageSegmenter:

    """ Takes in stereo camera depth image and produces estimates of detected
    obstacles
    """

    def __init__(self):

        rospy.init_node("image_segmenter")

        # TODO: remove hard coded constant
        self.NUM_BUCKETS = 4
        self.bridge = CvBridge()
        self.detector = ObjectDetector()

        img_topic = rospy.get_param('~image_topic', '/stereo/image_proc_right/image_rect_color')
        depth_img_topic = rospy.get_param('~image_topic', '/stereo/image_proc_right/image_rect_color')
        pub_topic = rospy.get_param('~tagged_image_topic', '/segmented_image')

        # subscriber and publishers
        self.image_mask_pub = rospy.Publisher(pub_topic, Image, queue_size=4)
        rospy.Subscriber(img_topic, Image, callback=self.segment_callback)
        # rospy.Subscriber(depth_img_topic, Image, callback=self.depth_callback)

        rospy.spin()

    def segment_callback(self, data):

        img = self.bridge.imgmsg_to_cv2(data, "passthrough")
        hsv_img = to_hsv(img)
        segmented_frame = self.detector.segment(hsv_img)

        try:
            self.image_mask_pub.publish(self.bridge.cv2_to_imgmsg(segmented_frame, "mono8"))
        except CvBridgeError as e:
            print(e)

    def depth_callback(self, data):
        img = self.bridge.imgmsg_to_cv2(data, "32FC1")
        depths = np.array(img, dtype=np.float32)
        width = len(depths[0])
        buckets = [[] for i in range(self.NUM_BUCKETS)]
        for row in depths:
            for i, val in enumerate(row):
                if not np.isnan(val):
                    deg = self._idx_to_degree(i, len(depths[0]))
                    idx = self._deg_to_bucket(deg, self.NUM_BUCKETS)
                    buckets[idx].append(val)


if __name__ == "__main__":
    ImageSegmenter()
#!/usr/bin/env python # Converts laser scan to planar distance # and segments into obstalces and just hills Handles the logic of detecting the cubesat, the base, and the base front. # color that ALWAYS occurs in a base connected component and # NEVER occurs in a cubesat or rover connected component # color range of base, cubesat and rover # Base Front Color Range # now grow mask to encompass some background and connect things up # now remove mask elements of outlier low pixels # only background detected (i.e. no component) # return early # no indication of "too small bits" # Now strip any connected components that don't have key color if key is specified # print('Pre-cut # comps:', conn_comps[0] - 1) # colors that should or shouldn't be there # cut out this component # print('Final # connected components:', comps[0] - 1) Takes in stereo camera depth image and produces estimates of detected     obstacles # TODO: remove hard coded constant # subscriber and publishers # rospy.Subscriber(depth_img_topic, Image, callback=self.depth_callback)