def draw_svg(self, drawing, g):
assert isinstance(drawing, svgwrite.Drawing)
glane = drawing.g()
glane.attribs['class'] = 'lane'
cp1 = self.control_points[0]
cp2 = self.control_points[-1]
rel = relative_pose(cp1.as_SE2(), cp2.as_SE2())
_, theta = geo.translation_angle_from_SE2(rel)
delta = int(np.sign(theta))
fill = {
-1: '#577094',
0: '#709457',
1: '#947057',
}[delta]
points = self.lane_profile(points_per_segment=10)
p = drawing.polygon(points=points, fill=fill, fill_opacity=0.5)
glane.add(p)
center_points = self.center_line_points(points_per_segment=10)
center_points = [
geo.translation_angle_from_SE2(_)[0] for _ in center_points
]
p = drawing.polyline(points=center_points,
stroke=fill,
fill='none',
stroke_dasharray="0.02",
stroke_width=0.01)
glane.add(p)
g.add(glane)
for x in self.control_points:
q = x.asmatrix2d().m
p1, _ = geo.translation_angle_from_SE2(q)
delta_arrow = np.array([self.width / 4, 0])
p2 = SE2_apply_R2(q, delta_arrow)
gp = drawing.g()
gp.attribs['class'] = 'control-point'
l = drawing.line(start=p1.tolist(),
end=p2.tolist(),
stroke='black',
stroke_width=self.width / 20.0)
gp.add(l)
c = drawing.circle(center=p1.tolist(),
r=self.width / 8,
fill='white',
stroke='black',
stroke_width=self.width / 20.0)
gp.add(c)
g.add(gp)
def graph_write(G, f,
vertex_command='VERTEX_XYT',
edge_command='EDGE_XYT', endline='\n'):
for u in G.nodes():
pose = G.node[u]['pose']
t, theta = translation_angle_from_SE2(pose)
f.write('%s %d %f %f %f %s' %
(vertex_command, u, t[0], t[1], theta, endline))
for u, v in G.edges():
if u > v: continue
diff = G[u][v]['pose']
t, theta = translation_angle_from_SE2(diff)
f.write('%s %d %d %f %f %f %s' %
(edge_command, u, v, t[0], t[1], theta, endline))
def get_fused_pose(self, req):
if req is None:
pose_resp = None
else:
pose = SE2_from_xytheta([req.x, req.y, req.theta])
pose = self.db_kinematics.step(self.encoder_ticks_left_delta_t, self.encoder_ticks_right_delta_t, pose)
trans, theta = translation_angle_from_SE2(pose)
pose_resp = PoseResponse(trans[0], trans[1], theta)
if not self.configure_flag:
self.pose = pose
self.configure_flag = True
if self.configure_flag:
self.pose = self.db_kinematics.step(self.encoder_ticks_left_delta_t, self.encoder_ticks_right_delta_t,
self.pose)
self.encoder_ticks_right_delta_t = 0
self.encoder_ticks_left_delta_t = 0
pose_SE3 = SE3_from_SE2(self.pose)
rot, trans = rotation_translation_from_SE3(pose_SE3)
quaternion_tf = quaternion_from_matrix(pose_SE3)
quaternion = Quaternion(quaternion_tf[0], quaternion_tf[1], quaternion_tf[2], quaternion_tf[3])
translation = Vector3(trans[0], trans[1], trans[2])
trafo = Transform(translation, quaternion)
self.tfs_msg.transform = trafo
if self.vel_left == 0.0 and self.vel_right == 0.0:
self.tfs_msg.header.stamp = rospy.Time.now()
else:
self.tfs_msg.header.stamp = self.encoder_timestamp
self.br.sendTransformMessage(self.tfs_msg)
self.pub_tf_enc_loc.publish(self.tfs_msg)
return pose_resp
def get_vxy_world(pose, vel):
(x, y), theta = translation_angle_from_SE2(pose)
_, omega = linear_angular_from_se2(vel)
vel2 = np.dot(pose, vel)
vx = vel2[0, 2]
vy = vel2[1, 2]
return x, y, theta, vx, vy, omega
def integrate(self, dt, commands):
"""
:param dt:
:param commands: an instance of CarCommands
:return:
"""
check_isinstance(commands, CarCommands)
# Your code comes here!
q,_ = self.TSE2_from_state()
_, direction = geo.translation_angle_from_SE2(q)
linear = [commands.linear_velocity, 0]
angular = (commands.linear_velocity / self.parameters.wheel_distance) * math.tan(commands.steering_angle)
# represent this as se(2)
commands_se2 = geo.se2_from_linear_angular(linear, angular)
# Call the "integrate" function of GenericKinematicsSE2
s1 = GenericKinematicsSE2.integrate(self, dt, commands_se2)
# new state
c1 = s1.q0, s1.v0
t1 = s1.t0
return CarDynamics(self.parameters, c1, t1)
def graph_write(G,
f,
vertex_command='VERTEX_XYT',
edge_command='EDGE_XYT',
endline='\n'):
for u in G.nodes():
pose = G.node[u]['pose']
t, theta = translation_angle_from_SE2(pose)
f.write('%s %d %f %f %f %s' %
(vertex_command, u, t[0], t[1], theta, endline))
for u, v in G.edges():
if u > v: continue
diff = G[u][v]['pose']
t, theta = translation_angle_from_SE2(diff)
f.write('%s %d %d %f %f %f %s' %
(edge_command, u, v, t[0], t[1], theta, endline))
def plot_poses_vels_xt(pylab, poses, vels, normalize=True):
cmd_style = dict(head_width=0.01, head_length=0.01, edgecolor='blue')
for pose, vel in zip(poses, vels):
(x, _), theta = translation_angle_from_SE2(pose)
omega = angular_from_se2(vel)
style = 'r.' if omega > 0 else 'b.'
pylab.plot(x, np.rad2deg(theta), style)
plot_arrow_se2_xt(pylab, pose, vel, normalize=normalize, **cmd_style)
def __init__(self, width, control_points, *args, **kwargs):
PlacedObject.__init__(self, *args, **kwargs)
self.width = float(width)
self.control_points = control_points
for p in control_points:
check_isinstance(p, SE2Transform)
for i in range(len(control_points) - 1):
a = control_points[i]
b = control_points[i + 1]
ta, _ = geo.translation_angle_from_SE2(a.as_SE2())
tb, _ = geo.translation_angle_from_SE2(b.as_SE2())
d = np.linalg.norm(ta - tb)
if d < 0.001:
msg = 'Two points are two close: \n%s\n%s' % (a, b)
raise ValueError(msg)
def show_sensor_data(pylab, vehicle, robot_pose=None, col='r'):
if robot_pose is None:
robot_pose = SE3.from_yaml(vehicle['pose'])
for attached in vehicle['sensors']:
sensor_pose = from_yaml(attached['current_pose'])
sensor_t, sensor_theta = \
translation_angle_from_SE2(SE2_from_SE3(sensor_pose))
print('robot: %s' % SE2.friendly(SE2_from_SE3(robot_pose)))
print(' sens: %s' % SE2.friendly(SE2_from_SE3(sensor_pose)))
# sensor_theta = -sensor_theta
sensor = attached['sensor']
if sensor['type'] == 'Rangefinder':
directions = np.array(sensor['directions'])
observations = attached['current_observations']
readings = np.array(observations['readings'])
valid = np.array(observations['valid'])
# directions = directions[valid]
# readings = readings[valid]
x = []
y = []
rho_min = 0.05
for theta_i, rho_i in zip(directions, readings):
print('theta_i: %s' % theta_i)
x.append(sensor_t[0] +
np.cos(sensor_theta + theta_i) * rho_min)
y.append(sensor_t[1] +
np.sin(sensor_theta + theta_i) * rho_min)
x.append(sensor_t[0] +
np.cos(sensor_theta + theta_i) * rho_i)
y.append(sensor_t[1] +
np.sin(sensor_theta + theta_i) * rho_i)
x.append(None)
y.append(None)
pylab.plot(x, y, color=col, markersize=0.5, zorder=2000)
elif sensor['type'] == 'Photoreceptors':
directions = np.array(sensor['directions'])
observations = attached['current_observations']
readings = np.array(observations['readings'])
luminance = np.array(observations['luminance'])
valid = np.array(observations['valid'])
readings[np.logical_not(valid)] = 0.6
rho_min = 0.5
for theta_i, rho_i, lum in zip(directions, readings, luminance):
x = []
y = []
x.append(sensor_t[0] +
np.cos(sensor_theta + theta_i) * rho_min)
y.append(sensor_t[1] +
np.sin(sensor_theta + theta_i) * rho_min)
x.append(sensor_t[0] +
np.cos(sensor_theta + theta_i) * rho_i)
y.append(sensor_t[1] +
np.sin(sensor_theta + theta_i) * rho_i)
pylab.plot(x, y, color=(lum, lum, lum),
markersize=0.5, zorder=2000)
else:
print('Unknown sensor type %r' % sensor['type'])
def end_condition(self, simulation):
if simulation.vehicle_collided:
return True
if simulation.timestamp > self.max_sim_time:
return True
pose = simulation.vehicle.get_pose()
t, theta = translation_angle_from_SE2(SE2_from_SE3(pose)) #@UnusedVariable
d = np.linalg.norm(t)
if d > self.failure_radius:
return True
def is_straight(self):
if not len(self.control_points) == 2:
return False
cp1 = self.control_points[0]
cp2 = self.control_points[1]
r = relative_pose(cp1.as_SE2(), cp2.as_SE2())
p, theta = geo.translation_angle_from_SE2(r)
return almost_equal(theta, 0) and almost_equal(p[1], 0)
def plot_ranges(cr, pose, directions, valid, readings,
rho_min=CC.plot_ranges_rho_min):
sensor_t, sensor_theta = translation_angle_from_SE2(pose)
directions = directions[valid]
readings = readings[valid]
for theta_i, rho_i in zip(directions, readings):
plot_ray(cr, sensor_t, sensor_theta + theta_i,
rho1=rho_min,
rho2=max(rho_i, rho_min),
color=(1, 1, 0)) # XXX
def run(self):
while not rospy.is_shutdown():
if self.image is None:
continue
if not self.camera_info_received:
continue
if self.rectify_flag:
self.rectify_image()
list_pose_tag = self.detect_april_tag()
if not list_pose_tag:
if self.encoder_conf_flag:
trans_map_base_2D, theta_map_base_2D = translation_angle_from_SE2(
self.pose_map_base_SE2)
self.pose_map_base_SE2 = encoder_localization_client(
trans_map_base_2D[0], trans_map_base_2D[1],
theta_map_base_2D)
self.tfs_msg_map_base.transform = self.pose2transform(
SE3_from_SE2(self.pose_map_base_SE2))
self.tfs_msg_map_base.header.stamp = rospy.Time.now()
self.pub_tf_fused_loc.publish(self.tfs_msg_map_base)
self.br_map_base.sendTransformMessage(self.tfs_msg_map_base)
else:
pose_dta_dtc = self.pose_inverse(list_pose_tag[0])
pose_april_cam = self.T_a_dta @ pose_dta_dtc @ self.T_dtc_c
pose_map_base = self.pose_map_at @ pose_april_cam @ self.pose_cam_base
quat_map_base = quaternion_from_matrix(pose_map_base)
euler = euler_from_quaternion(quat_map_base)
theta = euler[2]
rot_map_base, translation_map_base = rotation_translation_from_SE3(
pose_map_base)
pose_map_base_SE2_enc = encoder_localization_client(
translation_map_base[0], translation_map_base[1], theta)
self.encoder_conf_flag = True
self.pose_map_base_SE2 = SE2_from_xytheta(
[translation_map_base[0], translation_map_base[1], theta])
self.tfs_msg_map_base.transform = self.pose2transform(
SE3_from_SE2(self.pose_map_base_SE2))
self.tfs_msg_map_base.header.stamp = rospy.Time.now()
self.pub_tf_fused_loc.publish(self.tfs_msg_map_base)
self.br_map_base.sendTransformMessage(self.tfs_msg_map_base)
self.tfs_msg_map_april.header.stamp = self.image_timestamp
self.static_tf_br_map_april.sendTransform(
self.tfs_msg_map_april)
self.tfs_msg_cam_base.header.stamp = self.image_timestamp
self.static_tf_br_cam_base.sendTransform(self.tfs_msg_cam_base)
self.tfs_msg_april_cam.transform = self.pose2transform(
pose_april_cam)
self.tfs_msg_april_cam.header.stamp = self.image_timestamp
self.br_april_cam.sendTransformMessage(self.tfs_msg_april_cam)
def end_condition(self, simulation):
end_line = self.corridor_length * 0.9
if simulation.vehicle_collided:
return True
pose = simulation.vehicle.get_pose()
t, theta = translation_angle_from_SE2(SE2_from_SE3(pose)) #@UnusedVariable
y = t[1]
if y > end_line:
return True
return False
def initialize(cls, c0, t0=0, seed=None):
"""
This class initializes the dynamics at a given configuration
"""
# pose, velocity in SE(2), se(2)
q, v = c0
# get position p from pose
p, theta = geo.translation_angle_from_SE2(q)
# get linear velocity from se(2)
v2d, _ = geo.linear_angular_from_se2(v)
# create the integrator2d initial state
return Integrator2D(p, v2d, t0)
def draw_axes(pylab, pose, cx='r', cy='g', size=1, L=0.3):
t, th = translation_angle_from_SE2(pose)
tx = [t[0] + L * np.cos(th),
t[1] + L * np.sin(th)]
ty = [t[0] + L * -np.sin(th),
t[1] + L * np.cos(th)]
pylab.plot([t[0], tx[0]],
[t[1], tx[1]], '-', color=cx, linewidth=size)
pylab.plot([t[0], ty[0]],
[t[1], ty[1]], '-', color=cy, linewidth=size)
def plot_photoreceptors(cr, pose, directions, valid, readings, luminance,
rho_min=CC.plot_ranges_rho_min):
sensor_t, sensor_theta = translation_angle_from_SE2(pose)
directions = directions[valid]
readings = readings[valid]
luminance = luminance[valid]
for theta_i, rho_i, lum in zip(directions, readings, luminance):
color = luminance2color(lum)
plot_ray(cr, sensor_t, sensor_theta + theta_i,
rho1=rho_min,
rho2=max(rho_i, rho_min),
color=color)
def dd_test():
radius = 0.1
radius_left = radius
radius_right = radius
wheel_distance = 0.5
dddp = DifferentialDriveDynamicsParameters(
radius_left=radius_left,
radius_right=radius_right,
wheel_distance=wheel_distance,
)
q0 = geo.SE2_from_translation_angle([0, 0], 0)
v0 = geo.se2.zero()
c0 = q0, v0
s0 = dddp.initialize(c0)
omega = 0.3
commands = WheelVelocityCommands(omega, omega)
dt = 4.0
s1 = s0.integrate(dt, commands)
q1, v1 = s1.TSE2_from_state()
print(geo.se2.friendly(v1))
print(geo.SE2.friendly(q1))
p1, theta = geo.translation_angle_from_SE2(q1)
assert_almost_equal(p1, [dt * radius * omega, 0])
omega_left = 0.3
omega_right = 0
commands = WheelVelocityCommands(omega_left, omega_right)
dt = 4.0
s1 = s0.integrate(dt, commands)
q1, v1 = s1.TSE2_from_state()
p1, theta = geo.translation_angle_from_SE2(q1)
print(geo.se2.friendly(v1))
print(geo.SE2.friendly(q1))
def simulate(self, dt, vehicle):
vehicle_pose = SE2_from_SE3(vehicle.get_pose())
target_pose = self.get_target_pose()
relative_pose = SE2.multiply(SE2.inverse(target_pose), vehicle_pose)
t, theta = translation_angle_from_SE2(relative_pose) #@UnusedVariable
# position on field of view
phi = np.arctan2(t[1], t[0])
diff = normalize_pi_scalar(self.target_stabilize_phi - phi)
# torque command
command = -np.sign(diff)
commands = [command]
self.target_state = self.target_dynamics.integrate(self.target_state,
commands, dt)
self.update_primitives()
return [self.target]
def get_delta(beta):
q0 = self.center_point(beta)
t0, _ = geo.translation_angle_from_SE2(q0)
d = np.linalg.norm(p - t0)
d1 = np.array([0, -d])
p1 = SE2_apply_R2(q0, d1)
d2 = np.array([0, +d])
p2 = SE2_apply_R2(q0, d2)
D2 = np.linalg.norm(p2 - p)
D1 = np.linalg.norm(p1 - p)
res = np.minimum(D1, D2)
# print('%10f: q %s %f' % (beta, geo.SE2.friendly(q0), res))
return res
def from_SE2(cls, q: SE2value) -> 'SE2Transform':
""" From a matrix """
translation, angle = geo.translation_angle_from_SE2(q)
return SE2Transform(translation, angle)
def main():
usage = """Implements the interface of the SLAM evaluation utilities"""
parser = OptionParser(usage=usage)
parser.add_option("--slow", default=False, action='store_true',
help='Enables sanity checks.')
parser.add_option("--max_dist", default=15, type='float',
help='[= %default] Maximum distance for graph simplification.')
parser.add_option("--min_nodes", default=250, type='float',
help='[= %default] Minimum number of nodes to simplify to.')
parser.add_option("--scale", default=10000, type='float',
help='[= %default] Controls the weight of angular vs linear .')
parser.add_option("--seed", default=42, type='int',
help='[= %default] Seed for random number generator.')
(options, args) = parser.parse_args() #@UnusedVariable
np.random.seed(options.seed)
if not options.slow: contracts.disable_all()
fin = sys.stdin
fout = sys.stdout
G = DiGraph()
progress = False
count = 0
def status():
return ('Read %5d commands, built graph with %5d nodes and %5d edges. \r' %
(count, G.number_of_nodes(), G.number_of_edges()))
for command in parse_command_stream(fin, raise_if_unknown=False):
if isinstance(command, (AddVertex2D, Equiv, AddEdge2D)):
graph_apply_operation(G, command)
elif isinstance(command, SolveState):
eprint(status())
algorithm = EFPNO_S
params = dict(max_dist=options.max_dist,
min_nodes=options.min_nodes,
scale=options.scale)
instance = algorithm(params)
results = instance.solve(G)
G = results['solution']
elif isinstance(command, QueryState):
nodes = command.ids if command.ids else G.nodes()
nodes = sorted(nodes)
fout.write('BEGIN\n')
for n in nodes:
t, theta = translation_angle_from_SE2(G.node[n]['pose'])
fout.write('VERTEX_XYT %d %g %g %g\n' %
(n, t[0], t[1], theta))
fout.write('END\n')
fout.flush()
if progress and count % 250 == 0:
eprint(status())
count += 1
def distance_poses(q1: SE2value, q2: SE2value) -> float:
SE2 = g.SE2
d = SE2.multiply(SE2.inverse(q1), q2)
t, _a = g.translation_angle_from_SE2(d)
return np.linalg.norm(t)
def discretize(tran):
def D(x):
return np.round(x, decimals=2)
p, theta = geo.translation_angle_from_SE2(tran.as_SE2())
return D(p[0]), D(p[1]), D(np.cos(theta)), D(np.sin(theta))
def distance_poses(q1, q2):
SE2 = geometry.SE2
d = SE2.multiply(SE2.inverse(q1), q2)
t, _a = geometry.translation_angle_from_SE2(d)
return np.linalg.norm(t)
def plot_arrow_SE2(pylab, pose, length=0.1, **style):
(x, y), theta = translation_angle_from_SE2(pose)
pylab.plot(x, y, 'rx')
a = np.cos(theta) * length
b = np.sin(theta) * length
pylab.arrow(x, y, a, b, **style)
def cairo_rototranslate(cr, pose):
t, r = geometry.translation_angle_from_SE2(pose)
with cairo_transform(cr, t=t, r=r) as cr:
yield cr
This script outputs a 'good' starting position and angle for an agent, given a map. That means, agent starts at a point
that is close to the center of a lane and starts at an angle that is close to zero, which means agent is aligned with
the lane.
"""
from duckietown_world.world_duckietown.sampling_poses import sample_good_starting_pose
import duckietown_world as dw
import geometry as geo
import numpy as np
import argparse
# construct the argument parser and parse the arguments
ap = argparse.ArgumentParser()
ap.add_argument("-m", "--map-name", required=True, type=str,
help="map name as in maps/ folder")
args = vars(ap.parse_args())
map_name = args["map_name"]
along_lane = 1
only_straight = True
m = dw.load_map(map_name)
q = sample_good_starting_pose(m, only_straight=only_straight, along_lane=along_lane)
translation, angle = geo.translation_angle_from_SE2(q)
propose_pos = np.array([translation[0], 0, translation[1]])
propose_angle = angle
print(f"\nReturning for the map: '{map_name}'\n"
f"Pose: {propose_pos}",
f"\t|\tAngle: {propose_angle}")
def evaluate(self, context: RuleEvaluationContext,
result: RuleEvaluationResult):
interval = context.get_interval()
lane_pose_seq = context.get_lane_pose_seq()
ego_pose_sequence = context.get_ego_pose_global()
timestamps = []
driven_any = []
driven_lanedir = []
tile_fqn2lane_fqn = {}
for idt in iterate_with_dt(interval):
t0, t1 = idt.v0, idt.v1 # not v
try:
name2lpr = lane_pose_seq.at(t0)
p0 = ego_pose_sequence.at(t0).as_SE2()
p1 = ego_pose_sequence.at(t1).as_SE2()
except UndefinedAtTime:
dr_any = dr_lanedir = 0.0
else:
prel = relative_pose(p0, p1)
translation, _ = geo.translation_angle_from_SE2(prel)
dr_any = np.linalg.norm(translation)
if name2lpr:
ds = []
for k, lpr in name2lpr.items():
if lpr.tile_fqn in tile_fqn2lane_fqn:
if lpr.lane_segment_fqn != tile_fqn2lane_fqn[
lpr.tile_fqn]:
# msg = 'Backwards detected'
# print(msg)
continue
tile_fqn2lane_fqn[lpr.tile_fqn] = lpr.lane_segment_fqn
assert isinstance(lpr, GetLanePoseResult)
c0 = lpr.center_point
ctas = geo.translation_angle_scale_from_E2(
c0.asmatrix2d().m)
c0_ = geo.SE2_from_translation_angle(
ctas.translation, ctas.angle)
prelc0 = relative_pose(c0_, p1)
tas = geo.translation_angle_scale_from_E2(prelc0)
# otherwise this lane should not be reported
# assert tas.translation[0] >= 0, tas
ds.append(tas.translation[0])
dr_lanedir = max(ds) if ds else 0.0
else:
# no lp
dr_lanedir = 0.0
driven_any.append(dr_any)
driven_lanedir.append(dr_lanedir)
timestamps.append(t0)
title = "Consecutive lane distance"
driven_lanedir_incremental = SampledSequence[float](timestamps,
driven_lanedir)
driven_lanedir_cumulative = accumulate(driven_lanedir_incremental)
if len(driven_lanedir_incremental) <= 1:
total = 0
else:
total = driven_lanedir_cumulative.values[-1]
description = textwrap.dedent("""\
This metric computes how far the robot drove **in the direction of the correct lane**,
discounting whenever it was driven in the wrong direction with respect to the start.
""")
result.set_metric(
name=("driven_lanedir_consec", ),
total=total,
incremental=driven_lanedir_incremental,
title=title,
description=description,
cumulative=driven_lanedir_cumulative,
)
def evaluate(self, context: RuleEvaluationContext,
result: RuleEvaluationResult):
interval = context.get_interval()
lane_pose_seq = context.get_lane_pose_seq()
ego_pose_sequence = context.get_ego_pose_global()
driven_any_builder = SampledSequenceBuilder[float]()
driven_lanedir_builder = SampledSequenceBuilder[float]()
for idt in iterate_with_dt(interval):
t0, t1 = idt.v0, idt.v1 # not v
try:
name2lpr = lane_pose_seq.at(t0)
p0 = ego_pose_sequence.at(t0).as_SE2()
p1 = ego_pose_sequence.at(t1).as_SE2()
except UndefinedAtTime:
dr_any = dr_lanedir = 0.0
else:
prel = relative_pose(p0, p1)
translation, _ = geo.translation_angle_from_SE2(prel)
dr_any = np.linalg.norm(translation)
if name2lpr:
ds = []
for k, lpr in name2lpr.items():
assert isinstance(lpr, GetLanePoseResult)
c0 = lpr.center_point
ctas = geo.translation_angle_scale_from_E2(
c0.asmatrix2d().m)
c0_ = geo.SE2_from_translation_angle(
ctas.translation, ctas.angle)
prelc0 = relative_pose(c0_, p1)
tas = geo.translation_angle_scale_from_E2(prelc0)
# otherwise this lane should not be reported
# assert tas.translation[0] >= 0, tas
ds.append(tas.translation[0])
dr_lanedir = max(ds)
else:
# no lp
dr_lanedir = 0.0
driven_any_builder.add(t0, dr_any)
driven_lanedir_builder.add(t0, dr_lanedir)
driven_any_incremental = driven_any_builder.as_sequence()
driven_any_cumulative = accumulate(driven_any_incremental)
if len(driven_any_incremental) <= 1:
total = 0
else:
total = driven_any_cumulative.values[-1]
title = "Distance"
description = textwrap.dedent("""\
This metric computes how far the robot drove.
""")
result.set_metric(
name=("driven_any", ),
total=total,
incremental=driven_any_incremental,
title=title,
description=description,
cumulative=driven_any_cumulative,
)
title = "Lane distance"
driven_lanedir_incremental = driven_lanedir_builder.as_sequence()
driven_lanedir_cumulative = accumulate(driven_lanedir_incremental)
if len(driven_lanedir_incremental) <= 1:
total = 0
else:
total = driven_lanedir_cumulative.values[-1]
description = textwrap.dedent("""\
This metric computes how far the robot drove
**in the direction of the lane**.
""")
result.set_metric(
name=("driven_lanedir", ),
total=total,
incremental=driven_lanedir_incremental,
title=title,
description=description,
cumulative=driven_lanedir_cumulative,
)
def main():
usage = """Implements the interface of the SLAM evaluation utilities"""
parser = OptionParser(usage=usage)
parser.add_option("--slow",
default=False,
action='store_true',
help='Enables sanity checks.')
parser.add_option(
"--max_dist",
default=15,
type='float',
help='[= %default] Maximum distance for graph simplification.')
parser.add_option(
"--min_nodes",
default=250,
type='float',
help='[= %default] Minimum number of nodes to simplify to.')
parser.add_option(
"--scale",
default=10000,
type='float',
help='[= %default] Controls the weight of angular vs linear .')
parser.add_option("--seed",
default=42,
type='int',
help='[= %default] Seed for random number generator.')
(options, args) = parser.parse_args() #@UnusedVariable
np.random.seed(options.seed)
if not options.slow: contracts.disable_all()
fin = sys.stdin
fout = sys.stdout
G = DiGraph()
progress = False
count = 0
def status():
return (
'Read %5d commands, built graph with %5d nodes and %5d edges. \r' %
(count, G.number_of_nodes(), G.number_of_edges()))
for command in parse_command_stream(fin, raise_if_unknown=False):
if isinstance(command, (AddVertex2D, Equiv, AddEdge2D)):
graph_apply_operation(G, command)
elif isinstance(command, SolveState):
eprint(status())
algorithm = EFPNO_S
params = dict(max_dist=options.max_dist,
min_nodes=options.min_nodes,
scale=options.scale)
instance = algorithm(params)
results = instance.solve(G)
G = results['solution']
elif isinstance(command, QueryState):
nodes = command.ids if command.ids else G.nodes()
nodes = sorted(nodes)
fout.write('BEGIN\n')
for n in nodes:
t, theta = translation_angle_from_SE2(G.node[n]['pose'])
fout.write('VERTEX_XYT %d %g %g %g\n' % (n, t[0], t[1], theta))
fout.write('END\n')
fout.flush()
if progress and count % 250 == 0:
eprint(status())
count += 1
def test_pwm1():
parameters = get_DB18_nominal(delay=0)
# initial configuration
init_pose = np.array([0, 0.8])
init_vel = np.array([0, 0])
q0 = geo.SE2_from_R2(init_pose)
v0 = geo.se2_from_linear_angular(init_vel, 0)
tries = {
"straight_50": (PWMCommands(+0.5, 0.5)),
"straight_max": (PWMCommands(+1.0, +1.0)),
"straight_over_max": (PWMCommands(+1.5, +1.5)),
"pure_left": (PWMCommands(motor_left=-0.5, motor_right=+0.5)),
"pure_right": (PWMCommands(motor_left=+0.5, motor_right=-0.5)),
"slight-forward-left": (PWMCommands(motor_left=0, motor_right=0.25)),
"faster-forward-right": (PWMCommands(motor_left=0.5, motor_right=0)),
# 'slight-right': (PWMCommands(-0.1, 0.1)),
}
dt = 0.03
t_max = 10
map_data_yaml = """
tiles:
- [floor/W,floor/W, floor/W, floor/W, floor/W]
- [straight/W , straight/W , straight/W, straight/W, straight/W]
- [floor/W,floor/W, floor/W, floor/W, floor/W]
tile_size: 0.61
"""
map_data = yaml.load(map_data_yaml)
root = construct_map(map_data)
timeseries = {}
for id_try, commands in tries.items():
seq = integrate_dynamics(parameters, q0, v0, dt, t_max, commands)
# return getattr(x, 'axis_left_ticks', None)
ground_truth = seq.transform_values(get_SE2transform)
poses = seq.transform_values(get_pose)
velocities = get_velocities_from_sequence(poses)
linear = velocities.transform_values(linear_from_se2)
angular = velocities.transform_values(angular_from_se2)
odo_left = seq.transform_values(odometry_left)
odo_right = seq.transform_values(odometry_right)
root.set_object(id_try, DB18(), ground_truth=ground_truth)
sequences = {}
sequences["motor_left"] = seq.transform_values(
lambda _: commands.motor_left)
sequences["motor_right"] = seq.transform_values(
lambda _: commands.motor_right)
plots = TimeseriesPlot(f"PWM commands", "pwm_commands", sequences)
timeseries[f"{id_try}/commands"] = plots
sequences = {}
sequences["linear_velocity"] = linear
sequences["angular_velocity"] = angular
plots = TimeseriesPlot(f"Velocities", "velocities", sequences)
timeseries[f"{id_try}/velocities"] = plots
sequences = {}
sequences["theta"] = poses.transform_values(
lambda x: geo.translation_angle_from_SE2(x)[1])
plots = TimeseriesPlot(f"Pose", "theta", sequences)
timeseries[f"{id_try}/pose"] = plots
sequences = {}
sequences["odo_left"] = odo_left
sequences["odo_right"] = odo_right
plots = TimeseriesPlot(f"Odometry", "odometry", sequences)
timeseries[f"{id_try}/odometry"] = plots
outdir = os.path.join(get_comptests_output_dir(), "together")
draw_static(root, outdir, timeseries=timeseries)
def raytracing(self, pose):
position, orientation = translation_angle_from_SE2(pose)
return self.query_sensor(position, orientation)
def friendly_from_pose(a: SE2value) -> FriendlyPose:
t, theta = geometry.translation_angle_from_SE2(a)
theta_deg = np.rad2deg(theta)
return FriendlyPose(float(t[0]), float(t[1]), float(theta_deg))
def discretize(tran: SE2Transform) -> PointLabel:
def D(x) -> float:
return np.round(x, decimals=2)
p, theta = geo.translation_angle_from_SE2(tran.as_SE2())
return D(p[0]), D(p[1]), D(np.cos(theta)), D(np.sin(theta))
def cairo_rototranslate(cr, pose):
t, r = geometry.translation_angle_from_SE2(pose)
with cairo_transform(cr, t=t, r=r)as cr:
yield cr
