toys/simulation-py-ui/tools/simple_chassis.py

#!/usr/bin/env python3
# -*- coding: utf-8; mode: python; tab-width: 4; indent-tabs-mode: nil -*-
# vim: tabstop=4 expandtab shiftwidth=4 softtabstop=4 fileencoding=utf-8

import sys

import socket
import threading

import argparse
import struct

__author__ = ['"anjingyu" <anjingyu@navinfo.com>']

## Globals
__VERSION__ = ".".join(map(lambda x: str(), [0, 1, 0]))


class VehiclePacketExtended(object):
    def __init__(self, data: bytes = None):
        self.steeringWheelAngle = float(0.0)
        self.leftFrontSpeed = float(0.0)
        self.rightFrontSpeed = float(0.0)
        self.leftRearSpeed = float(0.0)
        self.rightRearSpeed = float(0.0)
        self.vehicleSpeed = float(0.0)
        self.yawRateFlag = float(0.0)
        self.yawRate = float(0.0)
        self.gear = float(0.0)
        self.vehicleAcc = float(0.0)
        self.systemTime = float(0.0)
        self.x = float(0.0)
        self.y = float(0.0)
        self.z = float(0.0)
        self.eastVelocity = float(0.0)
        self.northVelocity = float(0.0)
        self.upVelocity = float(0.0)
        self.pitch = float(0.0)
        self.roll = float(0.0)
        self.heading = float(0.0)
        self.xAcc = float(0.0)
        self.yAcc = float(0.0)
        self.zAcc = float(0.0)
        self.headingRate = float(0.0)
        self.pitchRate = float(0.0)
        self.rollRate = float(0.0)
        self.horizontalAccuracy = float(0.0)
        self.verticalAccuracy = float(0.0)
        self.speedAccuracy = float(0.0)
        self.headingAccuracy = float(0.0)
        self.gpsSpeed = float(0.0)
        self.gpsHeading = float(0.0)
        self.a0Error = float(0.0)
        self.laneColor = float(0.0)
        self.laneType = float(0.0)

        if data:
            (self.steeringWheelAngle, self.leftFrontSpeed,
             self.rightFrontSpeed, self.leftRearSpeed, self.rightRearSpeed,
             self.vehicleSpeed, self.yawRateFlag, self.yawRate, self.gear,
             self.vehicleAcc, self.systemTime, self.x, self.y, self.z,
             self.eastVelocity, self.northVelocity, self.upVelocity,
             self.pitch, self.roll, self.heading, self.xAcc, self.yAcc,
             self.zAcc, self.headingRate, self.pitchRate, self.rollRate,
             self.horizontalAccuracy, self.verticalAccuracy,
             self.speedAccuracy, self.headingAccuracy, self.gpsSpeed,
             self.gpsHeading, self.a0Error, self.laneColor,
             self.laneType) = struct.unpack("35d", data)

    def pack(self) -> bytes:
        return struct.pack(
            '35d', self.steeringWheelAngle, self.leftFrontSpeed,
            self.rightFrontSpeed, self.leftRearSpeed, self.rightRearSpeed,
            self.vehicleSpeed, self.yawRateFlag, self.yawRate, self.gear,
            self.vehicleAcc, self.systemTime, self.x, self.y, self.z,
            self.eastVelocity, self.northVelocity, self.upVelocity, self.pitch,
            self.roll, self.heading, self.xAcc, self.yAcc, self.zAcc,
            self.headingRate, self.pitchRate, self.rollRate,
            self.horizontalAccuracy, self.verticalAccuracy, self.speedAccuracy,
            self.headingAccuracy, self.gpsSpeed, self.gpsHeading, self.a0Error,
            self.laneColor, self.laneType)


class ControlCommandPacket:
    def __init__(self, data: bytes = None):
        self.timestamp = float(0.0)
        self.targetAcceleration = float(0.0)
        self.targetSteeringWheelAngle = float(0.0)
        self.targetGear = 3

        if data:
            (self.timestamp, self.targetAcceleration,
             self.targetSteeringWheelAngle,
             self.targetGear) = struct.unpack("3dq", data)

    def pack(self) -> bytes:
        return struct.pack("3dq", self.timestamp, self.targetAcceleration,
                           self.targetSteeringWheelAngle, self.targetGear)


class VehicleChassis:
    def __init__(self):
        self.chassis_pkg = VehiclePacketExtended()
        self.cmd_pkg = ControlCommandPacket()

    def update(self):
        pass

    def send(self):
        pass


class SimpleCarSimServer:
    def __init__(self):
        # Create a datagram socket
        self._socket = socket.socket(family=socket.AF_INET,
                                     type=socket.SOCK_DGRAM)
        self._chassis = VehicleChassis()

    def bind(self, ip: str, port: int):
        self._socket.bind((ip, port))

    def send_msg(self):
        pass

    def recv_msg(self):
        pass

    def run(self):
        t1 = threading.Thread(target=self.send_msg)
        t1.start()

        t2 = threading.Thread(target=self.recv_msg)
        t2.start()

def main():
    parser = argparse.ArgumentParser(
        description="Run a simple kinematic vehicle model.",
        usage='Use "python %(prog)s --help" for more information.',
        formatter_class=argparse.RawTextHelpFormatter,
        epilog="Typical usage:\n"
        "\tpython %(prog)s -a <address> -p <port>")
    parser.add_argument("-v",
                        "--version",
                        action="version",
                        version=f"{__VERSION__}")
    parser.add_argument("-a", "--address", required=True, help="Address.")
    parser.add_argument("-p", "--port", required=True, help="Port.")

    args = sys.argv[1:]
    if len(args) == 0:
        parser.print_help()
        sys.exit(0)


if __name__ == '__main__':
    main()