def process_navigation(self, c_teleop, c_inference):
# This runs at the service process frequency.
# Leave the state as is on empty teleop state.
if c_teleop is not None:
self._navigator.handle_teleop_state(c_teleop)
try:
# Peek the first command in execution order.
command = self._navigation_queue[0]
if command.get_sleep() is None or timestamp(
) > command.get_time() + command.get_sleep() * 1e6:
# Execute the command now.
command = self._navigation_queue.popleft()
if command.get_direction() is not None:
self._set_direction(
_translate_navigation_direction(
command.get_direction()))
if command.get_speed() is not None:
self.set_cruise_speed(command.get_speed())
except LookupError:
pass
# Fill the queue with the next instructions in order.
c_inference = {} if c_inference is None else c_inference
navigation_instructions = self._navigator.update(c_inference)
if navigation_instructions is not None:
self._navigation_queue.extend([
c.set_time(timestamp())
for c in navigation_instructions.get_commands()
])
def _detect(self):
# self._up += 1
# if self._up >= 2:
h_val = 1e6 / (timestamp() - self._detect_time)
self._rps = self._moment * h_val + (1. - self._moment) * self._rps
self._detect_time = timestamp()
self._up = 0
def test_command_history_reset():
relay = MyRelay()
publisher = CollectPublisher(topic='test/status')
platform = MyPlatform()
# With hz=1 the command history threshold is 180.
application = MainApplication(relay=relay, hz=1)
application.platform = platform
application.publisher = publisher
application.setup()
# Send the first commands to do valid communication.
command = dict(steering=0.1, throttle=0.2, reverse=0)
list(
map(
lambda _: (platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=command)
), application.step()), list(range(10))))
platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=dict(wakeup=1)))
application.step()
assert not relay.is_open()
publisher.clear()
# Send zero commands until just below the threshold.
zero = dict(steering=0, throttle=0, reverse=0)
list(
map(
lambda _: (platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=zero)),
application.step()), list(range(180))))
assert not relay.is_open()
publisher.clear()
# And then a good command and some zero commands again.
# The relay should remain open.
platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=command))
application.step()
list(
map(
lambda _: (platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=zero)),
application.step()), list(range(10))))
assert not relay.is_open()
publisher.clear()
application.finish()
def state():
x, y = 0, 0
return dict(x_coordinate=x,
y_coordinate=y,
heading=0,
velocity=0,
time=timestamp())
def start(self):
# The receiver thread is not restartable.
self._receiver = ReceiverThread(url=('{}:5555'.format(self._ras_uri)),
topic=b'ras/drive/status')
self._receiver.add_listener(self._on_receive)
self._receiver.start()
self._values.append((0, timestamp()))
def _on_receive(self, msg):
if 'velocity' in msg.keys():
value = float(msg.get('velocity'))
else:
value = float(
msg.get('motor_effort')) * self._motor_effort_speed_factor
self._values.append((value, timestamp()))
def step(self):
n_violations = self._integrity.check()
if n_violations > 5:
self._chassis.relay_violated()
self._integrity.reset()
return
c_config, c_drive = self._pop_config(), self._pop_drive()
self._chassis.set_configuration(c_config)
v_steering = 0 if c_drive is None else c_drive.get('steering', 0)
v_throttle = 0 if c_drive is None else c_drive.get('throttle', 0)
v_wakeup = False if c_drive is None else bool(c_drive.get('wakeup'))
self._cmd_history.touch(steering=v_steering, throttle=v_throttle, wakeup=v_wakeup)
if self._cmd_history.is_missing():
self._chassis.relay_violated()
elif n_violations < -5:
self._chassis.relay_ok()
# Immediately zero out throttle when violations start occurring.
v_throttle = 0 if n_violations > 0 else v_throttle
_effort = self._chassis.drive(v_steering, v_throttle)
_data = dict(time=timestamp(), configured=int(self._chassis.is_configured()), motor_effort=_effort)
if self._odometer.is_enabled():
_data.update(dict(velocity=self._odometer.velocity()))
# Let the communication partner know we are operational.
self.publisher.publish(data=_data)
def __init__(self,
runner=None,
config_dir=os.getcwd(),
internal_models=os.getcwd(),
user_models=None,
navigation_routes=None):
super(InferenceApplication, self).__init__()
self._config_dir = config_dir
self._internal_models = internal_models
self._user_models = user_models
if user_models is not None and not os.path.exists(user_models):
_mask = os.umask(000)
os.makedirs(user_models, mode=0o775)
os.umask(_mask)
if runner is None:
runner = TFRunner(navigator=Navigator(user_models, internal_models,
navigation_routes))
self._runner = runner
self.publisher = None
self.camera = None
self.ipc_server = None
self.teleop = None
self.pilot = None
self.ipc_chatter = None
self._last_known_active_time = timestamp()
def test_relay_wakeup_reset():
relay = MyRelay()
publisher = CollectPublisher(topic='test/status')
platform = MyPlatform()
# With hz=1 the command history threshold is 180.
application = MainApplication(relay=relay, hz=1)
application.platform = platform
application.publisher = publisher
application.setup()
# Send the first commands to do valid communication.
command = dict(steering=0.1, throttle=0.2, reverse=0)
list(
map(
lambda _: (platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=command)
), application.step()), list(range(10))))
platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=dict(wakeup=1)))
application.step()
assert not relay.is_open()
publisher.clear()
# Send the zero commands and wakeup.
zero = dict(steering=0, throttle=0, reverse=0)
list(
map(
lambda _: (platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=zero)),
application.step()), list(range(180))))
platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=dict(wakeup=1)))
application.step()
assert not relay.is_open()
publisher.clear()
# After wakeup the counters need to have been reset in order not to revert immediately.
list(
map(
lambda _: (platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=zero)),
application.step()), list(range(10))))
assert not relay.is_open()
publisher.clear()
application.finish()
def publish(self, _img, topic=None):
_topic = self._topic if topic is None else topic
self._publisher.send_multipart([
_topic,
json.dumps(dict(time=timestamp(), shape=_img.shape)),
np.ascontiguousarray(_img, dtype=np.uint8)
],
flags=zmq.NOBLOCK)
def _detect(self):
with self._lock:
_now = timestamp()
self._detect_duration = _now - self._detect_time
_rps = 1e6 / self._detect_duration
self._rps = (self._moment * _rps + (1. - self._moment) * self._rps) if self._rps > 0 else _rps
self._detect_time = _now
if self._debug:
self._num_detections += 1
def get(self):
try:
value, ts = self._values[-1]
_duration = timestamp() - ts
# Half a second is an eternity.
if _duration > 5e5:
raise AssertionError(_duration)
return value
except IndexError:
raise AssertionError("Not yet started.")
def _unpack_commands(self, teleop, external, ros, vehicle, inference):
_patience, _ts = self._patience_micro, timestamp()
# The teleop, vehicle or inference commands could be none, old or repeated.
teleop = teleop if teleop is not None and (
_ts - teleop.get('time') < _patience) else None
vehicle = vehicle if vehicle is not None and (
_ts - vehicle.get('time') < _patience) else None
inference = inference if inference is not None and (
_ts - inference.get('time') < _patience) else None
# The external and ros commands need to be handled each occurrence.
return teleop, external, ros, vehicle, inference
def on_message(self, message_timestamp_micro):
# This is our time in microseconds.
local_time = timestamp()
if local_time - self._last_protocol_time > self._max_delay_micro:
self._violation()
elif message_timestamp_micro - self._last_message_time > self._max_age_micro:
self._violation()
else:
self._success()
self._last_message_time = message_timestamp_micro
self._last_protocol_time = local_time
def main():
parser = argparse.ArgumentParser(description='Exr main.')
parser.add_argument('--config',
type=str,
default='/config',
help='Config directory path.')
args = parser.parse_args()
parser = SafeConfigParser()
[parser.read(_f) for _f in glob.glob(os.path.join(args.config, '*.ini'))]
cfg = dict(parser.items('vehicle'))
cfg.update(dict(parser.items('platform')))
_process_frequency = int(cfg.get('clock.hz'))
_patience_micro = float(cfg.get('patience.ms', 200)) * 1000
logger.info("Processing at {} Hz and a patience of {} ms.".format(
_process_frequency, _patience_micro / 1000))
state_publisher = JSONPublisher(url='ipc:///byodr/vehicle.sock',
topic='aav/vehicle/state')
image_publisher = ImagePublisher(url='ipc:///byodr/camera.sock',
topic='aav/camera/0')
_interface = cfg.get('can.interface')
_steering_scale = float(cfg.get('drive.motor.steering.scale'))
vehicle = TwistHandler(
bus_name=_interface,
steering_scale=_steering_scale,
fn_callback=(lambda im: image_publisher.publish(im)))
threads = []
pilot = ReceiverThread(url='ipc:///byodr/pilot.sock',
topic=b'aav/pilot/output',
event=quit_event)
threads.append(pilot)
[t.start() for t in threads]
_period = 1. / _process_frequency
while not quit_event.is_set():
command = pilot.get_latest()
_command_time = 0 if command is None else command.get('time')
_command_age = timestamp() - _command_time
_on_time = _command_age < _patience_micro
if _on_time:
vehicle.drive(command)
else:
vehicle.noop()
state_publisher.publish(vehicle.state())
time.sleep(_period)
logger.info("Waiting on vehicle to quit.")
vehicle.quit()
logger.info("Waiting on threads to stop.")
[t.join() for t in threads]
def state(self):
y_vel, trust_velocity = 0, 0
try:
if self._odometer is not None:
y_vel, trust_velocity = self._odometer.get(), 1
except AssertionError:
pass
latitude, longitude, bearing = self._track()
return dict(latitude_geo=latitude,
longitude_geo=longitude,
heading=bearing,
velocity=y_vel,
trust_velocity=trust_velocity,
time=timestamp())
def load_routes(self):
self._check_exists()
if not self._exists:
self._reset()
else:
_now = timestamp() # In micro seconds.
if _now - self._load_timestamp > 1e6:
# Each route is a sub-directory of the base folder.
self.routes = [
d for d in os.listdir(self.directory)
if not d.startswith('.')
]
self._load_timestamp = _now
logger.info(
"Directory '{}' contains the following routes {}.".format(
self.directory, self.routes))
def state(self):
ap_active, ap_steering, ap_throttle = False, 0, 0
if self._actor is not None and self._actor.is_alive and self._in_carla_autopilot:
ap_active = True
ap_steering = self._actor.get_control().steer
ap_throttle = self._actor.get_control().throttle
latitude, longitude, bearing = self._geo_tracker.get()
return dict(latitude_geo=latitude,
longitude_geo=longitude,
heading=bearing,
velocity=self._velocity(),
trust_velocity=1,
auto_active=ap_active,
auto_steering=ap_steering,
auto_throttle=ap_throttle,
time=timestamp())
def on_message(self, json_message):
_response = json.dumps(dict(control='viewer'))
if self._is_operator():
_response = json.dumps(dict(control='operator'))
_micro_time = timestamp()
# Throttle very fast operator connections to protect our processing resources.
_last_msg_micro = self.operator_access_control[
-1] if self.operator_access_control else 0
if _micro_time - _last_msg_micro > self._operator_throttle_micro:
msg = json.loads(json_message)
msg['time'] = _micro_time
self.operator_access_control.append(_micro_time)
self._fn_control(msg)
try:
self.write_message(_response)
except websocket.WebSocketClosedError:
pass
def _send_drive(self,
throttle=0.,
steering=0.,
reverse_gear=False,
wakeup=False):
if self._pi_client is not None:
throttle = max(-1., min(1., throttle))
steering = max(-1., min(1., steering))
_reverse = 1 if reverse_gear else 0
_wakeup = 1 if wakeup else 0
self._pi_client.call(
dict(time=timestamp(),
method='ras/servo/drive',
data=dict(steering=steering,
throttle=throttle,
reverse=_reverse,
wakeup=_wakeup)))
def forward(self, image, route=None):
_out = self._navigator.forward(image, route)
action, critic, surprise, brake, brake_critic, nav_point_id, nav_image_id, nav_distance, command, path = _out
_command_index = int(np.argmax(command))
_steer_penalty = max(
0,
self._fn_steer_mu(surprise=max(0, surprise),
loss=abs(surprise - critic)))
_obstacle_penalty = max(
0,
self._fn_brake_mu(surprise=max(0, brake),
loss=max(0, brake_critic)))
_total_penalty = max(
0,
min(
1,
self._penalty_filter.calculate(_steer_penalty +
_obstacle_penalty)))
return dict(
time=timestamp(),
action=float(self._dnn_steering(action)),
obstacle=float(brake),
surprise_out=float(surprise),
critic_out=float(critic),
brake_critic_out=float(brake_critic),
steer_penalty=float(_steer_penalty),
brake_penalty=float(_obstacle_penalty),
total_penalty=float(_total_penalty),
dagger=int(0),
internal=[float(0)],
navigation_point=int(-1 if nav_point_id is None else nav_point_id),
navigation_image=int(-1 if nav_image_id is None else nav_image_id),
navigation_distance=float(
1 if nav_distance is None else nav_distance),
navigation_command=int(_command_index),
navigation_path=[float(v) for v in path])
def next_action(self, teleop, vehicle, inference):
with self._lock:
# The blob time is now.
# If downstream processes need the teleop time then use an extra attribute.
_nav_active = self._navigator.is_active()
_nav_route = self._navigator.get_navigation_route(
) if _nav_active else None
_nav_match_image = self._navigator.get_match_image_id(
) if _nav_active else None
_nav_match_distance = self._navigator.get_match_distance(
) if _nav_active else None
_nav_match_point = self._navigator.get_match_point(
) if _nav_active else None
_inference_brake = 0. if inference is None else inference.get(
'obstacle', 0.)
# Report the driver activation time in milliseconds.
_driver_activation_time = self._driver.get_activation_timestamp()
if _driver_activation_time is not None:
_driver_activation_time = (timestamp() -
_driver_activation_time) * 1e-3
blob = Blob(driver=self._driver_ctl,
driver_activation_time=_driver_activation_time,
cruise_speed=self._pilot_state.cruise_speed,
instruction=self._pilot_state.instruction,
navigation_active=_nav_active,
navigation_route=_nav_route,
navigation_match_image=_nav_match_image,
navigation_match_distance=_nav_match_distance,
navigation_match_point=_nav_match_point,
inference_brake=_inference_brake,
**teleop)
# Scale teleop before interpretation by the driver.
blob.steering = self._principal_steer_scale * blob.steering
self._driver.next_action(blob, vehicle, inference)
blob.steering = self._steering_stabilizer.update(blob.steering)
return blob
def __init__(self, **kwargs):
super(Blob, self).__init__(**kwargs)
self.time = timestamp()
self.cruise_speed = kwargs.get('cruise_speed')
self.desired_speed = kwargs.get('desired_speed')
self.driver = kwargs.get('driver')
self.driver_activation_time = kwargs.get('driver_activation_time')
self.forced_acceleration = kwargs.get('forced_acceleration')
self.forced_deceleration = kwargs.get('forced_deceleration')
self.forced_steering = kwargs.get('forced_steering')
self.forced_throttle = kwargs.get('forced_throttle')
self.instruction = kwargs.get('instruction')
self.save_event = kwargs.get('save_event')
self.speed_driver = kwargs.get('speed_driver')
self.steering = kwargs.get('steering', 0)
self.steering_driver = kwargs.get('steering_driver')
self.throttle = kwargs.get('throttle', 0)
self.arrow_up = kwargs.get('arrow_up')
self.arrow_down = kwargs.get('arrow_down')
self.button_left = kwargs.get('button_left')
self.button_right = kwargs.get('button_right')
self.navigation_active = kwargs.get('navigation_active')
self.navigation_route = kwargs.get('navigation_route')
self.navigation_match_image = kwargs.get('navigation_match_image', -1)
self.navigation_match_distance = kwargs.get(
'navigation_match_distance', 1)
self.navigation_match_point = kwargs.get('navigation_match_point')
self.inference_brake = kwargs.get('inference_brake', 0)
if self.forced_steering is None:
self.forced_steering = _is_forced_value(self.steering)
if self.forced_throttle is None:
self.forced_throttle = _is_forced_value(self.throttle)
if self.forced_acceleration is None:
self.forced_acceleration = self.forced_throttle and self.throttle > 0
if self.forced_deceleration is None:
self.forced_deceleration = self.forced_throttle and self.throttle < 0
def _reset_activation_timestamp(self):
self._activation_timestamp = timestamp()
def check(self):
if timestamp() - self._last_protocol_time > self._max_delay_micro:
self._violation()
return self._n_violations
def _send_config(self, data):
if self._pi_client is not None and data is not None:
self._pi_client.call(
dict(time=timestamp(), method='ras/driver/config', data=data))
def recompile(self, seconds=300):
_sleeping = (timestamp() -
self._last_known_active_time) > seconds * 1e6
if _sleeping:
self._runner.recompile()
def _touch(self, c_pilot):
# Keep track of the activity to have the network update in case it has a new model and the robot is not in use.
if c_pilot is not None and (abs(c_pilot.get('steering', 0)) > 1e-3
or abs(c_pilot.get('throttle', 0)) > 1e-3):
self._last_known_active_time = timestamp()
def _latest_or_none(candidate, patience):
_time = 0 if candidate is None else candidate.get('time')
_on_time = (timestamp() - _time) < patience
return candidate if _on_time else None
def test_relay_cycle():
relay = MyRelay()
publisher = CollectPublisher(topic='test/status')
platform = MyPlatform()
application = MainApplication(relay=relay)
application.platform = platform
application.publisher = publisher
application.setup()
# Without reliable communication the relay is open.
application.step()
assert relay.is_open()
publisher.clear()
# A non-zero command.
command = dict(steering=0.1, throttle=0.2, reverse=0)
# Send the first commands to do valid communication.
# The integrity protocol does not assume valid by default.
list(
map(
lambda _: (platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=command)
), application.step()), list(range(10))))
assert relay.is_open()
publisher.clear()
# Send wakeup to close the relay after startup.
platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=dict(wakeup=1)))
application.step()
assert not relay.is_open()
publisher.clear()
# Simulate communication violations.
list(
map(
lambda i: (platform.send(
dict(time=timestamp() + i * 1e6,
method='ras/servo/drive',
data=command)), application.step()), list(range(10))))
assert relay.is_open()
publisher.clear()
# And resume.
list(
map(
lambda _: (platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=command)
), application.step()), list(range(10))))
assert not relay.is_open()
publisher.clear()
# Pretend missing commands but valid communication.
_null_command = dict(steering=0, throttle=0, reverse=0)
list(
map(
lambda _: (platform.send(
dict(time=timestamp(),
method='ras/servo/drive',
data=_null_command)), application.step()),
list(range(5000))))
assert relay.is_open()
publisher.clear()
# The communication requirements must still be met to let the other side know we are operational.
platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=_null_command))
application.step()
assert len(publisher.collect()) > 0
publisher.clear()
# Wakeup again.
platform.send(
dict(time=timestamp(), method='ras/servo/drive', data=dict(wakeup=1)))
application.step()
assert not relay.is_open()
application.finish()
assert relay.is_open()
