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
