def _handle_move_relative(
self,
# pylint: disable=C0103 # invalid-name
x: Optional[float] = None,
y: Optional[float] = None,
z: Optional[float] = None,
f: Optional[float] = None) -> None:
""" Handler for the "command:move_relative" event.
Move machine head to specified coordinates. """
distance_mode_save = self.state.gcode_modal.get(b"distance", b"G91")
gcode = "G91 G00 "
if x is not None:
gcode += "X%s " % x
if y is not None:
gcode += "Y%s " % y
if z is not None:
gcode += "Z%s " % z
if f is not None:
gcode += "F%s " % f
self._handle_gcode(Line(gcode).block)
if distance_mode_save != b"G91":
# Restore modal distance_mode.
self._handle_gcode(Line(distance_mode_save.decode()).block)
def test_incoming_gcode_g92p1(self) -> None:
""" Local G92.1 implementation.
G92.1: GCodeResetCoordSystemOffset is not handled by pygcode's VM so we
manually compute the offset. """
# Use G92 to create a diff between machine_pos and work_pos.
fake_event = ("command:gcode", Line("G92 X10 Y20").block)
self.controller._delivered.append(fake_event)
self.controller.update()
self.controller._delivered.clear()
self.assertEqual(len(self.controller.log), 1)
self.assertEqual(self.controller.state.machine_pos,
{"x": 0, "y": 0, "z": 0, "a": 0, "b": 0})
self.assertEqual(self.controller.state.work_pos,
{"x": 10, "y": 20, "z": 0, "a": 0, "b": 0})
# Now G92.1
fake_event = ("command:gcode", Line("G92.1").block)
self.controller._delivered.append(fake_event)
self.controller.update()
self.controller._delivered.clear()
# G92.1 should have reset the diff between machine_pos and work_pos.
self.assertEqual(len(self.controller.log), 2)
self.assertEqual(self.controller.state.machine_pos,
{"x": 0, "y": 0, "z": 0, "a": 0, "b": 0})
self.assertEqual(self.controller.state.work_pos,
{"x": 0, "y": 0, "z": 0, "a": 0, "b": 0})
def test_incoming_gcode_g92(self) -> None:
""" Local G92 implementation.
G92: GCodeCoordSystemOffset is not handled by pygcode's VM so we manually
compute the offset. """
fake_event = ("command:gcode", Line("G92 X10 Y20").block)
self.controller._delivered.append(fake_event)
self.controller.update()
self.controller._delivered.clear()
# G92 should have created a diff between machine_pos and work_pos.
self.assertEqual(len(self.controller.log), 1)
self.assertEqual(self.controller.state.machine_pos,
{"x": 0, "y": 0, "z": 0, "a": 0, "b": 0})
self.assertEqual(self.controller.state.work_pos,
{"x": 10, "y": 20, "z": 0, "a": 0, "b": 0})
# Now move and observe diff between machine_pos and work_pos.
fake_event = ("command:gcode", Line("G0 X10 Y20").block)
self.controller._delivered.append(fake_event)
self.controller.update()
self.controller._delivered.clear()
self.assertEqual(len(self.controller.log), 2)
self.assertEqual(self.controller.state.machine_pos,
{"x": 10, "y": 20, "z": 0, "a": 0, "b": 0})
self.assertEqual(self.controller.state.work_pos,
{"x": 20, "y": 40, "z": 0, "a": 0, "b": 0})
# Further G92's work as expected.
fake_event = ("command:gcode", Line("G92 X0 Y0").block)
self.controller._delivered.append(fake_event)
self.controller.update()
self.controller._delivered.clear()
self.assertEqual(len(self.controller.log), 3)
self.assertEqual(self.controller.state.machine_pos,
{"x": 10, "y": 20, "z": 0, "a": 0, "b": 0})
self.assertEqual(self.controller.state.work_pos,
{"x": 0, "y": 0, "z": 0, "a": 0, "b": 0})
fake_event = ("command:gcode", Line("G0 X0 Y0").block)
self.controller._delivered.append(fake_event)
self.controller.update()
self.controller._delivered.clear()
self.assertEqual(len(self.controller.log), 4)
self.assertEqual(self.controller.state.machine_pos,
{"x": 0, "y": 0, "z": 0, "a": 0, "b": 0})
self.assertEqual(self.controller.state.work_pos,
{"x": -10, "y": -20, "z": 0, "a": 0, "b": 0})
def parse_gcode_file(self, file_name):
vector = []
with open(file_name, 'r') as f:
x_word_pre = Word('X0')
y_word_pre = Word('Y0')
z_word_pre = Word('Z0')
for line_text in f.readlines():
line = Line(line_text)
self.log_display.append("GCODE = {}\n".format(str(line.block)))
x_word = line.block.X
y_word = line.block.Y
z_word = line.block.Z
if x_word is None and y_word is None and z_word is None:
continue
x_word, x_word_pre = self.update_gcode_coord(
x_word, x_word_pre)
y_word, y_word_pre = self.update_gcode_coord(
y_word, y_word_pre)
z_word, z_word_pre = self.update_gcode_coord(
z_word, z_word_pre)
point = [x_word.value, y_word.value, z_word.value]
self.log_display.append('parse point {}'.format(point))
vector.append(GCodePoint(point, str(line.block)))
return vector
def readGcode(self, filepath):
m = Machine()
with open(filepath, 'r') as fh:
for line_text in fh.readlines():
self.traj.append((m.pos.values['X'], m.pos.values['Y']))
line = Line(line_text)
m.process_block(line.block)
def from_string(cls, string: str, *args, **kwargs):
prev_gui_end = kwargs.get('prev_gui_end', Point2())
prev_gcode_end = kwargs.get('prev_gcode_end', Point2())
cnc_lines = [Line(l) for l in string.strip().split('\n')]
assert len(cnc_lines) == 3
line1, line2, line3 = cnc_lines
index = line1.gcodes[0].number
spill = line1.block.modal_params[1].value
speed = line2.gcodes[0].word.value
params = line3.gcodes[0].params
geom_end_point = Point2(params['X'].value, params['Y'].value)
x = geom_end_point.x
y = geom_end_point.y
return cls(index=index,
x=x,
y=y,
speed=speed,
spill=spill,
prev_gui_end=prev_gui_end,
prev_gcode_end=prev_gcode_end)
def _raw_gcode(self, raw_gcode: str) -> bool:
try:
line = Line(str(raw_gcode).strip())
except GCodeWordStrError:
return False
self.publish("command:gcode", line.block)
# print(line.block)
return True
def unpack_contents(filename):
return_list = []
with open(filename, 'r') as r_gcode:
for this_line in r_gcode.readlines():
try:
line = Line(this_line)
return_list.append(str(line))
except:
continue
return return_list
def process_g1(line: str, params: PrintParams):
if "Z" in line:
x = Line(line)
g1: GCodeLinearMove = first(x.gcodes, condition=find_linear_move)
fr: GCodeFeedRate = first(x.gcodes,
condition=find_feed_rate,
default=None)
z_value = g1.params["Z"].value - params.width + 0.001
f_value = fr.word.value if fr and fr.word else 900
return f"G1 Z{z_value:.3f} F{f_value:.3f}\n"
return line
def from_string(cls, string: str):
cnc_lines = [Line(l) for l in string.strip().split('\n')]
assert len(cnc_lines) == 2
line1, line2 = cnc_lines
assert line1.gcodes[0].word_letter == 'N'
assert line2.gcodes[0].word == 'G04'
index = line1.gcodes[0].number
spill = line1.block.modal_params[1].value
delay = line1.block.modal_params[2].value * 1000
return cls(index=index, spill=spill, delay=delay)
def _gcode_parse_line(self,
last_point: np.array,
count: int,
line: str) -> (str, Any, Any, ParsedLine):
section_name = ""
errors = []
point = np.array([np.nan, np.nan, np.nan])
gcode_word_key = None
distance = None
gcode_line = None
enabled = None
expanded = None
try:
gcode_line = Line(line)
except GCodeWordStrError:
errors.append("Invalid gcode")
if gcode_line:
point, point_errors, gcode_word_key = self._gcode_to_point(gcode_line, last_point)
errors += point_errors
distance = self._dist_between_points(last_point, point)
if np.isnan(distance):
distance = None
if isinstance(gcode_line, Line) and not gcode_line.block and gcode_line.comment:
# Only a comment in this line. No other gcode.
comment = str(gcode_line.comment)
if comment.upper().replace(" ", "").startswith("(BLOCK-NAME:"):
new_section_name = comment.split(":", 1)[1].rstrip(")").strip()
section_name = self._increment_name(new_section_name)
elif comment.upper().replace(" ", "").startswith("(BLOCK-ENABLE:"):
value = comment.split(":", 1)[1].rstrip(")").strip()
try:
enabled = bool(int(value))
except ValueError:
enabled = False
elif comment.upper().replace(" ", "").startswith("(BLOCK-EXPAND:"):
value = comment.split(":", 1)[1].rstrip(")").strip()
try:
expanded = bool(int(value))
except ValueError:
expanded = False
metadata = GcodeMetadata(point, distance)
gcode_iteration = GcodeIteration(gcode_line, errors, metadata)
return (section_name,
enabled,
expanded,
ParsedLine(line, gcode_word_key, count, [gcode_iteration]))
def read_line(line_text: str):
'''Parse a single line of GCODE using pygcode.Line
Args:
line: A single line of GCODE
Returns:
A Line object, or None
'''
try:
line = Line(line_text)
return line
except (GCodeWordStrError, AttributeError):
print("Warning: Could not read line!")
def test_incoming_event(self) -> None:
""" Incoming events gets processed by controller. """
fake_event = ("command:gcode", Line("G0 X10 Y20").block)
self.controller._delivered.append(fake_event)
self.controller.update()
self.controller._delivered.clear()
self.assertEqual(len(self.controller.log), 1)
self.assertEqual(self.controller.state.machine_pos,
{"x": 10, "y": 20, "z": 0, "a": 0, "b": 0})
# Further calls to self.controller.update() should have no affect as
# there is no new data.
self.controller.update()
self.assertEqual(len(self.controller.log), 1)
self.assertEqual(self.controller.state.machine_pos,
{"x": 10, "y": 20, "z": 0, "a": 0, "b": 0})
def main(args):
gcodes = []
offset = 0
line_no = 0
line = args.input.readline()
while line:
for gcode in Line(line).block.gcodes:
gcodes.append({'line_no': line_no+1, 'offset':offset, 'line': line, 'gcode':gcode,})
offset = args.input.tell()
line_no+=1
line = args.input.readline()
clearance_height = getClearanceHeight(gcodes)
print("GCode Z clearance_height is {}".format(clearance_height))
if args.command=="list": list(args, gcodes, clearance_height)
if args.command=="cut": cut(args, gcodes, clearance_height)
def test_omit_redundant_modes(self):
lines = [
Line(line_str) for line_str in re.split(
r'\s*\n\s*', '''
g1 x0 y0 ; yes
g1 x10 y-20 ; no
g0 x-3 y2 ; yes
g0 x0 y0 ; no
g0 x1 y1 ; no
g1 x20 y20 z5 ; yes
''') if line_str
]
gcodes = [l.gcodes[0] for l in lines]
comments = [l.comment for l in lines]
for (i, g) in enumerate(omit_redundant_modes(gcodes)):
comment = comments[i].text if comments[i] else None
if comment == 'no':
self.assertIsNotNone(re.search(r'^\s', str(g)))
elif comment == 'yes':
self.assertIsNone(re.search(r'^\s', str(g)))
def parse(self):
# use the machine to populate the all_positions variable with xyz coordinates
t1 = dt.now()
previous_position = []
for i in self.gcode.split('\n'):
line = Line(i)
block = line.block.gcodes
for g in block:
self.machine.process_gcodes(g)
keys = []
for key in self.machine.pos.values.keys():
keys.append(self.machine.pos.values[key])
this_position = self.machine.pos.values
if this_position != previous_position:
self.all_positions.append(this_position)
previous_position = this_position
timer(t1, 'parsing')
self.get_cities()
return self.all_positions
def from_string(cls, string: str, *args, **kwargs):
prev_gui_end = kwargs.get('prev_gui_end', Point2())
prev_gcode_end = kwargs.get('prev_gcode_end', Point2())
cnc_lines = [Line(l) for l in string.strip().split('\n')]
assert len(cnc_lines) == 4
line1, line2, line3, line4 = cnc_lines
index = line1.gcodes[0].number
spill = line1.block.modal_params[1].value
speed = line2.gcodes[0].word.value
params1 = line3.gcodes[0].params
params2 = line4.gcodes[0].params
arc1_end = Point2(float(params1['X'].value), float(params1['Y'].value))
arc1_center = Point2(float(params1['I'].value),
float(params1['J'].value))
arc2_end = Point2(float(params2['X'].value), float(params2['Y'].value))
arc2_center = Point2(float(params2['I'].value),
float(params2['J'].value))
geom_end_point = arc2_end
r = math.sqrt(
pow(geom_end_point.x - arc1_center.x, 2) +
pow(geom_end_point.y - arc1_center.y, 2))
x = geom_end_point.x
y = geom_end_point.y
return cls(index=index,
x=x,
y=y,
r=r,
speed=speed,
spill=spill,
prev_gui_end=prev_gui_end,
prev_gcode_end=prev_gcode_end)
def get_positions(data):
t1 = dt.now()
machine = Machine()
positions = []
last = []
for i in data.split('\n'):
line = Line(i)
block = line.block.gcodes
for g in block:
machine.process_gcodes(g)
keys = []
for key in machine.pos.values.keys():
keys.append(machine.pos.values[key])
#positions.append(keys)
position = machine.pos.values
if position != last:
positions.append(position)
last = position
timer(t1, "processing gcode")
return positions
#print(waypoint)
if waypoint != "":
ax.plot([last_recorder_x,recorder_x], [last_recorder_y,recorder_y], "y", linewidth='1', markersize=1)
pose_list.append(waypoint)
#UR5_pose = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
#UR5_pose.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
#UR5_pose.bind(('', PORT2)) # Bind to the port
#read gcode file
with open('/home/eason/桌面/UR5/dynami_0002.ngc', 'r') as fh:
print('Open file')
for line_text in fh.readlines():
#print line_text,
line = Line(line_text)
#print(line) # will print the line (with cosmetic changes)
line.block.gcodes # is your list of gcodes
line.block.modal_params # are all parameters not assigned to a gcode, assumed to be motion modal parameters
line.block.words
waypoint = ''
if len(line.block.words) != 0:
a = words.words2dict(line.block.words)
if 'G' in a.keys():
if a['G']==0:
if 'X' in a.keys():
offset_x = a.get('X',0)
last_pose_x = offset_x
def processFile(infile, mods, modStartZ):
m = Machine()
outfile = getOutputFilename(infile, mods)
print("Processing {inf} to {outf}".format(inf=infile, outf=outfile))
with open(infile, 'r') as fhIn:
fhOut = open(outfile, 'w')
lineNumber = 0
# Set all initial positions to 0
curE = prevX = prevY = prevZ = prevE = lastZ = float(0)
for line_text in fhIn.readlines():
lineNumber = lineNumber + 1
line = Line(line_text)
if (line.comment is not None):
debug("{line}: {comment}".format(line=lineNumber,
comment=line.comment))
if (line.block.words is not None):
hasX = hasY = hasZ = hasE = False
origX, origY, origZ, origE = float(0)
for word in line.block.words:
isMovement = False
if word.letter == "G" and word.value == 1:
isMovement = True
if (word.letter == "G" & word.value == 92):
prevE = 0 # Simplify3D resets E to 0 for each layer, so if we see a G92 we should consider the current position to be 0
hasX, origX = wordHasLetter(word, "X")
hasY, origY = wordHasLetter(word, "Y")
hasZ, origZ = wordHasLetter(word, "Z")
hasE, origE = wordHasLetter(word, "E")
if hasZ:
lastZ = origZ # save last seen Z
if isMovement:
# start by assuming the new XYZE will be the same as in the file
newX, newY, newE = origX, origY, origE
if (hasZ):
newZ = origZ
else:
# the line didn't specify Z so we have to use the last one we saw
origZ = lastZ
newZ = lastZ
for mod in mods:
adjustRoutine = mod["mod"]
extrudeOnly = mod["extrudeOnly"]
if (newZ >= modStartZ) and hasX and hasY and (
hasE or not extrudeOnly):
newX, newY, newZ, newE = adjustRoutine(
mod, modStartZ, m.abs_pos.X, m.abs_pos.Y,
m.abs_pos.Z, prevE, newX, newY, newZ, newE)
line = replaceOrAddLetter(line, "X", "{:.3f}".format(newX))
line = replaceOrAddLetter(line, "Y", "{:.3f}".format(newY))
line = replaceOrAddLetter(line, "Z", "{:.3f}".format(newZ))
line = replaceOrAddLetter(line, "E", "{:.4f}".format(newE))
#debug("*** X: {X} -> {nX}, Y: {Y} -> {nY}, Z: {Z} -> {nZ}, E: {E} -> {nE}".format(X = origX, Y=origY, Z= origZ, E=origE, nX = newX, nY = newY, nZ = newZ, nE = newE))
if (origZ != newZ) or (not hasZ):
line.comment = "; z {oz} to {nz}".format(oz=origZ,
nz=newZ)
prevX, prevY, prevZ, prevE = newX, newY, newZ, newE
try:
m.process_block(line.block)
except MachineInvalidState:
#debug("here")
continue
finally:
print(str(line), file=fhOut)
def _gcode_append_comment(self, line: Line, comment: str) -> None:
""" Add new comment or append to existing comment. """
line.comment = "%s ; %s" % (line.comment, comment)
def cut(args, gcodes, clearance_height):
non_motion = []
# Select where to cut
m = Machine()
last = m.pos
checkpoint = {'gcode': None, 'pos': m.pos, 'mode': m.mode.distance}
for idx, gcode in enumerate(gcodes):
m.process_gcodes(gcode['gcode'])
if gcode["line_no"] > args.line:
break
if m.pos == last: non_motion.append(gcode)
last = m.pos
if clearance_height == m.pos._value['Z']:
checkpoint = {'idx': idx, 'gcode': gcode, 'pos': m.pos, 'mode': m.mode.distance}
#print("Checkpoint at {}".format(checkpoint))
# If the user wants to cut before the first clearance height, do nothing
if checkpoint['gcode'] is None:
print("The provided line is not a valid one, use list subcommand for more info. Exiting")
return
# Dump the non motion commands first
for gcode in non_motion:
# if not isinstance(gcode['gcode'], GCodeFeedRate): # Ignore feedrates please
print(FORMAT.format(str(gcode['gcode']), str(gcode['line_no']), gcode['gcode'].__class__.__name__), file=args.output)
# Insert the travel until the checkpoint position
gcode = GCodeAbsoluteDistanceMode()
print(FORMAT.format(str(gcode), "Cut comp.", gcode.__class__.__name__), file=args.output)
gcode = GCodeRapidMove(Z=clearance_height)
print(FORMAT.format(str(gcode), "Cut comp.", gcode.__class__.__name__), file=args.output)
gcode = GCodeRapidMove(X=checkpoint['pos']._value['X'], Y=checkpoint['pos']._value['Y'], Z=checkpoint['pos']._value['Z'])
print(FORMAT.format(str(gcode), "Cut comp.", gcode.__class__.__name__), file=args.output)
gcode = checkpoint['mode']
print(FORMAT.format(str(gcode), "Cut comp.", gcode.__class__.__name__), file=args.output)
print(CHECKPOINT_TAG, file=args.output)
# Dump the rest of the codes from the original file (to preserve formatting)
try:
args.input.seek(gcodes[checkpoint['idx']+1]['offset'])
for line in args.input:
print(line, file=args.output, end="")
except: pass
if args.verify:
s = Machine()
t = Machine()
# Run input gcodes until the selected
for gcode in gcodes[:checkpoint['idx']+1]:
s.process_gcodes(gcode['gcode'])
# Run output gcodes until checkpoint
args.output.seek(0)
line = args.output.readline()
while line:
if CHECKPOINT_TAG in line:
break
for gcode in Line(line).block.gcodes:
t.process_gcodes(gcode)
line = args.output.readline()
args.input.seek(gcodes[checkpoint['idx']+1]['offset'])
sline = args.output.readline()
tline = args.input.readline()
while sline and tline:
for gcode in Line(sline).block.gcodes: s.process_gcodes(gcode)
for gcode in Line(tline).block.gcodes: t.process_gcodes(gcode)
if s.pos!=t.pos:
print("\nOutput gcode not verified (source: {}, target: {})".format(s.pos, t.pos))
return
sline = args.output.readline()
tline = args.input.readline()
print("\nOutput gcode verified")