def send_bsms(self):
"""
Collect the latest filtered measurements from each confirmed track,
and if the tracked object has not yet been served, send a BSM
:return:
"""
for (track_id, track) in self._track_list.items():
if track.track_state == TrackState.UNCONFIRMED or \
track.track_state == TrackState.CONFIRMED or track.track_state == TrackState.ZOMBIE:
self.log_track(track_id, track)
if track.track_state == TrackState.CONFIRMED and not track.served:
# only need to send 1 BSM per fused tracks
if not track.state_estimator.fused_track and len(
track.fused_track_ids) > 0:
continue
else:
try:
self._bsm_writer.sendto(
track.sensor.bsm(track_id, track),
("localhost", self._bsm_port))
except socket.error as e:
# log the error
ops.show(
" [*] Couldn't send BSM for track [{}]"
" due to error: {0}\n".format(track_id, e),
self._verbose)
def delete_track(self, track_id):
"""remove it from the track list."""
ops.show(" [*] Dropping track {}\n".format(track_id), self._verbose)
self.log_track(track_id, self._track_list[track_id])
msg_id = None
for k, v in self._sensor_id_map.items():
if v == track_id:
msg_id = k
if msg_id is None: # track_id has already been removed
return
del self._sensor_id_map[msg_id]
del self._track_list[track_id]
def merge_tracks(self, dsrc_track_id, radar_track_id):
"""
:param dsrc_track_id:
:param radar_track_id:
:return:
"""
ops.show(
" [*] Fusing DSRC track {} and radar track {}\n".format(
dsrc_track_id, radar_track_id), self._verbose)
self._track_list[dsrc_track_id].state_estimator.fused_track = True
self._track_list[dsrc_track_id].fused_track_ids.append(radar_track_id)
# set the type of the radar track to be connected or automated
self._track_list[radar_track_id].type = DSRC.get_default_vehicle_type(
id=dsrc_track_id)
self._track_list[radar_track_id].fused_track_ids.append(dsrc_track_id)
def create_track(self, msg, topic):
"""
A new UNCONFIRMED track is created and msg is associated with it.
:param msg:
:param topic:
:return:
"""
fusion_method = None
sensor = None
if topic == DSRC.topic():
sensor = DSRC
fusion_method = tf.covariance_intersection
elif topic == Radar.topic():
sensor = Radar
# Add the new (key, value) pair to the sensor id map
self._sensor_id_map[msg['id']] = self._sensor_id_idx
self._sensor_id_idx += 1
# Create the new track
self._track_list[self._sensor_id_map[msg['id']]] = Track(
self._period,
msg,
sensor,
self._motion_model,
self._n_scan,
fusion_method=fusion_method,
use_bias_estimation=False,
track_filter=self._filter)
# Add the new track to the track list
self._track_list[self._sensor_id_map[msg['id']]].store(
msg, self._track_list)
ops.show(
" [*] Creating track {} for {} track with ID {}\n".format(
self._sensor_id_map[msg['id']], topic, msg['id']),
self._verbose)
def __init__(
self,
sensors,
bsm_port,
run_for,
track_logger,
n_scan,
association=data_associator.single_hypothesis_track_association,
association_threshold=35,
verbose=False,
frequency=5,
track_filter='KF',
motion_model='CV',
track_confirmation_threshold=5,
track_zombie_threshold=5,
track_drop_threshold=10,
max_n_tracks=15):
self._context = zmq.Context()
self._subscribers = {}
self._topic_filters = []
self._track_list = {}
self._sensor_id_map = {} # maps individual sensor ids to track ids
self._sensor_id_idx = 0
self._period = 1 / frequency # seconds
self._run_for = run_for
self._max_n_tracks = max_n_tracks
self._n_scan = n_scan
self._verbose = verbose
self._logger = track_logger
self._association_threshold = association_threshold
self._filter = track_filter
self._motion_model = motion_model
self.track_confirmation_threshold = track_confirmation_threshold
self.track_zombie_threshold = track_zombie_threshold
self.track_drop_threshold = track_drop_threshold
self.track_association = association
topic_filters = sensors['topic_filters']
sensor_ports = sensors['sensor_ports']
for pair in list(zip(topic_filters, sensor_ports)):
topic_filter = pair[0]
port = pair[1]
if isinstance(topic_filter, bytes):
topic_filter = topic_filter.decode('ascii')
self._topic_filters.append(topic_filter)
self._subscribers[topic_filter] = self._context.socket(zmq.SUB)
self._subscribers[topic_filter].connect(
"tcp://localhost:{}".format(port))
self._subscribers[topic_filter].setsockopt_string(
zmq.SUBSCRIBE, topic_filter)
self._bsm_port = bsm_port
# Open a socket for sending tracks
self._bsm_writer = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self._bsm_writer.setblocking(0)
ops.show(
" [*] Opened UDP socket connection to send BSMs to port {}\n".
format(self._bsm_port), self._verbose)
def measurement_association(self, topic, msg):
"""
Track-to-track and measurement-to-track association.
For DSRC messages, vehicle IDs can be used.
For radar tracks and zone detections, we use global nearest neighbors.
New tracks are created here if no existing tracks can be associated.
:param topic: The sensor topic for this message
:param msg: The new sensor measurement
:return:
"""
if topic == Radar.topic() and msg['objZone'] > -1:
# measurement-to-track association
result, match_id = self.track_association(
self.track_list,
msg,
threshold=self._association_threshold,
method="measurement-to-track",
verbose=self._verbose)
if result == 0x5:
# send BSM for new conventional vehicle.
try:
self._bsm_writer.sendto(
Radar.zone_bsm(msg, self._sensor_id_idx),
("localhost", self._bsm_port))
self._sensor_id_idx += 1
except socket.error as e:
# log the error
ops.show(
" [!] Couldn't send BSM for detected conventional vehicle "
"[{}] due to error: {}\n".format(match_id, e.message),
self._verbose)
elif result == 0x6:
# match_id is that of the corresponding DSRC track. Add code
# for updating GUI
pass
else:
if msg['id'] in self._sensor_id_map:
track_id = self._sensor_id_map[msg['id']]
track = self._track_list.get(track_id)
# Occasionally, the synchronizer may allow 2 messages
# to arrive at 1 cycle. Only accept one by enforcing
# the invariant "each track receives <= 1 message per cycle"
if not track.received_measurement:
track.store(msg, self._track_list)
if track.track_state == TrackState.UNCONFIRMED and \
track.n_consecutive_measurements >= \
self.track_confirmation_threshold:
track.track_state = TrackState.CONFIRMED
# attempt to fuse tracks
result, match_id = self.track_association(
self.track_list, (track_id, track),
threshold=self._association_threshold,
verbose=self._verbose)
if result == 0x1:
track.type = VehicleType.CONVENTIONAL
elif result == 0x2:
self.merge_tracks(dsrc_track_id=match_id,
radar_track_id=track_id)
elif result == 0x3:
pass # nothing to do for this case
elif result == 0x4:
self.merge_tracks(dsrc_track_id=track_id,
radar_track_id=match_id)
elif track.track_state == TrackState.ZOMBIE:
track.track_state = TrackState.UNCONFIRMED
else:
# create new unconfirmed track
self.create_track(msg, topic)
def run(self):
"""
Loop for a set amount of time. Each loop lasts for self._period seconds.
A loop consists of attempts at reading self._max_n_tracks messages from each sensor.
New sensor measurements are first associated to existing or new tracks, and then
all tracks are updated by the new measurements. Tracks that didn't receive new measurements
are also updated accordingly.
"""
t_end = time.time() + self._run_for
while time.time() < t_end:
loop_start = time.time()
message_queue = []
# since there could be many vehicles being tracked via DSRC/Radar/etc,
# we need to attempt to read messages 2*max_n_tracks times
# to get them all. This is because there could be N AVs, which
# are tracked both by radar and DSRC, so we want to be
# able to read all of them
for i in range(2 * self._max_n_tracks):
for (topic, subscriber) in self._subscribers.items():
try:
string = subscriber.recv_string(flags=zmq.NOBLOCK)
m_topic, msg = string.split(" ", 1)
message_queue.append((m_topic, pickle.loads(str(msg))))
except zmq.Again as e:
continue
for msg in message_queue:
self.measurement_association(msg[0], msg[1])
# update track state estimates, or handle no measurement
if len(self._track_list) != 0:
for (track_id, track) in self._track_list.items():
if track.received_measurement:
track.step()
track.received_measurement = False
else:
track.n_consecutive_measurements = 0
track.n_consecutive_missed += 1
if track.track_state == TrackState.ZOMBIE and \
track.n_consecutive_missed >= self.track_drop_threshold:
track.track_state = TrackState.DEAD
# if the track is a fused track, tell the other
# track to stop fusing estimates with it
if len(track.fused_track_ids) > 0:
for fused_track_id in track.fused_track_ids:
# check if the track already got deleted
# during this iteration
if fused_track_id in self._track_list:
# remove the track being deleted from
# the other track's fusion list
self._track_list[
fused_track_id].fused_track_ids.remove(
track_id)
# if the other track is no longer fusing estimates with any other track,
# reset it's fusion state
if len(self._track_list[fused_track_id]
.fused_track_ids) == 0:
self._track_list[
fused_track_id].state_estimator.fused_track = False
self.delete_track(track_id)
else:
# generate a new time stamp anyways
prev_ts = track.state_estimator.t[-1]
prev_ts.s = str(
float(prev_ts.s) +
track.state_estimator.sensor_cfg.dt * 1000)
track.state_estimator.store(msg=None, time=prev_ts)
# do a step without receiving a new measurement
# by propagating the predicted state
track.step()
if (track.track_state == TrackState.UNCONFIRMED or
track.track_state == TrackState.CONFIRMED) and \
track.n_consecutive_missed >= self.track_zombie_threshold:
track.track_state = TrackState.ZOMBIE
# fuse estimates
for (track_id, track) in self._track_list.items():
if track.state_estimator.fused_track and track.fusion_method is not None:
# collect all tracks being fused with this track
fused_tracks = []
for (other_track_id,
other_track) in self._track_list.items():
if track_id == other_track_id:
continue
else:
if other_track_id in track.fused_track_ids:
fused_tracks.append(other_track)
if len(fused_tracks) > 0:
track.fuse_estimates(fused_tracks)
elif len(track.state_estimator.x_k) == len(
track.state_estimator.x_k_fused) - 1:
track.fuse_empty()
# sleep the track specialist so it runs at the given frequency
diff = self._period - (time.time() - loop_start)
if diff > 0:
time.sleep(diff)
# Send measurements from confirmed tracks to the optimization
self.send_bsms()
self._bsm_writer.close()
ops.show(" [*] Closed UDP port {}\n".format(self._bsm_port),
self._verbose)
def single_hypothesis_track_association(track_list, query_track_info, threshold, method="track-to-track", verbose=False):
"""
Single-hypothesis association
Using Chi-squared tables for 4 degrees of freedom (one for each
dimension of the state vector), for different p-values
p = 0.05 -> 9.49
p = 0.01 -> 13.28
p = 0.001 -> 18.47
For 2 DOF
p = 0.05 -> 5.99
p = 0.01 -> 9.21
Result codes
============
0x1 : query is a radar track and association w/ DSRC failed; change type to CONVENTIONAL.
Associated track id is the query track id.
0x2 : query is a radar track and association w/ DSRC succeeded; **Can eliminate old radar track and dsrc track.**
Create a new CONFIRMED fused track.
0x3 : query is a DSRC track and association w/ radar failed; send BSM if not yet served
Associated track id is the query track id.
0x4 : query is a DSRC track and association w/ radar succeeded; change type of the radar track to CONNECTED/AUTOMATED
and update radar track id to match that of query_track. **Can drop radar track.**
0x5: query is a radar measurement from a zone detection and association with a DSRC track failed. Send BSM for a
new conventional vehicle.
0x6: query is a radar measurement from a zone detection and association with a DSRC track succeeded. Change track
state to fused. Send BSM if not yet served.
:param track_list: List of (id, track) pairs
:param threshold: chi-squared threshold
:param query_track_info: Tuple of (id, track) for track-to-track, otherwise it is the measurement that is to be associated
:param method: options are "track-to-track" and "measurement-to-track"
:param verbose: display verbose information
:return: tuple (result code, associated track id)
"""
###########
# For Debug
###########
# t = TimeStamp(radar_msg['h'], radar_msg['m'], radar_msg['s'])
#
# gnn_dict = {'time': t.to_fname_string(), 'radar': radar_measurement}
# ops.dump(gnn_dict,
# "C:\\Users\\pemami\\Workspace\\Github\\sensible\\tests\\data\\trajectories\\radar-" + t.to_fname_string() + ".pkl")
results = []
for (track_id, track) in track_list.items():
if method == "track-to-track":
query_track_id = query_track_info[0]
query_track = query_track_info[1]
# First condition ensures the query track isn't fused with itself,
# and the second condition ensures two radar or two DSRC tracks
# aren't fused together
if track == query_track or track.sensor == query_track.sensor or track_id in query_track.fused_track_ids:
continue
# track-to-track association between tracks
md, ts1, ts2 = track.state_estimator.TTTA(query_track)
elif method == "measurement-to-track":
query_track_id = "UNKNOWN"
md, ts1, ts2 = track.state_estimator.TTMA(query_track_info)
ops.show(" [TTMA] Track {} has a mahalanobis distance of {} "
"to the query track {} with time-alignment of {} and {} respectively\n".format(track_id, md, query_track_id, ts1, ts2),
verbose)
if md <= threshold:
results.append((track_id, md))
# radar_t_stamp = TimeStamp(radar_msg['h'], radar_msg['m'], radar_msg['s'])
# radar_log_str = " [GNN] Radar msg: {},{},{},{},{}\n".format(
# radar_t_stamp.to_string(), radar_measurement[0], radar_measurement[2],
# radar_measurement[1], radar_measurement[3]
# )
# print(radar_log_str)
if method == "track-to-track":
sensor = query_track.sensor.topic()
elif method == "measurement-to-track":
sensor = Radar.topic()
# Association "failed"
if len(results) == 0:
# measurement didn't fall near any tracked vehicles, so if a radar track tentatively
# associate as a conventional vehicle
if sensor == Radar.topic():
if method == "track-to-track":
ops.show(" [TTTA] New conventional vehicle detected\n", verbose)
return 0x1, None
elif method == "measurement-to-track":
ops.show(" [TTMA] No matching DSRC track for radar detection, classifying as conventional\n", verbose)
return 0x5, None
else:
ops.show(" [TTTA] No matching radar track for DSRC track {}\n".format(query_track_id), verbose)
return 0x3, None
else:
if len(results) > 1:
ops.show(" [Warning] {} vehicles within gating region of radar detection!\n".format(len(results)), verbose)
# choose the closest
sorted_results = sorted(results, key=lambda pair: pair[1])
res_id = sorted_results[0][0]
ops.show(" [TTTA] Associating with closest track {}\n".format(res_id), verbose)
if sensor == Radar.topic():
if method == "track-to-track":
return 0x2, res_id
elif method == "measurement-to-track":
return 0x6, res_id
else:
return 0x4, res_id
else:
r = results[0]
if sensor == Radar.topic():
if method == "track-to-track":
ops.show(" [TTTA] Associating radar track {} with DSRC track {}\n".format(query_track_id, r[0]), verbose)
return 0x2, r[0]
elif method == "measurement-to-track":
ops.show(" [TTMA] Associating radar detection with track {}\n".format(r[0]), verbose)
return 0x6, r[0]
else:
ops.show(" [TTTA] Associating DSRC track {} with radar track {}\n".format(query_track_id, r[0]), verbose)
return 0x4, r[0]