Ejemplo n.º 1
0
    def __init__(self,
                 collision_avoid_queue,
                 log_dsrc=True,
                 log_radar=True,
                 dsrc_log_file=None,
                 radar_log_file=None,
                 dsrc_enabled=True,
                 radar_enabled=True,
                 log_level="DEBUG",
                 log_config=None):
        """ Setup Combiner, initialize DSRC+Radar event dispatcher. """
        self.dsrc_data = None
        self.radar_data = None

        self.data_count = 0

        self.dsrc_enabled = dsrc_enabled
        self.radar_enabled = radar_enabled

        self.data_queue = Queue()
        self.combined_data_queue = collision_avoid_queue
        # self.callback = callback

        if self.dsrc_enabled:
            self.dsrc_event_dispatcher = DsrcEventDispatcher(
                self.data_queue,
                log=log_dsrc,
                log_file=dsrc_log_file,
                log_level=log_level,
                log_config=log_config)
        if self.radar_enabled:
            self.radar_event_dispatcher = RadarEventDispatcher(
                self.data_queue,
                log=log_radar,
                log_file=radar_log_file,
                log_level=log_level,
                log_config=log_config)

        self.logger = logging.getLogger('debug_combined')
    def __init__(self, collision_avoid_queue, log_dsrc=True, log_radar=True,
                 dsrc_log_file=None, radar_log_file=None, dsrc_enabled=True,
                 radar_enabled=True, log_level="DEBUG", log_config=None):
        """ Setup Combiner, initialize DSRC+Radar event dispatcher. """
        self.dsrc_data = None
        self.radar_data = None

        self.data_count = 0

        self.dsrc_enabled = dsrc_enabled
        self.radar_enabled = radar_enabled

        self.data_queue = Queue()
        self.combined_data_queue = collision_avoid_queue
        # self.callback = callback


        if self.dsrc_enabled:
            self.dsrc_event_dispatcher = DsrcEventDispatcher(self.data_queue, log=log_dsrc, log_file=dsrc_log_file, log_level=log_level, log_config=log_config)
        if self.radar_enabled:
            self.radar_event_dispatcher = RadarEventDispatcher(self.data_queue, log=log_radar, log_file=radar_log_file, log_level=log_level, log_config=log_config)

        self.logger = logging.getLogger('debug_combined')
