Merge pull request #52 from flynneva/devel

added stream gcode function

Author: flynneva

grbl_ros/__init__.py

@@ -28,9 +28,17 @@
 #   Future implementations might include control for other
 #   GCODE compatible systems
 
+from ._logging import MODE
+
 
 class grbl:
 
+    from ._logging import getStatus, decodeStatus, STATUS, MODE
+    from ._configure import getPose, setSpeed, setOrigin, ensureMovementMode, \
+        blockUntilIdle, clearAlarm, enableSteppers, disableSteppers
+    from ._control import home, moveTo, moveRel, moveToOrigin
+    from ._command import startup, shutdown, gcode, stream
+
     def __init__(self):
         """
         Summary line.
@@ -39,6 +47,7 @@ def __init__(self):
 
         """
         # Default parameter values set in startup
+        self.mode = MODE.NORMAL
         self.s = None    # serial port object
         self.abs_move = None     # GRBL has 2 movement modes: relative and absolute
         self.baudrate = 0
@@ -59,9 +68,3 @@ def __init__(self):
         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]
-
-    # Import Methods
-    from ._logging import getStatus, decodeStatus, STATUS
-    from ._configure import getPose, setSpeed, setOrigin, ensureMovementMode, blockUntilIdle, \
-        clearAlarm, enableSteppers, disableSteppers
-    from ._control import home, moveTo, moveRel, moveToOrigin

grbl_ros/_command.py

