Example #1
0
    def __init__(self):

        # Pull constants from config file
        self.lower = rospy.get_param('~lower_color_threshold', [0, 0, 80])
        self.upper = rospy.get_param(
            '~higher_color_threshold', [200, 200, 250])
        self.min_contour_area = rospy.get_param('~min_contour_area', .001)
        self.max_contour_area = rospy.get_param('~max_contour_area', 400)
        self.min_trans = rospy.get_param('~min_trans', .05)
        self.max_velocity = rospy.get_param('~max_velocity', 1)
        self.timeout = rospy.Duration(
            rospy.get_param('~timeout_seconds'), 250000)
        self.min_observations = rospy.get_param('~min_observations', 8)
        self.camera = rospy.get_param('~camera_topic',
                                      '/camera/front/left/image_rect_color')

        # Instantiate remaining variables and objects
        self._observations = deque()
        self._pose_pairs = deque()
        self._times = deque()
        self.last_image_time = None
        self.last_image = None
        self.tf_listener = tf.TransformListener()
        self.status = ''
        self.est = None
        self.visual_id = 0
        self.enabled = False
        self.bridge = CvBridge()

        # Image Subscriber and Camera Information

        self.image_sub = Image_Subscriber(self.camera, self.image_cb)
        self.camera_info = self.image_sub.wait_for_camera_info()
        '''
        These variables store the camera information required to perform
        the transformations on the coordinates to move from the subs
        perspective to our global map perspective. This information is
        also necessary to perform the least squares intersection which
        will find the 3D coordinates of the torpedo board based on 2D
        observations from the Camera. More info on this can be found in
        sub8_vision_tools.
        '''

        self.camera_info = self.image_sub.wait_for_camera_info()
        self.camera_model = PinholeCameraModel()
        self.camera_model.fromCameraInfo(self.camera_info)
        self.frame_id = self.camera_model.tfFrame()

        # Ros Services so mission can be toggled and info requested
        rospy.Service('~enable', SetBool, self.toggle_search)
        self.multi_obs = MultiObservation(self.camera_model)
        rospy.Service('~pose', VisionRequest, self.request_board3d)
        self.image_pub = Image_Publisher("torp_vision/debug")
        self.point_pub = rospy.Publisher(
            "torp_vision/points", Point, queue_size=1)
        self.mask_image_pub = rospy.Publisher(
            'torp_vison/mask', Image, queue_size=1)

        # Debug
        self.debug = rospy.get_param('~debug', True)
    def __init__(self):
        self.tf_listener = tf.TransformListener()

        self.enabled = False
        self.last_image = None
        self.last_image_time = None
        self.camera_model = None
        self.circle_finder = CircleFinder(
            1.0
        )  # Model radius doesn't matter beacause it's not being used for 3D pose

        # Various constants for tuning, debugging. See buoy_finder.yaml for more info
        self.min_observations = rospy.get_param('~min_observations')
        self.debug_ros = rospy.get_param('~debug/ros', True)
        self.debug_cv = rospy.get_param('~debug/cv', False)
        self.min_contour_area = rospy.get_param('~min_contour_area')
        self.max_circle_error = rospy.get_param('~max_circle_error')
        self.max_velocity = rospy.get_param('~max_velocity')
        self.roi_y = rospy.get_param('~roi_y')
        self.roi_height = rospy.get_param('~roi_height')
        camera = rospy.get_param('~camera_topic',
                                 '/camera/front/right/image_rect_color')

        self.buoys = {}
        for color in ['red', 'yellow', 'green']:
            self.buoys[color] = Buoy(color, debug_cv=self.debug_cv)
        if self.debug_cv:
            cv2.waitKey(1)
            self.debug_images = {}

        # Keep latest odom message for sanity check
        self.last_odom = None
        self.odom_sub = rospy.Subscriber('/odom',
                                         Odometry,
                                         self.odom_cb,
                                         queue_size=3)

        self.image_sub = Image_Subscriber(camera, self.image_cb)
        if self.debug_ros:
            self.rviz = rviz.RvizVisualizer(topic='~markers')
            self.mask_pub = Image_Publisher('~mask_image')
            rospy.Timer(rospy.Duration(1), self.print_status)

        self.camera_info = self.image_sub.wait_for_camera_info()
        self.camera_model = PinholeCameraModel()
        self.camera_model.fromCameraInfo(self.camera_info)
        self.frame_id = self.camera_model.tfFrame()
        self.multi_obs = MultiObservation(self.camera_model)

        rospy.Service('~enable', SetBool, self.toggle_search)
        rospy.Service('~2D', VisionRequest2D, self.request_buoy)
        rospy.Service('~pose', VisionRequest, self.request_buoy3d)

        rospy.loginfo("BUOY FINDER: initialized successfully")
Example #3
0
    def image_cb(self, image):
        '''Hang on to last image'''
        self.last_image = image
        self.last_image_time = self.image_sub.last_image_time
        if self.camera_model is None:
            if self.image_sub.camera_info is None:
                return

            self.camera_model = image_geometry.PinholeCameraModel()
            self.camera_model.fromCameraInfo(self.image_sub.camera_info)
            self.multi_obs = MultiObservation(self.camera_model)
Example #4
0
    def __init__(self):

        # Pull constants from config file
        self.override = False
        self.lower = [0, 0, 0]
        self.upper = [0, 0, 0]
        self.min_trans = 0
        self.max_velocity = 0
        self.timeout = 0
        self.min_observations = 0
        self.camera = rospy.get_param('~camera_topic',
                                      '/camera/down/image_rect_color')
        self.goal = None
        self.last_config = None
        self.reconfigure_server = DynamicReconfigureServer(
            VampireIdentifierConfig, self.reconfigure)

        # Instantiate remaining variables and objects
        self._observations = deque()
        self._pose_pairs = deque()
        self._times = deque()
        self.last_image_time = None
        self.last_image = None
        self.tf_listener = tf.TransformListener()
        self.status = ''
        self.est = None
        self.visual_id = 0
        self.enabled = False
        self.bridge = CvBridge()

        # Image Subscriber and Camera Information

        self.image_sub = Image_Subscriber(self.camera, self.image_cb)
        self.camera_info = self.image_sub.wait_for_camera_info()

        self.camera_info = self.image_sub.wait_for_camera_info()
        self.camera_model = PinholeCameraModel()
        self.camera_model.fromCameraInfo(self.camera_info)
        self.frame_id = self.camera_model.tfFrame()

        # Ros Services so mission can be toggled and info requested
        rospy.Service('~enable', SetBool, self.toggle_search)
        self.multi_obs = MultiObservation(self.camera_model)
        rospy.Service('~pose', VisionRequest, self.request_buoy)
        self.image_pub = Image_Publisher("drac_vision/debug")
        self.point_pub = rospy.Publisher("drac_vision/points",
                                         Point,
                                         queue_size=1)
        self.mask_image_pub = rospy.Publisher('drac_vision/mask',
                                              Image,
                                              queue_size=1)

        # Debug
        self.debug = rospy.get_param('~debug', True)
Example #5
0
    def __init__(self):

        # Pull constants from config file
        self.min_trans = rospy.get_param('~min_trans', .25)
        self.max_velocity = rospy.get_param('~max_velocity', 1)
        self.min_observations = rospy.get_param('~min_observations', 30)
        self.camera = rospy.get_param('~camera_topic',
                                      '/camera/front/left/image_rect_color')
        # Instantiate remaining variables and objects
        self._observations = deque()
        self._pose_pairs = deque()
        self._times = deque()
        self._observations1 = deque()
        self._pose_pairs1 = deque()
        self._times1 = deque()
        self._observations2 = deque()
        self._pose_pairs2 = deque()
        self._times2 = deque()
        self.last_image_time = None
        self.last_image = None
        self.tf_listener = tf.TransformListener()
        self.status = ''
        self.est = None
        self.est1 = None
        self.est2 = None
        self.plane = None
        self.visual_id = 0
        self.enabled = False
        self.bridge = CvBridge() 
        self.print_info = FprintFactory(title=MISSION).fprint
        # Image Subscriber and Camera Information
        self.point_sub = rospy.Subscriber(
            '/roi_pub', RegionOfInterest, self.acquire_targets)
        self.image_sub = Image_Subscriber(self.camera, self.image_cb)

        self.camera_info = self.image_sub.wait_for_camera_info()
        self.camera_model = PinholeCameraModel()
        self.camera_model.fromCameraInfo(self.camera_info)
        self.frame_id = 'front_left_cam_optical'
        self.markerPub0 = rospy.Publisher('estMarker0', Marker, queue_size=1)
        self.markerPub1 = rospy.Publisher('estMarker1', Marker, queue_size=1)
        self.markerPub2 = rospy.Publisher('estMarker2', Marker, queue_size=1)
        # Ros Services so mission can be toggled and info requested
        rospy.Service('~enable', SetBool, self.toggle_search)
        self.multi_obs = MultiObservation(self.camera_model)
        rospy.Service('~pose', VisionRequest, self.request_board3d)

        # Debug
        self.debug = rospy.get_param('~debug', True)
Example #6
0
    def image_cb(self, image):
        '''Hang on to last image'''
        self.last_image = image
        self.last_image_time = self.image_sub.last_image_time
        if self.camera_model is None:
            if self.image_sub.camera_info is None:
                return

            self.camera_model = image_geometry.PinholeCameraModel()
            self.camera_model.fromCameraInfo(self.image_sub.camera_info)
            self.multi_obs = MultiObservation(self.camera_model)
Example #7
0
    def __init__(self):
        self.tf_listener = tf.TransformListener()

        self.enabled = False
        self.last_image = None
        self.last_image_time = None
        self.camera_model = None
        self.circle_finder = CircleFinder(1.0)  # Model radius doesn't matter beacause it's not being used for 3D pose

        # Various constants for tuning, debugging. See buoy_finder.yaml for more info
        self.min_observations = rospy.get_param('~min_observations')
        self.debug_ros = rospy.get_param('~debug/ros', True)
        self.debug_cv = rospy.get_param('~debug/cv', False)
        self.min_contour_area = rospy.get_param('~min_contour_area')
        self.max_circle_error = rospy.get_param('~max_circle_error')
        self.max_velocity = rospy.get_param('~max_velocity')
        self.roi_y = rospy.get_param('~roi_y')
        self.roi_height = rospy.get_param('~roi_height')
        camera = rospy.get_param('~camera_topic', '/camera/front/right/image_rect_color')

        self.buoys = {}
        for color in ['red', 'yellow', 'green']:
            self.buoys[color] = Buoy(color, debug_cv=self.debug_cv)
        if self.debug_cv:
            cv2.waitKey(1)
            self.debug_images = {}

        # Keep latest odom message for sanity check
        self.last_odom = None
        self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size=3)

        self.image_sub = Image_Subscriber(camera, self.image_cb)
        if self.debug_ros:
            self.rviz = rviz.RvizVisualizer(topic='~markers')
            self.mask_pub = Image_Publisher('~mask_image')
            rospy.Timer(rospy.Duration(1), self.print_status)

        self.camera_info = self.image_sub.wait_for_camera_info()
        self.camera_model = PinholeCameraModel()
        self.camera_model.fromCameraInfo(self.camera_info)
        self.frame_id = self.camera_model.tfFrame()
        self.multi_obs = MultiObservation(self.camera_model)

        rospy.Service('~enable', SetBool, self.toggle_search)
        rospy.Service('~2D', VisionRequest2D, self.request_buoy)
        rospy.Service('~pose', VisionRequest, self.request_buoy3d)

        rospy.loginfo("BUOY FINDER: initialized successfully")
