def options():
for loc in result:
p0, p1 = utils.range(loc['range'])
uri = loc['uri']
action = edit_uri_select(uri, [(p0, p1)])
line0, _ = p0
yield u'{}:{}'.format(uri, line0), action
def _(timestamp, pipe, disabled):
client.diagnostics[filetype, buffile] = defaultdict(list)
client.diagnostics[filetype, buffile]['timestamp'] = timestamp
flags = [str(timestamp), '1| ']
from_severity = [
u'', u'{red}\u2022 ', u'{yellow}\u2022 ', u'{blue}\u2022 ',
u'{green}\u2022 '
]
for diag in params['diagnostics']:
if disabled and re.match(disabled, diag['message']):
continue
(line0, col0), end = utils.range(diag['range'])
flags.append(
str(line0) + '|' + from_severity[diag.get('severity', 1)])
client.diagnostics[filetype, buffile][line0].append({
'col':
col0,
'end':
end,
'message':
diag['message']
})
# todo: Set for the other buffers too (but they need to be opened)
msg = 'try %{add-highlighter window/ flag_lines default lsp_flags}\n'
msg += 'set buffer=' + buffile + ' lsp_flags '
msg += utils.single_quoted(':'.join(flags))
pipe(msg)
def _(timestamp, pipe):
flags = [str(timestamp)]
for hl in params['symbols']:
face = face_for_symbol(hl)
if face is None:
print("No face found for {}".format(hl))
continue
for range in hl['ranges']:
(line0, col0), (line1, col1) = utils.range(range)
flags.append("{}.{},{}.{}|{}".format(
line0, col0, line1, col1, face))
# todo:Set for the other buffers too (but they need to be opened)
msg = 'try %{add-highlighter buffer/ ranges cquery_semhl}\n'
msg += 'set buffer=' + buffile + ' cquery_semhl '
msg += utils.single_quoted(':'.join(flags))
print(msg)
pipe(msg)
def lsp_references(arg1, pwd, result):
"""
Find the references to the identifier at the main cursor.
Takes one argument, whether to include the declaration or not.
(default: true)
"""
m = defaultdict(list)
for loc in result:
m[loc['uri']].append(utils.range(loc['range']))
if m:
def options():
for uri, pos in six.iteritems(m):
loc = utils.drop_prefix(utils.uri_to_file(uri), pwd).lstrip('/') or uri
entry = u'{} ({} references)'.format(loc, len(pos))
yield entry, edit_uri_select(uri, pos)
return libkak.menu(options())
else:
return 'echo No results.'
def __init__(self, triple='simple'):
self.path = []
if triple == 'simple':
# start
self.path.append(easy_Odom(x=0, y=0, v=0.0, heading=0.0, frame='map'))
# out
self.path.append(easy_Odom(x=0.5, y=0, v=0.0, heading=0.0, frame='map'))
# Figure-8 for x:8m x y:6m box. (forms a 6m-8m-10m right triangle)
elif triple == 'f8_8x6':
# Start in middle facing top left beacon (8mx6m point)
self.path.append(easy_Odom(x=4.0000, y=3.0000, v=0.5, heading= 0.6458, frame='map'))
# Draw partial CW circle with 1.5m radius on right
self.path.append(easy_Odom(x=5.6000, y=4.2000, v=0.5, heading= 0.6458, frame='map'))
self.path.append(easy_Odom(x=6.3653, y=4.4939, v=0.5, heading= 0.0873, frame='map'))
self.path.append(easy_Odom(x=7.1708, y=4.3416, v=0.5, heading=-0.4712, frame='map'))
self.path.append(easy_Odom(x=7.7760, y=3.7886, v=0.5, heading=-1.0123, frame='map'))
self.path.append(easy_Odom(x=8.0000, y=3.0000, v=0.5, heading=-1.5708, frame='map'))
self.path.append(easy_Odom(x=7.7760, y=2.2114, v=0.5, heading=-2.1293, frame='map'))
self.path.append(easy_Odom(x=7.1708, y=1.6584, v=0.5, heading=-2.6704, frame='map'))
self.path.append(easy_Odom(x=6.3653, y=1.5061, v=0.5, heading= 3.0543, frame='map'))
self.path.append(easy_Odom(x=5.6000, y=1.8000, v=0.5, heading= 2.4958, frame='map'))
# Done with partial CW circle on right -> go back to middle
self.path.append(easy_Odom(x=4.0000, y=3.0000, v=0.5, heading= 2.4958, frame='map'))
# Draw partial CCW circle with 1.5m radius on left
self.path.append(easy_Odom(x=2.4000, y=4.2000, v=0.5, heading= 2.4958, frame='map'))
self.path.append(easy_Odom(x=1.6347, y=4.4939, v=0.5, heading= 3.0543, frame='map'))
self.path.append(easy_Odom(x=0.8292, y=4.3416, v=0.5, heading=-2.6704, frame='map'))
self.path.append(easy_Odom(x=0.2240, y=3.7886, v=0.5, heading=-2.1293, frame='map'))
self.path.append(easy_Odom(x=0.0000, y=3.0000, v=0.5, heading=-1.5708, frame='map'))
self.path.append(easy_Odom(x=0.2240, y=2.2114, v=0.5, heading=-1.0123, frame='map'))
self.path.append(easy_Odom(x=0.8292, y=1.6584, v=0.5, heading=-0.4712, frame='map'))
self.path.append(easy_Odom(x=1.6347, y=1.5061, v=0.5, heading= 0.0873, frame='map'))
self.path.append(easy_Odom(x=2.4000, y=1.8000, v=0.5, heading= 0.6458, frame='map'))
# Done with partial CCW circle on left -> go back to middle
self.path.append(easy_Odom(x=4.0000, y=3.0000, v=0.5, heading= 0.6458, frame='map'))
# Stop
# Figure-8 for ShowCASE Demo
elif triple == 'figure_eight':
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
#back to start
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
elif triple == 'roltatedI':
# start
self.path.append(easy_Odom(x=3.0, y=2.0, v=0.65, heading=pi*3/4.0, frame='map'))
self.path.append(easy_Odom(x=0.5, y=4.5, v=0.35, heading=pi*3/4.0, frame='map'))
self.path.append(easy_Odom(x=0.5, y=5.0, v=0.35, heading=pi*1/4.0, frame='map'))
self.path.append(easy_Odom(x=1.0, y=5.0, v=0.55, heading=pi*-1/4.0, frame='map'))
self.path.append(easy_Odom(x=2.5, y=3.5, v=0.55, heading=pi*-1/4.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.35, heading=pi*-1/4.0, frame='map'))
self.path.append(easy_Odom(x=3.25, y=2.7, v=0.35, heading=pi*1/4.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=3.5, v=0.55, heading=pi*3/4.0, frame='map'))
self.path.append(easy_Odom(x=0.5, y=6.5, v=0.55, heading=pi*3/4.0, frame='map'))
elif triple == 'halfI':
# start
self.path.append(easy_Odom(x=2, y=2, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=1.75, y=3, v=0.5, heading=pi/2, frame='map'))
# out 1
self.path.append(easy_Odom(x=1.75, y=7, v=0.5, heading=pi/2, frame='map'))
# out 2
self.path.append(easy_Odom(x=1.5, y=7.5, v=0.5, heading=pi/2, frame='map'))
# over
self.path.append(easy_Odom(x=2, y=8, v=0.5, heading=0.0, frame='map'))
# turned around
self.path.append(easy_Odom(x=2.5, y=7.5, v=0.5, heading=-pi/2, frame='map'))
# back 1
self.path.append(easy_Odom(x=2.25, y=7, v=0.5, heading=-pi/2, frame='map'))
# back 2
self.path.append(easy_Odom(x=2.25, y=2, v=0.5, heading=-pi/2, frame='map'))
elif triple == 'I':
# start
self.path.append(easy_Odom(x=1.75, y=2, v=0.5, heading=pi/2, frame='map'))
# offset
self.path.append(easy_Odom(x=1.75, y=3, v=0.5, heading=pi/2, frame='map'))
# out 1
self.path.append(easy_Odom(x=1.75, y=13, v=0.5, heading=pi/2, frame='map'))
# out 2
self.path.append(easy_Odom(x=1.5, y=13.5, v=0.5, heading=pi/2, frame='map'))
# over
self.path.append(easy_Odom(x=2, y=14, v=0.5, heading=0.0, frame='map'))
# turned around
self.path.append(easy_Odom(x=2.5, y=13.5, v=0.5, heading=-pi/2, frame='map'))
# back 1
self.path.append(easy_Odom(x=2.25, y=13, v=0.5, heading=-pi/2, frame='map'))
# back 2
self.path.append(easy_Odom(x=2.25, y=2, v=0.5, heading=-pi/2, frame='map'))
# turn 1
self.path.append(easy_Odom(x=2.5, y=1.5, v=0.5, heading=-pi/2, frame='map'))
# turn 2
self.path.append(easy_Odom(x=2, y=1, v=0.5, heading=pi, frame='map'))
# turn 3
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2, frame='map'))
# back to start
self.path.append(easy_Odom(x=1.75, y=2, v=0.5, heading=pi/2, frame='map'))
elif triple == 'III' or triple == 'hamburger' or triple == 'zigzag':
# start
self.path.append(easy_Odom(x=3.25, y=1, v=0.5, heading=pi/2, frame='map'))
# start 2
self.path.append(easy_Odom(x=3.25, y=2, v=0.5, heading=pi/2, frame='map'))
# back and forth
path_width = float(.7)
# top left
self.path.append(easy_Odom(x=3.25, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# bubble
self.path.append(easy_Odom(x=3, y = 12.5, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5, y= 13, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=4, y = 12.5, v=0.5, heading=-pi/2, frame='map'))
# top right
self.path.append(easy_Odom(x=3.75, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# inside out spiral
# center back
self.path.append(easy_Odom(x=3.75, y=3+path_width, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.75, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 1
self.path.append(easy_Odom(x=3.25-path_width, y=3+path_width, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.25-path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 1 top
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 1
self.path.append(easy_Odom(x=3.75+path_width, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.75+path_width, y=3+3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# right 1 bottom
self.path.append(easy_Odom(x=3.5, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 2
self.path.append(easy_Odom(x=3.25-path_width, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=3.25-2*path_width, y=3+3*path_width/2, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.25-2*path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 2 top
self.path.append(easy_Odom(x=3.25-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 2
self.path.append(easy_Odom(x=3.75+2*path_width, y=13-5*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.75+2*path_width, y=2.0, v=0.5, heading=-pi/2, frame='map'))
# garage
self.path.append(easy_Odom(x=4.5, y=1, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=3.0, y=1, v=0.5, heading=pi, frame='map'))
elif triple == 'IxI':
# start
self.path.append(easy_Odom(x=1, y=1, v=0.5, heading=pi/2, frame='map'))
# start 2
self.path.append(easy_Odom(x=1, y=2, v=0.5, heading=pi/2, frame='map'))
# back and forth
path_width = float(.75)
# lateral passes
for i in range(3.0, 11.5, 2*path_width):
self.path.append(easy_Odom(x=2.65, y=i+path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=5, y=i+path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=5+path_width/2, y=i+path_width, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=5, y=i+path_width*3/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=2.65, y=i+path_width*3/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=2.65-path_width/2, y=i+2*path_width, v=0.5, heading=pi/2, frame='map'))
# last across
## self.path.append(easy_Odom(x=2, y=12.5, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# inside out spiral
# center
self.path.append(easy_Odom(x=3.5, y=3+path_width, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width/2, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 1
self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 1 top
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 1
self.path.append(easy_Odom(x=3.5+path_width, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5+path_width, y=3+3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# right 1 bottom
self.path.append(easy_Odom(x=3.5, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 2
self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=2.25, y=3+3*path_width/2, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=2.25, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 2 top
self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 2
self.path.append(easy_Odom(x=3.5+2*path_width, y=13-5*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5+2*path_width, y=3.0, v=0.5, heading=-pi/2, frame='map'))
# garage
self.path.append(easy_Odom(x=4.0, y=2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=3.0, y=2, v=0.5, heading=pi, frame='map'))
elif triple == 'snownado':
# start
self.path.append(easy_Odom(x=2, y=1, v=0.5, heading=pi/2, frame='map'))
# start 2
self.path.append(easy_Odom(x=2, y=2, v=0.5, heading=pi/2, frame='map'))
# back and forth
path_width = float(.75)
# lateral passes
for i in range(3.0, 12.1, path_width):
self.path.append(easy_Odom(x=2.25, y=i-path_width/2, v=0.5, heading=pi/2, frame='map')) # 1
self.path.append(easy_Odom(x=2.25+path_width, y=i+path_width/2, v=0.5, heading=0, frame='map')) # 2
self.path.append(easy_Odom(x=4.75-path_width, y=i+path_width/2, v=0.5, heading=0, frame='map')) # 3
self.path.append(easy_Odom(x=4.75, y=i-path_width/2, v=0.5, heading=-pi/2, frame='map')) # 4
self.path.append(easy_Odom(x=4.75-path_width, y=i-3*path_width/2, v=0.5, heading=pi, frame='map')) # 5
self.path.append(easy_Odom(x=2.25+2*path_width, y=i-3*path_width/2, v=0.5, heading=pi, frame='map')) # 6
# last across
self.path.append(easy_Odom(x=2, y=12.5, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# finish without spiral
self.path.append(easy_Odom(x=3.0, y=2.5, v=0.5, heading=-pi/2, frame='map'))
# # inside out spiral
# # center
# self.path.append(easy_Odom(x=3.5, y=3+path_width, v=0.5, heading=-pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5-path_width/2, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# # left 1
# self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width, v=0.5, heading=pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5-path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# # left 1 top
# self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# # right 1
# self.path.append(easy_Odom(x=3.5+path_width, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5+path_width, y=3+3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# # right 1 bottom
# self.path.append(easy_Odom(x=3.5, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# # left 2
# self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# self.path.append(easy_Odom(x=3.5-2*path_width, y=3+3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5-2*path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# # left 2 top
# self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# # right 2
# self.path.append(easy_Odom(x=3.5+2*path_width, y=13-5*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5+2*path_width, y=3.0, v=0.5, heading=-pi/2, frame='map'))
# # garage
# self.path.append(easy_Odom(x=4.0, y=2, v=0.5, heading=pi, frame='map'))
# self.path.append(easy_Odom(x=3.0, y=2, v=0.5, heading=pi, frame='map'))
elif triple == 'demo':
self.path.append(easy_Odom(x=3.5, y=1, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5, y=12.5, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=4.0, y=13, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=4.5, y=12.5, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=4.5, y = 2, v=0.5, heading=-pi/2, frame='map'))
self.index = 0
self.rolling_index = -1
# ROS publishers, etc. defined on start_server
self.goal = None
self.start = None
self.start_pub = None
self.back = None
self.next = None
self.current = None
self.rolling = None
def __init__(self, triple='simple'):
self.path = []
if triple == 'simple':
# start
self.path.append(easy_Odom(x=0, y=0, v=0.0, heading=0.0, frame='map'))
# out
self.path.append(easy_Odom(x=0.5, y=0, v=0.0, heading=0.0, frame='map'))
elif triple == 'roltatedI':
# start
self.path.append(easy_Odom(x=3.0, y=2.0, v=0.65, heading=pi*3/4.0, frame='map'))
self.path.append(easy_Odom(x=0.5, y=4.5, v=0.35, heading=pi*3/4.0, frame='map'))
self.path.append(easy_Odom(x=0.5, y=5.0, v=0.35, heading=pi*1/4.0, frame='map'))
self.path.append(easy_Odom(x=1.0, y=5.0, v=0.55, heading=pi*-1/4.0, frame='map'))
self.path.append(easy_Odom(x=2.5, y=3.5, v=0.55, heading=pi*-1/4.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.35, heading=pi*-1/4.0, frame='map'))
self.path.append(easy_Odom(x=3.25, y=2.7, v=0.35, heading=pi*1/4.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=3.5, v=0.55, heading=pi*3/4.0, frame='map'))
self.path.append(easy_Odom(x=0.5, y=6.5, v=0.55, heading=pi*3/4.0, frame='map'))
elif triple == 'halfI':
# start
self.path.append(easy_Odom(x=2, y=2, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=1.75, y=3, v=0.5, heading=pi/2, frame='map'))
# out 1
self.path.append(easy_Odom(x=1.75, y=7, v=0.5, heading=pi/2, frame='map'))
# out 2
self.path.append(easy_Odom(x=1.5, y=7.5, v=0.5, heading=pi/2, frame='map'))
# over
self.path.append(easy_Odom(x=2, y=8, v=0.5, heading=0.0, frame='map'))
# turned around
self.path.append(easy_Odom(x=2.5, y=7.5, v=0.5, heading=-pi/2, frame='map'))
# back 1
self.path.append(easy_Odom(x=2.25, y=7, v=0.5, heading=-pi/2, frame='map'))
# back 2
self.path.append(easy_Odom(x=2.25, y=2, v=0.5, heading=-pi/2, frame='map'))
elif triple == 'I':
# start
self.path.append(easy_Odom(x=2, y=2, v=0.5, heading=pi/2, frame='map'))
# offset
self.path.append(easy_Odom(x=1.75, y=3, v=0.5, heading=pi/2, frame='map'))
# out 1
self.path.append(easy_Odom(x=1.75, y=10, v=0.5, heading=pi/2, frame='map'))
# out 2
self.path.append(easy_Odom(x=1.5, y=10.5, v=0.5, heading=pi/2, frame='map'))
# over
self.path.append(easy_Odom(x=2, y=11, v=0.5, heading=0.0, frame='map'))
# turned around
self.path.append(easy_Odom(x=2.5, y=10.5, v=0.5, heading=-pi/2, frame='map'))
# back 1
self.path.append(easy_Odom(x=2.25, y=10, v=0.5, heading=-pi/2, frame='map'))
# back 2
self.path.append(easy_Odom(x=2.25, y=2.5, v=0.5, heading=-pi/2, frame='map'))
elif triple == 'III' or triple == 'hamburger' or triple == 'zigzag':
# start
self.path.append(easy_Odom(x=3.25, y=1, v=0.5, heading=pi/2, frame='map'))
# start 2
self.path.append(easy_Odom(x=3.25, y=2, v=0.5, heading=pi/2, frame='map'))
# back and forth
path_width = float(.7)
# top left
self.path.append(easy_Odom(x=3.25, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# bubble
self.path.append(easy_Odom(x=3, y = 12.5, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5, y= 13, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=4, y = 12.5, v=0.5, heading=-pi/2, frame='map'))
# top right
self.path.append(easy_Odom(x=3.75, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# inside out spiral
# center back
self.path.append(easy_Odom(x=3.75, y=3+path_width, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.75, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 1
self.path.append(easy_Odom(x=3.25-path_width, y=3+path_width, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.25-path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 1 top
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 1
self.path.append(easy_Odom(x=3.75+path_width, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.75+path_width, y=3+3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# right 1 bottom
self.path.append(easy_Odom(x=3.5, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 2
self.path.append(easy_Odom(x=3.25-path_width, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=3.25-2*path_width, y=3+3*path_width/2, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.25-2*path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 2 top
self.path.append(easy_Odom(x=3.25-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 2
self.path.append(easy_Odom(x=3.75+2*path_width, y=13-5*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.75+2*path_width, y=2.0, v=0.5, heading=-pi/2, frame='map'))
# garage
self.path.append(easy_Odom(x=4.5, y=1, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=3.0, y=1, v=0.5, heading=pi, frame='map'))
elif triple == 'IxI':
# start
self.path.append(easy_Odom(x=1, y=1, v=0.5, heading=pi/2, frame='map'))
# start 2
self.path.append(easy_Odom(x=1, y=2, v=0.5, heading=pi/2, frame='map'))
# back and forth
path_width = float(.75)
# lateral passes
for i in range(3.0, 11.5, 2*path_width):
self.path.append(easy_Odom(x=2.65, y=i+path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=5, y=i+path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=5+path_width/2, y=i+path_width, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=5, y=i+path_width*3/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=2.65, y=i+path_width*3/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=2.65-path_width/2, y=i+2*path_width, v=0.5, heading=pi/2, frame='map'))
# last across
## self.path.append(easy_Odom(x=2, y=12.5, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# inside out spiral
# center
self.path.append(easy_Odom(x=3.5, y=3+path_width, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width/2, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 1
self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 1 top
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 1
self.path.append(easy_Odom(x=3.5+path_width, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5+path_width, y=3+3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# right 1 bottom
self.path.append(easy_Odom(x=3.5, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 2
self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=2.25, y=3+3*path_width/2, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=2.25, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 2 top
self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 2
self.path.append(easy_Odom(x=3.5+2*path_width, y=13-5*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5+2*path_width, y=3.0, v=0.5, heading=-pi/2, frame='map'))
# garage
self.path.append(easy_Odom(x=4.0, y=2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=3.0, y=2, v=0.5, heading=pi, frame='map'))
elif triple == 'snownado':
# start
self.path.append(easy_Odom(x=2, y=1, v=0.5, heading=pi/2, frame='map'))
# start 2
self.path.append(easy_Odom(x=2, y=2, v=0.5, heading=pi/2, frame='map'))
# back and forth
path_width = float(.75)
# lateral passes
for i in range(3.0, 12.1, path_width):
self.path.append(easy_Odom(x=2.25, y=i-path_width/2, v=0.5, heading=pi/2, frame='map')) # 1
self.path.append(easy_Odom(x=2.25+path_width, y=i+path_width/2, v=0.5, heading=0, frame='map')) # 2
self.path.append(easy_Odom(x=4.75-path_width, y=i+path_width/2, v=0.5, heading=0, frame='map')) # 3
self.path.append(easy_Odom(x=4.75, y=i-path_width/2, v=0.5, heading=-pi/2, frame='map')) # 4
self.path.append(easy_Odom(x=4.75-path_width, y=i-3*path_width/2, v=0.5, heading=pi, frame='map')) # 5
self.path.append(easy_Odom(x=2.25+2*path_width, y=i-3*path_width/2, v=0.5, heading=pi, frame='map')) # 6
# last across
self.path.append(easy_Odom(x=2, y=12.5, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# finish without spiral
self.path.append(easy_Odom(x=3.0, y=2.5, v=0.5, heading=-pi/2, frame='map'))
# # inside out spiral
# # center
# self.path.append(easy_Odom(x=3.5, y=3+path_width, v=0.5, heading=-pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5-path_width/2, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# # left 1
# self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width, v=0.5, heading=pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5-path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# # left 1 top
# self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# # right 1
# self.path.append(easy_Odom(x=3.5+path_width, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5+path_width, y=3+3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# # right 1 bottom
# self.path.append(easy_Odom(x=3.5, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# # left 2
# self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# self.path.append(easy_Odom(x=3.5-2*path_width, y=3+3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5-2*path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# # left 2 top
# self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# # right 2
# self.path.append(easy_Odom(x=3.5+2*path_width, y=13-5*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5+2*path_width, y=3.0, v=0.5, heading=-pi/2, frame='map'))
# # garage
# self.path.append(easy_Odom(x=4.0, y=2, v=0.5, heading=pi, frame='map'))
# self.path.append(easy_Odom(x=3.0, y=2, v=0.5, heading=pi, frame='map'))
elif triple == 'demo':
self.path.append(easy_Odom(x=3.5, y=1, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5, y=12.5, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=4.0, y=13, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=4.5, y=12.5, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=4.5, y = 2, v=0.5, heading=-pi/2, frame='map'))
self.index = 0
self.rolling_index = -1
# ROS publishers, etc. defined on start_server
self.goal = None
self.start = None
self.start_pub = None
self.back = None
self.next = None
self.current = None
self.rolling = None
# real64 = np.load('/home/buaaren/lidar-sr-dataset/carla64.npy')
# sample = real64[1030]
# low_res_index = range(0, 64, 4)
real128 = np.load('/home/buaaren/lidar-sr-dataset/carla64.npy')
pred128 = np.load('/home/buaaren/lidar-sr/src/prediction64.npy')
pc_index = 4000
pc_real = real128[pc_index + 16]
pc_pred = pred128[pc_index]
pc_real = np.squeeze(pc_real)
pc_pred = np.squeeze(pc_pred)
print(pc_real.shape)
print(pc_pred.shape)
low_res_index = range(0, 64, 4)
print(low_res_index)
# print(sample.shape)
# print(sample)
pc_pred[low_res_index, :] = pc_real[low_res_index, :]
pc_pred[0:48, :] = pc_real[0:48, :]
pc_32 = pc_real[low_res_index, :]
sample = pc_32
# sample = pc_pred
# sample = pc_real
plt.imshow(sample, cmap='plasma')
#plt.show()
# sample = range_2_pointcloud(sample,
# channels = 128,
def apply_textedit(textedit):
return libkak.change(utils.range(textedit['range']), textedit['newText'])
