def __init__(self, sessions, photos, **kwargs): super(EventHandler, self).__init__() self._hash = hash_dict(**kwargs) self._record_dir = sessions self._photo_dir = photos self._quit_event = threading.Event() _errors = [] self._process_frequency = parse_option('clock.hz', int, 100, _errors, **kwargs) self._publish_frequency = parse_option('publish.hz', int, 2, _errors, **kwargs) self._vehicle = parse_option('constant.vehicle.type', str, 'vehicle.byodr.2020', _errors, **kwargs) self._config = parse_option('constant.vehicle.config', str, 'latest', _errors, **kwargs) _im_width, _im_height = parse_option('image.persist.scale', str, '320x240', _errors, **kwargs).split('x') self._im_height = int(_im_height) self._im_width = int(_im_width) self._session_active = False self._session_log = ImageEventLog() self._photo_log = ImageEventLog() self._recorder = get_or_create_recorder( mode='record.mode.interventions', directory=self._record_dir, vehicle_type=self._vehicle, vehicle_config=self._config) self._errors = _errors
def internal_start(self, **kwargs): _errors = [] _remote = parse_option('host.location', str, '127.0.0.1:2000', _errors, **kwargs) _img_wh = parse_option('camera.image.input.shape', str, '640x480', errors=_errors, **kwargs) carla_host, carla_port = _remote, 2000 if ':' in carla_host: host, port = carla_host.split(':') carla_host, carla_port = host, int(port) carla_client = carla.Client(carla_host, carla_port) carla_client.set_timeout(5.) _shape = [int(x) for x in _img_wh.split('x')] _shape = (_shape[1], _shape[0], 3) self._image_shape = _shape self._rand_weather_seconds = parse_option( 'weather.random.each.seconds', int, 180, _errors, **kwargs) self._spawn_preferred_id = parse_option('world.spawn.preferred.id', int, -1, _errors, **kwargs) self._spawn_preferred_weather = parse_option( 'world.spawn.preferred.weather', str, 'ClearNoon', _errors, **kwargs) self._world = carla_client.get_world() self._traffic_manager = carla_client.get_trafficmanager(self._tm_port) self._traffic_manager.global_percentage_speed_difference(65) self._change_weather_time = time.time() + self._rand_weather_seconds self._vehicle_tick = self._world.on_tick(lambda x: self.tick(x)) self.reset() return _errors
def _reboot(self): if self._pi_client is not None: self._pi_client.quit() if self._status is not None: self._status.quit() errors = [] _config = self._config() _master_uri = parse_option('ras.master.uri', str, 'tcp://192.168.1.32', errors, **_config) _steering_offset = parse_option('ras.driver.steering.offset', float, 0.0, errors, **_config) _motor_scale = parse_option('ras.driver.motor.scale', float, 1.0, errors, **_config) self._patience_micro = parse_option('patience.ms', int, 100, errors, ** _config) * 1000. self._servo_config = dict(app_version=2, steering_offset=_steering_offset, motor_scale=_motor_scale) self._pi_client = self._client_factory.create(_master_uri) self._status = self._status_factory.create(_master_uri) self._status.add_listener(self._on_receive) self._status.start() self._integrity.reset() self._send_config(self._servo_config) logger.info("Processing master at uri '{}'.".format(_master_uri)) return errors
def internal_start(self, **kwargs): errors = [] self._process_frequency = parse_option('clock.hz', int, 100, errors, **kwargs) self._patience_micro = parse_option('patience.ms', int, 100, errors, ** kwargs) * 1000. self._vehicle.restart(**kwargs) errors.extend(self._vehicle.get_errors()) if not self._gst_sources: front_camera = ImagePublisher(url='ipc:///byodr/camera_0.sock', topic='aav/camera/0') rear_camera = ImagePublisher(url='ipc:///byodr/camera_1.sock', topic='aav/camera/1') self._gst_sources.append( ConfigurableImageGstSource('front', image_publisher=front_camera)) self._gst_sources.append( ConfigurableImageGstSource('rear', image_publisher=rear_camera)) if not self._ptz_cameras: self._ptz_cameras.append(PTZCamera('front')) self._ptz_cameras.append(PTZCamera('rear')) for item in self._gst_sources + self._ptz_cameras: item.restart(**kwargs) errors.extend(item.get_errors()) return errors
def internal_start(self, **kwargs): errors = [] ptz_enabled = parse_option(self._name + '.camera.ptz.enabled', (lambda x: bool(int(x))), 1, errors, **kwargs) if ptz_enabled: _server = parse_option(self._name + '.camera.ip', str, '192.168.1.64', errors=errors, **kwargs) _user = parse_option(self._name + '.camera.user', str, 'user1', errors=errors, **kwargs) _password = parse_option(self._name + '.camera.password', str, 'HaikuPlot876', errors=errors, **kwargs) _protocol = 'http' _path = '/ISAPI/PTZCtrl/channels/1' _flip = 'tilt' _speed = 1.9 _flipcode = [1, 1] if _flip in ('pan', 'tilt', 'both'): _flipcode[0] = -1 if _flip in ('pan', 'both') else 1 _flipcode[1] = -1 if _flip in ('tilt', 'both') else 1 _port = 80 if _protocol == 'http' else 443 _url = '{protocol}://{server}:{port}{path}'.format(**dict( protocol=_protocol, server=_server, port=_port, path=_path)) logger.info("PTZ camera url={}.".format(_url)) if self._worker is None: self._worker = CameraPtzThread(_url, _user, _password, speed=_speed, flip=_flipcode) self._worker.start() elif len(errors) == 0: self._worker.set_url(_url) self._worker.set_auth(_user, _password) self._worker.set_speed(_speed) self._worker.set_flip(_flipcode) # Already under lock. elif self._worker: self._worker.quit() return errors
def internal_start(self, **kwargs): _errors = [] _steer_low_momentum = parse_option( 'driver.handler.steering.low_pass.momentum', float, 0.40, _errors, **kwargs) self._principal_steer_scale = parse_option( 'driver.steering.teleop.scale', float, 0.45, _errors, **kwargs) self._cruise_speed_step = parse_option('driver.cc.static.gear.step', float, 0.50, _errors, **kwargs) self._steering_stabilizer = LowPassFilter(alpha=_steer_low_momentum) self._navigator.initialize() self._driver_cache.clear() _errors.extend(self._fill_driver_cache(**kwargs)) self._driver = None self._driver_ctl = None self.switch_ctl() return _errors
def internal_start(self, **kwargs): _errors = [] self._driver.restart(**kwargs) self._process_frequency = parse_option('clock.hz', int, 100, _errors, **kwargs) self._patience_micro = parse_option('patience.ms', int, 200, _errors, **kwargs) * 1000. self._button_north_ctl = parse_option('controller.button.north.mode', str, 'driver_mode.inference.dnn', _errors, **kwargs) self._button_west_ctl = parse_option('controller.button.west.mode', str, 'ignore', _errors, **kwargs) # Avoid processing the same command more than once. # TTL is specified in seconds. self._cache = cachetools.TTLCache(maxsize=100, ttl=(self._patience_micro * 1e-6)) return _errors + self._driver.get_errors()
def __init__(self, control, **kwargs): super(AbstractCruiseControl, self).__init__(control) _errors = [] self._min_desired_speed = 0 self._max_desired_speed = 0 # Internal speed values are in meters per second. self._min_desired_speed = parse_option('driver.cc.static.speed.min', float, (0.1 / 3.6), _errors, **kwargs) self._max_desired_speed = parse_option('driver.cc.static.speed.max', float, (5.0 / 3.6), _errors, **kwargs) # _control_type = parse_option('driver.cc.control.type', str, 'pid', _errors, **kwargs) if _control_type == 'pid': p = (parse_option('driver.cc.throttle.pid_controller.p', float, 0.002, _errors, **kwargs)) i = (parse_option('driver.cc.throttle.pid_controller.i', float, 2.0, _errors, **kwargs)) d = (parse_option('driver.cc.throttle.pid_controller.d', float, 0, _errors, **kwargs)) stop_p = (parse_option('driver.cc.stop.pid_controller.p', float, 1.0, _errors, **kwargs)) self._throttle_control = PidThrottleControl( (p, i, d), stop_p=stop_p, min_desired_speed=self._min_desired_speed, max_desired_speed=self._max_desired_speed) else: _throttle_up_momentum = parse_option( 'driver.throttle.direct.up.momentum', float, 0.05, _errors, **kwargs) _throttle_down_momentum = parse_option( 'driver.throttle.direct.down.momentum', float, 1.0, _errors, **kwargs) _throttle_cutoff = parse_option('driver.throttle.direct.minimum', float, 0.002, _errors, **kwargs) self._throttle_control = DirectThrottleControl( min_desired_speed=self._min_desired_speed, max_desired_speed=self._max_desired_speed, throttle_up_momentum=_throttle_up_momentum, throttle_down_momentum=_throttle_down_momentum, throttle_cutoff=_throttle_cutoff) self._errors = _errors
def internal_start(self, **kwargs): errors = [] _master_uri = parse_option('ras.master.uri', str, 'tcp://192.168.1.32', errors, **kwargs) self._odometer = RasSpeedOdometer(_master_uri) self._odometer.start() if not self._gps_poller.is_alive(): self._gps_poller.start() return errors
def _build_expression(key, default_value, errors, **kwargs): _expression = lambda x, y: 100 _equation = parse_option(key, str, default_value, errors, **kwargs) try: _expression = Expression(_equation) _expression(surprise=0, loss=0) except (TypeError, IndexError, ZeroDivisionError) as te: errors.append(PropertyError(key, str(te))) return _expression
def internal_start(self, **kwargs): _errors = [] self._gpu_id = parse_option('gpu.id', int, 0, _errors, **kwargs) self._process_frequency = parse_option('clock.hz', int, 20, _errors, **kwargs) self._steering_scale_left = parse_option( 'driver.dnn.steering.scale.left', lambda x: abs(float(x)), -1, _errors, **kwargs) self._steering_scale_right = parse_option( 'driver.dnn.steering.scale.right', float, 1, _errors, **kwargs) _penalty_up_momentum = parse_option( 'driver.autopilot.filter.momentum.up', float, 0.15, _errors, **kwargs) _penalty_down_momentum = parse_option( 'driver.autopilot.filter.momentum.down', float, 0.25, _errors, **kwargs) _penalty_ceiling = parse_option('driver.autopilot.filter.ceiling', float, 2.0, _errors, **kwargs) self._penalty_filter = DynamicMomentum(up=_penalty_up_momentum, down=_penalty_down_momentum, ceiling=_penalty_ceiling) self._fn_steer_mu = _build_expression( 'driver.dnn.steer.mu.equation', '(7.0 * (-0.50 + surprise + loss)) **7', _errors, **kwargs) self._fn_brake_mu = _build_expression( 'driver.dnn.brake.mu.equation', '2.0 * surprise + 2.5 * (loss > 0.60) * (loss - 0.60)', _errors, **kwargs) _fn_dave_image = get_registered_function('dnn.image.transform.dave', 'dave__320_240__200_66__0', _errors, **kwargs) _fn_alex_image = get_registered_function('dnn.image.transform.alex', 'alex__200_100', _errors, **kwargs) _nav_threshold = parse_option('navigator.point.recognition.threshold', float, 0.100, _errors, **kwargs) _rt_compile = parse_option('runtime.graph.compilation', int, 1, _errors, **kwargs) self._navigator.restart(fn_dave_image=_fn_dave_image, fn_alex_image=_fn_alex_image, recognition_threshold=_nav_threshold, gpu_id=self._gpu_id, runtime_compilation=_rt_compile) return _errors
def internal_start(self, **kwargs): # This method runs under lock. self._close() _errors = [] input_width, input_height = parse_width_height( 'camera.image.input.shape', '640x480', _errors, **kwargs) bitrate = parse_option(self._name + '.video.encoding.bitrate', int, 1024, errors=_errors, **kwargs) self._stream_width, self._stream_height = parse_width_height( self._name + '.video.output.shape', '640x480', _errors, **kwargs) if len(_errors) == 0: _args = dict(input_width=input_width, input_height=input_height, bitrate=bitrate, stream_width=self._stream_width, stream_height=self._stream_height) command = "appsrc name=source emit-signals=True is-live=True " \ "caps=video/x-raw,format=BGR,width={input_width},height={input_height} ! " \ "videoconvert ! queue ! " \ "videoscale ! video/x-raw,width={stream_width},height={stream_height} ! " \ "queue ! x264enc bframes=0 bitrate={bitrate} b-adapt=0 tune=zerolatency key-int-max=60 ! " \ "video/x-h264,profile=baseline,stream-format=\"byte-stream\" ! queue ! " \ "appsink name=sink emit-signals=true sync=false async=false max-buffers=1 drop=true".format(**_args) pipeline = Gst.parse_launch(command) loop = GObject.MainLoop() bus = pipeline.get_bus() bus.add_signal_watch() bus.connect("message", bus_message, loop) src = pipeline.get_by_name('source') # src.set_property("format", Gst.Format.TIME) # src.set_property("block", True) src.set_property('format', 'time') src.set_property('do-timestamp', True) _caps = "video/x-raw,format=BGR,width={input_width},height={input_height},framerate={fps}/1".format( **dict(input_width=input_width, input_height=input_height, fps=20)) video_sink = pipeline.get_by_name('sink') video_sink.connect('new-sample', self._publish) pipeline.set_state(Gst.State.PLAYING) self._caps = Gst.Caps.from_string(_caps) self._pipeline = pipeline self._source = src logger.info("Setup video stream '{}' with input caps '{}'.".format( self._name, _caps)) return _errors
def main(): parser = argparse.ArgumentParser(description='Steering and throttle driver.') parser.add_argument('--config', type=str, default='/config/driver.ini', help='Configuration file.') args = parser.parse_args() config_file = args.config assert os.path.exists(config_file) and os.path.isfile(config_file) parser = SafeConfigParser() parser.read(config_file) kwargs = dict(parser.items('driver')) if parser.has_section('driver') else {} kwargs.update(dict(parser.items('odometer')) if parser.has_section('odometer') else {}) drive_type = parse_option('drive.type', str, **kwargs) relay = SearchUsbRelayFactory().get_relay() assert relay.is_attached(), "The device is not attached." if drive_type in ('gpio', 'gpio_with_hall'): driver = GPIODriver(relay, **kwargs) elif drive_type == 'odrive': driver = ODriveSerialDriver(relay, **kwargs) else: raise AssertionError("Unknown drive type '{}'.".format(drive_type)) # driver = NoopDriver(relay) try: application = MainApplication(chassis=driver, hz=50, **kwargs) quit_event = application.quit_event application.publisher = JSONPublisher(url='tcp://0.0.0.0:5555', topic='ras/drive/status') application.platform = JSONServerThread(url='tcp://0.0.0.0:5550', event=quit_event, receive_timeout_ms=50) threads = [application.platform] if quit_event.is_set(): return 0 [t.start() for t in threads] application.run() logger.info("Waiting on threads to stop.") [t.join() for t in threads] finally: relay.open()
def internal_start(self, **kwargs): self._close() _errors = [] _type = parse_option(self._name + '.camera.type', str, 'h264/rtsp', errors=_errors, **kwargs) assert _type in gst_commands.keys( ), "Unrecognized camera type '{}'.".format(_type) framerate = (parse_option(self._name + '.camera.framerate', int, 25, errors=_errors, **kwargs)) out_width, out_height = [ int(x) for x in parse_option(self._name + '.camera.shape', str, '320x240', errors=_errors, **kwargs).split('x') ] if _type == 'h264/rtsp': config = { 'ip': (parse_option(self._name + '.camera.ip', str, '192.168.1.64', errors=_errors, **kwargs)), 'port': (parse_option(self._name + '.camera.port', int, 554, errors=_errors, **kwargs)), 'user': (parse_option(self._name + '.camera.user', str, 'user1', errors=_errors, **kwargs)), 'password': (parse_option(self._name + '.camera.password', str, 'HaikuPlot876', errors=_errors, **kwargs)), 'path': (parse_option(self._name + '.camera.path', str, '/Streaming/Channels/102', errors=_errors, **kwargs)), 'height': out_height, 'width': out_width, 'framerate': framerate } else: _type = 'h264/udp' config = { 'port': (parse_option(self._name + '.camera.port', int, 5000, errors=_errors, **kwargs)), 'height': out_height, 'width': out_width, 'framerate': framerate } self._shape = (out_height, out_width, 3) self._ptz = parse_option(self._name + '.camera.ptz.enabled', int, 1, errors=_errors, **kwargs) _command = gst_commands.get(_type).format(**config) self._sink = create_image_source(self._name, shape=self._shape, command=_command) self._sink.add_listener(self._publish) logger.info("Gst '{}' command={}".format(self._name, _command)) return _errors
def __init__(self, **kwargs): self._cm_per_revolution = parse_option('odometer.distance.cm_per_revolution', float, 15, **kwargs) self._debug = parse_option('odometer.debug', int, 0, **kwargs) == 1 self._alpha = parse_option('odometer.moment.alpha', float, 0.10, **kwargs) self._enabled = parse_option('drive.type', str, **kwargs) == 'gpio_with_hall' self._hall = None
def parse_width_height(key, default_value, errors, **kwargs): return [ int(x) for x in parse_option( key, str, default_value, errors=errors, **kwargs).split('x') ]
def __init__(self, relay, **kwargs): super().__init__(relay) # Our relay is expected to be wired on the motor power line. self._relay.open() self._steer_servo_config = dict(pin=parse_option('servo.steering.pin.nr', int, 13, **kwargs), min_pw=parse_option('servo.steering.min_pulse_width.ms', float, 0.5, **kwargs), max_pw=parse_option('servo.steering.max_pulse_width.ms', float, 2.5, **kwargs), frame=parse_option('servo.steering.frame_width.ms', float, 20.0, **kwargs)) self._motor_servo_config = dict(pin=parse_option('servo.motor.pin.nr', int, 12, **kwargs), min_pw=parse_option('servo.motor.min_pulse_width.ms', float, 0.5, **kwargs), max_pw=parse_option('servo.motor.max_pulse_width.ms', float, 2.5, **kwargs), frame=parse_option('servo.motor.frame_width.ms', float, 20.0, **kwargs)) self._steering_config = dict(scale=parse_option('steering.domain.scale', float, 1.0, **kwargs)) self._throttle_config = dict(reverse=parse_option('throttle.reverse.gear', int, 0.0, **kwargs), forward_shift=parse_option('throttle.domain.forward.shift', float, 0.0, **kwargs), backward_shift=parse_option('throttle.domain.backward.shift', float, 0.0, **kwargs), scale=parse_option('throttle.domain.scale', float, 2.0, **kwargs)) self._steer_servo = None self._motor_servo = None
def create_stream(config_file): parser = SafeConfigParser() parser.read(config_file) kwargs = dict(parser.items('camera')) name = os.path.basename(os.path.splitext(config_file)[0]) _type = parse_option('camera.type', str, **kwargs) assert _type in list( gst_commands.keys()), "Unrecognized camera type '{}'.".format(_type) if _type == 'h264/rtsp': out_width, out_height = [ int(x) for x in parse_option('camera.output.shape', str, '640x480', **kwargs).split('x') ] config = { 'ip': (parse_option('camera.ip', str, '192.168.1.64', **kwargs)), 'port': (parse_option('camera.port', int, 554, **kwargs)), 'user': (parse_option('camera.user', str, 'user1', **kwargs)), 'password': (parse_option('camera.password', str, 'HaikuPlot876', **kwargs)), 'path': (parse_option('camera.path', str, '/Streaming/Channels/103', **kwargs)) } else: _type = 'raw/usb/h264/udp' src_width, src_height = [ int(x) for x in parse_option('camera.source.shape', str, '640x480', **kwargs).split('x') ] udp_width, udp_height = [ int(x) for x in parse_option('camera.udp.shape', str, '320x240', ** kwargs).split('x') ] out_width, out_height = [ int(x) for x in parse_option('camera.output.shape', str, '480x320', **kwargs).split('x') ] config = { 'uri': (parse_option('camera.uri', str, '/dev/video0', **kwargs)), 'src_width': src_width, 'src_height': src_height, 'video_flip': (parse_option('camera.flip.method', str, 'none', **kwargs)), 'udp_width': udp_width, 'udp_height': udp_height, 'udp_bitrate': (parse_option('camera.udp.bitrate', int, 1024000, **kwargs)), 'udp_host': (parse_option('camera.udp.host', str, '192.168.1.100', **kwargs)), 'udp_port': (parse_option('camera.udp.port', int, 5000, **kwargs)), 'out_width': out_width, 'out_height': out_height, 'out_bitrate': (parse_option('camera.output.bitrate', int, 1024000, **kwargs)) } _command = gst_commands.get(_type).format(**config) _socket_ref = parse_option('camera.output.class', str, 'http-live', **kwargs) logger.info("Socket '{}' ref '{}' gst command={}".format( name, _socket_ref, _command)) return create_video_source(name, shape=(out_height, out_width, 3), command=_command), _socket_ref