simplify.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rosweb
00004 
00005 from nav_msgs.msg import Path
00006 
00007 import math
00008 import time
00009 
00010 class Simplifier(rosweb.ROSWebSubTopic):
00011   def __init__(self, topic, factory, main_rwt):
00012     rosweb.ROSWebSubTopic.__init__(self, topic, factory, main_rwt)
00013 
00014   def simplify_points (self, pts, tolerance): 
00015       anchor  = 0
00016       floater = len(pts) - 1
00017       stack   = []
00018       keep    = set()
00019 
00020       stack.append((anchor, floater))  
00021       while stack:
00022           anchor, floater = stack.pop()
00023         
00024           # initialize line segment
00025           if pts[floater] != pts[anchor]:
00026               anchorX = float(pts[floater].pose.position.x - pts[anchor].pose.position.x)
00027               anchorY = float(pts[floater].pose.position.y - pts[anchor].pose.position.y)
00028               seg_len = math.sqrt(anchorX ** 2 + anchorY ** 2)
00029               # get the unit vector
00030               anchorX /= seg_len
00031               anchorY /= seg_len
00032           else:
00033               anchorX = anchorY = seg_len = 0.0
00034       
00035           # inner loop:
00036           max_dist = 0.0
00037           farthest = anchor + 1
00038           for i in range(anchor + 1, floater):
00039               dist_to_seg = 0.0
00040               # compare to anchor
00041               vecX = float(pts[i].pose.position.x - pts[anchor].pose.position.x)
00042               vecY = float(pts[i].pose.position.y - pts[anchor].pose.position.y)
00043               seg_len = math.sqrt( vecX ** 2 + vecY ** 2 )
00044               # dot product:
00045               proj = vecX * anchorX + vecY * anchorY
00046               if proj < 0.0:
00047                   dist_to_seg = seg_len
00048               else: 
00049                   # compare to floater
00050                   vecX = float(pts[i].pose.position.x - pts[floater].pose.position.x)
00051                   vecY = float(pts[i].pose.position.y - pts[floater].pose.position.y)
00052                   seg_len = math.sqrt( vecX ** 2 + vecY ** 2 )
00053                   # dot product:
00054                   proj = vecX * (-anchorX) + vecY * (-anchorY)
00055                   if proj < 0.0:
00056                       dist_to_seg = seg_len
00057                   else:  # calculate perpendicular distance to line (pythagorean theorem):
00058                       dist_to_seg = math.sqrt(abs(seg_len ** 2 - proj ** 2))
00059                   if max_dist < dist_to_seg:
00060                       max_dist = dist_to_seg
00061                       farthest = i
00062 
00063           if max_dist <= tolerance: # use line segment
00064               keep.add(anchor)
00065               keep.add(floater)
00066           else:
00067               stack.append((anchor, farthest))
00068               stack.append((farthest, floater))
00069 
00070       keep = list(keep)
00071       keep.sort()
00072       return [pts[i] for i in keep]
00073 
00074   def transform(self, msg):
00075     newmsg = Path()
00076     newmsg.poses = self.simplify_points(msg.poses, 0.05)
00077     return newmsg  


map_tiler
Author(s): Rob Wheeler
autogenerated on Thu Jan 2 2014 11:46:19