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