class BuoyFinder(object):
    '''
    Node to find red, green, and yellow buoys in a single camera frame.

    Combines several observations and uses a least-squares approach to get a 3D
    position estimate of a buoy when requested.

    Intended to be modular so other approaches can be tried. Adding more sophistication
    to segmentation would increase reliability.

    TODO: Use same mask for yellow/green
    '''
    def __init__(self):
        self.tf_listener = tf.TransformListener()

        self.enabled = False
        self.last_image = None
        self.last_image_time = None
        self.camera_model = None
        self.circle_finder = CircleFinder(
            1.0
        )  # Model radius doesn't matter beacause it's not being used for 3D pose

        # Various constants for tuning, debugging. See buoy_finder.yaml for more info
        self.min_observations = rospy.get_param('~min_observations')
        self.debug_ros = rospy.get_param('~debug/ros', True)
        self.debug_cv = rospy.get_param('~debug/cv', False)
        self.min_contour_area = rospy.get_param('~min_contour_area')
        self.max_circle_error = rospy.get_param('~max_circle_error')
        self.max_velocity = rospy.get_param('~max_velocity')
        self.roi_y = rospy.get_param('~roi_y')
        self.roi_height = rospy.get_param('~roi_height')
        camera = rospy.get_param('~camera_topic',
                                 '/camera/front/right/image_rect_color')

        self.buoys = {}
        for color in ['red', 'yellow', 'green']:
            self.buoys[color] = Buoy(color, debug_cv=self.debug_cv)
        if self.debug_cv:
            cv2.waitKey(1)
            self.debug_images = {}

        # Keep latest odom message for sanity check
        self.last_odom = None
        self.odom_sub = rospy.Subscriber('/odom',
                                         Odometry,
                                         self.odom_cb,
                                         queue_size=3)

        self.image_sub = Image_Subscriber(camera, self.image_cb)
        if self.debug_ros:
            self.rviz = rviz.RvizVisualizer(topic='~markers')
            self.mask_pub = Image_Publisher('~mask_image')
            rospy.Timer(rospy.Duration(1), self.print_status)

        self.camera_info = self.image_sub.wait_for_camera_info()
        self.camera_model = PinholeCameraModel()
        self.camera_model.fromCameraInfo(self.camera_info)
        self.frame_id = self.camera_model.tfFrame()
        self.multi_obs = MultiObservation(self.camera_model)

        rospy.Service('~enable', SetBool, self.toggle_search)
        rospy.Service('~2D', VisionRequest2D, self.request_buoy)
        rospy.Service('~pose', VisionRequest, self.request_buoy3d)

        rospy.loginfo("BUOY FINDER: initialized successfully")

    def odom_cb(self, odom):
        self.last_odom = odom

    def toggle_search(self, srv):
        '''
        Callback for standard ~enable service. If true, start
        looking at frames for buoys.
        '''
        if srv.data:
            rospy.loginfo("BUOY FINDER: enabled")
            self.enabled = True
        else:
            rospy.loginfo("BUOY FINDER: disabled")
            self.enabled = False

        return SetBoolResponse(success=True)

    def request_buoy(self, srv):
        '''
        Callback for 2D vision request. Returns centroid
        of buoy found with color specified in target_name
        if found.
        '''
        if not self.enabled or srv.target_name not in self.buoys:
            return VisionRequest2DResponse(found=False)
        response = self.find_single_buoy(srv.target_name)
        if response is False or response is None:
            return VisionRequest2DResponse(found=False)
        center, radius = response
        return VisionRequest2DResponse(header=Header(
            stamp=self.last_image_time, frame_id=self.frame_id),
                                       pose=Pose2D(
                                           x=center[0],
                                           y=center[1],
                                       ),
                                       max_x=self.last_image.shape[0],
                                       max_y=self.last_image.shape[1],
                                       camera_info=self.image_sub.camera_info,
                                       found=True)

    def request_buoy3d(self, srv):
        '''
        Callback for 3D vision request. Uses recent observations of buoy
        specified in target_name to attempt a least-squares position estimate.
        As buoys are spheres, orientation is meaningless.
        '''
        if srv.target_name not in self.buoys or not self.enabled:
            return VisionRequestResponse(found=False)
        buoy = self.buoys[srv.target_name]
        if buoy.est is None:
            return VisionRequestResponse(found=False)
        return VisionRequestResponse(pose=PoseStamped(
            header=Header(stamp=self.last_image_time, frame_id='/map'),
            pose=Pose(position=Point(*buoy.est))),
                                     found=True)

    def image_cb(self, image):
        '''
        Run each time an image comes in from ROS. If enabled,
        attempt to find each color buoy.
        '''
        if not self.enabled:
            return

        # Crop out some of the top and bottom to exclude the floor and surface reflections
        height = image.shape[0]
        roi_y = int(self.roi_y * height)
        roi_height = height - int(self.roi_height * height)
        self.roi = (0, roi_y, roi_height, image.shape[1])
        self.last_image = image[self.roi[1]:self.roi[2],
                                self.roi[0]:self.roi[3]]
        self.image_area = self.last_image.shape[0] * self.last_image.shape[1]

        if self.debug_ros:
            # Create a blacked out debug image for putting masks in
            self.mask_image = np.zeros(self.last_image.shape,
                                       dtype=image.dtype)
        if self.last_image_time is not None and self.image_sub.last_image_time < self.last_image_time:
            # Clear tf buffer if time went backwards (nice for playing bags in loop)
            self.tf_listener.clear()
        self.last_image_time = self.image_sub.last_image_time
        self.find_buoys()
        if self.debug_ros:
            self.mask_pub.publish(self.mask_image)

    def print_status(self, _):
        '''
        Called at 1 second intervals to display the status (not found, n observations, FOUND)
        for each buoy.
        '''
        if self.enabled:
            rospy.loginfo("STATUS: RED='%s', GREEN='%s', YELLOW='%s'",
                          self.buoys['red'].status, self.buoys['green'].status,
                          self.buoys['yellow'].status)

    def find_buoys(self):
        '''
        Run find_single_buoy for each color of buoy
        '''
        for buoy_name in self.buoys:
            self.find_single_buoy(buoy_name)

    def is_circular_contour(self, cnt):
        '''
        Check that a contour is close enough to a circle (using hue invariants)
        to be a pottential buoy.
        '''
        return self.circle_finder.verify_contour(cnt) <= self.max_circle_error

    def get_best_contour(self, contours):
        '''
        Attempts to find a good buoy contour among those found within the
        thresholded mask. If a good one is found, it return (contour, centroid, area),
        otherwise returns None. Right now the best contour is just the largest.

        @param contours Numpy array of contours from a particular buoy's mask
        @return A tuple (contour, error) where contour will be the best contour
                in an image or None if no contours pass criteria. Error will be a string
                describing why no good contour was found, or None if contour is not None.
        '''
        if len(contours) == 0:
            return None, 'no contours in mask'
        circular_contours = filter(self.is_circular_contour, contours)
        if len(circular_contours) == 0:
            return None, 'fails circularity test'
        circles_sorted = sorted(circular_contours,
                                key=cv2.contourArea,
                                reverse=True)
        if cv2.contourArea(
                circles_sorted[0]) < self.min_contour_area * self.image_area:
            return None, 'fails area test'
        return circles_sorted[
            0], None  # Return the largest contour that pases shape test

    def find_single_buoy(self, buoy_type):
        '''
        Attempt to find one color buoy in the image.
        1) Create mask for buoy's color in colorspace specified in paramaters
        2) Select the largest contour in this mask
        3) Approximate a circle around this contour
        4) Store the center of this circle and the current tf between /map and camera
           as an observation
        5) If observations for this buoy is now >= min_observations, approximate buoy
           position using the least squares tool imported
        '''
        assert buoy_type in self.buoys.keys(
        ), "Buoys_2d does not know buoy color: {}".format(buoy_type)
        buoy = self.buoys[buoy_type]
        mask = buoy.get_mask(self.last_image)
        kernel = np.ones((5, 5), np.uint8)
        mask = cv2.erode(mask, kernel, iterations=2)
        mask = cv2.dilate(mask, kernel, iterations=2)

        _, contours, _ = cv2.findContours(mask,
                                          cv2.RETR_TREE,
                                          cv2.CHAIN_APPROX_SIMPLE,
                                          offset=(self.roi[0], self.roi[1]))
        if self.debug_ros:
            cv2.add(self.mask_image.copy(),
                    buoy.cv_colors,
                    mask=mask,
                    dst=self.mask_image)
        if self.debug_cv:
            self.debug_images[buoy_type] = mask.copy()

        cnt, err = self.get_best_contour(contours)
        if cnt is None:
            buoy.clear_old_observations()
            buoy.status = '{} w/ {} obs'.format(err, buoy.size())
            return
        center, radius = cv2.minEnclosingCircle(cnt)

        if self.debug_ros:
            cv2.circle(
                self.mask_image,
                (int(center[0] - self.roi[0]), int(center[1]) - self.roi[1]),
                int(radius), buoy.cv_colors, 4)

        try:
            self.tf_listener.waitForTransform('/map', self.frame_id,
                                              self.last_image_time,
                                              rospy.Duration(0.2))
        except tf.Exception as e:
            rospy.logwarn("Could not transform camera to map: {}".format(e))
            return False

        if not self.sanity_check(center, self.last_image_time):
            buoy.status = 'failed sanity check'
            return False

        (t, rot_q) = self.tf_listener.lookupTransform('/map', self.frame_id,
                                                      self.last_image_time)
        R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q)

        buoy.add_observation(center, (np.array(t), R), self.last_image_time)

        observations, pose_pairs = buoy.get_observations_and_pose_pairs()
        if len(observations) > self.min_observations:
            buoy.est = self.multi_obs.lst_sqr_intersection(
                observations, pose_pairs)
            buoy.status = 'Pose found'
            if self.debug_ros:
                self.rviz.draw_sphere(buoy.est,
                                      color=buoy.draw_colors,
                                      scaling=(0.2286, 0.2286, 0.2286),
                                      frame='/map',
                                      _id=buoy.visual_id)
        else:
            buoy.status = '{} observations'.format(len(observations))
        return center, radius

    def sanity_check(self, coordinate, timestamp):
        '''
        Check if the observation is unreasonable. More can go here if we want.
        '''
        if self.last_odom is None:
            return False

        linear_velocity = rosmsg_to_numpy(self.last_odom.twist.twist.linear)
        if np.linalg.norm(linear_velocity) > self.max_velocity:
            return False

        return True
