def process_log(self, bag_in, prefix: str, bag_out, prefix_out: str, utils: ProcessorUtilsInterface):
algo_db = get_easy_algo_db()
line_detector = algo_db.create_instance(FAMILY_LINE_DETECTOR, self.line_detector)
image_prep = algo_db.create_instance("image_prep", self.image_prep)
vehicle = dbu.which_robot(bag_in)
topic = f"/{vehicle}/camera_node/image/compressed"
context = FakeContext()
transform = None
frame = 0
for compressed_img_msg in dbu.d8n_bag_read_with_progress(bag_in, topic):
with context.phase("decoding"):
try:
image_cv = dtu.bgr_from_jpg(compressed_img_msg.data)
except ValueError as e:
msg = f"Could not decode image: {e}"
dtu.raise_wrapped(ValueError, e, msg)
segment_list = image_prep.process(context, image_cv, line_detector, transform)
rendered = vs_fancy_display(image_prep.image_cv, segment_list)
rendered = dtu.d8_image_zoom_linear(rendered, 2)
log_name = "log_name"
time = 12
rendered = dtu.add_duckietown_header(rendered, log_name, time, frame)
out = dru.d8n_image_msg_from_cv_image(rendered, "bgr8", same_timestamp_as=compressed_img_msg)
# Write to the bag
bag_out.write("processed", out)
# out = d8n_image_msg_from_cv_image(image_cv, "bgr8", same_timestamp_as=compressed_img_msg)
bag_out.write("image", compressed_img_msg)
frame += 1
def r(m):
try:
yield
except EvaluationError as e:
msg = f"Cannot evaluate binary operation: error during {m}"
msg += "\n" + str(self)
dtu.raise_wrapped(EvaluationError, e, msg, compact=True)
def parse_date_spec(d: str) -> datetime:
from dateutil.parser import parse
try:
return parse(d)
except ValueError as e:
msg = f"Cannot parse date {d!r}."
dtu.raise_wrapped(RTParseError, e, msg, compact=True)
def __call__(self, a, b):
try:
expect_float(a)
expect_float(b)
val = self.f(a, b)
desc = f"{a} {self.which} {b}"
return ResultWithDescription(val, desc)
except EvaluationError as e:
msg = f"While evaluating {self.f.__name__}({a}, {b})"
dtu.raise_wrapped(EvaluationError, e, msg, compact=True)
def load_configuration_publishers(data: dict) -> dict:
res = {}
for k, v in list(data.items()):
try:
check_good_name(k)
res[k] = load_configuration_publisher(k, v)
except dtu.DTConfigException as e:
msg = f"Invalid publisher entry {k!r}:"
dtu.raise_wrapped(dtu.DTConfigException, e, msg, compact=True)
return res
def load_configuration(realpath, contents) -> EasyNodeConfig:
# TODO: load "version" string
try:
try:
data = dtu.yaml_load(contents)
except Exception as e:
msg = "Could not parse YAML file properly:"
dtu.raise_wrapped(dtu.DTConfigException, e, msg, compact=True)
raise # ide not smart
if not isinstance(data, dict):
msg = f"Expected a dict, got {type(data).__name__}."
raise dtu.DTConfigException(msg)
try:
parameters = data.pop("parameters")
subscriptions = data.pop("subscriptions")
publishers = data.pop("publishers")
contracts = data.pop("contracts")
description = data.pop("description")
except KeyError as e:
key = e.args[0]
msg = f"Invalid configuration: missing field {key!r}."
raise dtu.DTConfigException(msg)
if not isinstance(description, (str, NoneType)):
msg = f"Description should be a string, not {type(description).__name__}."
raise dtu.DTConfigException(msg)
if data:
msg = f"Spurious fields found: {sorted(data)}"
raise dtu.DTConfigException(msg)
parameters = load_configuration_parameters(parameters)
subscriptions = load_configuration_subscriptions(subscriptions)
contracts = load_configuration_contracts(contracts)
publishers = load_configuration_publishers(publishers)
return EasyNodeConfig(
filename=realpath,
parameters=parameters,
contracts=contracts,
subscriptions=subscriptions,
publishers=publishers,
package_name=None,
description=description,
node_type_name=None,
)
except dtu.DTConfigException as e:
msg = f"Invalid configuration at {realpath}: "
dtu.raise_wrapped(dtu.DTConfigException, e, msg, compact=True)
def _on_parameters_changed(self, first_time, values):
try:
values1 = UpdatedParameters(**values)
values1.set_allowed(list(self._configuration.parameters))
self.on_parameters_changed(first_time, values1)
except dtu.DTConfigException as e:
msg = "Configuration error raised by on_parameters_changed()"
msg += "\n\n" + dtu.indent(dtu.yaml_dump(values), " ",
"Configuration: ")
dtu.raise_wrapped(dtu.DTConfigException, e, msg, compact=True)
except Exception as e:
msg = "Configuration error raised by on_parameters_changed()."
msg += "\n\n" + dtu.indent(dtu.yaml_dump(values), " ",
"Configuration: ")
dtu.raise_wrapped(dtu.DTConfigException, e, msg)
def __init__(
self,
logs,
processors: Optional[List[object]] = None,
analyzers: Optional[List[str]] = None,
checks: Optional[List[str]] = None,
topic_videos: Optional[List[str]] = None,
topic_images: Optional[List[str]] = None,
):
processors = processors or []
analyzers = analyzers or []
checks = checks or []
topic_videos = topic_videos or []
topic_images = topic_images or []
self.logs = logs
self.processors = []
for p in processors:
p = copy.deepcopy(p)
processor = p.pop("processor")
prefix_in = p.pop("prefix_in", "")
prefix_out = p.pop("prefix_out", "")
if p:
msg = f"Extra keys: {p}"
raise ValueError(msg)
p2 = ProcessorEntry(prefix_in=prefix_in,
processor=processor,
prefix_out=prefix_out)
self.processors.append(p2)
self.analyzers = analyzers
self.topic_videos = topic_videos
self.topic_images = topic_images
check_isinstance(checks, list)
try:
self.cwcs = parse_list_of_checks(checks)
except RTParseError as e:
msg = "Cannot parse list of checks."
msg += "\n" + dtu.indent(dtu.yaml_dump_pretty(checks), "",
"parsing: ")
dtu.raise_wrapped(RTParseError, e, msg, compact=True)
def _parse_regression_test_check(line: str) -> Wrapper:
line = line.strip()
delim = " "
tokens = line.split(delim)
if len(tokens) != 3:
msg = f'I expect exactly 3 tokens with delimiter {delim}.\nLine: "{line}"\nTokens: {tokens}'
raise dtu.DTConfigException(msg)
try:
ref1 = parse_reference(tokens[0])
binary = parse_binary(tokens[1])
ref2 = parse_reference(tokens[2])
evaluable = BinaryEval(ref1, binary, ref2)
except RTParseError as e:
msg = f'Cannot parse string "{line}".'
dtu.raise_wrapped(RTParseError, e, msg, compact=True)
raise
return Wrapper(evaluable)
def message_class_from_string(s):
if not "/" in s:
msg = ""
msg += f'Invalid message name "{s}".\n'
msg += 'I expected that the name of the message is in the format "PACKAGE/MSG".\n '
msg += 'E.g. "sensor_msgs/Joy" or "duckietown_msgs/BoolStamped".'
raise dtu.DTConfigException(msg)
# e.g. "std_msgs/Header"
i = s.index("/")
package = s[:i]
name = s[i + 1:]
symbol = f"{package}.msg.{name}"
try:
msgclass = dtu.import_name(symbol)
return msgclass
except ValueError as e:
msg = f'Cannot import type for message "{s}" ({symbol}).'
dtu.raise_wrapped(dtu.DTConfigException, e, msg, compact=True)
def camera_info_from_yaml(calib_data: dict) -> CameraInfo:
try:
cam_info = CameraInfo()
cam_info.width = calib_data["image_width"]
cam_info.height = calib_data["image_height"]
# cam_info.K = np.matrix(calib_data['camera_matrix']['data']).reshape((3,3))
# cam_info.D = np.matrix(calib_data['distortion_coefficients']['data']).reshape((1,5))
# cam_info.R = np.matrix(calib_data['rectification_matrix']['data']).reshape((3,3))
# cam_info.P = np.matrix(calib_data['projection_matrix']['data']).reshape((3,4))
cam_info.K = calib_data["camera_matrix"]["data"]
cam_info.D = calib_data["distortion_coefficients"]["data"]
cam_info.R = calib_data["rectification_matrix"]["data"]
cam_info.P = calib_data["projection_matrix"]["data"]
cam_info.distortion_model = calib_data["distortion_model"]
return cam_info
except Exception as e:
msg = "Could not interpret data:"
msg += "\n\n" + dtu.indent(yaml.dump(calib_data), " ")
dtu.raise_wrapped(InvalidCameraInfo, e, msg)
def interpret_config_file(filename: str) -> ConfigInfo:
"""
Returns a ConfigInfo.
"""
try:
basename = os.path.basename(filename)
base = basename.replace(SUFFIX, "")
# now we have something like
# package-node.config_name.date
# or
# package-node.config_name
if not "." in base:
msg = f"Invalid filename {filename!r}."
raise dtu.DTConfigException(msg)
tokens = base.split(".")
if len(tokens) > 3:
msg = f"Too many periods/tokens (tokens={tokens})"
raise dtu.DTConfigException(msg)
if len(tokens) <= 2:
# package-node.config_name
package_node = tokens[0]
if not "-" in package_node:
msg = f'Expected a "-" in "{package_node}".'
raise dtu.DTConfigException(msg)
i = package_node.index("-")
package_name = package_node[:i]
node_name = package_node[i + 1 :]
else:
package_name = node_name = None # FIXME: should we bail?
config_name = tokens[1]
if len(tokens) == 3:
# package-node.config_name.date
date_effective = tokens[2]
else:
date_effective = "20170101"
try:
date_effective = parse(date_effective)
except:
msg = f'Cannot interpret "{date_effective}" as a date.'
raise dtu.DTConfigException(msg)
# now read file
with open(filename) as f:
contents = f.read()
try:
try:
data = yaml.load(contents, Loader=yaml.Loader)
except YAMLError as e:
dtu.raise_wrapped(dtu.DTConfigException, e, "Invalid YAML", compact=True)
raise
if not isinstance(data, dict):
msg = "Expected a dictionary inside."
raise dtu.DTConfigException(msg)
for field in ["description", "values"]:
if not field in data:
msg = f'Missing field "{field}".'
raise dtu.DTConfigException(msg)
description = data.pop("description")
if not isinstance(description, str):
msg = f'I expected that "description" is a string, obtained {description!r}.'
raise dtu.DTConfigException(msg)
extends = data.pop("extends", [])
if not isinstance(extends, list):
msg = f'I expected that "extends" is a list, obtained {extends!r}.'
raise dtu.DTConfigException(msg)
values = data.pop("values")
if not isinstance(values, dict):
msg = f'I expected that "values" is a dictionary, obtained {type(values)}.'
raise dtu.DTConfigException(msg)
# Freeze the data
extends = tuple(extends)
values = frozendict(values)
except dtu.DTConfigException as e:
msg = "Could not interpret the contents of the file\n"
msg += f" {dtu.friendly_path(filename)}\n"
msg += "Contents:\n" + dtu.indent(contents, " > ")
dtu.raise_wrapped(dtu.DTConfigException, e, msg, compact=True)
raise
return ConfigInfo(
filename=filename,
package_name=package_name,
node_name=node_name,
config_name=config_name,
date_effective=date_effective,
extends=extends,
description=description,
values=values,
# not decided
valid=None,
error_if_invalid=None,
)
except dtu.DTConfigException as e:
msg = f"Invalid file {dtu.friendly_path(filename)}"
dtu.raise_wrapped(dtu.DTConfigException, e, msg, compact=True)
