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)
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)
