def select_artf_for_report(self, artf_xyz): """ There are cases when two or more robots discover artefact independently and they do not have the base answer yet. Never-the-less only one representative (team identical) should be selected and reported. """ # first sort all known artifacts into three groups scored_true, scored_false, scored_unknown = [], [], [] for item in artf_xyz: artf_type, pos, src, scored = item if scored is True: scored_true.append(item) elif scored is False: scored_false.append(item) else: assert scored is None, scored scored_unknown.append(item) # remove all unknown too close to already successfully scored artifacts tmp = [] for item in scored_unknown: _, pos, _, _ = item for _, pos2, _, _ in scored_true: # we want to filter out also other artifacts of different type (i.e. probably wrongly recognized?) if distance3D(pos, pos2) / 1000.0 < RADIUS: break else: tmp.append(item) scored_unknown = tmp # now remove all already reported close to false reports tmp = [] for item in scored_unknown: artf_type, pos, _, _ = item for artf_type2, pos2, _, _ in scored_false: if artf_type == artf_type2 and distance3D( pos, pos2) / 1000.0 < RADIUS_FALSE: # close to wrongly reported artifact with the same type break else: tmp.append(item) scored_unknown = tmp # finally pick only one representative (any, just ordered) # ... and handle other reports once this is confirmed # TODO confirmation from base can be lost several times ... is it OK? if len(scored_unknown) > 0: return [min(scored_unknown, key=artf_sort_fcn)] else: return []
def follow_trace(self, trace, timeout, max_target_distance=5.0, safety_limit=None): print('Follow trace') END_THRESHOLD = 2.0 start_time = self.sim_time_sec print('MD', self.xyz, distance3D(self.xyz, trace.trace[0]), trace.trace) while distance3D(self.xyz, trace.trace[0]) > END_THRESHOLD and self.sim_time_sec - start_time < timeout.total_seconds(): if self.update() == 'scan': target_x, target_y = trace.where_to(self.xyz, max_target_distance)[:2] x, y = self.xyz[:2] # print((x, y), (target_x, target_y)) desired_direction = math.atan2(target_y - y, target_x - x) - self.yaw safety = self.go_safely(desired_direction) if safety_limit is not None and safety < safety_limit: print('Danger! Safety limit for follow trace reached!', safety, safety_limit) break print('End of follow trace(sec)', self.sim_time_sec - start_time)
def should_deploy(self, xyz): if self.radius is None: return False for loc in self.locations: if distance3D(xyz, loc) < self.radius: return False return True
def entrance_reached(sim): corrected = [(rr - oo) for rr, oo in zip(sim.xyz, sim.origin)] goal = [ 0.5, 0, 0 ] # note, that in real run the Y coordinate depends on choise left/righ if distance3D(corrected, goal) < 2: return True return False
def return_home(self, timeout, home_threshold=None): if home_threshold is None: HOME_THRESHOLD = 5.0 else: HOME_THRESHOLD = home_threshold SHORTCUT_RADIUS = 2.3 MAX_TARGET_DISTANCE = 5.0 MIN_TARGET_DISTANCE = 1.0 assert(MAX_TARGET_DISTANCE > SHORTCUT_RADIUS) # Because otherwise we could end up with a target point more distant from home than the robot. print('Wait and get ready for return') self.send_speed_cmd(0, 0) self.wait(dt=timedelta(seconds=3.0)) original_trace = copy.deepcopy(self.trace) self.trace.prune(SHORTCUT_RADIUS) self.wait(dt=timedelta(seconds=2.0)) print('done.') start_time = self.sim_time_sec target_distance = MAX_TARGET_DISTANCE count_down = 0 while distance3D(self.xyz, (0, 0, 0)) > HOME_THRESHOLD and self.sim_time_sec - start_time < timeout.total_seconds(): channel = self.update() if (channel == 'scan' and not self.flipped) or (channel == 'scan_back' and self.flipped) or (channel == 'scan360'): if target_distance == MIN_TARGET_DISTANCE: target_x, target_y = original_trace.where_to(self.xyz, target_distance)[:2] else: target_x, target_y = self.trace.where_to(self.xyz, target_distance)[:2] # print(self.time, self.xyz, (target_x, target_y), math.degrees(self.yaw)) x, y = self.xyz[:2] desired_direction = math.atan2(target_y - y, target_x - x) - self.yaw if self.flipped: desired_direction += math.pi # symmetry for angle in self.joint_angle_rad: desired_direction -= angle safety = self.go_safely(desired_direction) if safety < 0.2: print(self.time, "Safety low!", safety, desired_direction) target_distance = MIN_TARGET_DISTANCE count_down = 300 if count_down > 0: count_down -= 1 if count_down == 0: target_distance = MAX_TARGET_DISTANCE print(self.time, "Recovery to original", target_distance) print('return_home: dist', distance3D(self.xyz, (0, 0, 0)), 'time(sec)', self.sim_time_sec - start_time)
def maybe_remember_artifact(self, artifact_data, artifact_xyz): for stored_data, (x, y, z) in self.artifacts: if distance3D((x, y, z), artifact_xyz) < 4.0: # in case of uncertain type, rather report both if stored_data == artifact_data: return False self.artifacts.append((artifact_data, artifact_xyz)) return True
def nearest_scored_artifact(self, artf_type, position): # TODO remove 1000x scaling to millimeters - source of headache dist = [ distance3D(position, artf[1]) for artf in self.artf_xyz_accumulated if artf[0] == artf_type and artf[-1] is True ] if len(dist) == 0: return None # no nearest for given filter return min(dist) / 1000.0 # due to mm scaling
def on_acc(self, data): vec = [x/1000.0 for x in data] value = abs(distance3D(vec, [0, 0, 0]) - 9.81) if value >= self.threshold: if self.verbose: print(f'acc trigger: {value} (buf size = {len(self.buf)})') for rec in self.buf: self.publish('crash_rgbd', rec) self.buf.clear()
def register_new_artifact(self, artifact_data, artifact_xyz): """ Register newly detected artifact and update internal structures :param artifact_data: type of artifact :param artifact_xyz: artifact 3D position :return: True if artifact should be new published """ # the breadcrumbs are sometimes wrongly classified as DRILL if artifact_data == DRILL: for x, y, z in self.breadcrumbs: if distance3D((x, y, z), artifact_xyz) < 4.0: if self.verbose: print('False detection - dist:', distance3D((x, y, z), artifact_xyz)) return False if self.verbose: self.debug_artf.append((artifact_data, artifact_xyz)) for i, (stored_data, (x, y, z)) in enumerate(self.artifacts): if distance3D((x, y, z), artifact_xyz) < 4.0: # in case of uncertain type, rather report both if stored_data == artifact_data: self.num_observations[i] += 1 # return true only when confirmation threshold was reached if self.verbose: print('Confirmed:', artifact_data, self.num_observations[i]) if artifact_data == GAS: return False # ignore confirmation - virtual detector is perfect return self.num_observations[ i] == self.min_observations # report only once self.artifacts.append((artifact_data, artifact_xyz)) self.num_observations.append(1) if self.min_observations > 1 and artifact_data != GAS: # new GAS should be reported independently on confirmation level return False return True
def on_base_station(self, data): p = data['artifact_position'] dist = [ distance3D(p, [x / 1000.0 for x in artf[1]]) for artf in self.artf_xyz_accumulated ] if len(dist) > 0 and min(dist) < 0.1: # i.e. it is our report and we have it in the list ... I would almost assert it min_i = dist.index(min(dist)) was_unknown = self.artf_xyz_accumulated[min_i][-1] is None self.artf_xyz_accumulated[min_i][-1] = (data['score_change'] > 0) # TODO? check type - we decided to ignore it for Cave Circuit if was_unknown: self.publish('artf_all', self.artf_xyz_accumulated) # broadcast new update # trigger sending next artifact if there is any self.publish_artf(self.artf_xyz_accumulated)
def evaluate_poses(poses, gt_poses, time_step_sec=1): if len(poses) == 0 or len(gt_poses) == 0: return [] time_limit = max(poses[0][0], gt_poses[0][0]) end_time = min(poses[-1][0], gt_poses[-1][0]) i, j = 0, 0 arr = [] while time_limit <= end_time: while poses[i][0] < time_limit: i += 1 while gt_poses[j][0] < time_limit: j += 1 dist = distance3D(poses[i][1:], gt_poses[j][1:]) diff = [a - b for a, b in zip(poses[i][1:], gt_poses[j][1:])] arr.append((time_limit, dist, *diff, gt_poses[j][1:])) time_limit += time_step_sec return arr
def sufficient_step(self, anchor_pose, robot_pose): return (anchor_pose is None or distance3D(anchor_pose[0], robot_pose[0]) >= self.min_dist_step or quaternion.angle_between( anchor_pose[1], robot_pose[1]) >= self.min_angle_step)
def entrance_reached(sim): corrected = [(rr - oo) for rr, oo in zip(sim.xyz, sim.origin)] goal = [2.5, 0, 0] if distance3D(corrected, goal) < 2: return True return False
def main(): import argparse import pathlib import sys parser = argparse.ArgumentParser(__doc__) parser.add_argument('logfiles', help='OSGAR logfile(s)', nargs='*') parser.add_argument('--ign', help='Ignition "state.tlog", default in current directory', default=str(pathlib.Path("./state.tlog"))) parser.add_argument('--pose3d', help='Stream with pose3d data') parser.add_argument('--name', help='Robot name, default is autodetect') parser.add_argument('--sec', help='duration of the analyzed part (seconds)', type=float, default=MAX_SIMULATION_DURATION) parser.add_argument('--draw', help="draw debug results", action='store_true') args = parser.parse_args() if args.logfiles == []: paths = pathlib.Path('.').glob("*.log") for p in paths: try: LogReader(p) args.logfiles.append(str(p)) except AssertionError: pass if len(args.logfiles) == 0: sys.exit("no logfiles found in current directory") args.logfiles.sort() ground_truth, breadcrumbs = ign.read_poses(args.ign, seconds=args.sec) artifacts = ign.read_artifacts(args.ign) img = ign.draw(ground_truth, artifacts, breadcrumbs) cv2.imwrite(args.ign+'.png', img) if args.draw: from subt.tools import startfile startfile.main(args.ign+'.png') print('Ground truth count:', len(ground_truth)) for logfile in args.logfiles: print("Processing logfile:", logfile) robot_name = args.name if robot_name is None: robot_name = autodetect_name(logfile) print(' Autodetected name:', robot_name) if robot_name.startswith('T'): print(' skiping teambase') continue pose3d_stream = args.pose3d if pose3d_stream is None: pose3d_stream = autodetect_pose3d(logfile) print(' Autodetect pose3d stream:', pose3d_stream) pose3d = read_pose3d(logfile, pose3d_stream, seconds=args.sec) print(' Trace reduced count:', len(pose3d)) tmp_poses = osgar2arr(pose3d) tmp_gt = ign2arr(ground_truth, robot_name=robot_name) print(' Ground truth seconds:', len(tmp_gt)) arr = evaluate_poses(tmp_poses, tmp_gt) if len(arr) == 0: print('EMPTY OVERLAP!') if len(tmp_poses) > 0: print('poses:', tmp_poses[0][0], tmp_poses[-1][0]) if len(tmp_gt) > 0: print('gt :', tmp_gt[0][0], tmp_gt[-1][0]) else: limits = iter([1,2,3,4,5]) current_limit = next(limits) dist3d = [] last_xyz = arr[0][-1] path_dist = 0 for dt, e, x, y, z, gt_xyz in arr: dist3d.append(e) path_dist += distance3D(gt_xyz, last_xyz) last_xyz = gt_xyz if current_limit is not None and e > current_limit: print(f" sim_time_sec: {dt:5d}, error: {e:5.2f}m, distance: {distance3D(gt_xyz, [0,0,0]):7.2f}m from origin, path: {path_dist:7.2f}m") try: while e > current_limit: current_limit = next(limits) except StopIteration: current_limit = None print(f" Maximum error: {max(dist3d):.2f}m -> {'OK' if max(dist3d) < 3 else 'FAIL'}") assert min(dist3d) < 0.1, min(dist3d) # the minimum should be almost zero for correct evaluation if args.draw: import matplotlib.pyplot as plt fig, ax1 = plt.subplots() ax1.set_ylabel('distance (m)') ax1.set_title(robot_name) x = [a[0] for a in arr] for index, label in enumerate(['dist3d', 'x', 'y', 'z']): y = [a[index + 1] for a in arr] ax1.plot(x, y, '-', linewidth=2, label=label) ax1.set_xlabel('sim time (s)') plt.legend() plt.show()