Example #9
0
class MultiObs:

    def __init__(self):

        # Pull constants from config file
        self.min_trans = rospy.get_param('~min_trans', .25)
        self.max_velocity = rospy.get_param('~max_velocity', 1)
        self.min_observations = rospy.get_param('~min_observations', 30)
        self.camera = rospy.get_param('~camera_topic',
                                      '/camera/front/left/image_rect_color')
        # Instantiate remaining variables and objects
        self._observations = deque()
        self._pose_pairs = deque()
        self._times = deque()
        self._observations1 = deque()
        self._pose_pairs1 = deque()
        self._times1 = deque()
        self._observations2 = deque()
        self._pose_pairs2 = deque()
        self._times2 = deque()
        self.last_image_time = None
        self.last_image = None
        self.tf_listener = tf.TransformListener()
        self.status = ''
        self.est = None
        self.est1 = None
        self.est2 = None
        self.plane = None
        self.visual_id = 0
        self.enabled = False
        self.bridge = CvBridge() 
        self.print_info = FprintFactory(title=MISSION).fprint
        # Image Subscriber and Camera Information
        self.point_sub = rospy.Subscriber(
            '/roi_pub', RegionOfInterest, self.acquire_targets)
        self.image_sub = Image_Subscriber(self.camera, self.image_cb)

        self.camera_info = self.image_sub.wait_for_camera_info()
        self.camera_model = PinholeCameraModel()
        self.camera_model.fromCameraInfo(self.camera_info)
        self.frame_id = 'front_left_cam_optical'
        self.markerPub0 = rospy.Publisher('estMarker0', Marker, queue_size=1)
        self.markerPub1 = rospy.Publisher('estMarker1', Marker, queue_size=1)
        self.markerPub2 = rospy.Publisher('estMarker2', Marker, queue_size=1)
        # Ros Services so mission can be toggled and info requested
        rospy.Service('~enable', SetBool, self.toggle_search)
        self.multi_obs = MultiObservation(self.camera_model)
        rospy.Service('~pose', VisionRequest, self.request_board3d)

        # Debug
        self.debug = rospy.get_param('~debug', True)

    def image_cb(self, image):
        '''
        Run each time an image comes in from ROS. If enabled,
        attempt to find the torpedo board.
        '''
        return None

    def toggle_search(self, srv):
        '''
        Callback for standard ~enable service. If true, start
        looking at frames for buoys.
        '''
        if srv.data:
            rospy.loginfo("TARGET ACQUISITION: enabled")
            self.enabled = True

        else:
            rospy.loginfo("TARGET ACQUISITION: disabled")
            self.enabled = False

        return SetBoolResponse(success=True)

    def request_board3d(self, srv):
        '''
        Callback for 3D vision request. Uses recent observations of target
        board  specified in target_name to attempt a least-squares position
        estimate. Ignoring orientation of board.
        '''
        if not self.enabled:
          print("REEEE")  
          return VisionRequestResponse(found=False)
        #buoy = self.buoys[srv.target_name]
        if self.est is None:
            self.print_info("NO ESTIMATE!")
            return VisionRequestResponse(found=False)
        # NOTE: returns normal vec encoded into a quaternion message (so not actually a quaternion)
        return VisionRequestResponse(
            pose=PoseStamped(
                header=Header(stamp=rospy.Time.now(), frame_id='/map'),
                pose=Pose(position=Point(*((self.est + self.est1 + self.est2) / 3)),
                          orientation=Quaternion(x=self.plane[0], y=self.plane[1], z=self.plane[2]))),
                found=True)

    def clear_old_observations(self):
        # Observations older than three seconds are discarded.
        # print(self._observations)
        # print(self._observations1)
        # print(self._observations2)
        return

    def size(self):
        return len(self._observations)

    def add_observation(self, obs, pose_pair, time):
        if len(self._pose_pairs) == 0 or np.linalg.norm(
                self._pose_pairs[-1][0] - pose_pair[0]) > self.min_trans:
            self._observations.append(obs)
            self._pose_pairs.append(pose_pair)
            self._times.append(time)

    def add_observation1(self, obs, pose_pair, time):
        if len(self._pose_pairs1) == 0 or np.linalg.norm(
                self._pose_pairs1[-1][0] - pose_pair[0]) > self.min_trans:
            self._observations1.append(obs)
            self._pose_pairs1.append(pose_pair)
            self._times1.append(time)
        return (self._observations1, self._pose_pairs1)

    def add_observation2(self, obs, pose_pair, time):
        self.clear_old_observations()
        if len(self._pose_pairs2) == 0 or np.linalg.norm(
                self._pose_pairs2[-1][0] - pose_pair[0]) > self.min_trans:
            self._observations2.append(obs)
            self._pose_pairs2.append(pose_pair)
            self._times2.append(time)
        return (self._observations2, self._pose_pairs2)

    def get_observations_and_pose_pairs(self):
        self.clear_old_observations()
        return (self._observations, self._pose_pairs)

    def generate_plane(self):
        ab = self.est - self.est1
        ac = self.est - self.est2
        # print("AB: ", ab)
        # print("AC: ", ac)
        x = np.cross(ab,ac)
        return x / np.linalg.norm(x)

    def marker_msg(self, point, point_name):
        robotMarker = Marker()
        robotMarker.header.frame_id = "/map"
        robotMarker.header.stamp    = rospy.get_rostime()
        robotMarker.ns = point_name
        robotMarker.id = 0
        robotMarker.type = 2 # sphere
        robotMarker.action = 0
        robotMarker.pose.position = Point(point[0], point[1], point[2])
        robotMarker.pose.orientation.x = 0
        robotMarker.pose.orientation.y = 0
        robotMarker.pose.orientation.z = 0
        robotMarker.pose.orientation.w = 1.0
        robotMarker.scale.x = .5
        robotMarker.scale.y = .5
        robotMarker.scale.z = .5

        robotMarker.color.r = 1.0
        robotMarker.color.g = 1.0
        robotMarker.color.b = 0.0
        robotMarker.color.a = 1.0

        robotMarker.lifetime = rospy.Duration(0)
        return robotMarker

    def acquire_targets(self, roi):
        if not self.enabled:
          return
        # NOTE: point.z contains the timestamp of the image when it was processed in the neural net.
        x0 = roi.x_offset
        y0 = roi.y_offset
        height = roi.height
        width = roi.width
        
        point0 = np.array([x0, y0])
        point1 = np.array([x0+width, y0])
        point2 = np.array([x0, y0+height])
        point3 = np.array([x0+width, y0+height])
        # print("p1: ", point1)
        # print("p2: ", point2)
        try:
            self.tf_listener.waitForTransform('/map',
                                                '/front_left_cam_optical',
                                                self.image_sub.last_image_time - rospy.Time(.2),
                                                rospy.Duration(0.2))
        except tf.Exception as e:
            rospy.logwarn(
                "Could not transform camera to map: {}".format(e))
            return False

        (t, rot_q) = self.tf_listener.lookupTransform(
            '/map', '/front_left_cam_optical', self.image_sub.last_image_time - rospy.Time(.2))
        R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q)

        self.add_observation(point0, (np.array(t), R),
                                self.image_sub.last_image_time)
        observations1, pose_pairs1 = self.add_observation1(point1, (np.array(t), R),
                                self.image_sub.last_image_time)
        observations2, pose_pairs2 = self.add_observation2(point2, (np.array(t), R),
                                self.image_sub.last_image_time)

        observations, pose_pairs = self.get_observations_and_pose_pairs()
        if len(observations) > self.min_observations:
            self.est = self.multi_obs.lst_sqr_intersection(
                observations, pose_pairs)
            self.est1 = self.multi_obs.lst_sqr_intersection(
                observations1, pose_pairs1)
            self.est2 = self.multi_obs.lst_sqr_intersection(
                observations2, pose_pairs2)
            # print('est1: ', self.est1)
            # print('est2: ', self.est2)
            self.status = 'Pose found'
            self.print_info("Pose Found!")
            self.plane = self.generate_plane()
            # print("Plane: ", self.plane)
            self.markerPub0.publish(self.marker_msg(self.est, 'est0'))
            self.markerPub1.publish(self.marker_msg(self.est1, 'est1'))
            self.markerPub2.publish(self.marker_msg(self.est2, 'est2'))
            print((self.est + self.est1 + self.est2) / 3)

        else:   
            self.status = '{} observations'.format(len(observations))
