def test_synchronization():
"""Test the Pub/Sub component of the DSRC thread"""
remote_port = 4200
local_port = 6666
sensor = DSRC()
dsrc = SocketThread(sensor=sensor,
ip_address="localhost",
port=remote_port,
msg_len=300,
name="DSRCThread")
synchronizer = Synchronizer(publish_freq=5,
queue=sensor.queue,
port=local_port,
topic=DSRC.topic(),
name="DSRCSynchronizer",
verbose=True)
# Subscriber
context = zmq.Context()
subscriber = context.socket(zmq.SUB)
subscriber.connect("tcp://localhost:{}".format(local_port))
t_filter = "DSRC"
if isinstance(t_filter, bytes):
t_filter = t_filter.decode('ascii')
subscriber.setsockopt_string(zmq.SUBSCRIBE, t_filter)
radio = SensorEmulator(port=remote_port,
pub_freq=21,
file_names=["data/test_csm.txt"])
# Connect and start the DSRC thread.
try:
synchronizer.start()
# start the DSRC thread
dsrc.start()
except Exception:
pytest.fail("Unable to connect DSRC thread.")
# Process for 2.2 seconds, 1 message will be dropped due to start-up time
t_end = time.time() + 2.2
msgs = []
radio.start()
while time.time() < t_end:
try:
string = subscriber.recv_string(flags=zmq.NOBLOCK)
topic, msg = string.split(" ")
msgs.append(pickle.loads(str(msg)))
except zmq.Again as e:
continue
# Stop the DSRC sender and receiver
dsrc.stop()
synchronizer.stop()
radio.stop()
# Expect to only receive 10 messages
assert len(msgs) == 10
def test_track_state_confirmed_to_zombie(tmpdir):
"""This tests that a CONFIRMED track
becomes a ZOMBIE after missing N consecutive messages,
where N is the zombie threshold."""
# Assume the frequency is 5 Hz
# Need to run for 1.2 seconds because after track creation,
# received_msg is set to 1 for one iteration of the loop
track_specialist = get_track_specialist(tmpdir, run_for=0.8)
msg = fake_msg()
track_specialist.create_track(msg, DSRC.topic())
track_id = track_specialist.sensor_id_map[msg['id']]
track = track_specialist.track_list[track_id]
assert track.track_state == TrackState.UNCONFIRMED
track.step()
track.fuse_empty()
track.received_measurement = False
track.n_consecutive_measurements = track_specialist.track_confirmation_threshold
track_specialist.measurement_association("DSRC", msg)
assert track.track_state == TrackState.CONFIRMED
# run for 0.8 seconds
track_specialist.run()
assert track_id in track_specialist.track_list
assert track.track_state == TrackState.ZOMBIE
assert track.n_consecutive_missed == 3
def test_push_msg0():
"""Test pushing new messages onto the queue."""
dsrc = DSRC()
synchronizer = Synchronizer(publish_freq=5,
queue=dsrc.queue,
port=4200,
topic=dsrc.topic(),
name="DSRCSynchronizer")
test0 = {
'msg_count': 0,
'id': 123,
'h': 8,
'm': 16,
's': 32000.00,
'lat': 29.12,
'lon': -87.234,
'heading': 0,
'speed': 0,
'lane': 0,
'veh_len': 15,
'max_accel': 10,
'max_decel': -17,
'served': 0
}
test1 = {
'msg_count': 0,
'id': 123,
'h': 8,
'm': 16,
's': 32000.00,
'lat': 29.12,
'lon': -87.234,
'heading': 0,
'speed': 0,
'lane': 0,
'veh_len': 15,
'max_accel': 10,
'max_decel': -17,
'served': 0
}
dsrc.add_to_queue(test0)
assert len(synchronizer.queue) == 1
dsrc.add_to_queue(test1)
# don't add message with same id and seconds
assert len(synchronizer.queue) == 1
test1['s'] = 34000.00
dsrc.add_to_queue(test1)
assert len(synchronizer.queue) == 2
def test_track_creation(tmpdir):
"""This tests whether a new track is created with
the correct state, i.e., UNCONFIRMED, by
track_specialist.create_track(msg). """
track_specialist = get_track_specialist(tmpdir)
msg = fake_msg()
track_specialist.create_track(msg, DSRC.topic())
assert msg['id'] in track_specialist.sensor_id_map
track_id = track_specialist.sensor_id_map[msg['id']]
track = track_specialist.track_list[track_id]
assert track.track_state == TrackState.UNCONFIRMED
assert track.n_consecutive_measurements == 1
assert track.n_consecutive_missed == 0
assert track.received_measurement
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 test_track_state_confirm(tmpdir):
"""This tests that an UNCONFIRMED track becomes
confirmed after M consecutive messages arrive for that
track, where M is the threshold."""
track_specialist = get_track_specialist(tmpdir)
msg = fake_msg()
# Try to associate a new msg - should call create_track
track_specialist.create_track(msg, DSRC.topic())
track_id = track_specialist.sensor_id_map[msg['id']]
track = track_specialist.track_list[track_id]
track.received_measurement = False
assert track.track_state == TrackState.UNCONFIRMED
track.n_consecutive_measurements = track_specialist.track_confirmation_threshold
# One more message than the threshold causes the state to change
# to CONFIRMED
track_specialist.measurement_association("DSRC", msg)
assert track.track_state == TrackState.CONFIRMED
def test_vehicle_id_association(tmpdir):
"""This tests whether new measurements can be matched to
existing tracks based on ID"""
track_specialist = get_track_specialist(tmpdir)
msg = fake_msg()
# 1st message
track_specialist.create_track(msg, DSRC.topic())
track_id = track_specialist.sensor_id_map[msg['id']]
track_specialist.track_list[track_id].received_measurement = False
# 2nd message
msg['s'] = 15
track_specialist.measurement_association("DSRC", msg)
track = track_specialist.track_list[track_id]
assert track.track_state == TrackState.UNCONFIRMED
assert track.n_consecutive_measurements == 2
assert track.n_consecutive_missed == 0
assert track.received_measurement
def test_track_drop(tmpdir):
"""Test that the main `run` method works as expected
1. Exits after `run_for` seconds pass.
2. Tracks that don't receive new measurements are dropped."""
track_specialist = get_track_specialist(tmpdir, run_for=3)
msg = fake_msg()
track_specialist.create_track(msg, DSRC.topic())
track_id = track_specialist.sensor_id_map[msg['id']]
track = track_specialist.track_list[track_id]
assert track.n_consecutive_measurements == 1
assert track.n_consecutive_missed == 0
# Run for 3 seconds with no messages incoming
track_specialist.run()
assert track.track_state == TrackState.DEAD
assert msg['id'] not in track_specialist.sensor_id_map
assert track_id not in track_specialist.track_list
def test_track_recovery(tmpdir):
"""This tests whether a track can recover from ZOMBIE
state to UNCONFIRMED if a new message for it arrives
before it is deleted"""
track_specialist = get_track_specialist(tmpdir, run_for=0.8)
msg = fake_msg()
track_specialist.create_track(msg, DSRC.topic())
track_id = track_specialist.sensor_id_map[msg['id']]
track = track_specialist.track_list[track_id]
assert track.track_state == TrackState.UNCONFIRMED
track_specialist.run()
assert track.track_state == TrackState.ZOMBIE
track_specialist.measurement_association("DSRC", msg)
assert track.track_state == TrackState.UNCONFIRMED
assert track.n_consecutive_measurements == 1
assert track.n_consecutive_missed == 0
def test_track_state_zombie_to_unconfirmed(tmpdir):
"""This tests that an UNCONFIRMED track
becomes a ZOMBIE after missing N consecutive messages,
where N is the zombie threshold."""
# Assume the frequency is 5 Hz
# Need to run for 1.2 seconds because after track creation,
# received_msg is set to 1 for one iteration of the loop
track_specialist = get_track_specialist(tmpdir, run_for=0.8)
msg = fake_msg()
# Try to associate a new msg - should call create_track
track_specialist.create_track(msg, DSRC.topic())
track_id = track_specialist.sensor_id_map[msg['id']]
track = track_specialist.track_list[track_id]
assert track.track_state == TrackState.UNCONFIRMED
# run for 0.8 seconds
track_specialist.run()
assert track_id in track_specialist.track_list
assert track.track_state == TrackState.ZOMBIE
assert track.n_consecutive_missed == 3
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 test_csm_parsing2():
with open("data/test_csm.txt") as f:
csm = f.read()
result = DSRC.parse(csm)
assert result['served'] == 0