def main():
    """ Main application entry point. """
    args = parse_args()

    configure_logs(getattr(logging, args.log_level.upper(), None))
    print_header()

    if args.test_logger:
        test_logger()

    collision_avoid_queue = Queue()

    # Init the collision avoidance class
    if args.visualize_dir:
        collision_avoid = CollisionAvoidance(collision_avoid_queue, args.video_file, 
            int(args.export_interval), args.export_path)

        collision_avoid.start()

    # Setup the Combiner to call collision_avoid.new_data_handler every time new data is available!
    combiner = Combiner(collision_avoid_queue,
        args.log_dsrc, args.log_radar,
        args.dsrc_log_file, args.radar_log_file,
        args.dsrc_enabled, args.radar_enabled,
        args.log_level.upper(), args.log_config)

    # This is a blocking call, will keep on going while parsers are going for dsrc and radar
    combiner.start()
def main():
    """ Main application entry point. """
    args = parse_args()

    configure_logs(getattr(logging, args.log_level.upper(), None))
    print_header()

    if args.test_logger:
        test_logger()

    collision_avoid_queue = Queue()

    # Init the collision avoidance class
    if args.visualize_dir:
        collision_avoid = CollisionAvoidance(collision_avoid_queue,
                                             args.video_file,
                                             int(args.export_interval),
                                             args.export_path)

        collision_avoid.start()

    # Setup the Combiner to call collision_avoid.new_data_handler every time new data is available!
    combiner = Combiner(collision_avoid_queue, args.log_dsrc, args.log_radar,
                        args.dsrc_log_file, args.radar_log_file,
                        args.dsrc_enabled, args.radar_enabled,
                        args.log_level.upper(), args.log_config)

    # This is a blocking call, will keep on going while parsers are going for dsrc and radar
    combiner.start()
    def run(self):
        """ Start reading data from the CAN Bus and sending full objects to the dispatcher. """
        configure_logs(getattr(logging, self.log_level, None))
        self.logger = logging.getLogger('debug_radar')
        # These are logging properly on Bryce's machinekk:w
        # self.logger.info( "welcome to the debug_radar logger!")
        # logging.getLogger('radar').info("this is a dummy message")

        msgToFunc = {
            1248: self.status_one,
            1249: self.status_two,
            1250: self.status_three,
            1251: self.status_four,
            1280: self.track_msg,
            1281: self.track_msg,
            1282: self.track_msg,
            1283: self.track_msg,
            1284: self.track_msg,
            1285: self.track_msg,
            1286: self.track_msg,
            1287: self.track_msg,
            1288: self.track_msg,
            1289: self.track_msg,
            1290: self.track_msg,
            1291: self.track_msg,
            1292: self.track_msg,
            1293: self.track_msg,
            1294: self.track_msg,
            1295: self.track_msg,
            1296: self.track_msg,
            1297: self.track_msg,
            1298: self.track_msg,
            1299: self.track_msg,
            1300: self.track_msg,
            1301: self.track_msg,
            1302: self.track_msg,
            1303: self.track_msg,
            1304: self.track_msg,
            1305: self.track_msg,
            1306: self.track_msg,
            1307: self.track_msg,
            1308: self.track_msg,
            1309: self.track_msg,
            1310: self.track_msg,
            1311: self.track_msg,
            1312: self.track_msg,
            1313: self.track_msg,
            1314: self.track_msg,
            1315: self.track_msg,
            1316: self.track_msg,
            1317: self.track_msg,
            1318: self.track_msg,
            1319: self.track_msg,
            1320: self.track_msg,
            1321: self.track_msg,
            1322: self.track_msg,
            1323: self.track_msg,
            1324: self.track_msg,
            1325: self.track_msg,
            1326: self.track_msg,
            1327: self.track_msg,
            1328: self.track_msg,
            1329: self.track_msg,
            1330: self.track_msg,
            1331: self.track_msg,
            1332: self.track_msg,
            1333: self.track_msg,
            1334: self.track_msg,
            1335: self.track_msg,
            1336: self.track_msg,
            1337: self.track_msg,
            1338: self.track_msg,
            1339: self.track_msg,
            1340: self.track_msg,
            1341: self.track_msg,
            1342: self.track_msg,
            1343: self.track_msg,
            1344: self.track_status_msg,
            1488: self.validation_msg_one,
            1489: self.validation_msg_two,
            1508: self.additional_status_one,
            1509: self.additional_status_two,
            1510: self.additional_status_three,
            1511: self.additional_status_four,
            1512: self.additional_status_five,
        }
        cl = canlib.canlib()
        channels = cl.getNumberOfChannels()
        ch = 0; # Hard-coded, might need to change!
        if ch >= channels:
            print("Invalid channel number")
            sys.exit()

        try:
            ch1 = cl.openChannel(ch, canlib.canOPEN_ACCEPT_VIRTUAL)
            print("Using channel: %s, EAN: %s" % (ch1.getChannelData_Name(),
            ch1.getChannelData_EAN()))

            ch1.setBusOutputControl(canlib.canDRIVER_NORMAL)
            ch1.setBusParams(canlib.canBITRATE_500K)
            ch1.busOn()
        except (canlib.canError) as ex:
            print(ex)

        # Initialize the Radar
        message = [0,0,0,0,0,0,191,0]
        ch1.write(1265,message,8)

        msg_counter = 0 # Variable that keeps track of the iteration of msg 1344 we are on
        while True:
            try:
                msgId, msg, dlc, flg, time = ch1.read()
                # self.logger.debug("In radar_data_parser and this is a message")
                # self.logger.debug("msgId: %9d  time: %9d  flg: 0x%02x  dlc: %d " % (msgId, time, flg, dlc))
                print_var = ''.join('{:02x}'.format(x) for x in msg)
                # self.logger.debug((msg,print_var))

                if msgId in msgToFunc:
                    # This message is valid, so we need to parse it
                    if msgId >= 1280 and msgId <= 1343:
                        msgToFunc[msgId](msgId, msg)
                    else:
                        # self.logger.debug("In radar_data_parser and this is msgId %d", msgId)
                        if (msgId == 1344):
                            msgToFunc[msgId](msg_counter, msg)
                            msg_counter += 1
                        elif (msgId > 1344 and msg_counter > 0):
                            msgToFunc[msgId](msg)
                            msg_counter = 0
                        else:
                            msgToFunc[msgId](msg)
                            if (msgId == 1512):
                                # self.callback(copy.deepcopy(self.data))
                                self.callback(self.data)
                                if self.log:
                                    # sends JSON data to radar log file
                                    logging.getLogger('radar').info(json.dumps(self.data))

                                self.data = {} # Start with a fresh object
            except (canlib.canNoMsg) as ex:
                pass
            except (canlib.canError) as ex:
                print(ex)