Example #10
0
class BuoyFinder:
    _min_size = 100
    '''
    TODO:
        Check for outliers in observations
    '''
    def __init__(self):
        self.transformer = tf.TransformListener()
        rospy.sleep(2.0)

        self.search = False
        self.last_image = None
        self.last_draw_image = None
        self.last_image_time = None
        self.camera_model = None
        self.multi_obs = None
        self.max_observations = 20
        self._id = 0  # Only for display

        self.rviz = rviz.RvizVisualizer()

        rospack = rospkg.RosPack()

        # TODO: consolidate all these dictionaries into one big one.
        self.observations = {
            'red':deque(),
            'yellow':deque(),
            'green':deque()
        }
        self.pose_pairs = {
            'red':deque(),
            'yellow':deque(),
            'green':deque()
        }

        if boost_to_the_moon:
            self.buoys = {
                # Boost classifier paths
                'yellow': os.path.join(rospack.get_path('sub8_perception'), 'ml_classifiers/buoys/yellow/' + YELLOW),
                'red': os.path.join(rospack.get_path('sub8_perception'), 'ml_classifiers/buoys/red/' + RED),
                'green': os.path.join(rospack.get_path('sub8_perception'), 'ml_classifiers/buoys/green/' + GREEN)
            }
        else:
            self.buoys = {
                #Threshold paths
                'green': '/color/buoy/green',
                'red': '/color/buoy/red',
                'yellow': '/color/buoy/yellow',
            }

        self.last_t = {
            'green': None,
            'red': None,
            'yellow': None
        }

        # For displaying each buoy in rviz
        self.draw_colors = {
            'green': (0.0, 1.0, 0.0, 1.0),
            'red': (1.0, 0.0, 0.0, 1.0),
            'yellow': (1.0, 1.0, 0.0, 1.0),
        }
        self.visual_id = {
            'green': 0,
            'red': 1,
            'yellow': 2,
        }

        if boost_to_the_moon:
            for color in self.buoys.keys():
                if self.buoys[color] is None:
                    rospy.logwarn('Classifier path for {} not found!'.format(color))
                    continue

                path = self.buoys[color]
                self.buoys[color] = cv2.Boost()
                rospy.loginfo("BUOY - Loading {} boost...".format(color))
                self.buoys[color].load(path)
                rospy.loginfo("BUOY - Classifier for {} buoy loaded.".format(color))

        self.image_sub = sub8_ros_tools.Image_Subscriber('/stereo/left/image_raw', self.image_cb)
        self.image_pub = sub8_ros_tools.Image_Publisher('/vision/buoy_2d/target_info')
        self.mask_pub = sub8_ros_tools.Image_Publisher('/vision/buoy_2d/mask')

        # Occasional status publisher
        self.timer = rospy.Timer(rospy.Duration(1), self.publish_target_info)

        self.toggle = rospy.Service('vision/buoys/search', SetBool, self.toggle_search)
        self.pose2d_service = rospy.Service('vision/buoys/2D', VisionRequest2D, self.request_buoy)
        self.pose_service = rospy.Service('vision/buoys/pose', VisionRequest, self.request_buoy3d)

        print "BUOY - Fueled up, ready to go!"

    def toggle_search(self, srv):
        if srv.data:
            rospy.loginfo("BUOY - Looking for buoys now.")
            self.search = True
        else:
            rospy.loginfo("BUOY - Done looking for buoys.")
            self.search = False

        return SetBoolResponse(success=srv.data)

    def request_buoy(self, srv):
        print 'requesting', srv
        timestamp = self.last_image_time
        response = self.find_single_buoy(np.copy(self.last_image), timestamp, srv.target_name)

        if response is False:
            print 'did not find'
            resp = VisionRequest2DResponse(
                header=sub8_ros_tools.make_header(frame='/stereo_front/left'),
                found=False
            )

        else:
            # Fill in
            center, radius = response
            resp = VisionRequest2DResponse(
                header=Header(stamp=timestamp, frame_id='/stereo_front/left'),
                pose=Pose2D(
                    x=center[0],
                    y=center[1],
                ),
                max_x=self.last_image.shape[0],
                max_y=self.last_image.shape[1],
                camera_info=self.image_sub.camera_info,
                found=True
            )
        return resp

    def request_buoy3d(self, srv):
        print "Requesting 3d pose"

        if (len(self.observations[srv.target_name]) > 5) and self.multi_obs is not None:
            estimated_pose = self.multi_obs.lst_sqr_intersection(self.observations[srv.target_name], self.pose_pairs[srv.target_name])

            self.rviz.draw_sphere(estimated_pose, color=self.draw_colors[srv.target_name],
                scaling=(0.5, 0.5, 0.5), frame='/map', _id=self.visual_id[srv.target_name])

            resp = VisionRequestResponse(
                pose=PoseStamped(
                    header=Header(stamp=self.last_image_time, frame_id='/map'),
                    pose=Pose(
                        position=Point(*estimated_pose)
                    )
                ),
                found=True
            )
        else:
            if len(self.observations[srv.target_name]) <= 5:
                rospy.logerr("Did not attempt search because we did not have enough observations ({})".format(self.observations[srv.target_name]))
            else:
                rospy.logerr("Did not attempt search because buoys_2d was not fully initialized")

            resp = VisionRequestResponse(
                pose=PoseStamped(
                    header=Header(stamp=self.last_image_time, frame_id='/map'),
                ),
                found=False
            )
        return resp

    def publish_target_info(self, *args):
        if not self.search or self.last_image is None:
            return

        self.find_buoys(np.copy(self.last_image), self.last_image_time)
        if self.last_draw_image is not None:
            self.image_pub.publish(self.last_draw_image)

    def image_cb(self, image):
        '''Hang on to last image'''
        self.last_image = image
        self.last_image_time = self.image_sub.last_image_time
        if self.camera_model is None:
            if self.image_sub.camera_info is None:
                return

            self.camera_model = image_geometry.PinholeCameraModel()
            self.camera_model.fromCameraInfo(self.image_sub.camera_info)
            self.multi_obs = MultiObservation(self.camera_model)

    def ncc(self, image, mean_thresh, scale=15):
        '''Compute normalized cross correlation w.r.t a shadowed pillbox fcn

        The expected scale will vary, so we don't cache it
        '''
        kernel = np.ones((scale, scale)) * -1
        midpoint = (scale // 2, scale // 2)
        cv2.circle(kernel, midpoint, midpoint[0], 1, -1)

        mean, std_dev = cv2.meanStdDev(image)

        # Check if the scene is brighter than our a priori target
        if mean > mean_thresh:
            kernel = -kernel

        normalized_cross_correlation = cv2.filter2D((image - mean) / std_dev, -1, kernel)
        renormalized = normalized_cross_correlation
        return renormalized

    def get_biggest(self, contours):
        if len(contours) > 0:
            cnt = max(contours, key=cv2.contourArea)
            area = cv2.contourArea(cnt)
            if area > self._min_size:
                M = cv2.moments(cnt)
                cx = int(M['m10'] / M['m00'])
                cy = int(M['m01'] / M['m00'])
                tpl_center = (int(cx), int(cy))
                return cnt, tpl_center, area
            return None
        else:
            return None

    def find_single_buoy(self, img, timestamp, buoy_type):
        assert buoy_type in self.buoys.keys(), "Buoys_2d does not know buoy color: {}".format(buoy_type)
        max_area = 0
        best_ret = None

        if boost_to_the_moon:
            # Segmentation here (machine learning right now)
            some_observations = machine_learning.boost.observe(img)
            prediction2 = [int(x) for x in [self.buoys[buoy_type].predict(obs) for obs in some_observations]]
            mask = np.reshape(prediction2, img[:, :, 2].shape).astype(np.uint8) * 255
        else:
            #Thresholding here
            hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
            low = np.array(rospy.get_param(self.buoys[buoy_type] + '/hsv_low')).astype(np.int32)
            high = np.array(rospy.get_param(self.buoys[buoy_type] + '/hsv_high')).astype(np.int32)
            mask = cv2.inRange(hsv, low, high)

        rospy.sleep(.5)

        kernel = np.ones((13,13),np.uint8)
        mask = cv2.dilate(mask, kernel, iterations = 2)
        mask = cv2.erode(mask, kernel, iterations = 2)

        draw_mask = np.dstack([mask] * 3)
        draw_mask[:,:,0] *= 0
        if buoy_type == 'red':
            draw_mask[:,:,1] *= 0
        if buoy_type == 'green':
            draw_mask[:,:,2] *= 0
        self.mask_pub.publish(draw_mask)

        contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

        ret = self.get_biggest(contours)
        if ret is None:
            print "BUOY {} - No contours found".format(buoy_type)
            return

        contour, tuple_center, area = ret
        if area > max_area:
            max_area = area
            best_ret = ret

        if best_ret is None:
            print "BUOY {} - best_ret is None".format(buoy_type)
            return False

        contour, tuple_center, area = best_ret
        true_center, rad = cv2.minEnclosingCircle(contour)

        if self.camera_model is not None:

            if not self.sanity_check(tuple_center, timestamp):
                return False

            (t, rot_q) = self.transformer.lookupTransform('/map', '/stereo_front/left', timestamp)
            trans = np.array(t)
            R = sub8_ros_tools.geometry_helpers.quaternion_matrix(rot_q)

            self.rviz.draw_ray_3d(tuple_center, self.camera_model, self.draw_colors[buoy_type], frame='/stereo_front/left',
                _id=self._id + 100, timestamp=timestamp)
            self._id += 1
            if self._id >= self.max_observations * 3:
                self._id = 0

            if (self.last_t[buoy_type] is None) or (np.linalg.norm(trans - self.last_t[buoy_type]) > 0.3):
                self.last_t[buoy_type] = trans
                self.observations[buoy_type].append(true_center)
                self.pose_pairs[buoy_type].append((t, R))

            print "BUOY {} - {} observations".format(buoy_type, len(self.observations[buoy_type]))
            if len(self.observations[buoy_type]) > 5:
                est = self.multi_obs.lst_sqr_intersection(self.observations[buoy_type], self.pose_pairs[buoy_type])

                self.rviz.draw_sphere(est, color=self.draw_colors[buoy_type],
                    scaling=(0.5, 0.5, 0.5), frame='/map', _id=self.visual_id[buoy_type])

            if len(self.observations[buoy_type]) > self.max_observations:
                self.observations[buoy_type].popleft()
                self.pose_pairs[buoy_type].popleft()
        else:
            print "BUOY {} - camera_model is None".format(buoy_type)

        return tuple_center, rad

    def find_buoys(self, img, timestamp):
        draw_image = np.copy(img)

        # This is only run if buoy_type is not None
        for buoy_name in self.buoys.keys():
            if self.buoys[buoy_name] is None:
                continue

            #rospy.loginfo("BUOY - Looking for {}".format(buoy_name))
            result = self.find_single_buoy(img, timestamp, buoy_name)
            if not result:
                continue

            center, rad = result
            cv2.circle(draw_image, center, int(rad), (255, 255, 0), 2)
            font = cv2.FONT_HERSHEY_SIMPLEX
            cv2.putText(draw_image, '{}'.format(buoy_name), center, font, 0.8, (20, 20, 240), 1)

        self.last_draw_image = np.copy(draw_image)

    def sanity_check(self, coordinate, timestamp):
        '''
        Check if the observation is unreasonable. More can go here if we want.
        '''
        sane = True
        if np.linalg.norm(self.transformer.lookupTwist('/map', '/stereo_front/left', timestamp, rospy.Duration(.5))) > 1:
            rospy.logerr("BUOY - Moving too fast. Not observing buoy.")
            sane = False

        return sane
Example #11
0
class BuoyFinder(object):
    '''
    Node to find red, green, and yellow buoys in a single camera frame.

    Combines several observations and uses a least-squares approach to get a 3D
    position estimate of a buoy when requested.

    Intended to be modular so other approaches can be tried. Adding more sophistication
    to segmentation would increase reliability.
    '''
    def __init__(self):
        self.tf_listener = tf.TransformListener()

        self.enabled = False
        self.last_image = None
        self.last_image_time = None
        self.camera_model = None

        # Various constants for tuning, debugging. See buoy_finder.yaml for more info
        self.min_observations = rospy.get_param('~min_observations')
        self.debug_ros = rospy.get_param('~debug/ros', True)
        self.debug_cv = rospy.get_param('~debug/cv', False)
        self.min_contour_area = rospy.get_param('~min_contour_area')
        self.max_velocity = rospy.get_param('~max_velocity')
        camera = rospy.get_param('~camera_topic', '/camera/front/right/image_rect_color')

        self.buoys = {}
        for color in ['red', 'yellow', 'green']:
            self.buoys[color] = Buoy(color, debug_cv=self.debug_cv)
            self.buoys[color].load_segmentation()
        if self.debug_cv:
            cv2.waitKey(1)
            self.debug_images = {}

        # Keep latest odom message for sanity check
        self.last_odom = None
        self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size=3)

        self.image_sub = Image_Subscriber(camera, self.image_cb)
        if self.debug_ros:
            self.rviz = rviz.RvizVisualizer(topic='~markers')
            self.mask_pub = Image_Publisher('~mask_image')
            rospy.Timer(rospy.Duration(1), self.print_status)

        self.camera_info = self.image_sub.wait_for_camera_info()
        self.camera_model = PinholeCameraModel()
        self.camera_model.fromCameraInfo(self.camera_info)
        self.frame_id = self.camera_model.tfFrame()
        self.multi_obs = MultiObservation(self.camera_model)

        rospy.Service('~enable', SetBool, self.toggle_search)
        rospy.Service('~2D', VisionRequest2D, self.request_buoy)
        rospy.Service('~pose', VisionRequest, self.request_buoy3d)

        rospy.loginfo("BUOY FINDER: initialized successfully")

    def odom_cb(self, odom):
        self.last_odom = odom

    def toggle_search(self, srv):
        '''
        Callback for standard ~enable service. If true, start
        looking at frames for buoys.
        '''
        if srv.data:
            rospy.loginfo("BUOY FINDER: enabled")
            self.enabled = True
        else:
            rospy.loginfo("BUOY FINDER: disabled")
            self.enabled = False

        return SetBoolResponse(success=True)

    def request_buoy(self, srv):
        '''
        Callback for 2D vision request. Returns centroid
        of buoy found with color specified in target_name
        if found.
        '''
        if not self.enabled or srv.target_name not in self.buoys:
            return VisionRequest2DResponse(found=False)
        response = self.find_single_buoy(srv.target_name)
        if response is False or response is None:
            return VisionRequest2DResponse(found=False)
        center, radius = response
        return VisionRequest2DResponse(
            header=Header(stamp=self.last_image_time, frame_id=self.frame_id),
            pose=Pose2D(
                x=center[0],
                y=center[1],
            ),
            max_x=self.last_image.shape[0],
            max_y=self.last_image.shape[1],
            camera_info=self.image_sub.camera_info,
            found=True
        )

    def request_buoy3d(self, srv):
        '''
        Callback for 3D vision request. Uses recent observations of buoy
        specified in target_name to attempt a least-squares position estimate.
        As buoys are spheres, orientation is meaningless.
        '''
        if srv.target_name not in self.buoys or not self.enabled:
            return VisionRequestResponse(found=False)
        buoy = self.buoys[srv.target_name]
        if buoy.est is None:
            return VisionRequestResponse(found=False)
        return VisionRequestResponse(
            pose=PoseStamped(
                header=Header(stamp=self.last_image_time, frame_id='/map'),
                pose=Pose(
                    position=Point(*buoy.est)
                )
            ),
            found=True
        )

    def image_cb(self, image):
        '''
        Run each time an image comes in from ROS. If enabled,
        attempt to find each color buoy.
        '''
        if not self.enabled:
            return

        # Crop out some of the top and bottom to exclude the floor and surface reflections
        height = image.shape[0]
        roi_y = int(0.2 * height)
        roi_height = height - int(0.2 * height)
        self.roi = (0, roi_y, roi_height, image.shape[1])
        self.last_image = image[self.roi[1]:self.roi[2], self.roi[0]:self.roi[3]]

        if self.debug_ros:
            # Create a blacked out debug image for putting masks in
            self.mask_image = np.zeros(self.last_image.shape, dtype=image.dtype)
        if self.last_image_time is not None and self.image_sub.last_image_time < self.last_image_time:
            # Clear tf buffer if time went backwards (nice for playing bags in loop)
            self.tf_listener.clear()
        self.last_image_time = self.image_sub.last_image_time
        self.find_buoys()
        if self.debug_ros:
            self.mask_pub.publish(self.mask_image)

    def print_status(self, _):
        '''
        Called at 1 second intervals to display the status (not found, n observations, FOUND)
        for each buoy.
        '''
        if self.enabled:
            rospy.loginfo("STATUS: RED='%s', GREEN='%s', YELLOW='%s'",
                          self.buoys['red'].status,
                          self.buoys['green'].status,
                          self.buoys['yellow'].status)

    def find_buoys(self):
        '''
        Run find_single_buoy for each color of buoy
        '''
        for buoy_name in self.buoys:
            self.find_single_buoy(buoy_name)

    def get_best_contour(self, contours):
        '''
        Attempts to find a good buoy contour among those found within the
        thresholded mask. If a good one is found, it return (contour, centroid, area),
        otherwise returns None. Right now the best contour is just the largest.

        TODO: Use smarter contour filtering methods, like checking this it is circle like
        '''
        if len(contours) > 0:
            cnt = max(contours, key=cv2.contourArea)
            area = cv2.contourArea(cnt)
            if area < self.min_contour_area:
                return None
            M = cv2.moments(cnt)
            cx = int(M['m10'] / M['m00'])
            cy = int(M['m01'] / M['m00'])
            tpl_center = (int(cx), int(cy))
            return cnt, tpl_center, area
        else:
            return None

    def find_single_buoy(self, buoy_type):
        '''
        Attempt to find one color buoy in the image.
        1) Create mask for buoy's color in colorspace specified in paramaters
        2) Select the largest contour in this mask
        3) Approximate a circle around this contour
        4) Store the center of this circle and the current tf between /map and camera
           as an observation
        5) If observations for this buoy is now >= min_observations, approximate buoy
           position using the least squares tool imported
        '''
        assert buoy_type in self.buoys.keys(), "Buoys_2d does not know buoy color: {}".format(buoy_type)
        buoy = self.buoys[buoy_type]
        mask = buoy.get_mask(self.last_image)
        kernel = np.ones((5, 5), np.uint8)
        mask = cv2.erode(mask, kernel, iterations=2)
        mask = cv2.dilate(mask, kernel, iterations=2)

        _, contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE,
                                          offset=(self.roi[0], self.roi[1]))
        ret = self.get_best_contour(contours)
        if ret is None:
            buoy.clear_old_observations()
            buoy.status = 'not seen w/ {} obs'.format(buoy.size())
            return
        contour, tuple_center, area = ret
        true_center, rad = cv2.minEnclosingCircle(contour)

        if self.debug_ros:
            cv2.add(self.mask_image.copy(), buoy.cv_colors, mask=mask, dst=self.mask_image)
            cv2.circle(self.mask_image, (int(true_center[0] - self.roi[0]), int(true_center[1]) - self.roi[1]),
                       int(rad), buoy.cv_colors, 2)
        if self.debug_cv:
            self.debug_images[buoy_type] = mask.copy()

        try:
            self.tf_listener.waitForTransform('/map', self.frame_id, self.last_image_time, rospy.Duration(0.2))
        except tf.Exception as e:
            rospy.logwarn("Could not transform camera to map: {}".format(e))
            return False

        if not self.sanity_check(tuple_center, self.last_image_time):
            buoy.status = 'failed sanity check'
            return False

        (t, rot_q) = self.tf_listener.lookupTransform('/map', self.frame_id, self.last_image_time)
        R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q)

        buoy.add_observation(true_center, (np.array(t), R), self.last_image_time)

        observations, pose_pairs = buoy.get_observations_and_pose_pairs()
        if len(observations) > self.min_observations:
            buoy.est = self.multi_obs.lst_sqr_intersection(observations, pose_pairs)
            buoy.status = 'Pose found'
            if self.debug_ros:
                self.rviz.draw_sphere(buoy.est, color=buoy.draw_colors,
                                      scaling=(0.2286, 0.2286, 0.2286),
                                      frame='/map', _id=buoy.visual_id)
        else:
            buoy.status = '{} observations'.format(len(observations))

        return tuple_center, rad

    def sanity_check(self, coordinate, timestamp):
        '''
        Check if the observation is unreasonable. More can go here if we want.
        '''
        if self.last_odom is None:
            return False

        linear_velocity = rosmsg_to_numpy(self.last_odom.twist.twist.linear)
        if np.linalg.norm(linear_velocity) > self.max_velocity:
            return False

        return True
