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()