From 6e6504475a0ef865ed1d47f78c89f56762965b3c Mon Sep 17 00:00:00 2001
From: wu_xinjun <bent20140204@outlook.com>
Date: 星期五, 25 三月 2022 15:56:36 +0800
Subject: [PATCH] float support for servo controller
---
src/microwave/servers.py | 18 +++------
env/SettingEnv.bat | 6 ++
src/protos/compile.bat | 5 +-
.vscode/settings.json | 5 +-
scripts_launch/TEST_SlamAuto.bat | 15 ++++---
env/JustDoubleClickMe.bat | 3 +
RUN/Test_Servo.pdb | 0
src/slam/autoMode.py | 7 ++-
src/Client.py | 7 +--
src/slam/core.py | 6 +++
src/Server.py | 11 +----
RUN/Test_Servo.dll | 0
scripts/slamAutoMode.py | 31 ++++++++-------
13 files changed, 58 insertions(+), 56 deletions(-)
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 4479c53..95a74cc 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -7,6 +7,7 @@
"path": "${workspaceFolder}/.env/python.exe"
}
],
- "python.envFile": "${workspaceFolder}/.env",
+ "python.envFile": "",
+ "python.terminal.activateEnvironment": true,
"python.defaultInterpreterPath": "${workspaceFolder}/.env/python.exe"
-}
\ No newline at end of file
+ }
\ No newline at end of file
diff --git a/RUN/Test_Servo.dll b/RUN/Test_Servo.dll
index bf249d6..8cdeaad 100644
--- a/RUN/Test_Servo.dll
+++ b/RUN/Test_Servo.dll
Binary files differ
diff --git a/RUN/Test_Servo.pdb b/RUN/Test_Servo.pdb
index e8760a3..7d98179 100644
--- a/RUN/Test_Servo.pdb
+++ b/RUN/Test_Servo.pdb
Binary files differ
diff --git a/env/JustDoubleClickMe.bat b/env/JustDoubleClickMe.bat
index b8dfd94..b4b9ffc 100644
--- a/env/JustDoubleClickMe.bat
+++ b/env/JustDoubleClickMe.bat
@@ -6,7 +6,8 @@
echo setting done!
echo ========== Testing Env ==============
echo test it with hello word...
-set aerialpython=%cd%/../../aerial_deploy/python.exe
+cd %~dp0
+set aerialpython=%cd%/../.env/python.exe
%aerialpython% -c "import cv2, pika, flask, pika; print('hello world!')"
echo =========== ALL DONE ================
diff --git a/env/SettingEnv.bat b/env/SettingEnv.bat
index 7d79291..b0d30b1 100644
--- a/env/SettingEnv.bat
+++ b/env/SettingEnv.bat
@@ -20,7 +20,11 @@
tar -xzf "./aerial.tar.gz" -C "../../aerial_deploy"
cd ..
-mklink /j ".env" "../aerial_deploy/"
+if not exist ".env" (
+ mklink /j ".env" "../aerial_deploy/")
+
+cd %~dp0
+call "../src/protos/compile.bat"
echo ALL DONE!
diff --git a/scripts/slamAutoMode.py b/scripts/slamAutoMode.py
index dc0e979..f687ec2 100644
--- a/scripts/slamAutoMode.py
+++ b/scripts/slamAutoMode.py
@@ -9,27 +9,28 @@
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/")
+ print("Usage: auto_slam.py [servo_path][port] [speed] [degre_step] [time_delay] [alpha] [beta] [gamma] [microwaveHost] [udp_packets] [deviceName] [saveFolder]")
+ print("EXAMPLE: auto_slam.py ./RUN/Test_Servo.exe COM11 100 5 5 0 20 auto 192.168.2.65 1000 pod1 ./data/run_test/")
exit(1)
- if len(sys.argv) != 12:
+ if len(sys.argv) != 13:
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 = os.path.abspath(str(sys.argv[11]))
+ servo_path = str(sys.argv[1])
+ port = str(sys.argv[2])
+ speed = str(sys.argv[3])
+ degre_step = float(sys.argv[4])
+ time_delay = int(sys.argv[5])
+ alpha = str(sys.argv[6])
+ beta = str(sys.argv[7])
+ gamma = str(sys.argv[8])
+ microwaveHost = str(sys.argv[9])
+ udp_packets = int(sys.argv[10])
+ deviceName = str(sys.argv[11])
+ saveFolder = os.path.abspath(str(sys.argv[12]))
except Exception as e:
print(e)
help()
- rotateAndMerge(port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder)
+ rotateAndMerge(servo_path,port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder)
print("Done")
\ No newline at end of file
diff --git a/scripts_launch/TEST_SlamAuto.bat b/scripts_launch/TEST_SlamAuto.bat
index cdb2170..4ac34c6 100644
--- a/scripts_launch/TEST_SlamAuto.bat
+++ b/scripts_launch/TEST_SlamAuto.bat
@@ -17,7 +17,8 @@
echo change to path: %cd%
@REM pre-defined parameters
-set port=COM11
+set servo_path=./RUN/Test_Servo.exe
+set port=COM14
set speed=200
set degree_step=2
set time_delay=5
@@ -30,10 +31,11 @@
set saveFolder=./data/run_test/
@REM re-defined parameters
-set /p port=Please input the serial port of rotation machine (example: COM11):
-set /P speed=Please input the speed of rotation machine (example: 200):
-set /P degree_step=Please input the degree step of rotation machine (example: 2):
-set /P time_delay=Please input the delay time of rotation machine (example: 5):
+set /P servo_path=Please input the path of servo controller (example: ./RUN/Test_Servo.exe):
+set /p port=Please input the serial port of servo (example: COM14):
+set /P speed=Please input the speed of servo (example: 200):
+set /P degree_step=Please input the degree step of servo (example: 2):
+set /P time_delay=Please input the delay time of servo (example: 5):
set /P alpha=Please input the alpha degree of microwave device (example: 0/-20/auto):
set /P beta=Please input the beta degree of microwave device (example: 0/-20/auto):
set /P gamma=Please input the gamma degree of microwave device (example: 0/-20/auto):
@@ -44,6 +46,7 @@
@REM verify the parameters
echo ################# parameters #################
+echo servo_path :%servo_path%
echo port :%port%
echo speed :%speed%
echo degree_step :%degree_step%
@@ -61,7 +64,7 @@
if %choice% == y (
- %aerialpython% ./scripts/slamAutoMode.py %port% %speed% %degree_step% %time_delay% %alpha% %beta% %gamma% %microwaveHost% %udp_packets% %deviceName% %saveFolder%
+ %aerialpython% ./scripts/slamAutoMode.py %servo_path% %port% %speed% %degree_step% %time_delay% %alpha% %beta% %gamma% %microwaveHost% %udp_packets% %deviceName% %saveFolder%
@REM -------------- Usage: plot_ply.py [PLYfile]"
%aerialpython% ./scripts/plot_ply.py %PLYfile%
) else (
diff --git a/src/Client.py b/src/Client.py
index 3758f0f..4bb0efb 100644
--- a/src/Client.py
+++ b/src/Client.py
@@ -1,12 +1,9 @@
import signal
import os, sys
-batfile_dir = os.path.join(__file__, "..", "protos/compile.bat" )
-batfile_path = os.path.abspath(batfile_dir)
-os.system(batfile_path)
-from engine.AcquisitionMain import AcquisitionMain
-from engine.AcquisitionMain import Config
+from .engine.AcquisitionMain import AcquisitionMain
+from .engine.AcquisitionMain import Config
def quit(signum, frame):
print('You choose to stop me.')
diff --git a/src/Server.py b/src/Server.py
index b2e41a4..3f1e26e 100644
--- a/src/Server.py
+++ b/src/Server.py
@@ -1,15 +1,10 @@
-from posixpath import abspath
import sys # NOQA: E402
import os
from flask import Flask, request
-batfile_dir = os.path.join(__file__, "..", "protos/compile.bat" )
-batfile_path = os.path.abspath(batfile_dir)
-os.system(batfile_path)
-
-from engine.DeviceManager import DeviceManager
-from engine.MessageQueueThread import MessageQueueThread
-from protos import aerial_pb2 as pb
+from .engine.DeviceManager import DeviceManager
+from .engine.MessageQueueThread import MessageQueueThread
+from .protos import aerial_pb2 as pb
diff --git a/src/microwave/servers.py b/src/microwave/servers.py
index e9c315b..c24edef 100644
--- a/src/microwave/servers.py
+++ b/src/microwave/servers.py
@@ -3,13 +3,7 @@
from socketserver import BaseRequestHandler, UDPServer
from .components import *
from .constants import *
-
-# add python path of src to sys.path
-src_path = os.path.join(__file__, *(['..'] * 2))
-src_path = os.path.abspath(src_path)
-sys.path.insert(0, src_path)
-
-from protos import aerial_pb2 as pb
+from ..protos import aerial_pb2 as pb
@@ -37,7 +31,7 @@
"""
receive data packet from udp
"""
- self.data_buf.extend(data)
+ self.data_buf.extend(bytes(data))
self.ReceivedDataLength += len(data)
if self.ReceivedDataLength == self.DataLength:
# if received data length equal expected data length
@@ -47,7 +41,7 @@
def _receive_footer(self, data) -> None:
# receive footer block
- self.footer_buf.extend(data)
+ self.footer_buf.extend(bytes(data))
self.ReceivedFooterLength += len(data)
if self.ReceivedFooterLength == sizeof(FooterBlock):
self.Footer = FooterBlock.from_buffer_copy(self.footer_buf)
@@ -95,10 +89,10 @@
# 鍒ゆ柇绗竴涓暟鎹寘涓槸鍚﹀寘鍚獸ooter Block
if (header_size + data_size) < buf_size:
- self._receive_footer(buf[header_size + data_size])
+ self._receive_footer(buf[header_size + data_size:])
elif self.state == UDP_STATE_DATA:
# receive data block
- self.DataFrame.extend(buf)
+ self.DataFrame.extend(bytes(buf))
remain_data_size = self.DataLength - self.ReceivedDataLength
data_size = min(buf_size, remain_data_size)
self._receive_data(buf[: data_size])
@@ -106,7 +100,7 @@
self._receive_footer(buf[data_size: ])
elif self.state == UDP_STATE_FOOTER:
# receive footer block
- self.DataFrame.extend(buf)
+ self.DataFrame.extend(bytes(buf))
self._receive_footer(buf)
diff --git a/src/protos/compile.bat b/src/protos/compile.bat
index bbdbfd3..2176bbf 100644
--- a/src/protos/compile.bat
+++ b/src/protos/compile.bat
@@ -2,10 +2,9 @@
@REM change to the filepath
cd %~dp0
-echo change to path: %cd%
-set protoc=%cd%/../../../aerial_deploy/Library/bin/protoc.exe
+set protoc=%cd%/../../.env/Library/bin/protoc.exe
%protoc% aerial.proto --python_out=%cd%
-echo compile done!
\ No newline at end of file
+echo protoc: aerial.proto compile done!
\ No newline at end of file
diff --git a/src/slam/autoMode.py b/src/slam/autoMode.py
index 18663f2..0d64093 100644
--- a/src/slam/autoMode.py
+++ b/src/slam/autoMode.py
@@ -4,12 +4,13 @@
from ..microwave.deviceTools import udp_get_tool,udp2bin_tool,bin2ply_tool
from .manualMode import manual_merge
-cudir = os.path.abspath(os.path.join(__file__, *(['..'] * 1)))
-exe_path = os.path.abspath(os.path.join(cudir,"../RUN/Test_Servo.exe"))
+# cudir = os.path.abspath(os.path.join(__file__, *(['..'] * 1)))
+# exe_path = os.path.abspath(os.path.join(cudir,"../RUN/Test_Servo.exe"))
-def rotateAndMerge(port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder):
+def rotateAndMerge(servo_path,port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder):
# 鏃嬭浆鏈烘瀯澶嶄綅
+ exe_path = os.path.abspath(servo_path)
os.system(f"{exe_path} {port} 400 0")
time.sleep(10)
diff --git a/src/slam/core.py b/src/slam/core.py
index 7bf6a94..7401954 100644
--- a/src/slam/core.py
+++ b/src/slam/core.py
@@ -67,6 +67,12 @@
return rotation_matrix
+def get_origin_coordinates():
+ """
+ 鑾峰彇姣背娉㈣澶囨暟鎹腑蹇冪浉瀵硅埖鏈哄師鐐圭殑鍧愭爣
+ """
+ pass
+
def transferTo(current,delta_x,delta_y,delta_z,alpha,beta,gamma,reverse=False):
# Notice: 鍋囧畾褰撳墠鍧愭爣绯讳负鍙虫墜绯�
# step 1: 骞崇Щ锛沜urrent_coor + delta(delta_x, delta_y, delta_z) = target_coor
--
Gitblit v1.9.1