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)