import os,sys
|
import numpy as np
|
import time
|
from slam.slam import compose_ply
|
from utils import read_ply,write_ply,file_fliter,udp_get_tool,udp2bin_tool,bin2ply_tool
|
from mannul_slam import run as compose_muti_ply
|
|
cudir = src_path = os.path.abspath(os.path.join(__file__, *(['..'] * 1)))
|
exe_path = os.path.abspath(os.path.join(cudir,"../RUN/Test_Servo.exe"))
|
|
def run(port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder):
|
|
# 旋转机构复位
|
os.system(f"{exe_path} {port} 400 0")
|
time.sleep(10)
|
|
# 间隔获取udp
|
for i in range(0,360,degre_step):
|
os.system(f"{exe_path} {port} {speed} {i}")
|
time.sleep(int(time_delay))
|
subFolder = str(-i)
|
print (f"Geting udp files with {subFolder} degree ...")
|
udp_get_tool(microwaveHost,udp_packets,deviceName,saveFolder,subFolder)
|
|
# 旋转机构复位
|
os.system(f"{exe_path} {port} 100 0")
|
|
# iterate the folder
|
for root, dirs, files in os.walk(saveFolder,topdown=True):
|
break
|
|
for dir in dirs:
|
# udp2bin bin2ply
|
subdir = os.path.join(saveFolder,dir)
|
udp2bin_tool(subdir)
|
bin2ply_tool(subdir)
|
|
|
# compose ply
|
print("merge ply files, processing ...")
|
compose_muti_ply(saveFolder,alpha,beta,gamma,"data")
|
|
if __name__ == "__main__":
|
def help():
|
print("Usage: auto_slam.py [port] [speed] [degre_step] [time_delay] [alpha] [beta] [gamma] [microwaveHost] [udp_packets] [deviceName] [saveFolder]")
|
print("EXAMPLE: auto_slam.py COM11 100 5 5 0 20 auto 192.168.2.65 1000 pod1 ./data/run_test/")
|
exit(1)
|
|
if len(sys.argv) != 12:
|
help()
|
try:
|
port = str(sys.argv[1])
|
speed = str(sys.argv[2])
|
degre_step = int(sys.argv[3])
|
time_delay = int(sys.argv[4])
|
alpha = str(sys.argv[5])
|
beta = str(sys.argv[6])
|
gamma = str(sys.argv[7])
|
microwaveHost = str(sys.argv[8])
|
udp_packets = int(sys.argv[9])
|
deviceName = str(sys.argv[10])
|
saveFolder = str(sys.argv[11])
|
except Exception as e:
|
print(e)
|
help()
|
|
run(port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder)
|
print("Done")
|