Example #12
0
class BuoyFinder:
    _min_size = 50

    def __init__(self):
        self.transformer = tf.TransformListener()
        rospy.sleep(1.0)
        self.done_once = False

        self.last_image = None
        self.last_draw_image = None
        self.last_poop_image = None
        self.last_image_time = None
        self.camera_model = None
        self.last_t = None

        self.observations = deque()
        self.pose_pairs = deque()

        self.rviz = rviz.RvizVisualizer()

        self.pose2d_service = rospy.Service('vision/buoys/2D', VisionRequest2D, self.request_buoy)
        self.pose_service = rospy.Service('vision/buoys/pose', VisionRequest, self.request_buoy3d)

        self.image_sub = sub8_ros_tools.Image_Subscriber('/stereo/right/image_rect_color', self.image_cb)
        self.image_pub = sub8_ros_tools.Image_Publisher('/vision/buoy_2d/target_info')

        # Occasional status publisher
        self.timer = rospy.Timer(rospy.Duration(1.0), self.publish_target_info)

        rospack = rospkg.RosPack()
        boost_path = os.path.join(
            rospack.get_path('sub8_perception'),
            'sub8_vision_tools',
            'classifiers',
            'boost.cv2'
        )

        self.boost = cv2.Boost()
        rospy.loginfo("Loading boost")
        self.boost.load(boost_path)
        rospy.loginfo("Boost loaded")

        self.buoys = {
            'green': '/color/buoy/green',
            'red': '/color/buoy/red',
            'yellow': '/color/buoy/yellow',
        }

        self.ppf = None
        self.multi_obs = None

        self.draw_colors = {
            'green': (0.0, 1.0, 0.0, 1.0),
            'red': (1.0, 0.0, 0.0, 1.0),
            'yellow': (1.0, 1.0, 0.0, 1.0),
        }

    def request_buoy(self, srv):
        print 'requesting', srv
        response = self.find_single_buoy(np.copy(self.last_image), srv.target_name)

        if response is False:
            print 'did not find'
            resp = VisionRequest2DResponse(
                header=sub8_ros_tools.make_header(frame='/stereo_front/right'),
                found=False
            )

        else:
            # Fill in
            center, radius = response
            resp = VisionRequest2DResponse(
                header=Header(stamp=self.last_image_time, frame_id='/stereo_front/right'),
                pose=Pose2D(
                    x=center[0],
                    y=center[1],
                ),
                max_x=self.last_image.shape[0],
                max_y=self.last_image.shape[1],
                camera_info=self.image_sub.camera_info,
                found=True
            )
        return resp

    def request_buoy3d(self, srv):
        print "Requesting 3d pose"

        if (len(self.observations) > 5) and self.multi_obs is not None:
            estimated_pose = self.multi_obs.multilaterate(self.observations, self.pose_pairs)
            self.rviz.draw_sphere(estimated_pose, color=(0.2, 0.8, 0.0, 1.0), scaling=(0.5, 0.5, 0.5), frame='/map')
            resp = VisionRequestResponse(
                pose=PoseStamped(
                    header=Header(stamp=self.last_image_time, frame_id='/map'),
                    pose=Pose(
                        position=Point(*estimated_pose)
                    )
                ),
                found=True
            )
        else:
            if len(self.observations) <= 5:
                rospy.logerr("Did not attempt search because we did not have enough observations")
            else:
                rospy.logerr("Did not attempt search because buoys_2d was not fully initialized")

            resp = VisionRequestResponse(
                pose=PoseStamped(
                    header=Header(stamp=self.last_image_time, frame_id='/map'),
                ),
                found=False
            )
        return resp

    def publish_target_info(self, *args):
        if self.last_image is None:
            return

        self.find_buoys(np.copy(self.last_image))
        if self.last_draw_image is not None:
            self.image_pub.publish(self.last_draw_image)

    def image_cb(self, image):
        '''Hang on to last image'''
        self.last_image = image
        self.last_image_time = self.image_sub.last_image_time
        if self.camera_model is None:
            if self.image_sub.camera_info is None:
                return

            self.camera_model = image_geometry.PinholeCameraModel()
            self.camera_model.fromCameraInfo(self.image_sub.camera_info)
            self.multi_obs = MultiObservation(self.camera_model)

    def ncc(self, image, mean_thresh, scale=15):
        '''Compute normalized cross correlation w.r.t a shadowed pillbox fcn

        The expected scale will vary, so we don't cache it
        '''
        kernel = np.ones((scale, scale)) * -1
        midpoint = (scale // 2, scale // 2)
        cv2.circle(kernel, midpoint, midpoint[0], 1, -1)

        mean, std_dev = cv2.meanStdDev(image)

        # Check if the scene is brighter than our a priori target
        if mean > mean_thresh:
            kernel = -kernel

        normalized_cross_correlation = cv2.filter2D((image - mean) / std_dev, -1, kernel)
        renormalized = normalized_cross_correlation
        return renormalized

    def get_biggest(self, contours):
        if len(contours) > 0:
            cnt = max(contours, key=cv2.contourArea)
            area = cv2.contourArea(cnt)
            if area > self._min_size:
                M = cv2.moments(cnt)
                cx = int(M['m10'] / M['m00'])
                cy = int(M['m01'] / M['m00'])
                tpl_center = (int(cx), int(cy))
                return cnt, tpl_center, area
        else:
            return None

    def find_single_buoy(self, img, buoy_type):
        if buoy_type != 'yellow':
            return

        assert buoy_type in self.buoys[buoy_type], "Buoys_2d does not know buoy color: {}".format(buoy_type)
        max_area = 0
        best_ret = None

        some_observations = machine_learning.boost.observe(img)
        prediction2 = [int(x) for x in [self.boost.predict(obs) for obs in some_observations]]
        mask = np.reshape(prediction2, img[:, :, 2].shape).astype(np.uint8)
        contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

        ret = self.get_biggest(contours)
        if ret is None:
            return

        contour, tuple_center, area = ret
        if area > max_area:
            max_area = area
            best_ret = ret

        if best_ret is None:
            return False

        contour, tuple_center, area = best_ret
        true_center, rad = cv2.minEnclosingCircle(contour)

        if self.camera_model is not None and (buoy_type == 'yellow'):
            self.rviz.draw_ray_3d(tuple_center, self.camera_model, self.draw_colors[buoy_type])
            (t, rot_q) = self.transformer.lookupTransform('/map', '/stereo_front/right', self.last_image_time - rospy.Duration(0.14))
            trans = np.array(t)
            R = sub8_ros_tools.geometry_helpers.quaternion_matrix(rot_q)

            if (self.last_t is None) or (np.linalg.norm(trans - self.last_t) > 0.3):
                self.last_t = trans
                self.observations.append(true_center)
                self.pose_pairs.append((t, R))

            if len(self.observations) > 5:
                est = self.multi_obs.multilaterate(self.observations, self.pose_pairs)
                self.rviz.draw_sphere(est, color=(0.9, 0.1, 0.0, 1.0), scaling=(0.3, 0.3, 0.3), frame='/map')

            if len(self.observations) > 10:
                self.observations.popleft()
                self.pose_pairs.popleft()

        return tuple_center, rad

    def find_buoys(self, img):
        draw_image = np.copy(img)

        # This is only run if buoy_type is not None
        for buoy_name in self.buoys.keys():
            result = self.find_single_buoy(img, buoy_name)
            if not result:
                continue

            center, rad = result
            cv2.circle(draw_image, center, int(rad), (255, 255, 0), 2)
            font = cv2.FONT_HERSHEY_SIMPLEX
            cv2.putText(draw_image, '{}'.format(buoy_name), center, font, 0.8, (20, 20, 240), 1)

        self.last_draw_image = np.copy(draw_image)
Example #13
0
class torp_vision:

    def __init__(self):

        # Pull constants from config file
        self.lower = rospy.get_param('~lower_color_threshold', [0, 0, 80])
        self.upper = rospy.get_param(
            '~higher_color_threshold', [200, 200, 250])
        self.min_contour_area = rospy.get_param('~min_contour_area', .001)
        self.max_contour_area = rospy.get_param('~max_contour_area', 400)
        self.min_trans = rospy.get_param('~min_trans', .05)
        self.max_velocity = rospy.get_param('~max_velocity', 1)
        self.timeout = rospy.Duration(
            rospy.get_param('~timeout_seconds'), 250000)
        self.min_observations = rospy.get_param('~min_observations', 8)
        self.camera = rospy.get_param('~camera_topic',
                                      '/camera/front/left/image_rect_color')

        # Instantiate remaining variables and objects
        self._observations = deque()
        self._pose_pairs = deque()
        self._times = deque()
        self.last_image_time = None
        self.last_image = None
        self.tf_listener = tf.TransformListener()
        self.status = ''
        self.est = None
        self.visual_id = 0
        self.enabled = False
        self.bridge = CvBridge()

        # Image Subscriber and Camera Information

        self.image_sub = Image_Subscriber(self.camera, self.image_cb)
        self.camera_info = self.image_sub.wait_for_camera_info()
        '''
        These variables store the camera information required to perform
        the transformations on the coordinates to move from the subs
        perspective to our global map perspective. This information is
        also necessary to perform the least squares intersection which
        will find the 3D coordinates of the torpedo board based on 2D
        observations from the Camera. More info on this can be found in
        sub8_vision_tools.
        '''

        self.camera_info = self.image_sub.wait_for_camera_info()
        self.camera_model = PinholeCameraModel()
        self.camera_model.fromCameraInfo(self.camera_info)
        self.frame_id = self.camera_model.tfFrame()

        # Ros Services so mission can be toggled and info requested
        rospy.Service('~enable', SetBool, self.toggle_search)
        self.multi_obs = MultiObservation(self.camera_model)
        rospy.Service('~pose', VisionRequest, self.request_board3d)
        self.image_pub = Image_Publisher("torp_vision/debug")
        self.point_pub = rospy.Publisher(
            "torp_vision/points", Point, queue_size=1)
        self.mask_image_pub = rospy.Publisher(
            'torp_vison/mask', Image, queue_size=1)

        # Debug
        self.debug = rospy.get_param('~debug', True)

    def image_cb(self, image):
        '''
        Run each time an image comes in from ROS. If enabled,
        attempt to find the torpedo board.
        '''
        if not self.enabled:
            return

        self.last_image = image

        if self.last_image_time is not None and \
                self.image_sub.last_image_time < self.last_image_time:
            # Clear tf buffer if time went backwards (nice for playing bags in
            # loop)
            self.tf_listener.clear()

        self.last_image_time = self.image_sub.last_image_time
        self.acquire_targets(image)

    def toggle_search(self, srv):
        '''
        Callback for standard ~enable service. If true, start
        looking at frames for buoys.
        '''
        if srv.data:
            rospy.loginfo("TARGET ACQUISITION: enabled")
            self.enabled = True

        else:
            rospy.loginfo("TARGET ACQUISITION: disabled")
            self.enabled = False

        return SetBoolResponse(success=True)

    def request_board3d(self, srv):
        '''
        Callback for 3D vision request. Uses recent observations of target
        board  specified in target_name to attempt a least-squares position
        estimate. Ignoring orientation of board.
        '''
        if not self.enabled:
            return VisionRequestResponse(found=False)
        # buoy = self.buoys[srv.target_name]
        if self.est is None:
            return VisionRequestResponse(found=False)
        return VisionRequestResponse(
            pose=PoseStamped(
                header=Header(stamp=self.last_image_time, frame_id='/map'),
                pose=Pose(position=Point(*self.est))),
            found=True)

    def clear_old_observations(self):
        # Observations older than two seconds are discarded.
        time = rospy.Time.now()
        i = 0
        while i < len(self._times):
            if time - self._times[i] > self.timeout:
                self._times.popleft()
                self._observations.popleft()
                self._pose_pairs.popleft()
            else:
                i += 1
        # print('Clearing')

    def size(self):
        return len(self._observations)

    def add_observation(self, obs, pose_pair, time):
        self.clear_old_observations()
        # print('Adding...')
        if self.size() == 0 or np.linalg.norm(
                self._pose_pairs[-1][0] - pose_pair[0]) > self.min_trans:
            self._observations.append(obs)
            self._pose_pairs.append(pose_pair)
            self._times.append(time)

    def get_observations_and_pose_pairs(self):
        self.clear_old_observations()
        return (self._observations, self._pose_pairs)

    def detect(self, c):
        # initialize the shape name and approximate the contour
        target = "unidentified"
        peri = cv2.arcLength(c, True)

        if peri < self.min_contour_area or peri > self.max_contour_area:
            return target
        approx = cv2.approxPolyDP(c, 0.04 * peri, True)

        if len(approx) == 4:
            target = "Target Aquisition Successful"

        elif len(approx) == 3 or len(approx) == 5:
            target = "Partial Target Acquisition"

        return target

    def CLAHE(self, cv_image):
        '''
        CLAHE (Contrast Limited Adaptive Histogram Equalization)
        This increases the contrast between color channels and allows us to
        better differentiate colors under certain lighting conditions.
        '''
        clahe = cv2.createCLAHE(clipLimit=5., tileGridSize=(4, 4))

        # convert from BGR to LAB color space
        lab = cv2.cvtColor(cv_image, cv2.COLOR_BGR2LAB)
        l, a, b = cv2.split(lab)  # split on 3 different channels

        l2 = clahe.apply(l)  # apply CLAHE to the L-channel

        lab = cv2.merge((l2, a, b))  # merge channels
        cv_image = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)

        return cv_image

    def mask_image(self, cv_image, lower, upper):
        mask = cv2.inRange(cv_image, lower, upper)
        # Remove anything not within the bounds of our mask
        output = cv2.bitwise_and(cv_image, cv_image, mask=mask)

        # Resize to emphasize shapes
        # gray = cv2.cvtColor(output, cv2.COLOR_BGR2GRAY)

        # Blur image so our contours can better find the full shape.
        # blurred = cv2.GaussianBlur(gray, (5, 5), 0)
        if (self.debug):
            try:
                # print(output)
                self.mask_image_pub.publish(
                    self.bridge.cv2_to_imgmsg(np.array(output), 'bgr8'))
            except CvBridgeError as e:
                print(e)

        return output

    def acquire_targets(self, cv_image):
        # Take in the data and get its dimensions.
        height, width, channels = cv_image.shape

        # Run CLAHE.
        cv_image = self.CLAHE(cv_image)

        # Now we generate a color mask to isolate only red in the image. This
        # is achieved through the thresholds which can be changed in the above
        # constants.

        # create NumPy arrays from the boundaries
        lower = np.array(self.lower, dtype="uint8")
        upper = np.array(self.upper, dtype="uint8")

        # Generate a mask based on the constants.
        blurred = self.mask_image(cv_image, lower, upper)
        blurred = cv2.cvtColor(blurred, cv2.COLOR_BGR2GRAY)
        # Compute contours
        cnts = cv2.findContours(blurred.copy(), cv2.RETR_EXTERNAL,
                                cv2.CHAIN_APPROX_SIMPLE)

        cnts = cnts[1]
        '''
        We use OpenCV to compute our contours and then begin processing them
        to ensure we are identifying a proper target.
        '''

        shape = ''
        peri_max = 0
        max_x = 0
        max_y = 0
        m_shape = ''
        for c in cnts:
            # compute the center of the contour, then detect the name of the
            # shape using only the contour
            M = cv2.moments(c)
            if M["m00"] == 0:
                M["m00"] = .000001
            cX = int((M["m10"] / M["m00"]))
            cY = int((M["m01"] / M["m00"]))
            shape = self.detect(c)

            # multiply the contour (x, y)-coordinates by the resize ratio,
            # then draw the contours and the name of the shape on the image

            c = c.astype("float")
            # c *= ratio
            c = c.astype("int")
            if shape == "Target Aquisition Successful":
                if self.debug:
                    try:
                        cv2.drawContours(cv_image, [c], -1, (0, 255, 0), 2)
                        cv2.putText(cv_image, shape, (cX, cY),
                                    cv2.FONT_HERSHEY_SIMPLEX, 0.5,
                                    (255, 255, 255), 2)
                        self.image_pub.publish(cv_image)
                    except CvBridgeError as e:
                        print(e)

                peri = cv2.arcLength(c, True)
                if peri > peri_max:
                    peri_max = peri
                    max_x = cX
                    max_y = cY
                    m_shape = shape
        '''
        This is Kevin's Code, adapted for this project. We are trying to find
        the 3D coordinates of the torpedo board/target to give us a better idea
        of where we are trying to go and perform more accurate movements
        to align with the target. The first thing we need to do is convert from
        camera coordinates in pixels to 3D coordinates.
        Every time we succesfully get a target aquisition we add it to the
        counter. Once we observe it enough times
        we can be confident we are looking at the correct target. We then
        perform an least squares intersection from multiple angles
        to derive the approximate 3D coordinates.
        '''

        if m_shape == "Target Aquisition Successful":
            try:
                self.tf_listener.waitForTransform('/map',
                                                  self.camera_model.tfFrame(),
                                                  self.last_image_time,
                                                  rospy.Duration(0.2))
            except tf.Exception as e:
                rospy.logwarn(
                    "Could not transform camera to map: {}".format(e))
                return False

            (t, rot_q) = self.tf_listener.lookupTransform(
                '/map', self.camera_model.tfFrame(), self.last_image_time)
            R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q)
            center = np.array([max_x, max_y])
            self.add_observation(center, (np.array(t), R),
                                 self.last_image_time)

            observations, pose_pairs = self.get_observations_and_pose_pairs()
            if len(observations) > self.min_observations:
                self.est = self.multi_obs.lst_sqr_intersection(
                    observations, pose_pairs)
                self.status = 'Pose found'

            else:
                self.status = '{} observations'.format(len(observations))
