示例#1
0
    def __init__(self):
        """
        Constructor for ObstacleAvoiderThread. It sets up the parameters for
        camera snapshot retrieval as well as the parameters for optical flow
        feature selection and time-to-collision (TTC) calculations.
        """
        Thread.__init__(self)

        # Initialize camera
        self.camera = AutoCamera()

        # Parameters for Shi-Tomasi corner detection
        self.feature_params = dict( maxCorners = 100,
                                    qualityLevel = 0.3,
                                    minDistance = 7,
                                    blockSize = 7 )

        # Parameters for Lucas-Kanade optical flow
        self.lk_params = dict( winSize = (15,15),
                               maxLevel = 2,
                               criteria = (cv2.TERM_CRITERIA_EPS | \
                                           cv2.TERM_CRITERIA_COUNT, 10, 0.03) )

        # Initialize optical flow drawing class
        self.drawer = OpticalFlowDrawer(100)

        # Create array for line equations
        self.lines = np.zeros((100, 3))
        self.local_scales = np.zeros((100, 1))
        self.left_scales = np.zeros((100, 1))
        self.right_scales = np.zeros((100, 1))

        # Median filtering for smoother TTC computations
        self.scale_filter = MedianFilter(3)

        self.old_gray = None
        self.p0 = None

        # Callbacks
        self.imgdisp_cb = None
        self.min_ttc_cb = None
        self.balance_strategy_cb = None
#!/usr/bin/env python
# license removed for brevity
import serial
import rospy
from sensor_msgs.msg import Imu
import math
import tf
from median_filter import MedianFilter
from common_algos.constants import *
import traceback

um6_imu = Imu()

velocity_mf = MedianFilter(max_queue_size=5, min_value=0.0005, max_value=2.05)

def get_angle(section, coef=1.00424):
    data = ord(section[3]) + ord(section[4]) * 256 * 256 + ord(section[5]) * 256
    data = data - (data >> 23) * 1024 * 1024 * 16
    data = data * coef / 100
    return data

def get_velocity(section):
    data = ord(section[0]) + ord(section[1]) * 256 * 256 + ord(section[2]) * 256
    data = data - (data >> 23) * 1024 * 1024 * 16
    data = data * 2500.0 / 8388608
    return data

def check(section):
    sum = 0
    for i in range(6):
        sum = sum + ord(section[i])