Пример #4
0
    def run(self):
        """ Start reading data from the DSRC API. """
        # Setup the UDP socket that we are listening on
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.sock.bind(("0.0.0.0", 5005))

        configure_logs(getattr(logging, self.log_level, None))
        self.logger = logging.getLogger('debug_dsrc')
        # These are working on Bryce's Machine
        # self.logger.info( "welcome to the debug_dsrc logger!")
        logging.getLogger('debug_dsrc').info("This is a test message for dsrc!")

        remote_messages = []

        while True:

            data, addr = self.sock.recvfrom(1024)

            # print "recieved message:", data
            first_line, xml = data.split('\n', 1)

            root = ElementTree.fromstring(xml)
            message_bytes = root[1].text.split() # Split the bytes on whitespace

            raw_message = {
                "message_id": self.hex_to_uint(message_bytes[0]), # 1 byte message id
                "tmp_id": self.hex_to_uint(''.join(message_bytes[1:5])), # 4 byte tmp id
                "current_second": self.hex_to_uint(''.join(message_bytes[5:7])), # 2 byte current seconds

                "lat": self.hex_to_int(''.join(message_bytes[7:11]), 32), # 4 byte lat
                "long": self.hex_to_int(''.join(message_bytes[11:15]), 32), # 4 byte long
                "elevation": self.hex_to_int(''.join(message_bytes[15:17]), 16), # 2 byte elevation
                "accuracy": self.hex_to_uint(''.join(message_bytes[17:21])), # 4 byte accuracy

                "speed": self.hex_to_uint(''.join(message_bytes[21:23])), # 2 byte speed
                "heading": self.hex_to_uint(''.join(message_bytes[23:25])), # 2 byte heading
                "wheel_angle": self.hex_to_int(''.join(message_bytes[25:26]), 8), # 1 byte wheel angle

                "accel_long": self.hex_to_int(''.join(message_bytes[26:28]), 16), # 2 byte accel long
                "accel_lat_set": self.hex_to_int(''.join(message_bytes[28:30]), 16), # 2 byte lateral accel
                "accel_vert": self.hex_to_int(''.join(message_bytes[30:31]), 8), # 1 byte accel vert
                "accel_yaw": self.hex_to_int(''.join(message_bytes[31:33]), 16), # 2 byte accel yaw

                "brakes": self.hex_to_uint(''.join(message_bytes[33:35])), # 2 byte brake status

                "vehicle_size": self.hex_to_uint(''.join(message_bytes[35:38])) # 3 byte vehicle size
            }

            message = {
                "message_id": raw_message["message_id"], # to number
                "tmp_id": raw_message["tmp_id"], # unknown
                "current_second": raw_message["current_second"], # to s

                "lat": float(raw_message["lat"]) / 1e7, # to minutes.seconds
                "long": float(raw_message["long"]) / 1e7, # to minutes.seconds
                "elevation": float(raw_message["elevation"]) / 1e1, # to m
                "accuracy": raw_message["accuracy"], # unknown

                "speed": raw_message["speed"] & 0x1FFF, # to km/h
                # "heading": float(raw_message["heading"]) / 8e1, # unknown
                # "wheel_angle": float(raw_message["wheel_angle"]) * 0.3,

                # "accel_long": float(raw_message["message_id"]) / 14e3, # to m/s^2
                # "accel_lat_set": float(raw_message["accel_lat_set"]) / 3e3, # to m/s^2
                # "accel_vert": float(raw_message["accel_vert"]) / 1e3, # to G's
                # "accel_yaw": float(raw_message["accel_yaw"]) / 175e2, # to degrees/s
                #
                # "brakes": {
                #     "aux_brakes": raw_message["brakes"] & 0x03,
                #     "brake_boost": (raw_message["brakes"] >> 2) & 0x03,
                #     "scs": (raw_message["brakes"] >> 4) & 0x03,
                #     "abs": (raw_message["brakes"] >> 6) & 0x03,
                #     "traction": (raw_message["brakes"] >> 8) & 0x03,
                #     "spare_bit": (raw_message["brakes"] >> 10) & 0x01,
                #     "wheel_brakes_unavailable": (raw_message["brakes"] >> 11) & 0x01,
                #     "wheel_brakes": (raw_message["brakes"] >> 12) & 0x0F
                # },
                #
                # "vehicle_size": {
                #     "length": raw_message["vehicle_size"] & 0x3FFF, # to cm
                #     "width": (raw_message["vehicle_size"] >> 14) & 0xFF # to cm
                # }
            }

            # If the first line contains Rx then this is a message from a remote DSRC
            # unit, if first_line contains Tx it was a message being sent to a remote DSRC
            if first_line.find('Rx') > 0:
                remote_messages.append(message)
            else:
                # compile all of the remote messages since the last local DSRC update plus
                # the most recent DSRC update
                data = {
                    "message": message,
                    "remote_messages": remote_messages # state of nearby vehicles
                }
                # Send the messages to the dispatcher
                self.callback(data)

                if self.log:
                    # sends JSON data to dsrc log file
                    logging.getLogger('dsrc').info(json.dumps(data))

                # once remote_messages have been saved with most recent local DSRC update,
                # clear the list and start anew
                remote_messages = []
    def run(self):
        """ Start reading data from the CAN Bus and sending full objects to the dispatcher. """
        configure_logs(getattr(logging, self.log_level, None))
        self.logger = logging.getLogger('debug_radar')
        # These are logging properly on Bryce's machinekk:w
        # self.logger.info( "welcome to the debug_radar logger!")
        # logging.getLogger('radar').info("this is a dummy message")

        msgToFunc = {
            1248: self.status_one,
            1249: self.status_two,
            1250: self.status_three,
            1251: self.status_four,
            1280: self.track_msg,
            1281: self.track_msg,
            1282: self.track_msg,
            1283: self.track_msg,
            1284: self.track_msg,
            1285: self.track_msg,
            1286: self.track_msg,
            1287: self.track_msg,
            1288: self.track_msg,
            1289: self.track_msg,
            1290: self.track_msg,
            1291: self.track_msg,
            1292: self.track_msg,
            1293: self.track_msg,
            1294: self.track_msg,
            1295: self.track_msg,
            1296: self.track_msg,
            1297: self.track_msg,
            1298: self.track_msg,
            1299: self.track_msg,
            1300: self.track_msg,
            1301: self.track_msg,
            1302: self.track_msg,
            1303: self.track_msg,
            1304: self.track_msg,
            1305: self.track_msg,
            1306: self.track_msg,
            1307: self.track_msg,
            1308: self.track_msg,
            1309: self.track_msg,
            1310: self.track_msg,
            1311: self.track_msg,
            1312: self.track_msg,
            1313: self.track_msg,
            1314: self.track_msg,
            1315: self.track_msg,
            1316: self.track_msg,
            1317: self.track_msg,
            1318: self.track_msg,
            1319: self.track_msg,
            1320: self.track_msg,
            1321: self.track_msg,
            1322: self.track_msg,
            1323: self.track_msg,
            1324: self.track_msg,
            1325: self.track_msg,
            1326: self.track_msg,
            1327: self.track_msg,
            1328: self.track_msg,
            1329: self.track_msg,
            1330: self.track_msg,
            1331: self.track_msg,
            1332: self.track_msg,
            1333: self.track_msg,
            1334: self.track_msg,
            1335: self.track_msg,
            1336: self.track_msg,
            1337: self.track_msg,
            1338: self.track_msg,
            1339: self.track_msg,
            1340: self.track_msg,
            1341: self.track_msg,
            1342: self.track_msg,
            1343: self.track_msg,
            1344: self.track_status_msg,
            1488: self.validation_msg_one,
            1489: self.validation_msg_two,
            1508: self.additional_status_one,
            1509: self.additional_status_two,
            1510: self.additional_status_three,
            1511: self.additional_status_four,
            1512: self.additional_status_five,
        }
        cl = canlib.canlib()
        channels = cl.getNumberOfChannels()
        ch = 0
        # Hard-coded, might need to change!
        if ch >= channels:
            print("Invalid channel number")
            sys.exit()

        try:
            ch1 = cl.openChannel(ch, canlib.canOPEN_ACCEPT_VIRTUAL)
            print("Using channel: %s, EAN: %s" %
                  (ch1.getChannelData_Name(), ch1.getChannelData_EAN()))

            ch1.setBusOutputControl(canlib.canDRIVER_NORMAL)
            ch1.setBusParams(canlib.canBITRATE_500K)
            ch1.busOn()
        except (canlib.canError) as ex:
            print(ex)

        # Initialize the Radar
        message = [0, 0, 0, 0, 0, 0, 191, 0]
        ch1.write(1265, message, 8)

        msg_counter = 0  # Variable that keeps track of the iteration of msg 1344 we are on
        while True:
            try:
                msgId, msg, dlc, flg, time = ch1.read()
                # self.logger.debug("In radar_data_parser and this is a message")
                # self.logger.debug("msgId: %9d  time: %9d  flg: 0x%02x  dlc: %d " % (msgId, time, flg, dlc))
                print_var = ''.join('{:02x}'.format(x) for x in msg)
                # self.logger.debug((msg,print_var))

                if msgId in msgToFunc:
                    # This message is valid, so we need to parse it
                    if msgId >= 1280 and msgId <= 1343:
                        msgToFunc[msgId](msgId, msg)
                    else:
                        # self.logger.debug("In radar_data_parser and this is msgId %d", msgId)
                        if (msgId == 1344):
                            msgToFunc[msgId](msg_counter, msg)
                            msg_counter += 1
                        elif (msgId > 1344 and msg_counter > 0):
                            msgToFunc[msgId](msg)
                            msg_counter = 0
                        else:
                            msgToFunc[msgId](msg)
                            if (msgId == 1512):
                                # self.callback(copy.deepcopy(self.data))
                                self.callback(self.data)
                                if self.log:
                                    # sends JSON data to radar log file
                                    logging.getLogger('radar').info(
                                        json.dumps(self.data))

                                self.data = {}  # Start with a fresh object
            except (canlib.canNoMsg) as ex:
                pass
            except (canlib.canError) as ex:
                print(ex)