class Planner():
def __init__(self, ctrl):
self.ctrl = ctrl
self.cmdq = CommandQueue()
self.logLock = threading.Lock()
self.logIntercept = {}
ctrl.state.add_listener(self._update)
self.reset()
def is_busy(self):
return self.is_running() or self.cmdq.is_active()
def is_running(self):
return self.planner.is_running()
def is_synchronizing(self):
return self.planner.is_synchronizing()
def set_position(self, position):
self.planner.set_position(position)
def get_position(self):
position = {}
for axis in 'xyzabc':
if not self.ctrl.state.is_axis_enabled(axis): continue
value = self.ctrl.state.get(axis + 'p', None)
if value is not None: position[axis] = value
return position
def update_position(self):
self.set_position(self.get_position())
def _get_config_vector(self, name, scale):
state = self.ctrl.state
v = {}
for axis in 'xyzabc':
motor = state.find_motor(axis)
if motor is not None and state.motor_enabled(motor):
value = state.get(str(motor) + name, None)
if value is not None: v[axis] = value * scale
return v
def _get_soft_limit(self, var, default):
limit = self._get_config_vector(var, 1)
for axis in 'xyzabc':
if not axis in limit or not self.ctrl.state.is_axis_homed(axis):
limit[axis] = default
return limit
def get_config(self, mdi, with_limits):
metric = self.ctrl.state.get('metric', True)
config = {
'default-units': 'METRIC' if metric else 'IMPERIAL',
'max-vel': self._get_config_vector('vm', 1000),
'max-accel': self._get_config_vector('am', 1000000),
'max-jerk': self._get_config_vector('jm', 1000000),
# TODO junction deviation & accel
}
if with_limits:
minLimit = self._get_soft_limit('tn', -math.inf)
maxLimit = self._get_soft_limit('tm', math.inf)
# If max <= min then no limit
for axis in 'xyzabc':
if maxLimit[axis] <= minLimit[axis]:
minLimit[axis], maxLimit[axis] = -math.inf, math.inf
config['min-soft-limit'] = minLimit
config['max-soft-limit'] = maxLimit
if not mdi:
program_start = self.ctrl.config.get('program-start')
if program_start: config['program-start'] = program_start
overrides = {}
tool_change = self.ctrl.config.get('tool-change')
if tool_change: overrides['M6'] = tool_change
program_end = self.ctrl.config.get('program-end')
if program_end: overrides['M2'] = program_end
if overrides: config['overrides'] = overrides
log.info('Config:' + json.dumps(config))
return config
def _update(self, update):
if 'id' in update:
id = update['id']
self.planner.set_active(id)
# Synchronize planner variables with execution id
self.cmdq.release(id)
def _get_var_cb(self, name, units):
value = 0
if len(name) and name[0] == '_':
value = self.ctrl.state.get(name[1:], 0)
if units == 'IMPERIAL': value /= 25.4 # Assume metric
log.info('Get: %s=%s (units=%s)' % (name, value, units))
return value
def log_intercept(self, cb):
with self.logLock:
self.logIntercept[threading.get_ident()] = cb
def _log_cb(self, line):
line = line.strip()
m = reLogLine.match(line)
if not m: return
level = m.group('level')
msg = m.group('msg')
filename = m.group('file')
line = m.group('line')
column = m.group('column')
where = ':'.join(filter(None.__ne__, [filename, line, column]))
if line is not None: line = int(line)
if column is not None: column = int(column)
# Per thread log intercept
with self.logLock:
tid = threading.get_ident()
if tid in self.logIntercept:
self.logIntercept[tid](level, msg, filename, line, column)
return
if where: extra = dict(where=where)
else: extra = None
if level == 'I': log.info(msg, extra=extra)
elif level == 'D': log.debug(msg, extra=extra)
elif level == 'W': log.warning(msg, extra=extra)
elif level == 'E': log.error(msg, extra=extra)
elif level == 'C': log.critical(msg, extra=extra)
else: log.error('Could not parse planner log line: ' + line)
def _enqueue_set_cmd(self, id, name, value):
log.info('set(#%d, %s, %s)', id, name, value)
self.cmdq.enqueue(id, self.ctrl.state.set, name, value)
def __encode(self, block):
type, id = block['type'], block['id']
if type != 'set': log.info('Cmd:' + json.dumps(block))
if type == 'line':
return Cmd.line(block['target'], block['exit-vel'],
block['max-accel'], block['max-jerk'],
block['times'], block.get('speeds', []))
if type == 'set':
name, value = block['name'], block['value']
if name == 'message':
self.cmdq.enqueue(id, self.ctrl.msgs.broadcast,
{'message': value})
if name in ['line', 'tool']:
self._enqueue_set_cmd(id, name, value)
if name == 'speed': return Cmd.speed(value)
if len(name) and name[0] == '_':
# Don't queue axis positions, can be triggered by set_position()
if len(name) != 2 or name[1] not in 'xyzabc':
self._enqueue_set_cmd(id, name[1:], value)
if name[0:1] == '_' and name[1:2] in 'xyzabc':
if name[2:] == '_home': return Cmd.set_axis(name[1], value)
if name[2:] == '_homed':
motor = self.ctrl.state.find_motor(name[1])
if motor is not None:
return Cmd.set_sync('%dh' % motor, value)
return
if type == 'input':
# TODO handle timeout
self.planner.synchronize(0) # TODO Fix this
return Cmd.input(block['port'], block['mode'], block['timeout'])
if type == 'output':
return Cmd.output(block['port'], int(float(block['value'])))
if type == 'dwell': return Cmd.dwell(block['seconds'])
if type == 'pause': return Cmd.pause(block['pause-type'])
if type == 'seek':
return Cmd.seek(block['switch'], block['active'], block['error'])
if type == 'end': return '' # Sends id
raise Exception('Unknown planner command "%s"' % type)
def _encode(self, block):
cmd = self.__encode(block)
if cmd is not None:
self.cmdq.enqueue(block['id'], None)
return Cmd.set_sync('id', block['id']) + '\n' + cmd
def reset(self):
if (hasattr(self.ctrl, 'mach')): self.ctrl.mach.stop()
self.planner = gplan.Planner()
self.planner.set_resolver(self._get_var_cb)
self.planner.set_logger(self._log_cb, 1, 'LinePlanner:3')
self.cmdq.clear()
def mdi(self, cmd, with_limits=True):
log.info('MDI:' + cmd)
self.planner.load_string(cmd, self.get_config(True, with_limits))
def load(self, path):
log.info('GCode:' + path)
self.planner.load(path, self.get_config(False, True))
def stop(self):
try:
self.planner.stop()
self.cmdq.clear()
self.update_position()
except Exception as e:
log.exception(e)
self.reset()
def restart(self):
try:
state = self.ctrl.state
id = state.get('id')
position = {}
for axis in 'xyzabc':
if state.has(axis + 'p'):
position[axis] = state.get(axis + 'p')
log.info('Planner restart: %d %s' % (id, json.dumps(position)))
self.planner.restart(id, position)
if self.planner.is_synchronizing():
# TODO pass actual probed position
self.planner.synchronize(1) # Indicate successful probe
except Exception as e:
log.exception(e)
self.reset()
def next(self):
try:
while self.planner.has_more():
cmd = self.planner.next()
cmd = self._encode(cmd)
if cmd is not None: return cmd
except Exception as e:
log.exception(e)
self.reset()
log.info('running: %s active %s' %
(self.is_running(), self.cmdq.is_active()))
class Planner():
def __init__(self, ctrl):
self.ctrl = ctrl
self.log = ctrl.log.get('Planner')
self.cmdq = CommandQueue(ctrl)
self.planner = None
self._position_dirty = False
self.where = ''
ctrl.state.add_listener(self._update)
self.reset(False)
self._report_time()
def is_busy(self):
return self.is_running() or self.cmdq.is_active()
def is_running(self):
return self.planner.is_running()
def position_change(self):
self._position_dirty = True
def _sync_position(self, force=False):
if not force and not self._position_dirty: return
self._position_dirty = False
self.planner.set_position(self.ctrl.state.get_position())
def get_config(self, mdi, with_limits):
state = self.ctrl.state
config = self.ctrl.config
cfg = {
# NOTE Must get current units not configured default units
'default-units': 'METRIC' if state.get('metric') else 'IMPERIAL',
'max-vel': state.get_axis_vector('vm', 1000),
'max-accel': state.get_axis_vector('am', 1000000),
'max-jerk': state.get_axis_vector('jm', 1000000),
'rapid-auto-off': config.get('rapid-auto-off'),
'max-blend-error': config.get('max-deviation'),
'max-merge-error': config.get('max-deviation'),
}
if with_limits:
minLimit = state.get_soft_limit_vector('tn', -math.inf)
maxLimit = state.get_soft_limit_vector('tm', math.inf)
# If max <= min then no limit
for axis in 'xyzabc':
if maxLimit[axis] <= minLimit[axis]:
minLimit[axis], maxLimit[axis] = -math.inf, math.inf
cfg['min-soft-limit'] = minLimit
cfg['max-soft-limit'] = maxLimit
if not mdi:
program_start = config.get('program-start')
if program_start: cfg['program-start'] = program_start
overrides = {}
tool_change = config.get('tool-change')
if tool_change: overrides['M6'] = tool_change
program_end = config.get('program-end')
if program_end: overrides['M2'] = program_end
if overrides: cfg['overrides'] = overrides
self.log.info('Config:' + log_json(cfg))
return cfg
def _update(self, update):
if 'id' in update:
id = update['id']
self.planner.set_active(id) # Release planner commands
self.cmdq.release(id) # Synchronize planner variables
def _get_var_cb(self, name, units):
value = 0
if len(name) and name[0] == '_':
value = self.ctrl.state.get(name[1:], 0)
if units == 'IMPERIAL': value /= 25.4 # Assume metric
self.log.info('Get: %s=%s (units=%s)' % (name, value, units))
return value
def _log_cb(self, line):
line = line.strip()
m = reLogLine.match(line)
if not m: return
level = m.group('level')
msg = m.group('msg')
filename = m.group('file')
line = m.group('line')
column = m.group('column')
where = ':'.join(filter(None.__ne__, [filename, line, column]))
if line is not None: line = int(line)
if column is not None: column = int(column)
if level == 'I': self.log.info(msg, where=where)
elif level == 'D': self.log.debug(msg, where=where)
elif level == 'W': self.log.warning(msg, where=where)
elif level == 'E': self.log.error(msg, where=where)
else: self.log.error('Could not parse planner log line: ' + line)
def _add_message(self, text):
self.ctrl.state.add_message(text)
line = self.ctrl.state.get('line', 0)
if 0 <= line: where = '%s:%d' % (self.where, line)
else: where = self.where
self.log.message(text, where=where)
def _enqueue_set_cmd(self, id, name, value):
self.log.info('set(#%d, %s, %s)', id, name, value)
self.cmdq.enqueue(id, self.ctrl.state.set, name, value)
def _report_time(self):
state = self.ctrl.state.get('xx', '')
if state in ('STOPPING', 'RUNNING') and self.move_start:
delta = time.time() - self.move_start
if self.move_time < delta: delta = self.move_time
plan_time = self.current_plan_time + delta
self.ctrl.state.set('plan_time', round(plan_time))
elif state != 'HOLDING':
self.ctrl.state.set('plan_time', 0)
self.ctrl.ioloop.call_later(1, self._report_time)
def _plan_time_restart(self):
self.plan_time = self.ctrl.state.get('plan_time', 0)
def _update_time(self, plan_time, move_time):
self.current_plan_time = plan_time
self.move_time = move_time
self.move_start = time.time()
def _enqueue_line_time(self, block):
if block.get('first', False) or block.get('seeking', False): return
# Sum move times
move_time = sum(block['times']) / 1000 # To seconds
self.cmdq.enqueue(block['id'], self._update_time, self.plan_time,
move_time)
self.plan_time += move_time
def _enqueue_dwell_time(self, block):
self.cmdq.enqueue(block['id'], self._update_time, self.plan_time,
block['seconds'])
self.plan_time += block['seconds']
def __encode(self, block):
type, id = block['type'], block['id']
if type != 'set': self.log.info('Cmd:' + log_json(block))
if type == 'line':
self._enqueue_line_time(block)
return Cmd.line(block['target'], block['exit-vel'],
block['max-accel'], block['max-jerk'],
block['times'], block.get('speeds', []))
if type == 'set':
name, value = block['name'], block['value']
if name == 'message':
self.cmdq.enqueue(id, self._add_message, value)
if name in ['line', 'tool']: self._enqueue_set_cmd(id, name, value)
if name == 'speed':
self._enqueue_set_cmd(id, name, value)
return Cmd.speed(value)
if len(name) and name[0] == '_':
# Don't queue axis positions, can be triggered by new position
if len(name) != 2 or name[1] not in 'xyzabc':
self._enqueue_set_cmd(id, name[1:], value)
if name == '_feed': # Must come after _enqueue_set_cmd() above
return Cmd.set_sync('if', 1 / value if value else 0)
if name[0:1] == '_' and name[1:2] in 'xyzabc':
if name[2:] == '_home': return Cmd.set_axis(name[1], value)
if name[2:] == '_homed':
motor = self.ctrl.state.find_motor(name[1])
if motor is not None:
return Cmd.set_sync('%dh' % motor, value)
return
if type == 'input':
# TODO handle timeout
self.planner.synchronize(0) # TODO Fix this
return Cmd.input(block['port'], block['mode'], block['timeout'])
if type == 'output':
return Cmd.output(block['port'], int(float(block['value'])))
if type == 'dwell':
self._enqueue_dwell_time(block)
return Cmd.dwell(block['seconds'])
if type == 'pause': return Cmd.pause(block['pause-type'])
if type == 'seek':
sw = self.ctrl.state.get_switch_id(block['switch'])
return Cmd.seek(sw, block['active'], block['error'])
if type == 'end': return '' # Sends id
raise Exception('Unknown planner command "%s"' % type)
def _encode(self, block):
cmd = self.__encode(block)
if cmd is not None:
self.cmdq.enqueue(block['id'], None)
return Cmd.set_sync('id', block['id']) + '\n' + cmd
def reset_times(self):
self.move_start = 0
self.move_time = 0
self.plan_time = 0
self.current_plan_time = 0
def close(self):
# Release planner callbacks
if self.planner is not None:
self.planner.set_resolver(None)
self.planner.set_logger(None)
def reset(self, stop=True):
if stop: self.ctrl.mach.stop()
self.planner = gplan.Planner()
self.planner.set_resolver(self._get_var_cb)
# TODO logger is global and will not work correctly in demo mode
self.planner.set_logger(self._log_cb, 1, 'LinePlanner:3')
self._position_dirty = True
self.cmdq.clear()
self.reset_times()
self.ctrl.state.reset()
def mdi(self, cmd, with_limits=True):
self.where = '<mdi>'
self.log.info('MDI:' + cmd)
self._sync_position()
self.planner.load_string(cmd, self.get_config(True, with_limits))
self.reset_times()
def load(self, path):
self.where = path
path = self.ctrl.get_path('upload', path)
self.log.info('GCode:' + path)
self._sync_position()
self.planner.load(path, self.get_config(False, True))
self.reset_times()
def stop(self):
try:
self.planner.stop()
self.cmdq.clear()
except:
self.log.exception()
self.reset()
def restart(self):
try:
id = self.ctrl.state.get('id')
position = self.ctrl.state.get_position()
self.log.info('Planner restart: %d %s' % (id, log_json(position)))
self.cmdq.clear()
self.cmdq.release(id)
self._plan_time_restart()
self.planner.restart(id, position)
except:
self.log.exception()
self.stop()
def next(self):
try:
while self.planner.has_more():
cmd = self.planner.next()
cmd = self._encode(cmd)
if cmd is not None: return cmd
except RuntimeError as e:
# Pass on the planner message
self.log.error(str(e))
self.stop()
except:
self.log.exception()
self.stop()
class Planner():
def __init__(self, ctrl):
self.ctrl = ctrl
self.cmdq = CommandQueue()
ctrl.state.add_listener(self._update)
self.reset()
def is_busy(self):
return self.is_running() or self.cmdq.is_active()
def is_running(self):
return self.planner.is_running()
def is_synchronizing(self):
return self.planner.is_synchronizing()
def set_position(self, position):
self.planner.set_position(position)
def update_position(self):
position = {}
for axis in 'xyzabc':
if not self.ctrl.state.is_axis_enabled(axis): continue
value = self.ctrl.state.get(axis + 'p', None)
if value is not None: position[axis] = value
self.set_position(position)
def _get_config_vector(self, name, scale):
state = self.ctrl.state
v = {}
for axis in 'xyzabc':
motor = state.find_motor(axis)
if motor is not None and state.motor_enabled(motor):
value = state.get(str(motor) + name, None)
if value is not None: v[axis] = value * scale
return v
def _get_soft_limit(self, var, default):
limit = self._get_config_vector(var, 1)
for axis in 'xyzabc':
if not axis in limit or not self.ctrl.state.is_axis_homed(axis):
limit[axis] = default
return limit
def _get_config(self, mdi, with_limits):
config = {
'max-vel': self._get_config_vector('vm', 1000),
'max-accel': self._get_config_vector('am', 1000000),
'max-jerk': self._get_config_vector('jm', 1000000),
# TODO junction deviation & accel
}
if with_limits:
config['min-soft-limit'] = self._get_soft_limit('tn', -math.inf)
config['max-soft-limit'] = self._get_soft_limit('tm', math.inf)
if not mdi:
program_start = self.ctrl.config.get('program-start')
if program_start: config['program-start'] = program_start
overrides = {}
tool_change = self.ctrl.config.get('tool-change')
if tool_change: overrides['M6'] = tool_change
program_end = self.ctrl.config.get('program-end')
if program_end: overrides['M2'] = program_end
if overrides: config['overrides'] = overrides
log.info('Config:' + json.dumps(config))
return config
def _update(self, update):
if 'id' in update:
id = update['id']
self.planner.set_active(id)
# Synchronize planner variables with execution id
self.cmdq.release(id)
def _get_var_cb(self, name):
value = 0
if len(name) and name[0] == '_':
value = self.ctrl.state.get(name[1:], 0)
log.info('Get: %s=%s' % (name, value))
return value
def _log_cb(self, line):
line = line.strip()
m = reLogLine.match(line)
if not m: return
level = m.group('level')
msg = m.group('msg')
where = m.group('where')
if where is not None: extra = dict(where=where)
else: extra = None
if level == 'I': log.info(msg, extra=extra)
elif level == 'D': log.debug(msg, extra=extra)
elif level == 'W': log.warning(msg, extra=extra)
elif level == 'E': log.error(msg, extra=extra)
elif level == 'C': log.critical(msg, extra=extra)
else: log.error('Could not parse planner log line: ' + line)
def _enqueue_set_cmd(self, id, name, value):
log.info('set(#%d, %s, %s)', id, name, value)
self.cmdq.enqueue(id, True, self.ctrl.state.set, name, value)
def __encode(self, block):
log.info('Cmd:' + json.dumps(block))
type, id = block['type'], block['id']
if type == 'line':
return Cmd.line(block['target'], block['exit-vel'],
block['max-accel'], block['max-jerk'],
block['times'])
if type == 'set':
name, value = block['name'], block['value']
if name == 'message':
self.cmdq.enqueue(id, True, self.ctrl.msgs.broadcast,
{'message': value})
if name in ['line', 'tool']:
self._enqueue_set_cmd(id, name, value)
if name == 'speed': return Cmd.speed(value)
if len(name) and name[0] == '_':
self._enqueue_set_cmd(id, name[1:], value)
if name[0:1] == '_' and name[1:2] in 'xyzabc':
if name[2:] == '_home': return Cmd.set_axis(name[1], value)
if name[2:] == '_homed':
motor = self.ctrl.state.find_motor(name[1])
if motor is not None: return Cmd.set('%dh' % motor, value)
return
if type == 'input':
# TODO handle timeout
self.planner.synchronize(0) # TODO Fix this
return Cmd.input(block['port'], block['mode'], block['timeout'])
if type == 'output':
return Cmd.output(block['port'], int(float(block['value'])))
if type == 'dwell': return Cmd.dwell(block['seconds'])
if type == 'pause': return Cmd.pause(block['pause-type'])
if type == 'seek':
return Cmd.seek(block['switch'], block['active'], block['error'])
raise Exception('Unknown planner command "%s"' % type)
def _encode(self, block):
cmd = self.__encode(block)
if cmd is not None:
self.cmdq.enqueue(block['id'], False, None)
return Cmd.set('id', block['id']) + '\n' + cmd
def reset(self):
self.planner = gplan.Planner()
self.planner.set_resolver(self._get_var_cb)
self.planner.set_logger(self._log_cb, 1, 'LinePlanner:3')
self.cmdq.clear()
def mdi(self, cmd, with_limits=True):
log.info('MDI:' + cmd)
self.planner.load_string(cmd, self._get_config(True, with_limits))
def load(self, path):
log.info('GCode:' + path)
self.planner.load(path, self._get_config(False, True))
def stop(self):
try:
self.planner.stop()
self.cmdq.clear()
self.update_position()
except Exception as e:
log.exception(e)
self.reset()
def restart(self):
try:
state = self.ctrl.state
id = state.get('id')
position = {}
for axis in 'xyzabc':
if state.has(axis + 'p'):
position[axis] = state.get(axis + 'p')
log.info('Planner restart: %d %s' % (id, json.dumps(position)))
self.planner.restart(id, position)
if self.planner.is_synchronizing(): self.planner.synchronize(1)
except Exception as e:
log.exception(e)
self.reset()
def has_move(self):
return self.planner.has_more()
def next(self):
try:
while self.planner.has_more():
cmd = self.planner.next()
cmd = self._encode(cmd)
if cmd is not None: return cmd
except Exception as e:
log.exception(e)
self.reset()