Example #14
0
class VampireIdentifier:
    def __init__(self):

        # Pull constants from config file
        self.override = False
        self.lower = [0, 0, 0]
        self.upper = [0, 0, 0]
        self.min_trans = 0
        self.max_velocity = 0
        self.timeout = 0
        self.min_observations = 0
        self.camera = rospy.get_param('~camera_topic',
                                      '/camera/down/image_rect_color')
        self.goal = None
        self.last_config = None
        self.reconfigure_server = DynamicReconfigureServer(
            VampireIdentifierConfig, self.reconfigure)

        # Instantiate remaining variables and objects
        self._observations = deque()
        self._pose_pairs = deque()
        self._times = deque()
        self.last_image_time = None
        self.last_image = None
        self.tf_listener = tf.TransformListener()
        self.status = ''
        self.est = None
        self.visual_id = 0
        self.enabled = False
        self.bridge = CvBridge()

        # Image Subscriber and Camera Information

        self.image_sub = Image_Subscriber(self.camera, self.image_cb)
        self.camera_info = self.image_sub.wait_for_camera_info()

        self.camera_info = self.image_sub.wait_for_camera_info()
        self.camera_model = PinholeCameraModel()
        self.camera_model.fromCameraInfo(self.camera_info)
        self.frame_id = self.camera_model.tfFrame()

        # Ros Services so mission can be toggled and info requested
        rospy.Service('~enable', SetBool, self.toggle_search)
        self.multi_obs = MultiObservation(self.camera_model)
        rospy.Service('~pose', VisionRequest, self.request_buoy)
        self.image_pub = Image_Publisher("drac_vision/debug")
        self.point_pub = rospy.Publisher("drac_vision/points",
                                         Point,
                                         queue_size=1)
        self.mask_image_pub = rospy.Publisher('drac_vision/mask',
                                              Image,
                                              queue_size=1)

        # Debug
        self.debug = rospy.get_param('~debug', True)

    @staticmethod
    def parse_string(threshes):
        ret = [float(thresh.strip()) for thresh in threshes.split(',')]
        if len(ret) != 3:
            raise ValueError('not 3')
        return ret

    def reconfigure(self, config, level):
        try:
            self.override = config['override']
            self.goal = config['target']
            self.lower = self.parse_string(config['dyn_lower'])
            self.upper = self.parse_string(config['dyn_upper'])
            self.min_trans = config['min_trans']
            self.max_velocity = config['max_velocity']
            self.timeout = config['timeout']
            self.min_observations = config['min_obs']

        except ValueError as e:
            rospy.logwarn('Invalid dynamic reconfigure: {}'.format(e))
            return self.last_config

        if self.override:
            # Dynamic Values for testing
            self.lower = np.array(self.lower)
            self.upper = np.array(self.upper)
        else:
            # Hard Set for use in Competition
            if self.goal == 'drac':
                self.lower = rospy.get_param('~dracula_low_thresh', [0, 0, 80])
                self.upper = rospy.get_param('~dracula_high_thresh',
                                             [0, 0, 80])
            else:
                raise ValueError('Invalid Target Name')
        self.last_config = config
        rospy.loginfo('Params succesfully updated via dynamic reconfigure')
        return config

    def image_cb(self, image):
        '''
        Run each time an image comes in from ROS.
        '''
        if not self.enabled:
            return

        self.last_image = image

        if self.last_image_time is not None and \
                self.image_sub.last_image_time < self.last_image_time:
            # Clear tf buffer if time went backwards (nice for playing bags in
            # loop)
            self.tf_listener.clear()

        self.last_image_time = self.image_sub.last_image_time
        self.acquire_targets(image)

    def toggle_search(self, srv):
        '''
        Callback for standard ~enable service. If true, start
        looking at frames for buoys.
        '''
        if srv.data:
            rospy.loginfo("TARGET ACQUISITION: enabled")
            self.enabled = True

        else:
            rospy.loginfo("TARGET ACQUISITION: disabled")
            self.enabled = False

        return SetBoolResponse(success=True)

    def request_buoy(self, srv):
        '''
        Callback for 3D vision request. Uses recent observations of target
        board  specified in target_name to attempt a least-squares position
        estimate. Ignoring orientation of board.
        '''
        if not self.enabled:
            return VisionRequestResponse(found=False)
        # buoy = self.buoys[srv.target_name]
        if self.est is None:
            return VisionRequestResponse(found=False)
        return VisionRequestResponse(pose=PoseStamped(
            header=Header(stamp=self.last_image_time, frame_id='/map'),
            pose=Pose(position=Point(*self.est))),
                                     found=True)

    def clear_old_observations(self):
        '''
        Observations older than two seconds are discarded.
        '''
        time = rospy.Time.now()
        i = 0
        while i < len(self._times):
            if time - self._times[i] > self.timeout:
                self._times.popleft()
                self._observations.popleft()
                self._pose_pairs.popleft()
            else:
                i += 1
        # print('Clearing')

    def add_observation(self, obs, pose_pair, time):
        '''
        Add a new observation associated with an object
        '''

        self.clear_old_observations()
        # print('Adding...')
        if slen(self._observations) == 0 or np.linalg.norm(
                self._pose_pairs[-1][0] - pose_pair[0]) > self.min_trans:
            self._observations.append(obs)
            self._pose_pairs.append(pose_pair)
            self._times.append(time)

    def get_observations_and_pose_pairs(self):
        '''
        Fetch all recent observations + clear old ones
        '''

        self.clear_old_observations()
        return (self._observations, self._pose_pairs)

    def detect(self, c):
        '''
        Verify the shape in the masked image is large enough to be a valid target.
        This changes depending on target Vampire, as does the number of targets we want.  
        '''
        target = "unidentified"
        peri = cv2.arcLength(c, True)

        if peri < self.min_contour_area or peri > self.max_contour_area:
            return target
        approx = cv2.approxPolyDP(c, 0.04 * peri, True)

        target = "Target Aquisition Successful"

        return target

    def mask_image(self, cv_image, lower, upper):
        mask = cv2.inRange(cv_image, lower, upper)
        # Remove anything not within the bounds of our mask
        output = cv2.bitwise_and(cv_image, cv_image, mask=mask)
        print('ree')

        if (self.debug):
            try:
                # print(output)
                self.mask_image_pub.publish(
                    self.bridge.cv2_to_imgmsg(np.array(output), 'bgr8'))
            except CvBridgeError as e:
                print(e)

        return output

    def acquire_targets(self, cv_image):
        # Take in the data and get its dimensions.
        height, width, channels = cv_image.shape

        # create NumPy arrays from the boundaries
        lower = np.array(self.lower, dtype="uint8")
        upper = np.array(self.upper, dtype="uint8")

        # Generate a mask based on the constants.
        blurred = self.mask_image(cv_image, lower, upper)
        blurred = cv2.cvtColor(blurred, cv2.COLOR_BGR2GRAY)
        # Compute contours
        cnts = cv2.findContours(blurred.copy(), cv2.RETR_EXTERNAL,
                                cv2.CHAIN_APPROX_SIMPLE)

        cnts = cnts[1]
        '''
        We use OpenCV to compute our contours and then begin processing them
        to ensure we are identifying a proper target.
        '''

        shape = ''
        peri_max = 0
        max_x = 0
        max_y = 0
        m_shape = ''
        for c in cnts:
            # compute the center of the contour, then detect the name of the
            # shape using only the contour
            M = cv2.moments(c)
            if M["m00"] == 0:
                M["m00"] = .000001
            cX = int((M["m10"] / M["m00"]))
            cY = int((M["m01"] / M["m00"]))
            self.point_pub.publish(Point(x=cX, y=cY))
            shape = self.detect(c)

            # multiply the contour (x, y)-coordinates by the resize ratio,
            # then draw the contours and the name of the shape on the image

            c = c.astype("float")
            # c *= ratio
            c = c.astype("int")
            if shape == "Target Aquisition Successful":
                if self.debug:
                    try:
                        cv2.drawContours(cv_image, [c], -1, (0, 255, 0), 2)
                        cv2.putText(cv_image, shape, (cX, cY),
                                    cv2.FONT_HERSHEY_SIMPLEX, 0.5,
                                    (255, 255, 255), 2)
                        self.image_pub.publish(cv_image)
                    except CvBridgeError as e:
                        print(e)

                # Grab the largest contour. Generally this is a safe bet but... We may need to tweak this for the three different vampires.
                peri = cv2.arcLength(c, True)
                if peri > peri_max:
                    peri_max = peri
                    max_x = cX
                    max_y = cY
                    m_shape = shape
        '''
        Approximate 3D coordinates.
        '''

        if m_shape == "Target Aquisition Successful":
            try:
                self.tf_listener.waitForTransform('/map',
                                                  self.camera_model.tfFrame(),
                                                  self.last_image_time,
                                                  rospy.Duration(0.2))
            except tf.Exception as e:
                rospy.logwarn(
                    "Could not transform camera to map: {}".format(e))
                return False

            (t, rot_q) = self.tf_listener.lookupTransform(
                '/map', self.camera_model.tfFrame(), self.last_image_time)
            R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q)
            center = np.array([max_x, max_y])
            self.add_observation(center, (np.array(t), R),
                                 self.last_image_time)

            observations, pose_pairs = self.get_observations_and_pose_pairs()
            if len(observations) > self.min_observations:
                self.est = self.multi_obs.lst_sqr_intersection(
                    observations, pose_pairs)
                self.status = 'Pose found'

            else:
                self.status = '{} observations'.format(len(observations))
