grid_creator_perf.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 asr_next_best_view
21 import subprocess, os, signal
22 import xml.etree.cElementTree as ET
23 import math
24 import rospkg
25 import datetime
26 
27 
28 from asr_next_best_view.srv import IsPositionReachable,GetDistance
29 from geometry_msgs.msg import Point
30 from std_msgs.msg import ColorRGBA
31 from visualization_msgs.msg import Marker, MarkerArray
32 
33 
34 def calcDist(x,y):
35  """
36  returns the distance between two points using the appropriate service call
37  """
38  return math.sqrt(math.pow(x.x-y.x,2)+math.pow(x.y-y.y,2))
39 
40 
41 def two_opt_with_cycle(orderedPoseListe):
42  """
43  returns an optimised list from an orderedpose using the 2-opt algo with creating a Hamilton cycle
44  """
45  improve = True #we iterate in the list as long as we find improvements
46  while improve == True:
47  improve = False
48  for i in range(len(orderedPoseListe)):
49  for j in range(i+2,len(orderedPoseListe)):
50  if i==len(orderedPoseListe)-1:
51  if calcDist(orderedPoseListe[i],orderedPoseListe[0])+calcDist(orderedPoseListe[j],orderedPoseListe[j+1]) > calcDist(orderedPoseListe[i],orderedPoseListe[j])+calcDist(orderedPoseListe[0],orderedPoseListe[j+1]):
52  orderedPoseListe[0], orderedPoseListe[j] = orderedPoseListe[j] , orderedPoseListe[0]
53  improve = True
54  elif j==len(orderedPoseListe)-1:
55  if calcDist(orderedPoseListe[i],orderedPoseListe[i+1])+calcDist(orderedPoseListe[j],orderedPoseListe[0]) > calcDist(orderedPoseListe[i],orderedPoseListe[j])+calcDist(orderedPoseListe[i+1],orderedPoseListe[0]):
56  orderedPoseListe[i+1], orderedPoseListe[j] = orderedPoseListe[j] , orderedPoseListe[i+1]
57  improve = True
58  else:
59  if calcDist(orderedPoseListe[i],orderedPoseListe[i+1])+calcDist(orderedPoseListe[j],orderedPoseListe[j+1]) > calcDist(orderedPoseListe[i],orderedPoseListe[j])+calcDist(orderedPoseListe[i+1],orderedPoseListe[j+1]):
60  orderedPoseListe[i+1], orderedPoseListe[j] = orderedPoseListe[j] , orderedPoseListe[i+1]
61  improve = True
62  return orderedPoseListe
63 
64 def nearest_neighbour(PoseListe):
65  """
66  returns an ordered list from pose using the nearest neighbout algo
67  """
68  orderedPoseListe = []
69  distance = 100000
70  startX = rospy.get_param("startX")
71  startY = rospy.get_param("startY")
72  for pose in PoseListe:
73  if(math.sqrt(math.pow((startX-pose.x),2)+math.pow((startY-pose.y),2)) < distance):
74  distance = math.sqrt(math.pow((startX-pose.x),2)+math.pow((startY-pose.y),2))
75  if len(orderedPoseListe)>0:
76  del orderedPoseListe[0]
77  orderedPoseListe.append(pose)
78  for idx,pose in enumerate(PoseListe):
79  if pose == orderedPoseListe[0]:
80  del PoseListe[idx]
81  break
82  while len(PoseListe)>0:
83  distance = float(10000.0)
84  ref = -1
85  for index in range(len(PoseListe)):
86  dist = calcDist(orderedPoseListe[-1],PoseListe[index])
87  if dist<distance:
88  ref = index
89  distance = dist
90  orderedPoseListe.append(PoseListe[ref])
91  del PoseListe[ref]
92  return orderedPoseListe
93 
94 
95 if __name__ == "__main__":
96  rospy.init_node('grid_creator', anonymous=True)
97  PoseListe = []
98  for size in range(20,2000,20):
99  for elt in range(0,size):
100  PoseListe.append(Point(*[random.uniform(0, 100),random.uniform(0, 100),0]))
101  print(str(PoseListe.size)+",")
102  start = time()
103  orderedPoseListe = nearest_neighbour(PoseListe)
104  print(str(time()-start)+",")
105  #2-opt algorithm
106  start = time()
107  orderedPoseListe = two_opt_with_cycle(orderedPoseListe)
108  print(str(time()-start)+";")
109 
110 
111 
112 
113 
114 
115 
116 
117 
118 
119 
120 
121 
122 
123 
124 
125 
126 
127 
128 
129 
130 
131 
def nearest_neighbour(PoseListe)
def two_opt_with_cycle(orderedPoseListe)


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