def __str__(self):
n = len(self.entries)
s = f"ResultsDB with {n} entries"
s += "\n" + dtu.indent(str(self.current), "", " current: ")
for i, p in enumerate(self.entries):
dtu.check_isinstance(p, ResultDBEntry)
s += "\n" + dtu.indent(str(p), "", f" {i + 1:2d} of {n}: ")
return s
def format_enc(enc: EasyNodeConfig, descriptions: bool = False) -> str:
s = f'Configuration for node "{enc.node_type_name}" in package "{enc.package_name}"'
s += "\n" + "=" * len(s)
S = " " * 4
s += "\n\n Parameters\n\n"
s += dtu.indent(format_enc_parameters(enc, descriptions), S)
s += "\n\n Subcriptions\n\n"
s += dtu.indent(format_enc_subscriptions(enc, descriptions), S)
s += "\n\n Publishers\n\n"
s += dtu.indent(format_enc_publishers(enc, descriptions), S)
return s
def compare_faster2():
variables = {}
variables["alpha"] = dict(min=-180,
max=180,
description="angle",
resolution=5,
units="deg",
units_display="deg")
variables["r"] = dict(min=3,
max=5,
description="distance",
resolution=0.2,
units="m",
units_display="cm")
# this will fail if precision is float32
gh = GridHelper(variables, precision="float64")
val_fast = gh.create_new()
val_fast.fill(0)
val_slow = gh.create_new()
val_slow.fill(0)
od = dtu.get_output_dir_for_test()
F = 1
alpha0 = 177
# r0 = 4
r0 = 4.1
w0 = 1
value = dict(alpha=alpha0, r=r0)
gh.add_vote(val_slow, value, w0, F)
values = np.zeros((2, 1))
values[0, 0] = alpha0
values[1, 0] = r0
weights = np.zeros(1)
weights[0] = w0
gh.add_vote_faster(val_fast, values, weights, F)
print(f"sum slow: {np.sum(val_slow)}")
print(f"sum fast: {np.sum(val_fast)}")
d = grid_helper_plot(gh, val_slow)
fn = os.path.join(od, "compare_faster_slow.jpg")
dtu.write_data_to_file(d.get_png(), fn)
d = grid_helper_plot(gh, val_fast)
fn = os.path.join(od, "compare_faster_fast.jpg")
dtu.write_data_to_file(d.get_png(), fn)
f = lambda x: f" {x:5f}" if x else " "
print((dtu.indent(array_as_string(val_fast, f), "val_fast ")))
print((dtu.indent(array_as_string(val_slow, f), "val_slow ")))
assert_almost_equal(val_fast, val_slow)
def reduce_stats(a, b, a_plus_b):
a_plus_b[
"num_log_segments"] = a["num_log_segments"] + b["num_log_segments"]
a_plus_b["length"] = a["length"] + b["length"]
a_plus_b["nsamples"] = a["nsamples"] + b["nsamples"]
a_plus_b["mean"] = (a["mean"] * a["nsamples"] +
b["mean"] * b["nsamples"]) / a_plus_b["nsamples"]
a_plus_b["min"] = min(a["min"], b["min"])
a_plus_b["max"] = max(a["max"], b["max"])
# Note: it is not possible to compute the median in an efficient manner
a_plus_b["median"] = (a["median"] + b["median"]) / 2.0
print((dtu.indent(a, "a: ")))
print((dtu.indent(b, "b: ")))
print((dtu.indent(a_plus_b, "a_plus_b: ")))
def _init_parameters(self):
parameters = self._configuration.parameters
class Config:
def __getattr__(self, name):
if not name in parameters:
msg = f"The user is trying to use {name!r}, which is not a parameter "
msg += "for this node.\n"
s = "\n- ".join(list(parameters))
msg += f"The declared parameters that can be used are:\n- {s}"
raise AttributeError(msg)
return object.__getattr__(self, name)
self.config = Config()
values = {}
# load the configuration
self.info("Loading parameters...")
with dtu.rospy_timeit_wall("getting configuration files"):
qr = get_user_configuration(self.package_name, self.node_type_name)
if not qr.is_complete():
msg = "\nThe configuration that I could load is not complete:\n"
msg += dtu.indent(str(qr), " | ")
msg = dtu.indent(
msg,
f"{self.package_name} / {self.node_type_name} fatal error > ")
raise dtu.DTConfigException(msg)
self.info(f"Loaded configuration:\n{qr}")
for p in list(parameters.values()):
try:
val = qr.values.get(p.name)
except KeyError:
msg = f"Could not load required parameter {p.name!r}."
raise dtu.DTConfigException(msg)
# write to parameter server, for transparency
if val is not None: # we cannot set None parameters
rospy.set_param("~" + p.name, val)
setattr(self.config, p.name, val)
values[p.name] = val
self._on_parameters_changed(first_time=True, values=values)
duration = self.config.en_update_params_interval
duration = rospy.Duration.from_sec(duration)
rospy.Timer(duration, self._update_parameters)
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 format_logs(logs):
if not logs:
s = "No logs found."
return s
else:
s = f"Found {len(logs)} logs.\n"
table = get_logs_description_table(logs)
dtu.remove_table_field(table, "filename")
dtu.remove_table_field(table, "topics")
dtu.remove_table_field(table, "description")
dtu.remove_table_field(table, "hash bag")
s += dtu.indent(dtu.format_table_plus(table, colspacing=4), "| ")
counts = defaultdict(lambda: set())
for l in list(logs.values()):
for rname, dtr_yaml in list(l.resources.items()):
counts[rname].add(dtr_yaml["name"])
s += "\n\nCount of resources: "
rsort = sorted(counts, key=lambda _: -len(counts[_]))
for rname in rsort:
rcount = len(counts[rname])
s += f"\n {rcount:3d} {rname}"
if rcount <= 3:
s += " " + " ".join(counts[rname])
return s
def print_results(analyzers: List[str], results_all, out: str):
base = os.path.join(out, "statistics")
yaml_data = dtu.yaml_dump_pretty(results_all)
dtu.write_str_to_file(yaml_data, os.path.join(base, "statistics.yaml"))
print((dtu.indent(yaml_data, "print_results ")))
for a in analyzers:
dtu.write_str_to_file(dtu.yaml_dump_pretty(results_all[a]),
os.path.join(base, f"{a}.table.yaml"))
s = ""
s += "\n" + "-" * 10 + f" Results for {a} " + "-" * 10
table = table_for_analyzer(results_all[a])
s += "\n" + dtu.indent(dtu.format_table_plus(table, colspacing=3),
" ")
s += "\n"
dtu.write_str_to_file(s, os.path.join(base, f"{a}.table.txt"))
def create_instance(self, family_name: str,
instance_name_or_spec: Union[dict, str]):
dtu.check_isinstance(instance_name_or_spec, (dict, str))
family = self.get_family(family_name)
if not family.valid:
msg = f"Cannot instantiate {instance_name_or_spec!r} because its family {family_name!r} is " \
f"invalid."
msg += "\n\n" + dtu.indent(family.error_if_invalid, " > ")
raise dtu.DTConfigException(msg)
if isinstance(instance_name_or_spec, str):
instance_name = instance_name_or_spec
dtu.check_is_in("instance", instance_name, family.instances)
instance = family.instances[instance_name]
if not instance.valid:
msg = f"Cannot instantiate {instance_name!r} because it is invalid:\n" \
f"{dtu.indent(instance.error_if_invalid, '> ')}"
raise dtu.DTConfigException(msg)
res = dtu.instantiate(instance.constructor, instance.parameters)
elif isinstance(instance_name_or_spec, dict):
_name, spec = _parse_inline_spec(instance_name_or_spec)
res = dtu.instantiate(spec.constructor, spec.parameters)
else:
assert False
interface = dtu.import_name(family.interface)
if not isinstance(res, interface):
msg = f"I expected that {instance_name_or_spec!r} would be a {interface.__name__} but it is a " \
f"{type(res).__name__}."
raise dtu.DTConfigException(msg)
return res
def __str__(self):
s = f"Configuration result for node `{self.node_name}` (package `{self.package_name}`)"
s += f"\nThe configuration sequence was {list(self.config_sequence)}."
s += "\nThe following is the list of parameters set and their origin:"
s += "\n" + dtu.indent(
config_summary(self.all_keys, self.values, self.origin), " ")
return s
def write_images(bag_filename, topic, basename):
""" Returns the name of the first created file """
dtu.logger.info(f"reading topic {topic!r} from {bag_filename!r}")
bag = rosbag.Bag(bag_filename)
nfound = bag.get_message_count(topic)
if nfound == 0:
msg = f"Found 0 messages for topic {topic}"
msg += "\n\n" + dtu.indent(dbu.get_summary_of_bag_messages(bag), " ")
raise ValueError(msg)
res = dbu.d8n_read_all_images_from_bag(bag, topic)
n = len(res)
filenames = []
for i in range(n):
rgb = res[i]["rgb"]
data: bytes = dtu.png_from_bgr(dtu.bgr_from_rgb(rgb))
if n == 1:
fn = basename + ".png"
else:
fn = basename + f"-{i:02d}.png"
filenames.append(fn)
dtu.write_data_to_file(data, fn)
bag.close()
return filenames[0]
def display_check_results(results, out):
s = ""
s += f"\n{len(results)} results to report"
for i, r in enumerate(results):
s += "\n" + dtu.indent(str(r), "", f"{i + 1} of {len(results)}: ")
print(s)
fn = os.path.join(out, "check_results.txt")
dtu.write_str_to_file(s, fn)
def fail_if_not_expected(results: List[CheckResult], expect):
statuses = [r.status for r in results]
summary = summarize_statuses(statuses)
if summary != expect:
msg = f"Expected status {expect!r}, but got {summary!r}."
for i, r in enumerate(results):
msg += "\n" + dtu.indent(str(r), "", f"{i + 1} of {len(results)}: ")
raise Exception(msg)
def from_yaml(data0):
data = deepcopy(data0)
try:
urls = data.pop("urls")
mime = data.pop("mime")
desc = data.pop("desc")
size = data.pop("size")
name = data.pop("name")
dtrv = data.pop("dtrv")
hashes = data.pop("hash")
except KeyError as e:
msg = "Could not interpret:"
msg += "\n\n" + dtu.indent(dtu.yaml_dump(data0), " >")
msg += "\n\n" + dtu.indent(str(e), " ")
raise Exception(msg)
return DTR(dtrv, size, name, mime, hashes, urls, desc)
def homography_from_yaml(data: dict) -> np.array:
try:
h = data["homography"]
res = np.array(h).reshape((3, 3))
return res
except Exception as e:
msg = "Could not interpret data:"
msg += "\n\n" + dtu.indent(yaml.dump(data), " ")
dtu.logger.error(msg)
raise InvalidHomographyInfo(msg) from e
def format_instances(family, colorize, verbose=False):
if not family.instances:
s = (
f'No instances files found for family "{family.family_name}" (pattern = '
f"{family.instances_pattern}).\n\n")
return s
else:
s = f'Found {len(family.instances)} instances of algorithm family "{family.family_name}":\n'
table = []
table.append([
"Instance name", "constructor", "parameters", "description",
"filename"
])
for _ in list(family.instances.values()):
row = []
name = _.instance_name
if (not _.valid) and colorize:
name = dtu.make_red(name)
row.append(name)
row.append(_.constructor)
row.append(dtu.yaml_dump_pretty(_.parameters))
row.append(_.description)
row.append(dtu.friendly_path(_.filename))
table.append(row)
if not verbose:
dtu.remove_table_field(table, "filename")
dtu.remove_table_field(table, "description")
s += dtu.indent(dtu.format_table_plus(table, colspacing=4), "| ")
for _ in list(family.instances.values()):
if not _.valid:
msg = _.error_if_invalid
s += dtu.make_red("\n" +
dtu.indent(msg, "", _.instance_name + ": "))
return s
def format_families(families: List[EasyAlgoFamily],
colorize: bool = True,
verbose: bool = True):
if not families:
s = "No algorithm families found."
return s
else:
table = [[
"Family name",
"interface",
"pattern",
"# found",
"valid",
"filename",
"description",
]]
for family in families:
assert isinstance(family, EasyAlgoFamily)
row = [
family.family_name, family.interface, family.instances_pattern
]
if not family.instances:
row.append("\n(none)")
else:
n_valid = len(
[_ for _ in list(family.instances.values()) if _.valid])
n_invalid = len(family.instances) - n_valid
ss = f"{len(family.instances)}"
if n_invalid:
ss += dtu.make_red(f" ({n_invalid} invalid)")
row.append(ss)
if family.valid:
ss = "yes"
else:
ss = "no: " + family.error_if_invalid
row.append(ss)
row.append(dtu.friendly_path(family.filename))
if (not family.valid) and colorize:
row = dtu.make_row_red(row)
row.append(family.description.strip())
table.append(row)
if not verbose:
dtu.remove_table_field(table, "filename")
s = f"Found {len(families)} algorithm families:\n\n"
s += dtu.indent(dtu.format_table_plus(table, colspacing=4), " ")
return s
def ros_from_misc(name, value, t):
if isinstance(value, float):
return Float64(value)
elif isinstance(value, int):
return Int64(value)
elif isinstance(value, np.ndarray):
msg = ros_from_np_array(value)
# print('%s -> %s' % (value, msg))
return msg
elif isinstance(value, list):
data = np.array(value)
return ros_from_np_array(data)
else:
m = f'Could not find a way to write "{name}" in ROS ({dtu.describe_type(value)})'
m += "\n" + dtu.indent(str(value), "> ")
raise ValueError(m)
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 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 expect(data, query, result_keys, filters=None):
result_keys = list(result_keys)
check_isinstance(data, OrderedDict)
if True:
spec = dtu.parse_match_spec(query, filters=filters)
print("-----")
print("Query: %s" % query)
print(dtu.indent(spec, "", "Spec: "))
res = dtu.fuzzy_match(query, data, filters=filters)
check_isinstance(res, OrderedDict)
if list(res) != result_keys:
msg = "Error:"
msg += "\n Data: %s" % data
msg += "\n Query: %s" % query
msg += "\n Result: %s" % list(res)
msg += "\n Expect: %s" % list(result_keys)
raise Exception(msg)
def query_results_one(self, branch, date, commit):
possible = self.query_results(branch, date, commit)
from easy_regression.conditions.eval import DataNotFound
if len(possible) == 0:
msg = "Could not find any match for the query."
msg += f"\n branch: {branch}"
msg += f"\n date: {date}"
msg += f"\n commit: {commit}"
raise DataNotFound(msg)
if len(possible) > 1:
n = len(possible)
msg = f"Found {n} matches for this query."
msg += f"\n branch: {branch}"
msg += f"\n date: {date}"
msg += f"\n commit: {commit}"
msg += "\nThese are the matches:"
for i, p in enumerate(possible):
dtu.check_isinstance(p, ResultDBEntry)
msg += "\n" + dtu.indent(str(p), f" {i + 1:2d} of {n}: ")
raise AmbiguousQuery(msg)
return possible[0]
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)
def on_received_image(self, context, image_msg):
if not self.active:
return
self.intermittent_counter += 1
with context.phase("decoding"):
# Decode from compressed image with OpenCV
try:
image_cv = dtu.bgr_from_jpg(image_msg.data)
except ValueError as e:
self.error(f"Could not decode image: {e}")
return
with context.phase("resizing"):
# Resize and crop image
hei_original, wid_original = image_cv.shape[0:2]
if self.config.img_size[0] != hei_original or self.config.img_size[1] != wid_original:
# image_cv = cv2.GaussianBlur(image_cv, (5,5), 2)
image_cv = cv2.resize(
image_cv,
(self.config.img_size[1], self.config.img_size[0]),
interpolation=cv2.INTER_NEAREST,
)
image_cv = image_cv[self.config.top_cutoff :, :, :]
with context.phase("correcting"):
# apply color correction
image_cv_corr = self.ai.applyTransform(image_cv)
# image_cv_corr = cv2.convertScaleAbs(image_cv_corr)
with context.phase("detection"):
# Set the image to be detected
self.detector.setImage(image_cv_corr)
# Detect lines and normals
white = self.detector.detectLines("white")
yellow = self.detector.detectLines("yellow")
red = self.detector.detectLines("red")
with context.phase("preparing-images"):
# SegmentList constructor
segmentList = SegmentList()
segmentList.header.stamp = image_msg.header.stamp
# Convert to normalized pixel coordinates, and add segments to segmentList
top_cutoff = self.config.top_cutoff
s0, s1 = self.config.img_size[0], self.config.img_size[1]
arr_cutoff = np.array((0, top_cutoff, 0, top_cutoff))
arr_ratio = np.array((1.0 / s1, 1.0 / s0, 1.0 / s1, 1.0 / s0))
if len(white.lines) > 0:
lines_normalized_white = (white.lines + arr_cutoff) * arr_ratio
segmentList.segments.extend(
toSegmentMsg(lines_normalized_white, white.normals, Segment.WHITE)
)
if len(yellow.lines) > 0:
lines_normalized_yellow = (yellow.lines + arr_cutoff) * arr_ratio
segmentList.segments.extend(
toSegmentMsg(lines_normalized_yellow, yellow.normals, Segment.YELLOW)
)
if len(red.lines) > 0:
lines_normalized_red = (red.lines + arr_cutoff) * arr_ratio
segmentList.segments.extend(toSegmentMsg(lines_normalized_red, red.normals, Segment.RED))
self.intermittent_log(
"# segments: white %3d yellow %3d red %3d"
% (len(white.lines), len(yellow.lines), len(red.lines))
)
# Publish segmentList
with context.phase("publishing"):
self.publishers.segment_list.publish(segmentList)
# VISUALIZATION only below
if self.config.verbose:
with context.phase("draw-lines"):
# Draw lines and normals
image_with_lines = np.copy(image_cv_corr)
drawLines(image_with_lines, white.lines, (0, 0, 0))
drawLines(image_with_lines, yellow.lines, (255, 0, 0))
drawLines(image_with_lines, red.lines, (0, 255, 0))
with context.phase("published-images"):
# Publish the frame with lines
out = dru.d8n_image_msg_from_cv_image(image_with_lines, "bgr8", same_timestamp_as=image_msg)
self.publishers.image_with_lines.publish(out)
with context.phase("pub_edge/pub_segment"):
out = dru.d8n_image_msg_from_cv_image(
self.detector.edges, "mono8", same_timestamp_as=image_msg
)
self.publishers.edge.publish(out)
colorSegment = color_segment(white.area, red.area, yellow.area)
out = dru.d8n_image_msg_from_cv_image(colorSegment, "bgr8", same_timestamp_as=image_msg)
self.publishers.color_segment.publish(out)
if self.intermittent_log_now():
self.info("stats from easy_node\n" + dtu.indent(context.get_stats(), "> "))
def __str__(self):
s = "CheckResult:"
s += "\n" + dtu.indent(self.status, " status: ")
s += "\n" + dtu.indent(self.summary, " summary: ")
s += "\n" + dtu.indent(self.details, "", " details: ")
return s
def compare_faster():
variables = {}
variables["alpha"] = dict(min=-180,
max=180,
description="angle",
resolution=5,
units="deg",
units_display="deg")
variables["r"] = dict(min=3,
max=5,
description="distance",
resolution=0.1,
units="m",
units_display="cm")
# this will fail if precision is float32
gh = GridHelper(variables, precision="float64")
val_fast = gh.create_new()
val_fast.fill(0)
val_slow = gh.create_new()
val_slow.fill(0)
od = dtu.get_output_dir_for_test()
F = 1
alpha0 = 7
# r0 = 4
r0 = 4.1
w0 = 1.0
value = dict(alpha=alpha0, r=r0)
gh.add_vote(val_slow, value, w0, F)
assert_equal(np.sum(val_slow > 0), 9)
values = np.zeros((2, 1))
values[0, 0] = alpha0
values[1, 0] = r0
weights = np.zeros(1)
weights[0] = w0
gh.add_vote_faster(val_fast, values, weights, F)
assert_equal(np.sum(val_fast > 0), 9)
d = grid_helper_plot(gh, val_slow)
fn = os.path.join(od, "compare_faster_slow.jpg")
dtu.write_data_to_file(d.get_png(), fn)
d = grid_helper_plot(gh, val_fast)
fn = os.path.join(od, "compare_faster_fast.jpg")
dtu.write_data_to_file(d.get_png(), fn)
D = val_fast - val_slow
diff = np.max(np.abs(D))
print(f"diff: {diff!r}")
if diff > 1e-8:
print(dtu.indent(array_as_string_sign(val_fast), "val_fast "))
print(dtu.indent(array_as_string_sign(val_slow), "val_slow "))
print(dtu.indent(array_as_string_sign(D), "Diff "))
print(f"non zero val_fast: {val_fast[val_fast > 0]}")
print(f"non zero val_slow: {val_slow[val_slow > 0]}")
assert_almost_equal(val_fast, val_slow)
def __str__(self):
s = f"MakeTimeSlice {{ {self.t0} : {self.t1} }}"
s += "\n" + dtu.indent(str(self.children[0]), " ")
return s
def __str__(self):
s = "Binary operation"
s += "\n" + dtu.indent(self.a, "", " a: ")
s += "\n" + dtu.indent(self.op, "", " op: ")
s += "\n" + dtu.indent(self.b, "", " b: ")
return s
def d8n_make_video_from_bag(
bag_filename: str,
topic: str,
out: str,
t0: Optional[float] = None,
t1: Optional[float] = None,
):
"""
Creates a video out of the topic in the bag.
topic: the topic name (any image-like topic)
out: an .mp4 file.
Returns the name of the created file.
raises NotEnoughFramesInSlice if there are less than 3 frames in slice
Note that needs a bunch more dependencies to be installed.
Until we fix the dependencies:
sudo pip install SystemCmd==1.2 ros_node_utils==1.0 ConfTools==1.8 QuickApp==1.2.2
sudo apt-get install -y mplayer mencoder
sudo add-apt-repository ppa:mc3man/trusty-media
sudo apt-get update
sudo apt-get install -y ffmpeg
"""
try:
import procgraph_ros
from procgraph import pg
except ImportError:
raise
# pg -m procgraph_ros bag2mp4 --bag $bag --topic $topic --out $out
stop_at = 10
min_messages = 5
actual_count, count, stopped_early = count_messages_in_slice(
bag_filename, topic, t0, t1, stop_at=stop_at)
msg = "Creating video for topic %r, which has %d messages " "in the entire log." % (
topic,
count,
)
logger.info(msg)
if not stopped_early and (actual_count != count):
msg = "However, the actual count in [%s, %s] is %s" % (t0, t1,
actual_count)
logger.info(msg)
if actual_count < min_messages:
msg = ("Topic %r has only %d messages in slice (%d total), too few"
" to make a video (min: %s).\nFile: %s" %
(topic, actual_count, count, min_messages, bag_filename))
msg += "\nt0: %s" % t0
msg += "\nt1: %s" % t1
bag = rosbag.Bag(bag_filename)
msg += "\nstart: %s" % bag.get_start_time()
msg += "\nend: %s" % bag.get_end_time()
if actual_count == count:
msg += "\n" + dtu.indent(get_summary_of_bag_messages(bag),
" info: ")
bag.close()
raise NotEnoughFramesInSlice(msg)
model = "bag2mp4_fixfps_limit"
tmpdir = dtu.create_tmpdir()
out_tmp = os.path.join(tmpdir, os.path.basename(out))
try:
logger.debug("Writing temp file to %s" % out_tmp)
logger.debug("(You can use mplayer to follow along.)")
pg(model,
config=dict(bag=bag_filename,
topic=topic,
out=out_tmp,
t0=t0,
t1=t1))
md = out_tmp + ".metadata.yaml"
if os.path.exists(md):
os.unlink(md)
dn = os.path.dirname(out)
if not os.path.exists(dn):
dtu.mkdirs_thread_safe(dn)
shutil.copyfile(out_tmp, out)
logger.info("Created: %s" % out)
info = out_tmp + ".info.yaml"
if os.path.exists(info):
os.unlink(info)
return out
finally:
if os.path.exists(out_tmp):
os.unlink(out_tmp)
if os.path.exists(tmpdir):
shutil.rmtree(tmpdir)
def raise_error(rdb, t, res, s):
msg = s
msg += "\n" + dtu.indent(str(res), "obtained: ")
msg += "\n" + dtu.indent(str(t), "", "test: ")
msg += "\n" + dtu.indent(str(rdb), "", "rdb: ")
raise Exception(msg)
