grid_creator.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 Copyright (c) 2016, Borella Jocelyn, Karrenbauer Oliver, Meissner Pascal
5 All rights reserved.
6 
7 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
8 
9 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
10 
11 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
12 
13 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
14 
15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
16 '''
17 
18 import sys
19 import rospy
20 import subprocess, os, signal
21 import xml.etree.cElementTree as ET
22 import math
23 import rospkg
24 import datetime
25 
26 
27 from asr_robot_model_services.srv import GetDistance, IsPositionAllowed
28 from geometry_msgs.msg import Point
29 from std_msgs.msg import ColorRGBA
30 from visualization_msgs.msg import Marker, MarkerArray
31 
32 
33 class GridCreator():
34 
35  def __init__(self):
36  rospy.init_node('grid_creator', anonymous=True)
37  self.Xmax = rospy.get_param("width")
38  self.Ymax = rospy.get_param("height")
39  self.GridType = rospy.get_param("GridType")
40  ExitPath = rospy.get_param("ExitXML")
41 
42  StepSizeMin = rospy.get_param("StepSizeMin")
43  StepSizeMax = rospy.get_param("StepSizeMax")
44  StepSizeStep = rospy.get_param("StepSizeStep")
45 
46  HtransMin = rospy.get_param("HtransMin")
47  HtransMax = rospy.get_param("HtransMax")
48  HtransStep = rospy.get_param("HtransStep")
49 
50  VtransMin = rospy.get_param("VtransMin")
51  VtransMax = rospy.get_param("VtransMax")
52  VtransStep = rospy.get_param("VtransStep")
53 
54  OffsetMin = rospy.get_param("OffsetMin")
55  OffsetMax = rospy.get_param("OffsetMax")
56  OffsetStep = rospy.get_param("OffsetStep")
57 
58 
59  bestPoseListe = []
60  bestMarkerArray = MarkerArray()
61  bestK = 0
62 
63  bestStepSize = 0.0
64  bestHtrans = 0.0
65  bestVtrans = 0.0
66  bestOffset = 0.0
67 
68  rospy.loginfo("begin of grid creations")
69  for currentStepSize in self.frange(StepSizeMin, StepSizeMax, StepSizeStep):
70  self.StepSize = currentStepSize
71  for currentHtrans in self.frange(HtransMin, HtransMax, HtransStep):
72  self.Htrans = currentHtrans
73  for currentVtrans in self.frange(VtransMin, VtransMax, VtransStep):
74  self.Vtrans = currentVtrans
75  for currentOffset in self.frange(OffsetMin, OffsetMax, OffsetStep):
76  #beginn = rospy.get_time()
77  self.Offset = currentOffset
78 
79  self.PoseListe = []
80  self.markerArray = MarkerArray()
81  self.k = 0 #marker id
82  self.createGrid()
83  if len(self.PoseListe) >= len(bestPoseListe):
84  bestPoseListe = self.PoseListe
85  bestMarkerArray = self.markerArray
86  bestK = self.k
87  bestStepSize = self.StepSize
88  bestHtrans = self.Htrans
89  bestVtrans = self.Vtrans
90  bestOffset = self.Offset
91  #rospy.loginfo('Iteration took ' + str(rospy.get_time() - beginn)+ 's to finish')
92 
93  rospy.loginfo("end of grid creations with number of best poses: " + str(len(bestPoseListe)))
94 
95  rospy.loginfo("bestStepSize: " + str(bestStepSize))
96  rospy.loginfo("bestHtrans: " + str(bestHtrans))
97  rospy.loginfo("bestVtrans: " + str(bestVtrans))
98  rospy.loginfo("bestOffset: " + str(bestOffset))
99 
100  #rospy.loginfo("Grid created")
101 
102  #rospy.loginfo("Begin nearest Neighbour")
103  #Nearest Neighbour
104  orderedPoseListe = self.nearest_neighbour(bestPoseListe)
105  #rospy.loginfo("End nearest Neighbour")
106 
107  #rospy.loginfo("Begin 2-Opt")
108  #2-opt algorithm
109  if rospy.get_param("HamiltonCycle") == True:
110  orderedPoseListe = self.two_opt_with_cycle(orderedPoseListe)
111  else:
112  orderedPoseListe = self.two_opt_without_cycle(orderedPoseListe)
113  #rospy.loginfo("End 2-Opt")
114 
115  #Save the ordered pose list in the defined xml file
116  root = ET.Element("Posen")
117  for elem in orderedPoseListe:
118  ET.SubElement(root, "Pose", X=str(elem.x), Y=str(elem.y), Z="0", QX="0", QY="0", QZ="0", QW="0")
119  tree = ET.ElementTree(root)
120  tree.write(ExitPath)
121  rospy.loginfo("config.xml created")
122 
123  text = 1
124  cpoints = []
125  for child in orderedPoseListe:
126  cpoints.append(Point(*[float(child.x),float(child.y),0]))
127 
128  b=0
129  for point in cpoints:
130  marker = self.getMarker(point,bestK,'GridPoints',0,0.7,1)
131  bestMarkerArray.markers.append(marker)
132  bestK += 1
133  b += 0.01
134  marker2 = self.getMarker(point,bestK,'NumPoints',1,0,0)
135  marker2.type = 9
136  marker2.scale.z = 0.5
137  marker2.text = str(text)
138  bestMarkerArray.markers.append(marker2)
139  bestK += 1
140  text += 1
141 
142 
143  p=0
144  for j in range(len(cpoints)-1):
145  points = []
146  points.append(Point(*[float(cpoints[j].x),float(cpoints[j].y),0]))
147  points.append(Point(*[float(cpoints[j+1].x),float(cpoints[j+1].y),0]))
148  marker = self.getMarkerArrow(points,bestK,"Arrows",p,1-p,0)
149  bestMarkerArray.markers.append(marker)
150  bestK += 1
151  p += 0.02
152 
153  pub = rospy.Publisher('direct_search/grid_visualization', MarkerArray, queue_size=100)
154  rate = rospy.Rate(10) # 10hz
155 
156  while not rospy.is_shutdown():
157  pub.publish(bestMarkerArray)
158  rate.sleep()
159 
160 
161  def createGrid(self):
162  #rospy.loginfo("Begin from grid creation")
163  #Quad grid creation
164  if self.GridType == "Quad":
165  i = 0
166  while i<self.Xmax:
167  j=0
168  test = 0 # 0 is not reachable, 1 is reachable
169  while j< self.Ymax:
170  TempPose = self.transformPoint(Point(*[i,j,0]),self.Offset,self.Htrans,self.Vtrans)
171  if self.isRobotPositionAllowed(TempPose):
172  self.PoseListe.append(TempPose)
173  test = 1
174  else:
175  marker = self.getMarker(TempPose,self.k,'UnreachablePoints',1,0.5,0)
176  self.markerArray.markers.append(marker)
177  self.k += 1
178  test = 0
179 
180  if test == 1:
181  marker = self.drawQuad(i,j,self.Offset,self.Htrans,self.Vtrans,self.StepSize,0,0,1,self.k,"QuadDrawReach")
182  self.k+=1
183  markerColor = self.colorQuad(i,j,self.Offset,self.Htrans,self.Vtrans,self.StepSize,0,0,1,self.k,"ColorQuadReach")
184  self.markerArray.markers.append(markerColor)
185  else:
186  marker = self.drawQuad(i,j,self.Offset,self.Htrans,self.Vtrans,self.StepSize,1,0,0,self.k,"QuadDrawUnreach")
187  marker.color.a = 0.2
188  self.k+=1
189  markerColor = self.colorQuad(i,j,self.Offset,self.Htrans,self.Vtrans,self.StepSize,1,0,0,self.k,"ColorQuadUnreach")
190  self.markerArray.markers.append(markerColor)
191 
192  self.markerArray.markers.append(marker)
193  self.k+=1
194  j+=self.StepSize
195  i+=self.StepSize
196 
197 
198  #Hexagonal grid creation
199  #The hexagonal grid is created in 2 steps; Each step create one out of two line of hexagon, because of the offset
200  elif self.GridType == "Hex":
201  i=0
202  while i < self.Xmax:
203  j=0
204  test = 0 # 0 is not reachable, 1 is reachable
205  while j < self.Ymax:
206  TempPose = self.transformPoint(Point(*[i,j,0]),self.Offset,self.Htrans,self.Vtrans)
207  if self.isRobotPositionAllowed(TempPose):
208  self.PoseListe.append(TempPose)
209  test = 1
210  else:
211  marker = self.getMarker(TempPose,self.k,'UnreachablePoints',1,0.5,0)
212  self.markerArray.markers.append(marker)
213  self.k += 1
214  test = 0
215 
216  if test == 1 :
217  marker = self.drawHexagon(i,j,self.Offset,self.Htrans,self.Vtrans,self.StepSize,0,0,1,self.k,"HexDrawReach")
218  self.k+=1
219  markerColor = self.colorHexagon(i,j,self.Offset,self.Htrans,self.Vtrans,self.StepSize,0,0,1,self.k,"ColorHexReach")
220  self.markerArray.markers.append(markerColor)
221  else:
222  marker = self.drawHexagon(i,j,self.Offset,self.Htrans,self.Vtrans,self.StepSize,1,0,0,self.k,"HexDrawUnreach")
223  marker.color.a = 0.2
224  self.k+=1
225  markerColor = self.colorHexagon(i,j,self.Offset,self.Htrans,self.Vtrans,self.StepSize,1,0,0,self.k,"ColorHexUnreach")
226  self.markerArray.markers.append(markerColor)
227  self.markerArray.markers.append(marker)
228  self.k += 1
229 
230  j += 3*self.StepSize
231  i+= math.sqrt(3)*self.StepSize
232 
233  i = math.sqrt(3)*self.StepSize/2
234  while i < self.Xmax:
235  test = 0 # 0 is not reachable, 1 is reachable
236  j = 1.5*self.StepSize
237  while j < self.Ymax:
238  TempPose = self.transformPoint(Point(*[i,j,0]),self.Offset,self.Htrans,self.Vtrans)
239  if self.isRobotPositionAllowed(TempPose):
240  self.PoseListe.append(TempPose)
241  test = 1
242  else:
243  marker = self.getMarker(TempPose,self.k,'UnreachablePoints',1,0.5,0)
244  self.markerArray.markers.append(marker)
245  self.k += 1
246  test = 0
247 
248  if test == 1 :
249  marker = self.drawHexagon(i,j,self.Offset,self.Htrans,self.Vtrans,self.StepSize,0,0,1,self.k,"HexDrawReach")
250  self.k+=1
251  markerColor = self.colorHexagon(i,j,self.Offset,self.Htrans,self.Vtrans,self.StepSize,0,0,1,self.k,"ColorHexReach")
252  self.markerArray.markers.append(markerColor)
253  else:
254  marker = self.drawHexagon(i,j,self.Offset,self.Htrans,self.Vtrans,self.StepSize,1,0,0,self.k,"HexDrawUnreach")
255  marker.color.a = 0.2
256  self.k+=1
257  markerColor = self.colorHexagon(i,j,self.Offset,self.Htrans,self.Vtrans,self.StepSize,1,0,0,self.k,"ColorHexUnreach")
258  self.markerArray.markers.append(markerColor)
259  self.markerArray.markers.append(marker)
260  self.k += 1
261 
262  j += 3*self.StepSize
263  i+= math.sqrt(3)*self.StepSize
264 
265 
266  def frange(self, start, stop, step):
267  i = start
268  while i <= stop:
269  yield i
270  i += step
271 
272 
273  def isRobotPositionAllowed(self, robotPosition):
274  """
275  returns if robotPosition is allowed using the appropriate service call
276  """
277  rospy.wait_for_service('/asr_robot_model_services/IsPositionAllowed')
278  try:
279  isReachableHandler = rospy.ServiceProxy('/asr_robot_model_services/IsPositionAllowed', IsPositionAllowed)
280  ret = isReachableHandler(robotPosition)
281  return ret.isAllowed
282  except rospy.ServiceException, e:
283  print "Service call failed: %s"%e
284  return 'aborted'
285 
286  def calcDist(self, x,y):
287  """
288  returns the distance between two points using the appropriate service call
289  """
290  rospy.wait_for_service('/asr_robot_model_services/GetDistance')
291  try:
292  GetDistanceHandler = rospy.ServiceProxy('/asr_robot_model_services/GetDistance', GetDistance)
293  ret = GetDistanceHandler(x, y)
294  return ret.distance
295  except rospy.ServiceException, e:
296  print "Service call failed: %s"%e
297 
298  def getMarker(self, pose,k,n_s,r,g,b):
299  """
300  returns a sphere marker
301  """
302  color = ColorRGBA(*[r,g,b,1])
303  marker = Marker()
304  marker.header.frame_id = "map"
305  marker.header.stamp = rospy.get_rostime()
306  marker.ns = n_s
307  marker.id = k
308  marker.type = 2
309  marker.action = 0
310  marker.pose.position = pose
311  marker.scale.x = 0.1
312  marker.scale.y = 0.1
313  marker.scale.z = 0.1
314  marker.color = color
315  return marker
316 
317  def getMarkerLine(self, points,k,n_s,r,g,b):
318  """
319  returns a line marker
320  """
321  marker = Marker()
322  color = ColorRGBA(*[r,g,b,1])
323  marker.header.frame_id = "map"
324  marker.header.stamp = rospy.get_rostime()
325  marker.ns = n_s
326  marker.id = k
327  marker.type = 4
328  marker.action = 0
329  marker.points = points
330  marker.pose.position = Point(*[0,0,0])
331  marker.pose.orientation.w = 1.0
332  marker.scale.x = 0.05
333  marker.color = color
334  return marker
335 
336  def getMarkerArrow(self, points,k,n_s,r,g,b):
337  """
338  returns an arrow marker
339  """
340  marker = Marker()
341  color = ColorRGBA(*[r,g,b,1])
342  marker.header.frame_id = "map"
343  marker.header.stamp = rospy.get_rostime()
344  marker.ns = n_s
345  marker.id = k
346  marker.type = 0
347  marker.action = 0
348  marker.points=points
349  marker.pose.orientation.w = 1.0
350  marker.scale.x = 0.05
351  marker.scale.y = 0.1
352  marker.scale.z = 0
353  marker.color = color
354  return marker
355 
356  def transformPoint(self, point,Offset,Htrans,Vtrans):
357  """
358  returns a transformed point using Offset rotation and (Htrans,Vtrans) translation
359  """
360  ret = Point()
361  ret.x = float(point.x)*math.cos(Offset) - math.sin(Offset)*float(point.y) + Htrans
362  ret.y = float(point.x)*math.sin(Offset) + math.cos(Offset)*float(point.y) + Vtrans
363  ret.z = 0
364  return ret
365 
366  def drawHexagon(self, i,j,Offset,Htrans,Vtrans,StepSize,r,g,b,k,ns):
367  """
368  returns a list of line marker to draw the Hexagonal grid
369  """
370  points = []
371  xplus = float(i)+StepSize*math.sqrt(3.0)/2
372  xminus = float(i)-StepSize*math.sqrt(3.0)/2
373  yplus = float(j)+float((StepSize/2.0))
374  yminus = float(j)-float((StepSize/2.0))
375  ypp = float(j)+float(StepSize)
376  ymm = float(j)-float(StepSize)
377  points.append(self.transformPoint(Point(*[i,ypp,0]),Offset,Htrans,Vtrans))
378  points.append(self.transformPoint(Point(*[xplus,yplus,0]),Offset,Htrans,Vtrans))
379  points.append(self.transformPoint(Point(*[xplus,yminus,0]),Offset,Htrans,Vtrans))
380  points.append(self.transformPoint(Point(*[i,ymm,0]),Offset,Htrans,Vtrans))
381  points.append(self.transformPoint(Point(*[xminus,yminus,0]),Offset,Htrans,Vtrans))
382  points.append(self.transformPoint(Point(*[xminus,yplus,0]),Offset,Htrans,Vtrans))
383  points.append(self.transformPoint(Point(*[i,ypp,0]),Offset,Htrans,Vtrans))
384  return self.getMarkerLine(points,k,ns,r,g,b)
385 
386  def colorHexagon(self, i,j,Offset,Htrans,Vtrans,StepSize,r,g,b,k,ns):
387  """
388  returns a list of triangle marker to color the Hexagonal grid
389  """
390  points = []
391  xplus = float(i)+StepSize*math.sqrt(3.0)/2
392  xminus = float(i)-StepSize*math.sqrt(3.0)/2
393  yplus = float(j)+float((StepSize/2.0))
394  yminus = float(j)-float((StepSize/2.0))
395  ypp = float(j)+float(StepSize)
396  ymm = float(j)-float(StepSize)
397 
398  #We use the points list to store all the triangle used for coloring the grid cells.
399  #Color depends on reachability. An hexagon is divided into 6 triangles
400  points.append(self.transformPoint(Point(*[i,ypp,0]),Offset,Htrans,Vtrans))
401  points.append(self.transformPoint(Point(*[xplus,yplus,0]),Offset,Htrans,Vtrans))
402  points.append(self.transformPoint(Point(*[i,j,0]),Offset,Htrans,Vtrans))
403 
404  points.append(self.transformPoint(Point(*[xplus,yplus,0]),Offset,Htrans,Vtrans))
405  points.append(self.transformPoint(Point(*[i,j,0]),Offset,Htrans,Vtrans))
406  points.append(self.transformPoint(Point(*[xplus,yminus,0]),Offset,Htrans,Vtrans))
407 
408  points.append(self.transformPoint(Point(*[i,j,0]),Offset,Htrans,Vtrans))
409  points.append(self.transformPoint(Point(*[xplus,yminus,0]),Offset,Htrans,Vtrans))
410  points.append(self.transformPoint(Point(*[i,ymm,0]),Offset,Htrans,Vtrans))
411 
412  points.append(self.transformPoint(Point(*[i,j,0]),Offset,Htrans,Vtrans))
413  points.append(self.transformPoint(Point(*[xminus,yminus,0]),Offset,Htrans,Vtrans))
414  points.append(self.transformPoint(Point(*[i,ymm,0]),Offset,Htrans,Vtrans))
415 
416  points.append(self.transformPoint(Point(*[i,j,0]),Offset,Htrans,Vtrans))
417  points.append(self.transformPoint(Point(*[xminus,yplus,0]),Offset,Htrans,Vtrans))
418  points.append(self.transformPoint(Point(*[xminus,yminus,0]),Offset,Htrans,Vtrans))
419 
420  points.append(self.transformPoint(Point(*[i,j,0]),Offset,Htrans,Vtrans))
421  points.append(self.transformPoint(Point(*[i,ypp,0]),Offset,Htrans,Vtrans))
422  points.append(self.transformPoint(Point(*[xminus,yplus,0]),Offset,Htrans,Vtrans))
423 
424  marker = Marker()
425  color = ColorRGBA(*[r,g,b,1])
426  colorM = ColorRGBA(*[r,g,b,0.5])
427  marker.header.frame_id = "map"
428  marker.header.stamp = rospy.get_rostime()
429  marker.ns = ns
430  marker.id = k
431  marker.type = 11
432  marker.action = 0
433  marker.points=points
434  marker.pose.orientation.w = 1.0
435  marker.scale.x = 1
436  marker.scale.y = 1
437  marker.scale.z = 1
438  marker.color = color
439  for point in points:
440  marker.colors.append(colorM)
441  return marker
442 
443  def drawQuad(self, i,j,Offset,Htrans,Vtrans,StepSize,r,g,b,k,ns):
444  """
445  returns a list of line marker to draw the quad grid
446  """
447  points = []
448  xplus = float(i)+float((StepSize/2.0))
449  xminus = float(i)-float((StepSize/2.0))
450  yplus = float(j)+float((StepSize/2.0))
451  yminus = float(j)-float((StepSize/2.0))
452  points.append(self.transformPoint(Point(*[xminus,yplus,0]),Offset,Htrans,Vtrans))
453  points.append(self.transformPoint(Point(*[xplus,yplus,0]),Offset,Htrans,Vtrans))
454  points.append(self.transformPoint(Point(*[xplus,yminus,0]),Offset,Htrans,Vtrans))
455  points.append(self.transformPoint(Point(*[xminus,yminus,0]),Offset,Htrans,Vtrans))
456  points.append(self.transformPoint(Point(*[xminus,yplus,0]),Offset,Htrans,Vtrans))
457  return self.getMarkerLine(points,k,ns,r,g,b)
458 
459  def colorQuad(self, i,j,Offset,Htrans,Vtrans,StepSize,r,g,b,k,ns):
460  """
461  returns a list of triangle marker to color the quad grid
462  """
463  points = []
464  xplus = float(i)+float((StepSize/2.0))
465  xminus = float(i)-float((StepSize/2.0))
466  yplus = float(j)+float((StepSize/2.0))
467  yminus = float(j)-float((StepSize/2.0))
468 
469  #We use the points list to store all the triangle used for coloring the grid cells.
470  #Color depends on reachability. A quad is divided into 4 triangles
471  points.append(self.transformPoint(Point(*[xminus,yplus,0]),Offset,Htrans,Vtrans))
472  points.append(self.transformPoint(Point(*[xplus,yplus,0]),Offset,Htrans,Vtrans))
473  points.append(self.transformPoint(Point(*[i,j,0]),Offset,Htrans,Vtrans))
474 
475  points.append(self.transformPoint(Point(*[xplus,yplus,0]),Offset,Htrans,Vtrans))
476  points.append(self.transformPoint(Point(*[xplus,yminus,0]),Offset,Htrans,Vtrans))
477  points.append(self.transformPoint(Point(*[i,j,0]),Offset,Htrans,Vtrans))
478 
479  points.append(self.transformPoint(Point(*[xplus,yminus,0]),Offset,Htrans,Vtrans))
480  points.append(self.transformPoint(Point(*[xminus,yminus,0]),Offset,Htrans,Vtrans))
481  points.append(self.transformPoint(Point(*[i,j,0]),Offset,Htrans,Vtrans))
482 
483  points.append(self.transformPoint(Point(*[xminus,yminus,0]),Offset,Htrans,Vtrans))
484  points.append(self.transformPoint(Point(*[xminus,yplus,0]),Offset,Htrans,Vtrans))
485  points.append(self.transformPoint(Point(*[i,j,0]),Offset,Htrans,Vtrans))
486 
487  marker = Marker()
488  color = ColorRGBA(*[r,g,b,0.5])
489  colorM = ColorRGBA(*[r,g,b,0.5])
490  marker.header.frame_id = "map"
491  marker.header.stamp = rospy.get_rostime()
492  marker.ns = ns
493  marker.id = k
494  marker.type = 11
495  marker.action = 0
496  marker.points=points
497  marker.pose.orientation.w = 1.0
498  marker.scale.x = 1
499  marker.scale.y = 1
500  marker.scale.z = 1
501  marker.color = color
502  for point in points:
503  marker.colors.append(colorM)
504  return marker
505 
506  def two_opt_with_cycle(self, orderedPoseListe):
507  """
508  returns an optimised list from an orderedpose using the 2-opt algo with creating a Hamilton cycle
509  """
510  improve = True #we iterate in the list as long as we find improvements
511  while improve == True:
512  improve = False
513  for i in range(len(orderedPoseListe)):
514  for j in range(i+2,len(orderedPoseListe)):
515  if i==len(orderedPoseListe)-1:
516  if self.calcDist(orderedPoseListe[i],orderedPoseListe[0])+self.calcDist(orderedPoseListe[j],orderedPoseListe[j+1]) > self.calcDist(orderedPoseListe[i],orderedPoseListe[j])+self.calcDist(orderedPoseListe[0],orderedPoseListe[j+1]):
517  orderedPoseListe[0], orderedPoseListe[j] = orderedPoseListe[j] , orderedPoseListe[0]
518  improve = True
519  elif j==len(orderedPoseListe)-1:
520  if self.calcDist(orderedPoseListe[i],orderedPoseListe[i+1])+self.calcDist(orderedPoseListe[j],orderedPoseListe[0]) > self.calcDist(orderedPoseListe[i],orderedPoseListe[j])+self.calcDist(orderedPoseListe[i+1],orderedPoseListe[0]):
521  orderedPoseListe[i+1], orderedPoseListe[j] = orderedPoseListe[j] , orderedPoseListe[i+1]
522  improve = True
523  else:
524  if self.calcDist(orderedPoseListe[i],orderedPoseListe[i+1])+self.calcDist(orderedPoseListe[j],orderedPoseListe[j+1]) > self.calcDist(orderedPoseListe[i],orderedPoseListe[j])+self.calcDist(orderedPoseListe[i+1],orderedPoseListe[j+1]):
525  orderedPoseListe[i+1], orderedPoseListe[j] = orderedPoseListe[j] , orderedPoseListe[i+1]
526  improve = True
527  return orderedPoseListe
528 
529  def two_opt_without_cycle(self, orderedPoseListe):
530  """
531  returns an optimised list from an orderedpose using the 2-opt algo without creating a Hamilton cycle
532  """
533  improve = True #we iterate in the list as long as we find improvements
534  while improve == True:
535  improve = False
536  for i in range(len(orderedPoseListe)-1):
537  for j in range(i+2,len(orderedPoseListe)-1):
538  if self.calcDist(orderedPoseListe[i],orderedPoseListe[i+1])+self.calcDist(orderedPoseListe[j],orderedPoseListe[j+1]) > self.calcDist(orderedPoseListe[i],orderedPoseListe[j])+self.calcDist(orderedPoseListe[i+1],orderedPoseListe[j+1]):
539  orderedPoseListe[i+1], orderedPoseListe[j] = orderedPoseListe[j] , orderedPoseListe[i+1]
540  improve = True
541  return orderedPoseListe
542 
543  def nearest_neighbour(self, PoseListe):
544  """
545  returns an ordered list from pose using the nearest neighbout algo
546  """
547  orderedPoseListe = []
548  distance = 100000
549  startX = rospy.get_param("startX")
550  startY = rospy.get_param("startY")
551  for pose in PoseListe:
552  if(math.sqrt(math.pow((startX-pose.x),2)+math.pow((startY-pose.y),2)) < distance):
553  distance = math.sqrt(math.pow((startX-pose.x),2)+math.pow((startY-pose.y),2))
554  if len(orderedPoseListe)>0:
555  del orderedPoseListe[0]
556  orderedPoseListe.append(pose)
557  for idx,pose in enumerate(PoseListe):
558  if pose == orderedPoseListe[0]:
559  del PoseListe[idx]
560  break
561  while len(PoseListe)>0:
562  distance = float(10000.0)
563  ref = -1
564  for index in range(len(PoseListe)):
565  dist = self.calcDist(orderedPoseListe[-1],PoseListe[index])
566  if dist<distance:
567  ref = index
568  distance = dist
569  orderedPoseListe.append(PoseListe[ref])
570  del PoseListe[ref]
571  return orderedPoseListe
572 
573 
574 if __name__ == "__main__":
575  gridCreator = GridCreator()
576 
577 
578 
579 
580 
581 
582 
583 
584 
585 
586 
587 
588 
589 
590 
591 
592 
593 
594 
595 
596 
597 
598 
599 
grid_creator.GridCreator.GridType
GridType
Definition: grid_creator.py:39
grid_creator.GridCreator.two_opt_with_cycle
def two_opt_with_cycle(self, orderedPoseListe)
Definition: grid_creator.py:506
grid_creator.GridCreator.Ymax
Ymax
Definition: grid_creator.py:38
grid_creator.GridCreator.two_opt_without_cycle
def two_opt_without_cycle(self, orderedPoseListe)
Definition: grid_creator.py:529
grid_creator.GridCreator.Xmax
Xmax
Definition: grid_creator.py:37
grid_creator.GridCreator.createGrid
def createGrid(self)
Definition: grid_creator.py:161
grid_creator.GridCreator.drawQuad
def drawQuad(self, i, j, Offset, Htrans, Vtrans, StepSize, r, g, b, k, ns)
Definition: grid_creator.py:443
grid_creator.GridCreator.Htrans
Htrans
Definition: grid_creator.py:72
grid_creator.GridCreator.__init__
def __init__(self)
Definition: grid_creator.py:35
grid_creator.GridCreator.Offset
Offset
Definition: grid_creator.py:77
grid_creator.GridCreator.transformPoint
def transformPoint(self, point, Offset, Htrans, Vtrans)
Definition: grid_creator.py:356
grid_creator.GridCreator.Vtrans
Vtrans
Definition: grid_creator.py:74
grid_creator.GridCreator.colorQuad
def colorQuad(self, i, j, Offset, Htrans, Vtrans, StepSize, r, g, b, k, ns)
Definition: grid_creator.py:459
grid_creator.GridCreator.StepSize
StepSize
Definition: grid_creator.py:70
grid_creator.GridCreator.nearest_neighbour
def nearest_neighbour(self, PoseListe)
Definition: grid_creator.py:543
grid_creator.GridCreator.drawHexagon
def drawHexagon(self, i, j, Offset, Htrans, Vtrans, StepSize, r, g, b, k, ns)
Definition: grid_creator.py:366
grid_creator.GridCreator.markerArray
markerArray
Definition: grid_creator.py:80
grid_creator.GridCreator.PoseListe
PoseListe
Definition: grid_creator.py:79
grid_creator.GridCreator.getMarker
def getMarker(self, pose, k, n_s, r, g, b)
Definition: grid_creator.py:298
grid_creator.GridCreator.getMarkerLine
def getMarkerLine(self, points, k, n_s, r, g, b)
Definition: grid_creator.py:317
grid_creator.GridCreator.getMarkerArrow
def getMarkerArrow(self, points, k, n_s, r, g, b)
Definition: grid_creator.py:336
grid_creator.GridCreator.frange
def frange(self, start, stop, step)
Definition: grid_creator.py:266
grid_creator.GridCreator.k
k
Definition: grid_creator.py:81
grid_creator.GridCreator.calcDist
def calcDist(self, x, y)
Definition: grid_creator.py:286
grid_creator.GridCreator.isRobotPositionAllowed
def isRobotPositionAllowed(self, robotPosition)
Definition: grid_creator.py:273
grid_creator.GridCreator
Definition: grid_creator.py:33
grid_creator.GridCreator.colorHexagon
def colorHexagon(self, i, j, Offset, Htrans, Vtrans, StepSize, r, g, b, k, ns)
Definition: grid_creator.py:386

asr_grid_creator
Author(s): Borella Jocelyn, Karrenbauer Oliver, Meißner Pascal
autogenerated on Thu Nov 21 2019 03:22:28