$search

gcode_server.py

Go to the documentation of this file.

#! /usr/bin/env python

import roslib; roslib.load_manifest("pr2_dremel_server")


from math import *

import rospy
import numpy
import tf

import pdb

def arcPoints(center, start, angle, rotaxis, stepSize = 0.01):
  """ Given the center point, the start location on an arc, and the rotation angle and axis
  This function computes points along the arc
  center-  center point of arc [ x, y, z]
  start - start point along arc [x,y,z]
  angle - theta to rotate about axis
  rotaxis - rotational axis
  stepsize = Optional parameter - radian increment of points along the arc
  """
  # bring lists into numpy
  
  c = list(center)
  s = list(start)
  c.extend([0])  
  s.extend([1])
  centerp = numpy.array(c)
  startp = numpy.array(s)
  angle = numpy.array(angle)
  
  #make the step size divide evenly into the angle that must be traversed
  steps = numpy.max(4, numpy.ceil(abs(angle)/stepSize))  
  stepAngle = angle/steps
  points = numpy.ones([steps+1, 4] )
  
  rot= tf.transformations.rotation_matrix(stepAngle, rotaxis)
  points[0] = (startp-centerp)
  
  for i in range(1, len(points) ):
    points[i] = numpy.dot(rot, points[i-1])
  
  points = points+centerp  
  return points[:,0:3]

def within(target, actual, tolerance):
  if (numpy.abs(target-actual) < tolerance):
    return true
  else:
    return false

def buildLineSegment(c, g):
  current = numpy.array(c)
  goal = numpy.array(g)
  dist = numpy.linalg.norm(goal - current)
  num_points = int(floor(dist / 20)) + 1
  inc = (1.0 / num_points) * (goal - current)
  points = []
  for i in range(1, num_points):
    points.append(current + inc * i)

  points.append(goal)
  
  return numpy.array(points)
  

# TODO: ignores cw / ccw distinction, instead it always takes shortest path along arc
def buildArc(current, center, goal, cw):
  p1 = numpy.array(current) - numpy.array(center)
  p2 = numpy.array(goal) - numpy.array(center)
  
  # the distance between current and center should be the same as between goal and center
  dist_current = sqrt(p1[1]*p1[1] + p1[2]*p1[2])
  dist_goal = sqrt(p2[1]*p2[1] + p2[2]*p2[2])
  if (abs(dist_current - dist_goal) > 0.01):
    print "#WARNING: arc is mis-specified"
    return numpy.array([goal])
  
  angle = atan2(p2[2], p2[1]) - atan2(p1[2], p1[1])
  if (angle > pi):
    angle = angle - 2*pi
    
  if (angle < -pi):
    angle = angle + 2*pi
  
  arc = arcPoints(center, current, angle, [1,0,0], numpy.pi/25)
  return arc
   
def loadGCode(path):
  f = open(path)
  lines = f.readlines()
  points = [] #numpy.zeros([lines.__len__(), 3])
  x = None
  y = None
  z = None
  i = None
  j = None
  c = 0
  
  command = 'G01'
  add_point = False
  for line in lines:
    for token in line.split():
      if (token == 'G00'):
        command = 'G01'
      if (token == 'G01'):
        command = 'G01'
      if (token == 'G02'):
        command = 'G02'
      if (token == 'G03'):
        command = 'G03'

      if token[0] == 'X':
        x = float(token[1:])
        add_point = True        
      if token[0] == 'Y':
        y = float(token[1:])
        add_point = True
      if token[0] == 'Z':
        z = float(token[1:])
        add_point = True
      if token[0] == 'I':
        i = float(token[1:])
        add_point = True
      if token[0] == 'J':
        j = float(token[1:])
        add_point = True
        
    if add_point and (x!=None) and (y!=None) and (z!=None):
      #pdb.set_trace()
      goal = [z,x,y]
      if (c == 0):
        current = list(goal)
      else:
        current = points[c-1]

      if command == 'G01':
        pts = buildLineSegment(current, goal)
        for p in pts:
          points.append(p)
          c = c + 1
      if command == 'G02': # assumes I,J are relative to coordinates to current X,Y
        center = [z, current[1]+i, current[2]+j]
        pts = buildArc(current, center, goal, True).tolist()
        for p in pts:
          points.append(p)
          c = c + 1
      if command == 'G03':
        center = [z, current[1]+i, current[2]+j]
        pts = buildArc(current, center, goal, False).tolist()
        for p in pts:
          points.append(p)
          c = c + 1

  # remove last 0's
  #while (pts[-1][1] == 0) and (pts[-1][2] == 0):
  #  pts = pts[0:-2]

  pts = numpy.array(points)
  return pts