class Combiner(object):
    """ Takes DSRC+Radar information and forms a single model of the environment.

    This package is responsible for taking data from each source, normalizing the
    format, and combining that information into a single model of the car's
    environment. Once the combination is complete the new model gets passed to
    the registered callback function, which in our case feeds data into our collision
    avoidance system.
    """

    def __init__(self, collision_avoid_queue, log_dsrc=True, log_radar=True,
                 dsrc_log_file=None, radar_log_file=None, dsrc_enabled=True,
                 radar_enabled=True, log_level="DEBUG", log_config=None):
        """ Setup Combiner, initialize DSRC+Radar event dispatcher. """
        self.dsrc_data = None
        self.radar_data = None

        self.data_count = 0

        self.dsrc_enabled = dsrc_enabled
        self.radar_enabled = radar_enabled

        self.data_queue = Queue()
        self.combined_data_queue = collision_avoid_queue
        # self.callback = callback


        if self.dsrc_enabled:
            self.dsrc_event_dispatcher = DsrcEventDispatcher(self.data_queue, log=log_dsrc, log_file=dsrc_log_file, log_level=log_level, log_config=log_config)
        if self.radar_enabled:
            self.radar_event_dispatcher = RadarEventDispatcher(self.data_queue, log=log_radar, log_file=radar_log_file, log_level=log_level, log_config=log_config)

        self.logger = logging.getLogger('debug_combined')

    def start(self):
        """ Start running the event dispatcher threads (we are ready to recieve data). """

        if self.dsrc_enabled:
            self.dsrc_event_dispatcher.start()
        if self.radar_enabled:
            self.radar_event_dispatcher.start()

        while self.dispatchers_alive():
            if self.data_queue.qsize() > 1:
                print 'WARNING: The queue depth is %s, we are behind real time!' % self.data_queue.qsize()

            try:
                dispatcher_data = self.data_queue.get(timeout=0.5)

                if dispatcher_data['from'] == 'dsrc':
                    self.dsrc_data_callback(dispatcher_data['data'])
                else:
                    self.radar_data_callback(dispatcher_data['data'])
            except Empty:
                pass
                # print 'Timeout'

        if self.dsrc_enabled:
            self.dsrc_event_dispatcher.terminate()
        if self.radar_enabled:
            self.radar_event_dispatcher.terminate()

    def dispatchers_alive(self):
        if self.dsrc_enabled and not self.dsrc_event_dispatcher.is_alive():
            return False

        if self.radar_enabled and not self.radar_event_dispatcher.is_alive():
            return False

        if not self.dsrc_enabled and not self.radar_enabled:
            return False

        return True

    def hex_to_int(self, h, d):
        # h = number to convert (can be integer)
        # d = number of bits in the number
        i = h
        if i >= 2**(d-1):
            i -= 2**d
        return i

    def dsrc_data_callback(self, data):
        """ Callback for when new DSRC data arrives. """
        data = self.data_normalize_dsrc(data)
        self.dsrc_data = data
        self._update_combined()

    def radar_data_callback(self, data):
        """ Callback for when new radar data arrives. """
        data = self.data_normalize_radar(data)
        self.radar_data = data
        self._update_combined()

    def data_normalize_radar(self, data):
        """
        Takes in raw radar data and normalizes the format.
        Dictionary that is going to have a bunch of metadata about the
        message, and then a list of entities that the radar data 'sees'

        Create a key in the dict called entities that is a list
        """
        new_data = {}

        # First we handle the status messages
        try:
            # Message 4E0
            new_data['scan_index'] = data['scan_index']
            new_data['vehicle_speed'] = data['vehicle_speed'] / 16
            new_data['yaw_rate'] = self.hex_to_int(data['yaw_rate'], 12) / 16
            new_data['radius_curvature'] = self.hex_to_int(data['radius_curvature'], 14)
            # Message 4E1
            new_data['steering_angle_ack'] = data['steering_angle_ack']
            new_data['radiating'] = data['radiating']
            new_data['maximum_tracks'] = data['maximum_tracks'] # Might not be necessary
            new_data['grouping_mode'] = data['grouping_mode']
            new_data['yaw_rate_bias'] = self.hex_to_int(data['yaw_rate_bias'], 8) / 8
            new_data['speed_comp_factor'] = (data['speed_comp_factor'] / 512) + 0.9375
            # Message 4E3
            new_data['auto_align_angle'] = self.hex_to_int(data['auto_align_angle'], 8) / 16
            # Message 5E8
            new_data['sideslip_angle'] = self.hex_to_int(data['sideslip_angle'], 10) / 8
        except KeyError:
            self.logger.debug("KeyError, printing data structure\n")
            self.logger.debug(json.dumps(data))

        # Now we deal with all of the tracks
        new_data['entities'] = list()
        track_id = 1
        vehicle_speed = new_data['vehicle_speed'] if 'vehicle_speed' in new_data else 0 # m/s
        for i in range(1,65):
            track_number = str(track_id)
            try:
                track_status = data[track_number + "_track_status"]
            except KeyError:
                track_id += 1
                continue
            # If we get here, then this track is valid, so add to the list
            try:
                track = {}
                track["track_number"] = track_number
                track[track_number + "_track_status"] = data[track_number + "_track_status"]
                track[track_number + "_track_range"] = data[track_number + "_track_range"] / 10.0
                track[track_number + "_track_range_rate"] = self.hex_to_int(data[track_number + "_track_range_rate"], 14) / 100.0
                track[track_number + "_track_range_accel"] = self.hex_to_int(data[track_number + "_track_range_accel"], 10) / 20.0
                track[track_number + "_track_angle"] = self.hex_to_int(data[track_number + "_track_angle"], 10) / 10.0
                track[track_number + "_track_width"] = data[track_number + "_track_width"] / 2.0
                track[track_number + "_track_oncoming"] = data[track_number + "_track_oncoming"]
                track[track_number + "_track_lat_rate"] = self.hex_to_int(data[track_number + "_track_lat_rate"], 6) / 4.0
                track[track_number + "_track_moving"] = data[track_number + "_track_moving"]
                track[track_number + "_track_power"] = -10 + data[track_number + "_track_power"]

                # Attempting to get absolute values for speeds
                # Note: Currently only basing this off range_rate and vehicle_speed
                absolute_speed = vehicle_speed # m/s
                angle = self.hex_to_int(track[track_number + "_track_angle"], 10) / 10.0
                lat_rate = self.hex_to_int(track[track_number + "_track_lat_rate"], 6) / 4.0
                range_rate = self.hex_to_int(track[track_number + "_track_range_rate"], 14) / 100.0

                radians = math.radians(angle)
                abs_speed_sin = lat_rate / math.sin(radians) if math.sin(radians) != 0 else lat_rate
                abs_speed_cos = range_rate / math.cos(radians) if math.cos(radians) != 0 else range_rate
                abs_speed_hypot = math.hypot(lat_rate, range_rate)

                track[track_number + "_track_absolute_rate"] = ((abs_speed_sin + abs_speed_cos + abs_speed_hypot) / 3) + absolute_speed
                #print("Angle: " + str(angle))
                #print("Lateral: " + str(lat_rate))
                #print("Rate: " + str(range_rate))
                #print("Speed: " + str(track[track_number + "_track_absolute_rate"]))
                if track[track_number + "_track_power"] > 2:
                    new_data['entities'].append(track)
                track_id += 1;
            except KeyError:
                # Shouldn't happen
                self.logger.debug("KeyError, printing data structure\n")
                self.logger.debug(json.dumps(data))
        #print(new_data['entities'])
        # self.logger.debug("FINISHED RADAR NORMALIZER, HERE IS NORMALIZED DATA\n")
        # self.logger.debug(json.dumps(new_data))

        return new_data

    def data_normalize_dsrc(self, data):
        """ Takes in raw dsrc data and normalizes the format. """
        # self.logger.debug("FINISHED DSRC NORMALIZER, HERE IS NORMALIZED DATA\n")
        # self.logger.debug(json.dumps(data))
        return data

    def _update_combined(self):
        """ Updates the combined model using the latest dsrc & radar data. """

        # Perform a simple union of data currently
        data = {'dsrc': None, 'radar': None}
        if self.dsrc_data:
            data['dsrc'] = self.dsrc_data
        if self.radar_data:
            data['radar'] = self.radar_data

        #data filtering happens here
        if self.dsrc_data and self.radar_data:
            if KALMAN_FILTER:
                data = kalman_filter(data)
            if KALMAN_FILTER2:
                data = kalman_filter2(data)
            if LOCATION_FILTER:
                data = location_filter(data)
        # Send updated information back to our callback function (Collision Avoidance)
        self.combined_data_queue.put(data)

        # sends logs to the combined file
        logging.getLogger('combined').info(json.dumps(data))

        self.data_count = self.data_count + 1
        if self.data_count > 20:
            if data['dsrc'] and data['radar']:
                msg = {
                    'dsrc': {
                        'speed': data['dsrc']['message']['speed'],
                        'lat': data['dsrc']['message']['lat'],
                        'long': data['dsrc']['message']['long'],
                        'remotes': len(data['dsrc']['remote_messages'])
                    },
                    'radar': {
                        'tracks': len(data['radar']['entities'])
                    }
                }
            else:
                msg = 'Missing data member dsrc or radar'
            self.logger.info(msg)
            self.data_count = 0
