I am trying to write a search algorithm that takes in a start point and then return the path to the end point, I originally tried just doing it via some nested for loops and a list of lists so that I could just loop through and try to find a path but the RAM requirements convinced me to try it using a class-based system. However, all it is doing is taking like 2gb of RAM and 100% of one of my CPU cores and just sitting there without exiting. If anyone sees a problem in my code, any help would be greatly appreciated.
import csv
import math
from multiprocessing import Process
from rplidar import RPLidar
import heapq
lidar = RPLidar('/dev/ttyUSB0')
file = "lidar01.csv"
def calc_offset():
# take in argos ros data and calculate offset
x_offset = 0
y_offset = 0
return x_offset, y_offset
def find_fix_quad_convert(x, y):
offset_x, offset_y = calc_offset()
if x >= 0 and y >= 0:
x = abs(x 12000 offset_x)
y = abs(y offset_y)
return x,y
elif x < 0 and y >= 0:
x = abs(x - 12000 offset_x)
y = abs(y offset_x)
return x,y
elif x < 0 and y < 0:
x = abs(x - 12000 offset_x)
y = abs(y - 12000 offset_y)
return x,y
elif x >= 0 and y < 0:
x = abs(x 12000 offset_x)
y = abs(y - 12000 offset_y)
return x,y
def scan():
try:
for scan in enumerate(lidar.iter_scans()):
list_version_data = list(scan)
for data in list_version_data:
if isinstance(data, list):
for indiv_data_points in data:
if isinstance(indiv_data_points, tuple):
list_indiv_data_points = list(indiv_data_points)
list_indiv_data_points.pop(0)
angle = list_indiv_data_points[0]
distance = list_indiv_data_points[1]
length = distance
angle = angle
angle = math.radians(angle)
x,y = (length * math.cos(angle)), (length * math.sin(angle))
x = int(x)
y = int(y)
new_x,new_y = find_fix_quad_convert(x,y)
with open(file=file, mode="w") as f:
writer = csv.writer(f)
writer.writerow([new_x,new_y])
except Exception as e:
print(e)
pass
def eliminate_duplicates():
unique_coords = set()
with open(file, 'r') as f:
reader = csv.reader(f)
for row in reader:
coord = (row[0], row[1])
if coord not in unique_coords:
unique_coords.add(coord)
with open(file, 'w') as f:
writer = csv.writer(f)
for coord in unique_coords:
writer.writerow(coord)
# create the node class that takes in the individual data points and creates a node for the nav graph
class Node:
def __init__(self, x, y):
self.x = x
self.y = y
self.neighbors = []
self.parent = None
def __eq__(self, other):
return self.x == other.x and self.y == other.y
def __lt__(self, other):
return self.f < other.f
def scan_eliminate_duplicates():
scan_process = Process(target=scan)
eliminate_duplicates_process = Process(target=eliminate_duplicates)
scan_process.start()
scan_process.join()
eliminate_duplicates_process.start()
eliminate_duplicates_process.join()
def find_path(start, end, nodes):
open_set = []
closed_set = set()
start.f = 0
heapq.heappush(open_set, start)
while open_set:
current_node = heapq.heappop(open_set)
closed_set.add(current_node)
if current_node == end:
print(f"Path found: {0}".format(construct_path(current_node)))
return construct_path(current_node)
for neighbor in current_node.neighbors:
if neighbor in closed_set:
continue
tentative_g = current_node.f 1
if neighbor not in open_set or tentative_g < neighbor.f:
neighbor.parent = current_node
neighbor.f = tentative_g
if neighbor not in open_set:
heapq.heappush(open_set, neighbor)
return None
def construct_path(node):
path = []
while node.parent:
path.append((node.x, node.y))
node = node.parent
return path[::-1]
if __name__ == "__main__":
scan_elim_dupl_process = Process(target=scan_eliminate_duplicates)
nodes = []
with open(file, "r") as f:
reader = csv.reader(f)
for row in reader:
node = Node(int(float(row[0])), int(float(row[1])))
nodes.append(node)
# set start and end nodes
start = Node(3201, 3201)
end = Node(23000, 23000)
# connect the nodes to their neighbors
for i, node in enumerate(nodes):
for j in range(i 1, len(nodes)):
if abs(node.x - nodes[j].x) <= 1 and abs(node.y - nodes[j].y) <= 1:
node.neighbors.append(nodes[j])
nodes[j].neighbors.append(node)
find_path_process = Process(target=find_path, args=(start, end, nodes))
scan_elim_dupl_process.start(), find_path_process.start()
scan_elim_dupl_process.join(), find_path_process.join()
CSV Data(example):
-224.45409129769087,-75.30553365940557
-225.4021550412925,-75.62361405501024
-221.37533513849013,-86.0186665341958
-222.02088232366805,-83.6318737815909
-219.05825287406182,-90.4570718504838
-216.1406631194247,-97.22249609167298
-212.35203834877927,-105.80252506022047
-210.74781416150145,-110.5864314739799
-209.03673236351906,-114.8298503124623
-207.00083783790242,-118.46055518359869
-202.61438759451943,-126.76123200607452
-200.80257079121006,-132.35776351858277
-198.46526749871208,-137.60010027854142
-200.72914689131528,-136.5114357417897
-198.8372035969446,-141.42053056663028
-195.46212772818174,-148.12872484421098
-192.555826974252,-155.49438413737627
-191.2678199044531,-159.4290471306835
-204.80806340541443,-686.6046221546457
-189.9329560617947,-692.9413555284663
-174.4435476087335,-698.0327346891975
-157.25903052807882,-703.8971230352976
-142.50543063768973,-710.3467126965301
-44.080364168658264,-424.9699801100761
-12.039081185863548,-420.3276228422303
151.3137816891034,-880.5943387683925
171.58805621421078,-880.1807209116937
192.6920068673774,-879.3860659513674
213.97191813826333,-877.540073585379
235.7914668768005,-874.2611933215878
257.2898049769299,-872.088022366397
279.8247876333225,-870.8993344388122
301.1827105821156,-869.3037299163104
323.32023991345227,-866.9207489513143
344.60980320968105,-865.141980273634
368.38864460533046,-862.6319067399766
390.5808612705762,-860.7811297357389
413.666591519788,-858.81634448839
437.78734499029486,-856.893985896942
462.98529035913396,-854.9354849408629
488.6789701052747,-851.7727773161384
513.1091380975454,-851.3254444692665
540.2315442531086,-849.5265982262719
566.5849433716348,-845.456423740787
593.946149447507,-843.3387347658589
620.2144841174974,-841.046368335817
649.1761458917417,-837.0071051700481
678.6559458023329,-834.341870714362
709.32872426918,-831.894418437014
739.9610013434269,-829.0562580373138
772.0166065569867,-826.5297086011094
807.4258588219725,-823.4373352026249
841.6994505337709,-821.2904693049519
878.8426320460152,-818.1908032961703
917.0058399786907,-814.8716782076648
953.9215320868914,-809.3421468211068
989.2825428144441,-801.5520026689394
1089.7385480638236,-803.6080492775999
1124.2452992304018,-789.0340659048534
1161.1577259536114,-774.4874420920189
1207.7231504414601,-765.7054210907444
1256.6619459378417,-758.3474244247931
1312.4869985934681,-749.6733478810021
1436.2817842205613,-736.2680465130896
560.4785752204706,-119.13339883857641
561.947341484636,-105.41939052351769
562.7845996626268,-91.97041308256136
562.0728347190211,-80.08864445677706
572.2556983202762,-67.73092528507895
3073.007418570301,775.0941671254502
3076.1322580025953,851.280443670507
3085.7160627583366,932.367218447319
3079.5934798899584,1010.8439845590693
3065.409617566463,1086.4593591253329
3049.6010101414113,1162.0524639380467
553.4062949898384,280.6451899919539
517.2886535413827,292.48164287240894
504.22866463801756,302.9711475658394
493.01596196440715,311.63411839577225
457.35133669092596,320.98491760053656
448.3587658812583,330.0681438088732
438.5893339206918,341.3172405124065
430.36674574745746,351.23313362315815
396.83008722312223,357.699516877629
385.27345578604894,365.4112262460958
375.7639852425002,372.7022805064037
366.43229041712107,379.4517446786384
355.57191272554525,387.6140024311522
346.70803875198897,395.5623033919553
332.8988762943624,398.49579754616076
315.8504082369599,398.19163993804005
300.8542343626438,400.6091981795568
289.23868520741775,402.93545758531144
277.5650855793172,406.2285357620096
272.3911822392343,417.2003191972799
262.73233641927243,427.16971966616535
253.20792971902577,432.01424377837924
249.1985319999312,447.5490522267691
292.7465904906362,640.521269167627
272.64249172224527,636.02678536952
804.8967316661586,3209.614034639233
724.5030467972489,3205.9040686959897
646.5556113779995,3209.2685932928116
567.8035492364211,3204.8395735160475
486.7038419116777,3179.46404607261
409.31126573218995,3155.564654343928
335.12423534291634,3147.2077698947405
111.3757146106589,3140.2755472561585
39.18188674771358,3123.254237130063
-35.079705269662,3137.303884592341
-108.12103095017433,1135.8656798522752
-135.12589586812848,1133.7257405412631
-478.8748400704411,2350.463879758102
-523.4289972333165,2298.6579421165134
-321.0672961603582,1006.7950902920995
-337.5896468300176,906.691502305599
-362.5912398939693,906.686187582095
-386.4405220459153,909.0180336059789
-348.11121502543114,476.6693240324135
-709.9009007847953,689.6707718650517
-705.7154352397234,654.50131738936
-225.61815673209847,73.37581245076781
-224.96174273211477,60.673011355377774
-221.7058096465867,57.33702092846706
-218.36099776953716,54.2537985130275
-217.26139804313215,45.62329361569259
-215.60048241436377,36.98640943229841
-212.9460118930461,31.409529108961046
-210.41143597187929,27.876336062182475
-209.29301055390292,20.037420824145237
-207.148062758457,18.806384432377264
-205.20651537090765,10.97889107922301
-203.40844501247898,6.103646254929753
-201.49568420532205,1.3188050004214316
-199.7214487838248,-3.3771875414384667
-198.08854835941534,-7.9993442768494845
-196.3529499548831,-12.493259943472601
-194.511648279817,-16.964114579569504
-192.81795218045707,-21.3831082150133
-191.27242817254086,-25.765446260062987
-189.63616139426645,-30.0354172877243
-188.37840041163838,-34.43585716012369
-226.49932324600476,-68.91022470651103
-226.1737175842578,-73.28377701863081
-224.04801115257553,-79.54590623391874
-221.56247177423467,-85.53549614804056
-217.15764842108885,-95.55165216898537
-215.7143937962088,-98.164659165782
-213.46548271945335,-102.96352600484683
-212.697138624531,-105.10703935006946
-210.6167482193979,-110.29799576368899
-205.56454558388867,-120.93579949249667
-203.34370277416045,-126.06308358156993
-202.659741276799,-128.5652723935235
-198.05436632376652,-137.315213942561
-200.01801379745294,-136.66695524713327
-195.89271630106168,-144.21028465470746
-196.10659851005252,-147.2744530487792
-191.887743885396,-155.12787063120754
-190.4663185563427,-159.99732496386864
-240.31744343455787,-628.0953561212486
-194.36681058896912,-689.8928217060037
-179.58107052974322,-695.954166312259
-162.78643452157806,-701.6128841717147
-148.54493769413048,-708.3420529556655
-48.61733572180791,-427.74595809582235
-36.52211770286281,-421.42037791081987
-14.701911453818868,-417.7413719032436
121.73474418061696,-881.8875861238097
142.58207691392846,-880.0242049755851
164.10647050731015,-880.0804672515084
185.3187865324572,-878.9254859532392
207.6388003465223,-877.5187696514856
227.2478526229215,-875.7459540176426
247.99909241264257,-872.9562991138248
269.3479160261434,-870.7950103970358
292.78503356447436,-868.7390210072583
313.92451252961064,-866.9114144092499
335.83498334245536,-864.2959137143787
358.6456719461373,-861.8558649764493
381.566664921735,-860.1673559374968
403.5993305416195,-860.0044071900775
427.212685906662,-858.0277215803786
451.6489095368339,-855.0451815630497
476.82947920642863,-854.1701881122556
501.87089118531856,-853.0713165268506
528.0321382457586,-850.695964184392
554.4110689448593,-846.6240999590187
579.1151539748655,-843.6031062867585
606.096392136026,-841.4369634379586
635.4714561227303,-839.0067213397381
664.7834755172479,-837.1522744872694
695.0210066681005,-833.8201546437099
724.3369310372825,-830.5058761595194
755.2483075753681,-828.7349132289179
790.0536662312429,-825.3008266532705
823.3915496973328,-822.0453794572566
857.6177770538111,-819.1994558599753
896.6246516997196,-816.2233052078068
933.9759389836386,-814.1314054866272
965.93739908313,-802.0256797961757
1106.8124778852348,-795.8748025270977
1145.3782516272215,-784.4403885569442
1185.8751583397407,-772.8779715017727
1231.3279001110159,-763.8275999256613
1283.8274490578733,-756.2321608114493
1339.6622193807364,-748.3283958021902
1399.0838013856292,-739.5071106489357
559.0470439861833,-113.7394065850111
556.9664328067189,-99.30486003493867
558.3537490936984,-86.40654415616522
565.1640657918545,-74.09169142097093
576.7394965367929,-61.412972037564295
3083.2240320377678,726.8903842841579
3075.974377735485,801.7687883765465
3077.959314383161,878.9581668099685
3081.5656872937416,958.6946737068328
3079.603817010731,1036.9621884393555
3065.787463056901,1112.0695993790728
3041.893912737369,1184.8382478850592
565.3389250472349,272.8771927560765
548.9161042252287,281.1968581300259
512.1181201590852,295.48535243684216
500.3702034741492,306.4395894385034
489.0168090088083,315.1120800712703
453.6658726040589,323.5967220699212
445.1879662404556,334.3325249129779
436.9264391821313,344.2518689036028
404.1328350154679,352.8581891672072
393.65549849104536,359.33876566238513
382.9718830476642,366.0158456883065
373.6310075466433,373.4272789977725
362.9032487936391,381.7941782099644
354.38182598682874,389.7166713270566
342.20930022858226,397.153443063338
CodePudding user response：
One problem is that this line does not behave like you seem to be expecting:
for scan in enumerate(lidar.iter_scans()):
Looking at the source code, this appears to iterate through scans as they come in. In other words, it's a continual stream of incoming data. You need to update your code to have a non-error exit condition. The README in the source repo has this as an example:
for i, scan in enumerate(lidar.iter_scans()):
print('%d: Got %d measurments' % (i, len(scan)))
if i > 10:
break
Another problem is that you've got multiple processes running, which makes debugging significantly more challenging. I would suggest simplifying your __main__ section to this until you've made sure your find_path method is correct:
if __name__ == "__main__":
nodes = []
with open(file, "r") as f:
reader = csv.reader(f)
for row in reader:
node = Node(int(float(row[0])), int(float(row[1])))
nodes.append(node)
# set start and end nodes
start = Node(3201, 3201)
end = Node(23000, 23000)
# connect the nodes to their neighbors
for i, node in enumerate(nodes):
for j in range(i 1, len(nodes)):
if abs(node.x - nodes[j].x) <= 1 and abs(node.y - nodes[j].y) <= 1:
node.neighbors.append(nodes[j])
nodes[j].neighbors.append(node)
find_path(start, end, nodes)
It would also be helpful for readability if you moved most of this into a separate read_nodes method.