def remove_duplicate_x(points):
  pts = []
  prev = points[0]
  approach_x = prev[0]
  for p in points:
    if (p[0] != prev[0]) or (prev[0] != approach_x):
      pts.append(prev)
    prev = p
  return numpy.array(pts)

def remove_duplicates(points):
  pts = []
  prev = numpy.array([-1, -1, -1])
  for p in points:
    if (p != prev).any():
      pts.append(p)
    prev = p
  return numpy.array(pts)

class Segment:
  def __init__(self):
    self.x = []
    self.y = []
  
  def append(self, x, y):
    self.x.append(x)
    self.y.append(y)
    
  def empty(self):
    return (self.x == [])

def points2segments(points):
  segments = []
  segment = Segment()  
  
  approach_x = points[0][0]
  
  for p in points:
    if p[0] == approach_x:
      if not segment.empty():
        segments.append(segment)
      segment = Segment()
    else:
      segment.append(p[1], p[2])
      
  if segment != []:
    segments.append(segment)
    
  return segments

def scale_points(pts, max_x, max_y):
  mx = pts.max(0)
    
  pts_scaled = pts
    
  x_scale = max_x / mx[1] 
  y_scale = max_y / mx[2]  
  
  if (x_scale > y_scale):
    pts_scaled[:,1:] = pts_scaled[:,1:] * y_scale    
  else:
    pts_scaled[:,1:] = pts_scaled[:,1:] * x_scale    
     
  return pts_scaled  

def center_points(pts):
  center = (pts.max(0) + pts.min(0)) / 2
  center[0] = 0 #don't center z
  pts_centered = pts - center
  return pts_centered

def flip_y(pts):
  pts_flipped = pts;
  pts_flipped[:,2] = -pts[:,2]
  return pts_flipped

def handle_parse_gcode(req):  
  flip_y = rospy.get_param("flip_y", False)
  max_x = rospy.get_param("max_x", 0.3)
  max_y = rospy.get_param("max_y", 0.3)
  
  pts = loadGCode(req.path)
  pts = removeDuplicates(pts)
  pts = center_points(pts)
  pts = scale_points(pts, max_x, max_y)        
    
  if flip_y:
    pts = flip_y(pts)
  
  resp = ParseGCodeResponse()
  resp.segments = points2segments(pts)
  
  return resp

#def gcode_server():
#  rospy.init_node("gcode_server")
#  s = rospy.Service("parse_gcode", ParseGCode, handle_parse_gcode)
#  rospy.spin()
#
## Example how to call the parse_gcode service
#def sample_client():
#  path = "/home/duhadway/ros/repos/bosch-ros-pkg/adam/pr2plotter/data/BOSCH.ncd"
#  parse_gcode = rospy.ServiceProxy('parse_gcode', ParseGCode)
#  resp = parse_gcode.call(path)
#  for segment in segments:
#    plt.plot(segment.x, segment.y)
#  
#if __name__ == "__main__":
#  gcode_server()

---

pr2_dremel_server
Author(s): Christian Bersch, Benjamin Pitzer (maintained by Benjamin Pitzer)
autogenerated on Sun Mar 3 12:07:41 2013