Example #15
0
class BuoyFinder:
    _min_size = 50
    '''
    TODO:
        Check for outliers in observations
    '''
    def __init__(self):
        self.transformer = tf.TransformListener()
        rospy.sleep(2.0)

        self.search = False
        self.last_image = None
        self.last_draw_image = None
        self.last_image_time = None
        self.camera_model = None
        self.multi_obs = None
        self.max_observations = 200
        self._id = 0  # Only for display

        self.rviz = rviz.RvizVisualizer()

        boosting = rospy.get_param("/buoys/use_boost")
        self.buoys = {}
        for color in ['red', 'yellow', 'green']:
            self.buoys[color] = Buoy(color, boosting)
            self.buoys[color].load_segmentation()

        self.image_sub = sub8_ros_tools.Image_Subscriber('/camera/front/left/image_raw', self.image_cb)
        self.image_pub = sub8_ros_tools.Image_Publisher('/vision/buoy_2d/target_info')
        self.mask_pub = sub8_ros_tools.Image_Publisher('/vision/buoy_2d/mask')

        # Occasional status publisher
        self.timer = rospy.Timer(rospy.Duration(.25), self.publish_target_info)

        self.toggle = rospy.Service('vision/buoys/search', SetBool, self.toggle_search)
        self.pose2d_service = rospy.Service('vision/buoys/2D', VisionRequest2D, self.request_buoy)
        self.pose_service = rospy.Service('vision/buoys/pose', VisionRequest, self.request_buoy3d)

        print "BUOY - Fueled up, ready to go!"

    def toggle_search(self, srv):
        if srv.data:
            rospy.loginfo("BUOY - Looking for buoys now.")
            self.search = True
        else:
            rospy.loginfo("BUOY - Done looking for buoys.")
            self.search = False

        return SetBoolResponse(success=srv.data)

    def request_buoy(self, srv):
        print 'requesting', srv
        timestamp = self.last_image_time
        response = self.find_single_buoy(np.copy(self.last_image), timestamp, srv.target_name)

        if response is False:
            print 'did not find'
            resp = VisionRequest2DResponse(
                header=sub8_ros_tools.make_header(frame='front_left_cam'),
                found=False
            )

        else:
            # Fill in
            center, radius = response
            resp = VisionRequest2DResponse(
                header=Header(stamp=timestamp, frame_id='front_left_cam'),
                pose=Pose2D(
                    x=center[0],
                    y=center[1],
                ),
                max_x=self.last_image.shape[0],
                max_y=self.last_image.shape[1],
                camera_info=self.image_sub.camera_info,
                found=True
            )
        return resp

    def request_buoy3d(self, srv):
        print "Requesting 3d pose"
        buoy = self.buoys[srv.target_name]
        if (len(buoy.observations) > 5) and self.multi_obs is not None:
            estimated_pose = self.multi_obs.lst_sqr_intersection(buoy.observations, buoy.pose_pairs)

            rospy.loginfo("===================================")
            rospy.loginfo("BUOY: est pose: {}".format(estimated_pose))
            rospy.loginfo("===================================")

            self.rviz.draw_sphere(estimated_pose, color=buoy.draw_colors,
                scaling=(0.5, 0.5, 0.5), frame='/map', _id=buoy.visual_id)

            resp = VisionRequestResponse(
                pose=PoseStamped(
                    header=Header(stamp=self.last_image_time, frame_id='/map'),
                    pose=Pose(
                        position=Point(*estimated_pose)
                    )
                ),
                found=True
            )
        else:
            if len(buoy.observations) <= 5:
                rospy.logerr("Did not attempt search because we did not have enough observations ({})".format(len(buoy.observations)))
            else:
                rospy.logerr("Did not attempt search because buoys_2d was not fully initialized")

            resp = VisionRequestResponse(
                pose=PoseStamped(
                    header=Header(stamp=self.last_image_time, frame_id='/map'),
                ),
                found=False
            )
        return resp

    def publish_target_info(self, *args):
        if not self.search or self.last_image is None:
            return

        self.find_buoys(np.copy(self.last_image), self.last_image_time)
        if self.last_draw_image is not None:
            self.image_pub.publish(self.last_draw_image)

    def image_cb(self, image):
        '''Hang on to last image'''
        self.last_image = image
        self.last_image_time = self.image_sub.last_image_time
        if self.camera_model is None:
            if self.image_sub.camera_info is None:
                return

            self.camera_model = image_geometry.PinholeCameraModel()
            self.camera_model.fromCameraInfo(self.image_sub.camera_info)
            self.multi_obs = MultiObservation(self.camera_model)

    def ncc(self, image, mean_thresh, scale=15):
        '''Compute normalized cross correlation w.r.t a shadowed pillbox fcn

        The expected scale will vary, so we don't cache it
        '''
        kernel = np.ones((scale, scale)) * -1
        midpoint = (scale // 2, scale // 2)
        cv2.circle(kernel, midpoint, midpoint[0], 1, -1)

        mean, std_dev = cv2.meanStdDev(image)

        # Check if the scene is brighter than our a priori target
        if mean > mean_thresh:
            kernel = -kernel

        normalized_cross_correlation = cv2.filter2D((image - mean) / std_dev, -1, kernel)
        renormalized = normalized_cross_correlation
        return renormalized

    def get_biggest(self, contours):
        if len(contours) > 0:
            cnt = max(contours, key=cv2.contourArea)
            area = cv2.contourArea(cnt)
            if area > self._min_size:
                M = cv2.moments(cnt)
                cx = int(M['m10'] / M['m00'])
                cy = int(M['m01'] / M['m00'])
                tpl_center = (int(cx), int(cy))
                return cnt, tpl_center, area
            return None
        else:
            return None

    def find_single_buoy(self, img, timestamp, buoy_type):
        assert buoy_type in self.buoys.keys(), "Buoys_2d does not know buoy color: {}".format(buoy_type)
        max_area = 0
        best_ret = None

        buoy = self.buoys[buoy_type]
        rospy.sleep(.1)
        mask = buoy.segment(img)
        kernel = np.ones((5,5),np.uint8)
        mask = cv2.erode(mask, kernel, iterations = 2)
        mask = cv2.dilate(mask, kernel, iterations = 2)

        draw_mask = np.dstack([mask] * 3)
        draw_mask[:,:,0] *= 0
        if buoy_type == 'red':
            draw_mask[:,:,1] *= 0
        if buoy_type == 'green':
            draw_mask[:,:,2] *= 0
        self.mask_pub.publish(draw_mask)

        contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

        ret = self.get_biggest(contours)
        if ret is None:
            print "BUOY {} - No contours found".format(buoy_type)
            return

        contour, tuple_center, area = ret
        if area > max_area:
            max_area = area
            best_ret = ret

        if best_ret is None:
            print "BUOY {} - best_ret is None".format(buoy_type)
            return False

        contour, tuple_center, area = best_ret
        true_center, rad = cv2.minEnclosingCircle(contour)

        if self.camera_model is not None:

            if not self.sanity_check(tuple_center, timestamp):
                return False

            (t, rot_q) = self.transformer.lookupTransform('/map', '/front_left_cam', timestamp)
            trans = np.array(t)
            R = mil_ros_tools.geometry_helpers.quaternion_matrix(rot_q)

            self.rviz.draw_ray_3d(tuple_center, self.camera_model, buoy.draw_colors, frame='/front_left_cam',
                _id=self._id + 100, timestamp=timestamp)
            self._id += 1
            if self._id >= self.max_observations * 3:
                self._id = 0

            if (buoy.last_t is None) or (np.linalg.norm(trans - buoy.last_t) > 0.1):
                buoy.last_t = trans
                buoy.observations.append(true_center)
                buoy.pose_pairs.append((t, R))

            print "BUOY {} - {} observations".format(buoy_type, len(buoy.observations))
            if len(buoy.observations) > 5:
                est = self.multi_obs.lst_sqr_intersection(buoy.observations, buoy.pose_pairs)

                rospy.loginfo("===================================")
                rospy.loginfo("BUOY: est pose: {}".format(est))
                rospy.loginfo("===================================")

                self.rviz.draw_sphere(est, color=buoy.draw_colors,
                    scaling=(0.5, 0.5, 0.5), frame='/map', _id=buoy.visual_id)

            if len(buoy.observations) > self.max_observations:
                buoy.observations.popleft()
                buoy.pose_pairs.popleft()
        else:
            print "BUOY {} - camera_model is None".format(buoy_type)

        return tuple_center, rad

    def find_buoys(self, img, timestamp):
        draw_image = np.copy(img)

        for buoy_name in self.buoys.keys():
            if self.buoys[buoy_name] is None:
                continue

            rospy.loginfo("BUOY - Looking for {}".format(buoy_name))
            result = self.find_single_buoy(img, timestamp, buoy_name)
            if not result:
                continue

            center, rad = result
            cv2.circle(draw_image, center, int(rad), (255, 255, 0), 2)
            font = cv2.FONT_HERSHEY_SIMPLEX
            cv2.putText(draw_image, '{}'.format(buoy_name), center, font, 0.8, (20, 20, 240), 1)

        self.last_draw_image = np.copy(draw_image)

    def sanity_check(self, coordinate, timestamp):
        '''
        Check if the observation is unreasonable. More can go here if we want.
        '''
        sane = True
        if np.linalg.norm(self.transformer.lookupTwist('/map', '/front_left_cam', timestamp, rospy.Duration(.5))) > 1:
            rospy.logerr("BUOY - Moving too fast. Not observing buoy.")
            sane = False

        return sane