Ejemplo n.º 4
0
class Combiner(object):
    """ Takes DSRC+Radar information and forms a single model of the environment.

    This package is responsible for taking data from each source, normalizing the
    format, and combining that information into a single model of the car's
    environment. Once the combination is complete the new model gets passed to
    the registered callback function, which in our case feeds data into our collision
    avoidance system.
    """
    def __init__(self,
                 collision_avoid_queue,
                 log_dsrc=True,
                 log_radar=True,
                 dsrc_log_file=None,
                 radar_log_file=None,
                 dsrc_enabled=True,
                 radar_enabled=True,
                 log_level="DEBUG",
                 log_config=None):
        """ Setup Combiner, initialize DSRC+Radar event dispatcher. """
        self.dsrc_data = None
        self.radar_data = None

        self.data_count = 0

        self.dsrc_enabled = dsrc_enabled
        self.radar_enabled = radar_enabled

        self.data_queue = Queue()
        self.combined_data_queue = collision_avoid_queue
        # self.callback = callback

        if self.dsrc_enabled:
            self.dsrc_event_dispatcher = DsrcEventDispatcher(
                self.data_queue,
                log=log_dsrc,
                log_file=dsrc_log_file,
                log_level=log_level,
                log_config=log_config)
        if self.radar_enabled:
            self.radar_event_dispatcher = RadarEventDispatcher(
                self.data_queue,
                log=log_radar,
                log_file=radar_log_file,
                log_level=log_level,
                log_config=log_config)

        self.logger = logging.getLogger('debug_combined')

    def start(self):
        """ Start running the event dispatcher threads (we are ready to recieve data). """

        if self.dsrc_enabled:
            self.dsrc_event_dispatcher.start()
        if self.radar_enabled:
            self.radar_event_dispatcher.start()

        while self.dispatchers_alive():
            if self.data_queue.qsize() > 1:
                print 'WARNING: The queue depth is %s, we are behind real time!' % self.data_queue.qsize(
                )

            try:
                dispatcher_data = self.data_queue.get(timeout=0.5)

                if dispatcher_data['from'] == 'dsrc':
                    self.dsrc_data_callback(dispatcher_data['data'])
                else:
                    self.radar_data_callback(dispatcher_data['data'])
            except Empty:
                pass
                # print 'Timeout'

        if self.dsrc_enabled:
            self.dsrc_event_dispatcher.terminate()
        if self.radar_enabled:
            self.radar_event_dispatcher.terminate()

    def dispatchers_alive(self):
        if self.dsrc_enabled and not self.dsrc_event_dispatcher.is_alive():
            return False

        if self.radar_enabled and not self.radar_event_dispatcher.is_alive():
            return False

        if not self.dsrc_enabled and not self.radar_enabled:
            return False

        return True

    def hex_to_int(self, h, d):
        # h = number to convert (can be integer)
        # d = number of bits in the number
        i = h
        if i >= 2**(d - 1):
            i -= 2**d
        return i

    def dsrc_data_callback(self, data):
        """ Callback for when new DSRC data arrives. """
        data = self.data_normalize_dsrc(data)
        self.dsrc_data = data
        self._update_combined()

    def radar_data_callback(self, data):
        """ Callback for when new radar data arrives. """
        data = self.data_normalize_radar(data)
        self.radar_data = data
        self._update_combined()

    def data_normalize_radar(self, data):
        """
        Takes in raw radar data and normalizes the format.
        Dictionary that is going to have a bunch of metadata about the
        message, and then a list of entities that the radar data 'sees'

        Create a key in the dict called entities that is a list
        """
        new_data = {}

        # First we handle the status messages
        try:
            # Message 4E0
            new_data['scan_index'] = data['scan_index']
            new_data['vehicle_speed'] = data['vehicle_speed'] / 16
            new_data['yaw_rate'] = self.hex_to_int(data['yaw_rate'], 12) / 16
            new_data['radius_curvature'] = self.hex_to_int(
                data['radius_curvature'], 14)
            # Message 4E1
            new_data['steering_angle_ack'] = data['steering_angle_ack']
            new_data['radiating'] = data['radiating']
            new_data['maximum_tracks'] = data[
                'maximum_tracks']  # Might not be necessary
            new_data['grouping_mode'] = data['grouping_mode']
            new_data['yaw_rate_bias'] = self.hex_to_int(
                data['yaw_rate_bias'], 8) / 8
            new_data['speed_comp_factor'] = (data['speed_comp_factor'] /
                                             512) + 0.9375
            # Message 4E3
            new_data['auto_align_angle'] = self.hex_to_int(
                data['auto_align_angle'], 8) / 16
            # Message 5E8
            new_data['sideslip_angle'] = self.hex_to_int(
                data['sideslip_angle'], 10) / 8
        except KeyError:
            self.logger.debug("KeyError, printing data structure\n")
            self.logger.debug(json.dumps(data))

        # Now we deal with all of the tracks
        new_data['entities'] = list()
        track_id = 1
        vehicle_speed = new_data[
            'vehicle_speed'] if 'vehicle_speed' in new_data else 0  # m/s
        for i in range(1, 65):
            track_number = str(track_id)
            try:
                track_status = data[track_number + "_track_status"]
            except KeyError:
                track_id += 1
                continue
            # If we get here, then this track is valid, so add to the list
            try:
                track = {}
                track["track_number"] = track_number
                track[track_number + "_track_status"] = data[track_number +
                                                             "_track_status"]
                track[track_number +
                      "_track_range"] = data[track_number +
                                             "_track_range"] / 10.0
                track[track_number + "_track_range_rate"] = self.hex_to_int(
                    data[track_number + "_track_range_rate"], 14) / 100.0
                track[track_number + "_track_range_accel"] = self.hex_to_int(
                    data[track_number + "_track_range_accel"], 10) / 20.0
                track[track_number + "_track_angle"] = self.hex_to_int(
                    data[track_number + "_track_angle"], 10) / 10.0
                track[track_number +
                      "_track_width"] = data[track_number +
                                             "_track_width"] / 2.0
                track[track_number +
                      "_track_oncoming"] = data[track_number +
                                                "_track_oncoming"]
                track[track_number + "_track_lat_rate"] = self.hex_to_int(
                    data[track_number + "_track_lat_rate"], 6) / 4.0
                track[track_number + "_track_moving"] = data[track_number +
                                                             "_track_moving"]
                track[track_number +
                      "_track_power"] = -10 + data[track_number +
                                                   "_track_power"]

                # Attempting to get absolute values for speeds
                # Note: Currently only basing this off range_rate and vehicle_speed
                absolute_speed = vehicle_speed  # m/s
                angle = self.hex_to_int(track[track_number + "_track_angle"],
                                        10) / 10.0
                lat_rate = self.hex_to_int(
                    track[track_number + "_track_lat_rate"], 6) / 4.0
                range_rate = self.hex_to_int(
                    track[track_number + "_track_range_rate"], 14) / 100.0

                radians = math.radians(angle)
                abs_speed_sin = lat_rate / math.sin(radians) if math.sin(
                    radians) != 0 else lat_rate
                abs_speed_cos = range_rate / math.cos(radians) if math.cos(
                    radians) != 0 else range_rate
                abs_speed_hypot = math.hypot(lat_rate, range_rate)

                track[track_number + "_track_absolute_rate"] = (
                    (abs_speed_sin + abs_speed_cos + abs_speed_hypot) /
                    3) + absolute_speed
                #print("Angle: " + str(angle))
                #print("Lateral: " + str(lat_rate))
                #print("Rate: " + str(range_rate))
                #print("Speed: " + str(track[track_number + "_track_absolute_rate"]))
                if track[track_number + "_track_power"] > 2:
                    new_data['entities'].append(track)
                track_id += 1
            except KeyError:
                # Shouldn't happen
                self.logger.debug("KeyError, printing data structure\n")
                self.logger.debug(json.dumps(data))
        #print(new_data['entities'])
        # self.logger.debug("FINISHED RADAR NORMALIZER, HERE IS NORMALIZED DATA\n")
        # self.logger.debug(json.dumps(new_data))

        return new_data

    def data_normalize_dsrc(self, data):
        """ Takes in raw dsrc data and normalizes the format. """
        # self.logger.debug("FINISHED DSRC NORMALIZER, HERE IS NORMALIZED DATA\n")
        # self.logger.debug(json.dumps(data))
        return data

    def _update_combined(self):
        """ Updates the combined model using the latest dsrc & radar data. """

        # Perform a simple union of data currently
        data = {'dsrc': None, 'radar': None}
        if self.dsrc_data:
            data['dsrc'] = self.dsrc_data
        if self.radar_data:
            data['radar'] = self.radar_data

        #data filtering happens here
        if self.dsrc_data and self.radar_data:
            if KALMAN_FILTER:
                data = kalman_filter(data)
            if KALMAN_FILTER2:
                data = kalman_filter2(data)
            if LOCATION_FILTER:
                data = location_filter(data)
        # Send updated information back to our callback function (Collision Avoidance)
        self.combined_data_queue.put(data)

        # sends logs to the combined file
        logging.getLogger('combined').info(json.dumps(data))

        self.data_count = self.data_count + 1
        if self.data_count > 20:
            if data['dsrc'] and data['radar']:
                msg = {
                    'dsrc': {
                        'speed': data['dsrc']['message']['speed'],
                        'lat': data['dsrc']['message']['lat'],
                        'long': data['dsrc']['message']['long'],
                        'remotes': len(data['dsrc']['remote_messages'])
                    },
                    'radar': {
                        'tracks': len(data['radar']['entities'])
                    }
                }
            else:
                msg = 'Missing data member dsrc or radar'
            self.logger.info(msg)
            self.data_count = 0