示例#3
0
class ObstacleAvoiderThread(Thread):
    """
    Class for running the obstacle avoidance algorithm in a Python thread.
    """
    def __init__(self):
        """
        Constructor for ObstacleAvoiderThread. It sets up the parameters for
        camera snapshot retrieval as well as the parameters for optical flow
        feature selection and time-to-collision (TTC) calculations.
        """
        Thread.__init__(self)

        # Initialize camera
        self.camera = AutoCamera()

        # Parameters for Shi-Tomasi corner detection
        self.feature_params = dict( maxCorners = 100,
                                    qualityLevel = 0.3,
                                    minDistance = 7,
                                    blockSize = 7 )

        # Parameters for Lucas-Kanade optical flow
        self.lk_params = dict( winSize = (15,15),
                               maxLevel = 2,
                               criteria = (cv2.TERM_CRITERIA_EPS | \
                                           cv2.TERM_CRITERIA_COUNT, 10, 0.03) )

        # Initialize optical flow drawing class
        self.drawer = OpticalFlowDrawer(100)

        # Create array for line equations
        self.lines = np.zeros((100, 3))
        self.local_scales = np.zeros((100, 1))
        self.left_scales = np.zeros((100, 1))
        self.right_scales = np.zeros((100, 1))

        # Median filtering for smoother TTC computations
        self.scale_filter = MedianFilter(3)

        self.old_gray = None
        self.p0 = None

        # Callbacks
        self.imgdisp_cb = None
        self.min_ttc_cb = None
        self.balance_strategy_cb = None

    def set_imgdisp_cb(self, imgdisp_cb):
        """
        Setter for the image display callback. imgdisp_cb takes on the
        following format:

           imgdisp_cb(cv2, img)

        where cv2 is the OpenCV object, and img is the image data that was
        retrieved (either in original form or in modified form with optical
        flows).
        """
        self.imgdisp_cb = imgdisp_cb

    def set_min_ttc_cb(self, min_ttc_cb):
        """
        Setter for the minimum TTC value callback, which is called when the
        computation of this value has finished for a particular camera
        snapshot. min_ttc_cb takes on the following format:

           min_ttc_cb(the_min_ttc)

        where the_min_ttc is the minimum TTC value in the entire snapshot.
        """
        self.min_ttc_cb = min_ttc_cb

    def set_balance_strategy_cb(self, balance_strategy_cb):
        """
        Setter for the balance strategy callback, which is called when the
        computation of the minimum TTC value for the left and right halves of
        a particular camera snapshot has finished. balance_strategy_cb takes
        on the following format:

           balance_strategy_cb(left_ttc, right_ttc)

        where left_ttc is the minimum TTC value in the left half of the
        snapshot, and right_ttc is the minimum TTC value in the right half of
        the snapshot.
        """
        self.balance_strategy_cb = balance_strategy_cb

    @staticmethod
    def find_neighborhoods(delaunay_triangles):
        """
        Helper function used internally by the TTC computation algorithm for
        finding nearby feature points corresponding to a particular feature
        point. This function should not be used directly outside this class.
        """
        neighbor_dict = {}
        for triangle_point_indices in delaunay_triangles:
            index_set = set(triangle_point_indices)
            for index in index_set:
                index_set_2 = index_set - set([index])
                if len(index_set_2) != 0:
                    if neighbor_dict.get(index):
                        neighbor_dict[index] = neighbor_dict[index] | \
                                               index_set_2
                    else:
                        neighbor_dict[index] = index_set_2
        return neighbor_dict

    @staticmethod
    def filter_local_scales(local_scales, num_localscales, threshold=None):
        """
        Helper function used internally by the TTC computation algorithm for
        filtering local scale changes of each feature point using some specific
        threshold. This function should not be used directly outside this
        class.
        """
        thresh_scales = local_scales[:num_localscales]
        the_threshold = threshold
        max_local_scale = 0
        if len(thresh_scales) != 0:
            if threshold is None:
                the_threshold = 0.1 * thresh_scales.max()
            thresh_scales = thresh_scales[thresh_scales > the_threshold]
        if len(thresh_scales) != 0:
            max_local_scale = np.max(thresh_scales)
        return the_threshold, max_local_scale, thresh_scales

    def run(self):
        """
        A function representing the thread runtime code. This function takes
        care of camera snapshot retrieval, optical flow imaging, optical flow
        feature selection, TTC computations, and optical flow imaging. TTC
        computations are computed for the entire snapshot, which is useful for
        detecting obstacles, as well as for the left and right halves of the
        snapshot, which is useful for deciding whether to make a left or right
        turn. The thread runs until the user decides to terminate it.

        This function should not be used directly outside this class.
        """
        filter_update_time = None
        for frame in self.camera.get_iterator():
            last_iter_time = time.time()
            the_frame = self.camera.get_frame(frame)
            self.drawer.set_frame(the_frame)

            if self.old_gray is None:
                # Take first frame and find corners in it
                self.old_gray = cv2.cvtColor(the_frame, cv2.COLOR_BGR2GRAY)
                self.p0 = cv2.goodFeaturesToTrack(self.old_gray, mask = None, \
                                                  **self.feature_params)

                # Reset the state of the optical flow drawing class
                self.drawer.reset()

                # Move on to next frame capture
                filter_update_time = last_iter_time
                self.scale_filter.reset_filter()
                continue

            frame_gray = cv2.cvtColor(the_frame, cv2.COLOR_BGR2GRAY)

            # Calculate optical flow
            p1, st, _ = cv2.calcOpticalFlowPyrLK(self.old_gray, frame_gray, \
                                                 self.p0, None, \
                                                 **self.lk_params)
            if p1 is None:
                # We lost all tracking at this point; reinitialize the obstacle
                # avoider
                self.old_gray = None
                self.imgdisp_cb(cv2, the_frame)
                k = cv2.waitKey(30) & 0xff
                if k == 27: # Was escape pressed?
                    break
                continue

            # Select good points
            good_new = p1[st == 1]
            good_old = self.p0[st == 1]

            # Start worker up
            worker = FrameWorker(self, frame_gray, last_iter_time, \
                filter_update_time, good_old, good_new)
            worker.start()

            # Once we have results from the TTC computation, use them
            worker.wait_on_ttc_computation()
            filter_update_time = worker.get_latest_ttc_update_time()
            min_ttc, left_ttc, right_ttc = worker.get_ttc_values()
            if min_ttc is not None:
                self.min_ttc_cb(min_ttc)
                self.balance_strategy_cb(left_ttc, right_ttc)

            # Now update the previous frame and previous points
            if self.old_gray is not None:
                self.old_gray = frame_gray.copy()
                self.p0 = good_new.reshape(-1,1,2)

            # Once we have results from the render, use them
            worker.wait_on_render()
            self.imgdisp_cb(cv2, cv2.add(the_frame, worker.get_updated_mask()))

            # Idle for whatever time we have left
            iter_time = time.time()
            if last_iter_time is not None and last_iter_time + 30 > iter_time:
                k = cv2.waitKey(int(ceil(30 - (iter_time - last_iter_time))))
            else:
                k = cv2.waitKey(1) & 0xff
            if k == 27: # Was escape pressed?
                break

        cv2.destroyAllWindows()
        self.camera.destroy()