@@ -60,18 +60,23 @@ def startup(self, port, baud, acc, maxx, maxy, maxz,
     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
-    self.s = serial.Serial(self.port, self.baudrate)
 
-    # set movement to Absolute coordinates
-    self.ensureMovementMode(True)
-    # start homing procedure
-    # TODO(flynneva): should this be done at startup?
-    # should probably be configurable by user
-    # 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()
+    # initiates the serial port
+    try:
+        self.s = serial.Serial(self.port, self.baudrate)
+        # set movement to Absolute coordinates
+        self.ensureMovementMode(True)
+        # start homing procedure
+        # TODO(flynneva): should this be done at startup?
+        # should probably be configurable by user
+        # 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):
@@ -80,6 +85,23 @@ def shutdown(self):
 
 
 def gcode(self, gcode):
-    # TODO(evanflynn): need to add some input checking
-    self.s.write(str.encode(gcode + '\r\n'))
-    return self.decodeStatus(self.s.readline().decode('utf-8').strip())
+    # TODO(evanflynn): need to add some input checking to make sure its valid GCODE
+    if(len(gcode) > 0):
+        if(self.mode == self.MODE.NORMAL):
+            self.s.write(str.encode(gcode + '\r\n'))
+            return self.decodeStatus(self.s.readline().decode('utf-8').strip())
+        elif(self.mode == self.MODE.DEBUG):
+            # in debug mode just return the GCODE that was input
+            return 'Sent: ' + gcode
+    else:
+        return 'Input GCODE was blank'
+
+
+def stream(self, gcode_fpath):
+    f = open(gcode_fpath, 'r')
+
+    for raw_line in f:
+        line = raw_line.strip()  # strip all EOL characters for consistency
+        status = gcode(self, line)
+        if(self.mode == self.MODE.DEBUG):
+            print('    ' + status)

grbl_ros/_logging.py

@@ -24,18 +24,23 @@
 
 def getStatus(self):
     # TODO(evanflynn): status should be ROS msg?
-    self.s.write(b'?\n')
-    time.sleep(0.1)
-    buffer_status = []
-    status = ''
-    while True:
-        if (self.s.inWaiting()):
-            status = self.decodeStatus(self.s.readline().decode('utf-8').strip())
-            buffer_status.append(status)
-            status = ''
-        else:
-            self.s.flushInput()
-            return buffer_status
+    if(self.mode == self.MODE.NORMAL):
+        self.s.write(b'?\n')
+        time.sleep(0.1)
+        buffer_status = []
+        status = ''
+        while True:
+            if (self.s.inWaiting()):
+                status = self.decodeStatus(self.s.readline().decode('utf-8').strip())
+                buffer_status.append(status)
+                status = ''
+            else:
+                self.s.flushInput()
+                return buffer_status
+    elif(self.mode == self.MODE.DEBUG):
+        return 'DEBUG GRBL device is happy!'
+    else:
+        return 'UNDEFINED GRBL MODE'
 
 
 def decodeStatus(self, status):
@@ -44,6 +49,12 @@ def decodeStatus(self, status):
     return status
 
 
+class MODE(Enum):
+    # enum class for operation modes helpful for debugging
+    NORMAL = 0
+    DEBUG = 1
+
+
 class STATUS(Enum):
     # enum class for grbl error list
     # http://domoticx.com/cnc-machine-grbl-error-list/

grbl_ros/grbl_device.py

@@ -36,17 +36,20 @@ class grbl_node(Node):
 
     def __init__(self):
         super().__init__(grbl_node_name)
+        self.get_logger().info('Initializing Publishers & Subscribers')
         self.pub_pos_ = self.create_publisher(Pose, grbl_node_name + '/position', 10)
         self.pub_status_ = self.create_publisher(String, grbl_node_name + '/status', 10)
         self.sub_cmd_ = self.create_subscription(
             String, grbl_node_name + '/send_gcode', self.gcodeCallback, 10)
+        self.sub_stream_ = self.create_subscription(
+            String, grbl_node_name + '/send_gcode_file', self.streamCallback, 10)
         self.sub_pose_ = self.create_subscription(
             Pose, grbl_node_name + '/set_pose', self.poseCallback, 10)
         self.sub_stop_ = self.create_subscription(
             String, grbl_node_name + '/stop', self.stopCallback, 10)
         self.sub_stop_  # prevent unused variable warning
 
-        # declare parameters
+        self.get_logger().info('Declaring ROS parameters')
         self.declare_parameter('port', '/dev/ttyUSB0')
         self.declare_parameter('baudrate', 115200)
         self.declare_parameter('acceleration', 1000)
@@ -61,6 +64,7 @@ def __init__(self):
         self.declare_parameter('y_steps_mm', 100)
         self.declare_parameter('z_steps_mm', 100)
 
+        self.get_logger().info('Setting ROS parameters')
         port = self.get_parameter('port')
         baud = self.get_parameter('baudrate')
         acc = self.get_parameter('acceleration')    # axis acceleration (mm/s^2)
@@ -75,7 +79,9 @@ def __init__(self):
         steps_y = self.get_parameter('y_steps_mm')      # axis steps per mm
         steps_z = self.get_parameter('z_steps_mm')      # axis steps per mm
 
+        self.get_logger().info('Initializing GRBL Device')
         self.grbl_obj = grbl()
+        self.get_logger().info('Starting up GRBL Device...')
         self.grbl_obj.startup(port.get_parameter_value().string_value,
                               baud.get_parameter_value().integer_value,
                               acc.get_parameter_value().integer_value,
@@ -89,9 +95,21 @@ def __init__(self):
                               steps_x.get_parameter_value().integer_value,
                               steps_y.get_parameter_value().integer_value,
                               steps_z.get_parameter_value().integer_value)
-        self.wake()
-        self.refreshStatus()
-        self.refreshPosition()
+        if(self.grbl_obj.s):
+            self.wake()
+            self.refreshStatus()
+            self.refreshPosition()
+            self.get_logger().info('GRBL device ready')
+
+        else:
+            self.get_logger().warn('Could not detect GRBL device '
+                                   'on serial port ' + self.grbl_obj.port)
+            self.get_logger().warn('Are you sure the GRBL device '
+                                   'is connected and powered on?')
+            # 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.grbl_obj.mode = self.grbl_obj.MODE.DEBUG
 
     def wake(self):
         self.grbl_obj.s.write(b'\r\n\r\n')
@@ -100,13 +118,19 @@ def wake(self):
 
     def refreshStatus(self):
         status = self.grbl_obj.getStatus()
-        for stat in status:
-            self.get_logger().info(stat)
-            if ('error' in stat):
-                # TODO(evanflynn): temp code to clear alarm error, should be user decision
-                self.get_logger().warn(self.grbl_obj.clearAlarm())
+        if(self.grbl_obj.mode == self.grbl_obj.MODE.NORMAL):
+            for stat in status:
+                self.get_logger().info(stat)
+                if ('error' in stat):
+                    # TODO(evanflynn): temp code to clear alarm error, should be user decision
+                    self.get_logger().warn(self.grbl_obj.clearAlarm())
+                ros_status = String()
+                ros_status.data = stat
+                self.pub_status_.publish(ros_status)
+        else:
+            self.get_logger().info(status)
             ros_status = String()
-            ros_status.data = stat
+            ros_status.data = status
             self.pub_status_.publish(ros_status)
 
     def refreshPosition(self):
@@ -137,11 +161,22 @@ def stopCallback(self, msg):
         elif msg.data == 'f':
             self.grbl_obj.enableSteppers()
 
+    def streamCallback(self, msg):
+        self.get_logger().info('Sending GCODE file: ')
+        self.get_logger().info('  ' + msg.data)
+        # stream gcode file to grbl device
+        status = self.grbl_obj.stream(msg.data)
+        # TODO(evanflynn): have stream method return something useful
+        self.get_logger().info(status)
+        self.get_logger().info('GCODE file complete!')
+        self.refreshStatus()
+        self.refreshPosition()
+
 
 def main():
     rclpy.init()
-    interface = grbl()
-    rclpy.spin(interface)
+    node = grbl_node()
+    rclpy.spin(node)
 
 
 if __name__ == '__main__':

grbl_ros/node.py

@@ -26,7 +26,7 @@
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'interface = grbl_ros.node:main'
+            'grbl_node = grbl_ros.node:main'
         ],
     },
 )