float support for servo controller

wu_xinjun

2022-03-25 a94b41088bb1a3bd71613e14ac2e97d2bb1e9a37

float support for servo controller

(cherry picked from commit f4af62ca256a0f5eed4f399f4501bc1dfc50975a)

 .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"
}

 RUN/Test_Servo.dll

Binary files differ

 RUN/Test_Servo.pdb

Binary files differ

 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 ================


 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!


 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")

 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 (

 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.')

 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




 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 @@
                
                # 判断第一个数据包中是否包含Footer 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)



 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!
echo protoc: aerial.proto compile done!

 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)


 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: 平移；current_coor + delta(delta_x, delta_y, delta_z) = target_coor