From afa05d949344b0ed65a731e440666a7eabf65797 Mon Sep 17 00:00:00 2001 From: Evan Flynn Date: Sat, 6 May 2023 17:04:03 -0700 Subject: [PATCH] Migrate grbl_ros to use pydantic Signed-off-by: Evan Flynn --- grbl_ros/__init__.py | 68 ------- grbl_ros/_command.py | 261 ------------------------- grbl_ros/_configure.py | 66 ------- grbl_ros/_control.py | 32 --- grbl_ros/_logging.py | 119 ----------- grbl_ros/device.py | 166 ++++++++-------- grbl_ros/models/__init__.py | 0 grbl_ros/models/command.py | 149 ++++++++++++++ grbl_ros/models/common.py | 60 ++++++ grbl_ros/models/connectors/__init__.py | 0 grbl_ros/models/connectors/uart.py | 64 ++++++ grbl_ros/models/grbl.py | 150 ++++++++++++++ requirements.txt | 1 + setup.cfg | 4 - setup.py | 8 +- 15 files changed, 511 insertions(+), 637 deletions(-) delete mode 100644 grbl_ros/_command.py delete mode 100644 grbl_ros/_configure.py delete mode 100644 grbl_ros/_control.py delete mode 100644 grbl_ros/_logging.py create mode 100644 grbl_ros/models/__init__.py create mode 100644 grbl_ros/models/command.py create mode 100644 grbl_ros/models/common.py create mode 100644 grbl_ros/models/connectors/__init__.py create mode 100644 grbl_ros/models/connectors/uart.py create mode 100644 grbl_ros/models/grbl.py delete mode 100644 setup.cfg diff --git a/grbl_ros/__init__.py b/grbl_ros/__init__.py index cf1b7d4..e69de29 100644 --- a/grbl_ros/__init__.py +++ b/grbl_ros/__init__.py @@ -1,68 +0,0 @@ -# Copyright 2020 Evan Flynn -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - -""" -Functions to initialize the GRBL device. - -This code was ported over to ROS2 from picatostas: -https://github.com/picatostas/cnc_interface -Special thanks to his work and the work before him - -The grbl device class initialized here imports the other control, -command, configure and logging files within this directory and takes -a ROS2 node as an argument in order to seperate out the ROS2 specific -code from the grbl device code. - -""" - -from ._command import command -from ._configure import configure -from ._control import control -from ._logging import logging - - -class grbl(control, command, configure, logging): - """Initializes the base grbl device class.""" - - def __init__(self, node): - self.mode = self.MODE.NORMAL - self.state = self.STATE.ALARM # initalize to alarm state for safety - self.node = node # so we can pass info to ROS - self.s = None # serial port object - self.abs_move = None # GRBL has 2 movement modes: relative and absolute - self.machine_id = 'cnc_000' - self.baudrate = 0 - self.port = '' - self.acceleration = 0 - self.x_max = 0 - self.y_max = 0 - self.z_max = 0 - self.defaultSpeed = 0 - self.x_max_speed = 0 - self.y_max_speed = 0 - self.z_max_speed = 0 - self.x_steps_mm = 0 # number of steps per centimeter - self.y_steps_mm = 0 # number of steps per centimeter - self.z_steps_mm = 0 # number of steps per centimeter - self.idle = True # machine is idle - self.pos = [0.0, 0.0, 0.0] # current position [X, Y, Z] - self.angular = [0.0, 0.0, 0.0, 0.0] # quaterion [X, Y, Z, W] - self.origin = [0.0, 0.0, 0.0] # minimum coordinates [X, Y, Z] - self.limits = [0.0, 0.0, 0.0] # maximum coordinates [X, Y, Z] diff --git a/grbl_ros/_command.py b/grbl_ros/_command.py deleted file mode 100644 index 09c6a54..0000000 --- a/grbl_ros/_command.py +++ /dev/null @@ -1,261 +0,0 @@ -# Copyright 2020 Evan Flynn -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. -""" -Functions to command the GRBL device. - -The grbl device command functions -""" - -from geometry_msgs.msg import Pose -from geometry_msgs.msg import TransformStamped - -from grbl_msgs.msg import State - -import serial - - -class command(object): - """Command class to hold all command functions for the grbl device class.""" - - def startup(self, machine_id, port, baud, acc, maxx, maxy, maxz, - spdf, spdx, spdy, spdz, stepsx, stepsy, stepsz): - """ - Startup the GRBL machine with the specified parameters. - - Args: - ---- - self (obj): the grbl object - machine_id (str): the name of the machine - port (str): the serial port that connects to the grbl device - baud (int): the baud rate to use for serial communication with the grbl device - acc (int): the grbl device axis acceleration (mm/s^2) - maxx (int): workable travel of the grbl device for its x axis (mm) - maxy (int): workable travel of the grbl device for its y axis (mm) - maxz (int): workable travel of the grbl device for tts z axis (mm) - spdf (int): default speed of the grbl device (mm/min) - spdx (int): maximum speed for x axis (mm/min) - spdy (int): maximum speed for y axis (mm/min) - spdz (int): maximum speed for z axis (mm/min) - stepsx (int): number of steps per mm for x axis (steps) - stepsy (int): number of steps per mm for y axis (steps) - stepsz (int): number of steps per mm for z axis (steps) - - """ - # initiate all CNC parameters read from .launch file - self.machine_id = machine_id - self.baudrate = baud - self.port = port - self.acceleration = acc - self.x_max = maxx - self.y_max = maxy - self.z_max = maxz - self.defaultSpeed = spdf - self.x_max_speed = spdx - self.y_max_speed = spdy - self.z_max_speed = spdz - self.x_steps_mm = stepsx - self.y_steps_mm = stepsy - self.z_steps_mm = stepsz - self.limits = [self.x_max, self.y_max, self.z_max] - # initiates the serial port - try: - self.s = serial.Serial(port=self.port, baudrate=self.baudrate) - # set movement to Absolute coordinates - self.ensureMovementMode(True) - # try to get current position - self.send('?') - # start homing procedure - # TODO(flynneva): should this be done at startup? - # should probably be configurable by user if they want to or not - # self.home() - # TODO(flynneva): could cause issues if user is resuming a job - # set the current position as the origin (GRBL sometimes starts with z not 0) - # self.setOrigin() - except serial.serialutil.SerialException: - # Could not detect GRBL device on serial port ' + self.port - # Are you sure the GRBL device is connected and powered on? - return - - def shutdown(self): - """Shutdown the GRBL machine.""" - # TODO(flynneva): probably should do some more serious shutdown logic here - # close the serial connection - self.s.close() - - def send(self, gcode): - """ - Send some specified GCODE to the GRBL machine. - - Args: - ---- - gcode (str): GCODE string to send - - Return: - ------ - str: response of GRBL device to GCODE - - """ - # TODO(evanflynn): need to add some input checking to make sure its valid GCODE - if(len(gcode) > 0): - responses = [] - if(self.mode == self.MODE.NORMAL): - self.s.write(str.encode(gcode + '\n')) - # wait until receive response with EOL character - r = self.s.readline().decode('utf-8').strip() - if(len(r) > 0): - responses.append(r) - # check to see if there are more lines in waiting - while (self.s.inWaiting() > 0): - responses.append(self.s.readline().decode('utf-8').strip()) - self.handle_responses(responses, gcode) - # last response should always be the state of grbl - return responses[-1] - elif(self.mode == self.MODE.DEBUG): - # in debug mode just return the GCODE that was input - return 'Sent: ' + gcode - else: - return 'MISSING_GCODE' - - def handle_responses(self, responses, cmd): - # iterate over each response line - for line in responses: - if(line.find('ok') == -1): - self.node.get_logger().info('[ ' + str(cmd) + ' ] ' + str(line)) - # check if line is grbl status report - if(line[0] == '<'): - self.parse_status(line) - - def parse_status(self, status): - try: - # seperate fields using pipe delimeter | - fields = status.split('|') - # clean first and last field of '<>' - fields[0] = fields[0][1:] - last_field = len(fields) - 1 - fields[last_field] = fields[last_field][:-1] - # for f in fields: - # self.node.get_logger().info(str(f)) - # followed grbl message construction docs - # https://github.com/gnea/grbl/wiki/Grbl-v1.1-Interface#grbl-response-messages - # The first (Machine State) and second (Current Position) data fields - # are always included in every report. - # handle machine state - state_msg = self.handle_state(fields[0]) - # parse current position - self.handle_current_pose(fields[1]) - # publish status - self.node.pub_state_.publish(state_msg) - except IndexError: - self.node.get_logger().warn( - 'Received status from machine does not have required fields') - self.node.get_logger().warn( - 'Status should always include machine state and current position') - self.node.get_logger().warn('Received: %s'.format(str(status))) - - def handle_current_pose(self, pose): - transforms = [] - machine_tf = TransformStamped() - machine_tf.header.frame_id = 'base_link' - machine_tf.header.stamp = self.node.get_clock().now().to_msg() - machine_tf.child_frame_id = self.machine_id + '_machine' - machine_pose = Pose() - - # parse coordinates from current position - coords = pose.split(':')[1].split(',') - x = float(coords[0]) / 1000.0 - y = float(coords[1]) / 1000.0 - z = float(coords[2]) / 1000.0 - - machine_tf.transform.translation.x = x - machine_tf.transform.translation.y = y - machine_tf.transform.translation.z = z - - machine_pose.position.x = x - machine_pose.position.y = y - machine_pose.position.z = z - - transforms.append(machine_tf) - self.node.pub_mpos_.publish(machine_pose) - self.node.pub_tf_.sendTransform(transforms) - return - - def handle_state(self, state): - state_msg = State() - state_msg.header.stamp = self.node.get_clock().now().to_msg() - state_msg.header.frame_id = self.machine_id - # TODO(flynneva): should probably be a switch/case? - if(state.upper() == self.STATE.IDLE.name): - state_msg.state = self.STATE.IDLE - state_msg.state_name = self.STATE.IDLE.name - self.state = self.STATE.IDLE - elif(state.upper() == self.STATE.RUN.name): - state_msg.state = self.STATE.RUN - state_msg.state_name = self.STATE.RUN.name - self.state = self.STATE.RUN - elif(state.upper() == self.STATE.ALARM.name): - state_msg.state = self.STATE.ALARM - state_msg.state_name = self.STATE.ALARM.name - self.state = self.STATE.ALARM - elif(state.upper() == self.STATE.JOG.name): - state_msg.state = self.STATE.JOG - state_msg.state_name = self.STATE.JOG.name - self.state = self.STATE.JOG - elif(state.upper() == self.STATE.HOLD.name): - state_msg.state = self.STATE.HOLD - state_msg.state_name = self.STATE.HOLD.name - self.state = self.STATE.HOLD - elif(state.upper() == self.STATE.DOOR.name): - state_msg.state = self.STATE.DOOR - state_msg.state_name = self.STATE.DOOR.name - self.state = self.STATE.DOOR - elif(state.upper() == self.STATE.CHECK.name): - state_msg.state = self.STATE.CHECK - state_msg.state_name = self.STATE.CHECK.name - self.state = self.STATE.CHECK - elif(state.upper() == self.STATE.HOME.name): - state_msg.state = self.STATE.HOME - state_msg.state_name = self.STATE.HOME.name - self.state = self.STATE.HOME - elif(state.upper() == self.STATE.SLEEP.name): - state_msg.state = self.STATE.SLEEP - state_msg.state_name = self.STATE.SLEEP.name - self.state = self.STATE.SLEEP - return state_msg - - def stream(self, fpath): - """ - Send an entire file of GCODE commands to the GRBL machine. - - Args: - ---- - fpath (str): filepath to GCODE (.nc, .gcode) file to send - - Return: - ------ - str: status of sending the file - - """ - f = open(fpath, 'r') - for raw_line in f: - line = raw_line.strip() # strip all EOL characters for consistency - status = self.send(line) - if(self.mode == self.MODE.DEBUG): - print(' ' + status) - return 'ok' diff --git a/grbl_ros/_configure.py b/grbl_ros/_configure.py deleted file mode 100644 index ea9fe9d..0000000 --- a/grbl_ros/_configure.py +++ /dev/null @@ -1,66 +0,0 @@ -# Copyright 2020 Evan Flynn -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. -""" -Functions to configure the GRBL device. - -The grbl device configure functions -""" - - -class configure(object): - """Configure class to hold all configure functions for the grbl device class.""" - - def setSpeed(self, speed): - self.defaultSpeed = speed - - def setOrigin(self, x=0, y=0, z=0): - # set current position to be (0,0,0), or a custom (x,y,z) - gcode = 'G92 x{} y{} z{}\n'.format(x, y, z) - self.send(self, gcode) - # update our internal location - self.pos = [x, y, z] - - def clearAlarm(self): - """Clear the alarm on the GRBL machine.""" - return self.send(self, r'\$X') - - def enableSteppers(self): - """Enable the motors on the GRBL machine.""" - return self.send(self, 'M17') - - def feedHold(self): - """Feed hold the GRBL machine.""" - return self.send(self, r'\!') - - def disableSteppers(self): - """Disable the motors on the GRBL machine.""" - return self.send(self, 'M17') - - def ensureMovementMode(self, absoluteMode=True): - # GRBL has two movement modes - # if necessary this function tells GRBL to switch modes - if self.abs_move == absoluteMode: - return - self.abs_move = absoluteMode - if absoluteMode: - self.s.write(b'G90\r\n') # absolute movement mode - else: - self.s.write(b'G91\r\n') # relative movement mode - self.s.readline() diff --git a/grbl_ros/_control.py b/grbl_ros/_control.py deleted file mode 100644 index a9b7664..0000000 --- a/grbl_ros/_control.py +++ /dev/null @@ -1,32 +0,0 @@ -# Copyright 2020 Evan Flynn -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. -""" -Functions to control the GRBL device. - -The grbl device control functions -""" - - -class control(object): - """Control class to hold all control functions for the grbl device class.""" - - def home(self): - """Home the GRBL device.""" - self.send(self, '$H') diff --git a/grbl_ros/_logging.py b/grbl_ros/_logging.py deleted file mode 100644 index 8dd4190..0000000 --- a/grbl_ros/_logging.py +++ /dev/null @@ -1,119 +0,0 @@ -# Copyright 2020 Evan Flynn -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. -""" -Functions to handle logging for the GRBL device. - -The grbl device logging functions -""" - -from enum import IntEnum - -from geometry_msgs.msg import Pose - - -class logging(object): - """Logging class to hold all logging functions for the grbl device class.""" - - def getStatus(self): - """Get the current status of the GRBL device.""" - # TODO(evanflynn): status should be ROS msg? - if(self.mode == self.MODE.NORMAL): - # ? returns the active GRBL state & current machine and work positions - return self.send('?') - elif(self.mode == self.MODE.DEBUG): - return 'DEBUG GRBL device is happy!' - else: - return 'UNDEFINED GRBL MODE' - - def getSettings(self): - """Get the current settings of the GRBL device.""" - # TODO(evanflynn): status should be ROS msg? - if(self.mode == self.MODE.NORMAL): - # ? returns the active GRBL state & current machine and work positions - return self.send('$$') - elif(self.mode == self.MODE.DEBUG): - return 'DEBUG GRBL device is happy!' - else: - return 'UNDEFINED GRBL MODE' - - def getPose(self): - """Get the last Pose recorded of the GRBL device.""" - pose = Pose() - pose.position.x = float(self.pos[0]) - pose.position.y = float(self.pos[1]) - pose.position.z = float(self.pos[2]) - # this parameters are set to 0 - # the cnc its a XYZ 3 DOF mechanism and doesnt need it - # TODO(evanflynn): could be useful for higher DOF machines? - pose.orientation.x = float(self.angular[0]) - pose.orientation.y = float(self.angular[1]) - pose.orientation.z = float(self.angular[2]) - pose.orientation.w = float(self.angular[3]) - return pose - - def decodeStatus(self, status): - if('error' in status): - err = status.split(':', 1)[1] - if(type(err) == int()): - return self.STATUS(int(status.split(':', 1)[1])).name - else: - return err - else: - return status - - class MODE(IntEnum): - """Operation modes of the GRBL device, helpful for debugging.""" - - NORMAL = 0 - DEBUG = 1 - - class STATE(IntEnum): - """Operation states of the GRBL device.""" - - IDLE = 0 - RUN = 1 - HOLD = 2 - JOG = 3 - ALARM = 4 - DOOR = 5 - CHECK = 6 - HOME = 7 - SLEEP = 8 - - class STATUS(IntEnum): - """ - Official GRBL error list. - - http://domoticx.com/cnc-machine-grbl-error-list/ - - """ - - NO_ERROR = 0 - MISSING_LETTER = 1 # G-code words consist of a letter and a value. Letter not found - INVALID_NUMERIC = 2 # Numeric value format is not valid or missing an expected value - COMMAND_NOT_SUPPORTED = 3 # Grbl '$' system command was not recognized or supported - NEGATIVE_POSITION = 4 # Negtaive value received for an expected positive value - DISABLED_HOMING = 5 # Homing cycle is not enabled via settings - MIN_STEP_TOO_SMALL = 6 # must be larger than 3usec - EEPROM_READ_FAIL = 7 # EEPROM read failed. Reset and restored to default values - GRBL_NOT_IDLE = 8 # Grbl '$' cmd cannot be used unless Grbl is IDLE - LOCKED_OUT = 9 # G-code locked out during alarm or jog state - HOMING_NOT_ENABLED = 10 # Soft limits cannot be enabled without homing also enabled - MAX_CHARS_PER_LINE = 11 # diff --git a/grbl_ros/device.py b/grbl_ros/device.py index 2d4b5fa..01092d6 100644 --- a/grbl_ros/device.py +++ b/grbl_ros/device.py @@ -25,7 +25,6 @@ from grbl_msgs.action import SendGcodeCmd, SendGcodeFile from grbl_msgs.msg import State from grbl_msgs.srv import Stop -from grbl_ros import grbl import rclpy from rclpy.action import ActionClient, ActionServer @@ -36,6 +35,10 @@ from tf2_ros.transform_broadcaster import TransformBroadcaster +from grbl_ros.models.common import VectorXYZ +from grbl_ros.models.connectors.uart import UART +from grbl_ros.models.grbl import GrblConfig, GrblDevice + class grbl_node(Node): """ @@ -47,115 +50,106 @@ class grbl_node(Node): """ - def __init__(self): + def __init__(self, node_name: str): # TODO(evanflynn): init node with machine_id param input or arg - super().__init__('grbl_device') + super().__init__(node_name) + + # Initialize the GRBL device objects + self.device = GrblDevice() - self.get_logger().info('Declaring ROS parameters') + self.get_logger().info("Declaring ROS parameters") self.declare_parameters( - namespace='', + namespace="", parameters=[ - ('machine_id', 'cnc_001'), - ('port', '/dev/ttyUSB0'), - ('baudrate', 115200), - ('acceleration', 50), # mm / min^2 - ('x_max', 300), # mm - ('y_max', 200), # mm - ('z_max', 150), # mm - ('default_v', 100), # mm / min - ('x_max_v', 150), # mm / min - ('y_max_v', 150), # mm / min - ('z_max_v', 150), # mm / min - ('x_steps', 100), # mm - ('y_steps', 100), # mm - ('z_steps', 100), # mm + ("machine_id", "cnc_001"), + ("port", "/dev/ttyUSB0"), + ("baudrate", 115200), + ("timeout", 5), # seconds + ("acceleration", 50), # mm / min^2 + ("x_max", 300), # mm + ("y_max", 200), # mm + ("z_max", 150), # mm + ("default_v", 100), # mm / min + ("x_max_v", 150), # mm / min + ("y_max_v", 150), # mm / min + ("z_max_v", 150), # mm / min + ("x_steps", 100), # mm + ("y_steps", 100), # mm + ("z_steps", 100), # mm ]) - self.machine_id = self.get_parameter('machine_id').get_parameter_value().string_value + # Update the parameters if any are available + self.get_params() + self.get_logger().info('Initializing Publishers & Subscribers') # Initialize Publishers self.pub_tf_ = TransformBroadcaster(self) - self.pub_mpos_ = self.create_publisher(Pose, self.machine_id + '/machine_position', 5) - self.pub_wpos_ = self.create_publisher(Pose, self.machine_id + '/work_position', 5) - self.pub_state_ = self.create_publisher(State, self.machine_id + '/state', 5) + self.pub_mpos_ = self.create_publisher(Pose, self.device.id + '/machine_position', 5) + self.pub_wpos_ = self.create_publisher(Pose, self.device.id + '/work_position', 5) + self.pub_state_ = self.create_publisher(State, self.device.id + '/state', 5) # Initialize Services self.srv_stop_ = self.create_service( - Stop, self.machine_id + '/stop', self.stopCallback) + Stop, self.device.id + '/stop', self.stopCallback) # Initialize Actions self.action_done_event = Event() self.callback_group = ReentrantCallbackGroup() self.action_send_gcode_ = ActionServer( self, SendGcodeCmd, - self.machine_id + '/send_gcode_cmd', + self.device.id + '/send_gcode_cmd', self.gcodeCallback) self.action_send_gcode_file_ = ActionServer( self, SendGcodeFile, - self.machine_id + '/send_gcode_file', + self.device.id + '/send_gcode_file', self.streamCallback) self.action_client_send_gcode_ = ActionClient( self, SendGcodeCmd, - self.machine_id + '/send_gcode_cmd', callback_group=self.callback_group) + self.device.id + '/send_gcode_cmd', callback_group=self.callback_group) self.action_done_event = Event() - self.get_logger().info('Getting ROS parameters') - port = self.get_parameter('port') - baud = self.get_parameter('baudrate') - acc = self.get_parameter('acceleration') # axis acceleration (mm/s^2) - max_x = self.get_parameter('x_max') # workable travel (mm) - max_y = self.get_parameter('y_max') # workable travel (mm) - max_z = self.get_parameter('x_max') # workable travel (mm) - default_speed = self.get_parameter('default_v') # mm/min - speed_x = self.get_parameter('x_max_v') # mm/min - speed_y = self.get_parameter('y_max_v') # mm/min - speed_z = self.get_parameter('z_max_v') # mm/min - steps_x = self.get_parameter('x_steps') # axis steps per mm - steps_y = self.get_parameter('y_steps') # axis steps per mm - steps_z = self.get_parameter('z_steps') # axis steps per mm - - self.get_logger().warn(' machine_id: ' + str(self.machine_id)) - self.get_logger().warn(' port: ' + str(port.get_parameter_value().string_value)) - self.get_logger().warn(' baudrate: ' + str(baud.get_parameter_value().integer_value)) - - self.get_logger().info('Initializing GRBL Device') - self.machine = grbl(self) + self.get_logger().warn(' machine_id: ' + str(self.device.id)) + self.get_logger().warn(' port: ' + str(self.device.connection.port)) + self.get_logger().warn(' baudrate: ' + str(self.device.connection.baudrate)) + self.get_logger().info('Starting up GRBL Device...') - self.machine.startup(self.machine_id, - port.get_parameter_value().string_value, - baud.get_parameter_value().integer_value, - acc.get_parameter_value().integer_value, - max_x.get_parameter_value().integer_value, - max_y.get_parameter_value().integer_value, - max_z.get_parameter_value().integer_value, - default_speed.get_parameter_value().integer_value, - speed_x.get_parameter_value().integer_value, - speed_y.get_parameter_value().integer_value, - speed_z.get_parameter_value().integer_value, - steps_x.get_parameter_value().integer_value, - steps_y.get_parameter_value().integer_value, - steps_z.get_parameter_value().integer_value) - if(self.machine.s): - self.machine.getStatus() - self.machine.getSettings() - else: - self.get_logger().warn('Could not detect GRBL device ' - 'on serial port ' + self.machine.port) - self.get_logger().warn('Are you sure the GRBL device ' - 'is connected and powered on?') - self.get_logger().warn( - '[ TIP ] Change the serial port and machine ID parameters') - self.get_logger().warn( - '[ TIP ] in the `grbl_ros/config` yaml file and use it at run-time:') - self.get_logger().warn( - '[ TIP ] ros2 run grbl_ros grbl_node ' - '--ros-args --params-file .yaml') - # TODO(evanflynn): set this to a different color so it stands out? - self.get_logger().info('Node running in `debug` mode') - self.get_logger().info('GRBL device operation may not function as expected') - self.machine.mode = self.machine.MODE.DEBUG + self.device.get_status() + self.device.get_settings() + + def get_params(self): + """Get the GRBL parameters and update the members with them.""" + self.get_logger().info('Getting ROS parameters') + self.device.id = self.get_str_param('machine_id') + + connection = UART( + port = self.get_str_param("port"), + baudrate = self.get_int_param("baudrate"), + timeout = self.get_int_param("timeout"), + ) + self.device.connection = connection + + config = GrblConfig( + acceleration = self.get_int_param("acceleration"), + max_travel = VectorXYZ( + x = self.get_int_param("x_max"), + y = self.get_int_param("y_max"), + z = self.get_int_param("z_max"), + ), + default_speed = self.get_int_param("default_v"), + max_speed = VectorXYZ( + x = self.get_int_param("x_max_v"), + y = self.get_int_param("y_max_v"), + z = self.get_int_param("z_max_v"), + ), + steps_per_mm = VectorXYZ( + x = self.get_int_param("x_steps"), + y = self.get_int_param("y_steps"), + z = self.get_int_param("z_steps"), + ) + ) + self.device.config = config def poseCallback(self, request, response): self.machine.moveTo(request.position.x, @@ -271,15 +265,21 @@ def stopCallback(self, request, response): # fire steppers elif request.data == 'f': self.machine.enableSteppers() + + def get_int_param(self, param_name: str) -> int: + return self.get_parameter(param_name).get_parameter_value().integer_value + def get_str_param(self, param_name: str) -> str: + return self.get_parameter(param_name).get_parameter_value().string_value def main(args=None): rclpy.init(args=args) - node = grbl_node() + # TODO(flynneva): make this a CLI arg + node = grbl_node("grbl_device") executor = MultiThreadedExecutor() rclpy.spin(node, executor) rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/grbl_ros/models/__init__.py b/grbl_ros/models/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/grbl_ros/models/command.py b/grbl_ros/models/command.py new file mode 100644 index 0000000..044a2f9 --- /dev/null +++ b/grbl_ros/models/command.py @@ -0,0 +1,149 @@ +# Copyright 2020 Evan Flynn +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +""" +Functions to command the GRBL device. + +The grbl device command functions +""" +# from typing import Optional +# from pydantic import BaseModel, root_validator +# +# from geometry_msgs.msg import Pose +# from geometry_msgs.msg import TransformStamped +# +# from grbl_msgs.msg import State +# +# from models.common import Limits, Coordinate +# import serial +# +# +# +# def handle_responses(self, responses, cmd): +# # iterate over each response line +# for line in responses: +# if(line.find('ok') == -1): +# self.node.get_logger().info('[ ' + str(cmd) + ' ] ' + str(line)) +# # check if line is grbl status report +# if(line[0] == '<'): +# self.parse_status(line) +# +# # for f in fields: +# # self.node.get_logger().info(str(f)) +# # followed grbl message construction docs +# # https://github.com/gnea/grbl/wiki/Grbl-v1.1-Interface#grbl-response-messages +# # The first (Machine State) and second (Current Position) data fields +# # are always included in every report. +# # handle machine state +# state_msg = self.handle_state(fields[0]) +# # parse current position +# self.handle_current_pose(fields[1]) +# # publish status +# self.node.pub_state_.publish(state_msg) +# +# def handle_current_pose(self, pose): +# transforms = [] +# machine_tf = TransformStamped() +# machine_tf.header.frame_id = 'base_link' +# machine_tf.header.stamp = self.node.get_clock().now().to_msg() +# machine_tf.child_frame_id = self.machine_id + '_machine' +# machine_pose = Pose() +# +# # parse coordinates from current position +# coords = pose.split(':')[1].split(',') +# x = float(coords[0]) / 1000.0 +# y = float(coords[1]) / 1000.0 +# z = float(coords[2]) / 1000.0 +# +# machine_tf.transform.translation.x = x +# machine_tf.transform.translation.y = y +# machine_tf.transform.translation.z = z +# +# machine_pose.position.x = x +# machine_pose.position.y = y +# machine_pose.position.z = z +# +# transforms.append(machine_tf) +# self.node.pub_mpos_.publish(machine_pose) +# self.node.pub_tf_.sendTransform(transforms) +# return +# +# def handle_state(self, state): +# state_msg = State() +# state_msg.header.stamp = self.node.get_clock().now().to_msg() +# state_msg.header.frame_id = self.machine_id +# # TODO(flynneva): should probably be a switch/case? +# if(state.upper() == self.STATE.IDLE.name): +# state_msg.state = self.STATE.IDLE +# state_msg.state_name = self.STATE.IDLE.name +# self.state = self.STATE.IDLE +# elif(state.upper() == self.STATE.RUN.name): +# state_msg.state = self.STATE.RUN +# state_msg.state_name = self.STATE.RUN.name +# self.state = self.STATE.RUN +# elif(state.upper() == self.STATE.ALARM.name): +# state_msg.state = self.STATE.ALARM +# state_msg.state_name = self.STATE.ALARM.name +# self.state = self.STATE.ALARM +# elif(state.upper() == self.STATE.JOG.name): +# state_msg.state = self.STATE.JOG +# state_msg.state_name = self.STATE.JOG.name +# self.state = self.STATE.JOG +# elif(state.upper() == self.STATE.HOLD.name): +# state_msg.state = self.STATE.HOLD +# state_msg.state_name = self.STATE.HOLD.name +# self.state = self.STATE.HOLD +# elif(state.upper() == self.STATE.DOOR.name): +# state_msg.state = self.STATE.DOOR +# state_msg.state_name = self.STATE.DOOR.name +# self.state = self.STATE.DOOR +# elif(state.upper() == self.STATE.CHECK.name): +# state_msg.state = self.STATE.CHECK +# state_msg.state_name = self.STATE.CHECK.name +# self.state = self.STATE.CHECK +# elif(state.upper() == self.STATE.HOME.name): +# state_msg.state = self.STATE.HOME +# state_msg.state_name = self.STATE.HOME.name +# self.state = self.STATE.HOME +# elif(state.upper() == self.STATE.SLEEP.name): +# state_msg.state = self.STATE.SLEEP +# state_msg.state_name = self.STATE.SLEEP.name +# self.state = self.STATE.SLEEP +# return state_msg +# +# def stream(self, fpath): +# """ +# Send an entire file of GCODE commands to the GRBL machine. +# +# Args: +# ---- +# fpath (str): filepath to GCODE (.nc, .gcode) file to send +# +# Return: +# ------ +# str: status of sending the file +# +# """ +# f = open(fpath, 'r') +# for raw_line in f: +# line = raw_line.strip() # strip all EOL characters for consistency +# status = self.send(line) +# if(self.mode == self.MODE.DEBUG): +# print(' ' + status) +# return 'ok' diff --git a/grbl_ros/models/common.py b/grbl_ros/models/common.py new file mode 100644 index 0000000..d3e2296 --- /dev/null +++ b/grbl_ros/models/common.py @@ -0,0 +1,60 @@ +from enum import IntEnum + +from pydantic import BaseModel + + +class Limits(BaseModel): + x_max: int = 0 + y_max: int = 0 + z_max: int = 0 + + +class VectorXYZ(BaseModel): + x: int = 0 + y: int = 0 + z: int = 0 + + +class Quaternion(BaseModel): + x: int = 0 + y: int = 0 + z: int = 0 + w: int = 0 + + +class OperationState(IntEnum): + """Operation states of the GRBL device.""" + IDLE = 0 + RUN = 1 + HOLD = 2 + JOG = 3 + ALARM = 4 + DOOR = 5 + CHECK = 6 + HOME = 7 + SLEEP = 8 + + +class MovementMode(IntEnum): + """GRBL has 2 move modes: relative and absolute.""" + RELATIVE = 0 + ABSOLUTE = 1 + + +class GrblError(IntEnum): + """ + Official GRBL error list. + http://domoticx.com/cnc-machine-grbl-error-list/ + """ + NO_ERROR = 0 + MISSING_LETTER = 1 # G-code words consist of a letter and a value. Letter not found + INVALID_NUMERIC = 2 # Numeric value format is not valid or missing an expected value + COMMAND_NOT_SUPPORTED = 3 # Grbl '$' system command was not recognized or supported + NEGATIVE_POSITION = 4 # Negtaive value received for an expected positive value + DISABLED_HOMING = 5 # Homing cycle is not enabled via settings + MIN_STEP_TOO_SMALL = 6 # must be larger than 3usec + EEPROM_READ_FAIL = 7 # EEPROM read failed. Reset and restored to default values + GRBL_NOT_IDLE = 8 # Grbl '$' cmd cannot be used unless Grbl is IDLE + LOCKED_OUT = 9 # G-code locked out during alarm or jog state + HOMING_NOT_ENABLED = 10 # Soft limits cannot be enabled without homing also enabled + MAX_CHARS_PER_LINE = 11 # diff --git a/grbl_ros/models/connectors/__init__.py b/grbl_ros/models/connectors/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/grbl_ros/models/connectors/uart.py b/grbl_ros/models/connectors/uart.py new file mode 100644 index 0000000..668e3a6 --- /dev/null +++ b/grbl_ros/models/connectors/uart.py @@ -0,0 +1,64 @@ +from serial import Serial +from serial.serialutil import SerialException +import sys +from time import sleep +from typing import List, Optional + +from pydantic import BaseModel, root_validator + + +class UART(BaseModel): + """UART connector to a GRBL device.""" + baudrate: int + port: str + timeout: int = 5 # Timeout in seconds + connection: Optional[Serial] + + class Config: + arbitrary_types_allowed = True + + @root_validator(pre=True) + def validate_root(cls, values): + port, baudrate = values.get("port"), values.get("baudrate") + if port and baudrate: + values["connection"] = Serial(port, baudrate) + return values + + def __del__(self): + self.connection.close() + + def connect(self): + """Connect to the GRBL device.""" + try: + self.conection = Serial(self.port, self.baudrate, timeout=self.timeout) + except SerialException: + # TODO(flynneva): log something here + sys.exit(1) + + def read(self) -> List[str]: + """Read all data available from the GRBL device via UART.""" + responses = [] + # wait until receive response with EOL character + resp = self.connection.readline().decode('utf-8').strip() + if(len(resp) > 0): + responses.append(resp) + # check to see if there are more lines in waiting + while (self.connection.inWaiting() > 0): + responses.append(self.read()) + return responses + + def send(self, data: str): + """ + Transmit data to the GRBL device. + + Appends an end of line character (\\n) to the provided string. + """ + self.connection.write(str.encode(data + "\n")) + + def send_and_read_response(self, data: str) -> List[str]: + """Transmit data to the GRBL device and read the response.""" + self.send(data) + sleep(0.05) # give some time for GRBL device + return self.read() + + diff --git a/grbl_ros/models/grbl.py b/grbl_ros/models/grbl.py new file mode 100644 index 0000000..85e117c --- /dev/null +++ b/grbl_ros/models/grbl.py @@ -0,0 +1,150 @@ +# Copyright 2020 Evan Flynn +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +""" +Functions to initialize the GRBL device. + +This code was ported over to ROS2 from picatostas: +https://github.com/picatostas/cnc_interface +Special thanks to his work and the work before him + +The grbl device class initialized here imports the other control, +command, configure and logging files within this directory and takes +a ROS2 node as an argument in order to seperate out the ROS2 specific +code from the grbl device code. + +""" +from typing import List, Optional +from pydantic import BaseModel + +from grbl_ros.models.common import ( + GrblError, + Limits, + MovementMode, + OperationState, + VectorXYZ, + Quaternion +) +from grbl_ros.models.connectors.uart import UART + + +class GrblConfig(BaseModel): + acceleration: int # the grbl device axis acceleration (mm/s^2) + max_travel: VectorXYZ # workable travel of the grbl device for each axis (mm) + default_speed: int # default speed of the grbl device (mm/min) + max_speed: VectorXYZ # maximum speed for each axis (mm/min) + steps_per_mm: VectorXYZ # number of steps per mm + limits: Limits # + + +class GrblDevice(BaseModel): + id: str = "cnc_000" + op_state: OperationState = OperationState.ALARM # initalize to alarm state for safety + movement_mode: MovementMode = MovementMode.RELATIVE + connection: Optional[UART] # serial port connection + position: VectorXYZ = VectorXYZ() # the current position of the device [X, Y, Z] + angular: Quaternion = Quaternion() # quaterion [X, Y, Z, W] + origin: VectorXYZ = VectorXYZ() # origin [X, Y, Z] + config: Optional[GrblConfig] + + def home(self): + """Home the GRBL device.""" + self.connection.send("$H") + + def is_idle(self) -> bool: + """Returns true if machine is idle.""" + if self.op_state == OperationState.IDLE: + return True + return False + + def get_status(self) -> List[str]: + """Get the current status of the GRBL device.""" + # ? returns the active GRBL state & current machine and work positions + return self.connection.send_and_read_response("?") + + def get_settings(self): + """Get the current settings of the GRBL device.""" + # $$ returns the current GRBL settings + return self.connection.send_and_read_response("$$") + + def decode_grbl_error(self, grbl_err_str: str) -> str: + if("error" in grbl_err_str): + err = grbl_err_str.split(":", 1)[1] + if(type(err) == int()): + return GrblError((int(err))).name + else: + return err + else: + return grbl_err_str + + def set_speed(self, speed): + self.defaultSpeed = speed + + def set_origin(self, new_origin: VectorXYZ): + """Set the origin of the device.""" + # set current position to be (0,0,0), or a custom (x,y,z) + gcode = "G92 x{} y{} z{}\n".format( + new_origin.x, + new_origin.y, + new_origin.z) + self.connection.send(self, gcode) + + def clear_alarm(self): + """Clear the alarm on the GRBL machine.""" + return self.connection.send(self, r"\$X") + + def enable_steppers(self): + """Enable the motors on the GRBL machine.""" + return self.connection.send(self, "M17") + + def disable_steppers(self): + """Disable the motors on the GRBL machine.""" + return self.connection.send(self, "M17") + + def feed_hold(self): + """Feed hold the GRBL machine.""" + return self.connection.send(self, r"\!") + + def ensure_movement_mode(self, absolute_mode=True): + """Ensure GRBL device is in the expected movement mode.""" + if self.abs_move == absolute_mode: + return + if absolute_mode: + self.connection.send("G90") # absolute movement mode + else: + self.connection.send("G91") # relative movement mode + self.abs_move = absolute_mode + return self.connection.read() + + def send_gcode(self, gcode: str) -> str: + """Send some specified GCODE to the GRBL machine.""" + # TODO(evanflynn): need to add some input checking to make sure its valid GCODE + return self.connection.send_and_read_response(gcode) + + def parse_status_fields(self, status: str) -> List[str]: + if "|" not in status: + # No fields to parse + return [] + # seperate fields using pipe delimeter | + fields = status.split('|') + # clean first and last field of '<>' + fields[0] = fields[0][1:] + fields[-1] = fields[-1][:-1] + return fields diff --git a/requirements.txt b/requirements.txt index 205196f..868125a 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1 +1,2 @@ pyserial==3.4 +pydantic==1.10.7 \ No newline at end of file diff --git a/setup.cfg b/setup.cfg deleted file mode 100644 index 3475195..0000000 --- a/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script-dir=$base/lib/grbl_ros -[install] -install-scripts=$base/lib/grbl_ros diff --git a/setup.py b/setup.py index dded705..03cf320 100644 --- a/setup.py +++ b/setup.py @@ -1,14 +1,14 @@ from glob import glob import os -import setuptools +from setuptools import find_packages, setup package_name = 'grbl_ros' -setuptools.setup( +setup( name=package_name, - version='0.0.16', - packages=[package_name], + version='0.1.0', + packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]),