Update
This commit is contained in:
Konstantin Lampalzer
parent
d1e385a2a1
commit
1d91792c56
35 changed files with 2275 additions and 237 deletions
312
client_s2/.gitignore
vendored
Normal file
312
client_s2/.gitignore
vendored
Normal file
|
|
@ -0,0 +1,312 @@
|
|||
# Created by https://www.toptal.com/developers/gitignore/api/macos,python,pycharm
|
||||
# Edit at https://www.toptal.com/developers/gitignore?templates=macos,python,pycharm
|
||||
|
||||
### macOS ###
|
||||
# General
|
||||
.DS_Store
|
||||
.AppleDouble
|
||||
.LSOverride
|
||||
|
||||
# Icon must end with two \r
|
||||
Icon
|
||||
|
||||
|
||||
# Thumbnails
|
||||
._*
|
||||
|
||||
# Files that might appear in the root of a volume
|
||||
.DocumentRevisions-V100
|
||||
.fseventsd
|
||||
.Spotlight-V100
|
||||
.TemporaryItems
|
||||
.Trashes
|
||||
.VolumeIcon.icns
|
||||
.com.apple.timemachine.donotpresent
|
||||
|
||||
# Directories potentially created on remote AFP share
|
||||
.AppleDB
|
||||
.AppleDesktop
|
||||
Network Trash Folder
|
||||
Temporary Items
|
||||
.apdisk
|
||||
|
||||
### macOS Patch ###
|
||||
# iCloud generated files
|
||||
*.icloud
|
||||
|
||||
### PyCharm ###
|
||||
# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider
|
||||
# Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839
|
||||
|
||||
# User-specific stuff
|
||||
.idea/**/workspace.xml
|
||||
.idea/**/tasks.xml
|
||||
.idea/**/usage.statistics.xml
|
||||
.idea/**/dictionaries
|
||||
.idea/**/shelf
|
||||
|
||||
# AWS User-specific
|
||||
.idea/**/aws.xml
|
||||
|
||||
# Generated files
|
||||
.idea/**/contentModel.xml
|
||||
|
||||
# Sensitive or high-churn files
|
||||
.idea/**/dataSources/
|
||||
.idea/**/dataSources.ids
|
||||
.idea/**/dataSources.local.xml
|
||||
.idea/**/sqlDataSources.xml
|
||||
.idea/**/dynamic.xml
|
||||
.idea/**/uiDesigner.xml
|
||||
.idea/**/dbnavigator.xml
|
||||
|
||||
# Gradle
|
||||
.idea/**/gradle.xml
|
||||
.idea/**/libraries
|
||||
|
||||
# Gradle and Maven with auto-import
|
||||
# When using Gradle or Maven with auto-import, you should exclude module files,
|
||||
# since they will be recreated, and may cause churn. Uncomment if using
|
||||
# auto-import.
|
||||
# .idea/artifacts
|
||||
# .idea/compiler.xml
|
||||
# .idea/jarRepositories.xml
|
||||
# .idea/modules.xml
|
||||
# .idea/*.iml
|
||||
# .idea/modules
|
||||
# *.iml
|
||||
# *.ipr
|
||||
|
||||
# CMake
|
||||
cmake-build-*/
|
||||
|
||||
# Mongo Explorer plugin
|
||||
.idea/**/mongoSettings.xml
|
||||
|
||||
# File-based project format
|
||||
*.iws
|
||||
|
||||
# IntelliJ
|
||||
out/
|
||||
|
||||
# mpeltonen/sbt-idea plugin
|
||||
.idea_modules/
|
||||
|
||||
# JIRA plugin
|
||||
atlassian-ide-plugin.xml
|
||||
|
||||
# Cursive Clojure plugin
|
||||
.idea/replstate.xml
|
||||
|
||||
# SonarLint plugin
|
||||
.idea/sonarlint/
|
||||
|
||||
# Crashlytics plugin (for Android Studio and IntelliJ)
|
||||
com_crashlytics_export_strings.xml
|
||||
crashlytics.properties
|
||||
crashlytics-build.properties
|
||||
fabric.properties
|
||||
|
||||
# Editor-based Rest Client
|
||||
.idea/httpRequests
|
||||
|
||||
# Android studio 3.1+ serialized cache file
|
||||
.idea/caches/build_file_checksums.ser
|
||||
|
||||
### PyCharm Patch ###
|
||||
# Comment Reason: https://github.com/joeblau/gitignore.io/issues/186#issuecomment-215987721
|
||||
|
||||
# *.iml
|
||||
# modules.xml
|
||||
# .idea/misc.xml
|
||||
# *.ipr
|
||||
|
||||
# Sonarlint plugin
|
||||
# https://plugins.jetbrains.com/plugin/7973-sonarlint
|
||||
.idea/**/sonarlint/
|
||||
|
||||
# SonarQube Plugin
|
||||
# https://plugins.jetbrains.com/plugin/7238-sonarqube-community-plugin
|
||||
.idea/**/sonarIssues.xml
|
||||
|
||||
# Markdown Navigator plugin
|
||||
# https://plugins.jetbrains.com/plugin/7896-markdown-navigator-enhanced
|
||||
.idea/**/markdown-navigator.xml
|
||||
.idea/**/markdown-navigator-enh.xml
|
||||
.idea/**/markdown-navigator/
|
||||
|
||||
# Cache file creation bug
|
||||
# See https://youtrack.jetbrains.com/issue/JBR-2257
|
||||
.idea/$CACHE_FILE$
|
||||
|
||||
# CodeStream plugin
|
||||
# https://plugins.jetbrains.com/plugin/12206-codestream
|
||||
.idea/codestream.xml
|
||||
|
||||
# Azure Toolkit for IntelliJ plugin
|
||||
# https://plugins.jetbrains.com/plugin/8053-azure-toolkit-for-intellij
|
||||
.idea/**/azureSettings.xml
|
||||
|
||||
### Python ###
|
||||
# Byte-compiled / optimized / DLL files
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
*$py.class
|
||||
|
||||
# C extensions
|
||||
*.so
|
||||
|
||||
# Distribution / packaging
|
||||
.Python
|
||||
build/
|
||||
develop-eggs/
|
||||
dist/
|
||||
downloads/
|
||||
eggs/
|
||||
.eggs/
|
||||
lib/
|
||||
lib64/
|
||||
parts/
|
||||
sdist/
|
||||
var/
|
||||
wheels/
|
||||
share/python-wheels/
|
||||
*.egg-info/
|
||||
.installed.cfg
|
||||
*.egg
|
||||
MANIFEST
|
||||
|
||||
# PyInstaller
|
||||
# Usually these files are written by a python script from a template
|
||||
# before PyInstaller builds the exe, so as to inject date/other infos into it.
|
||||
*.manifest
|
||||
*.spec
|
||||
|
||||
# Installer logs
|
||||
pip-log.txt
|
||||
pip-delete-this-directory.txt
|
||||
|
||||
# Unit test / coverage reports
|
||||
htmlcov/
|
||||
.tox/
|
||||
.nox/
|
||||
.coverage
|
||||
.coverage.*
|
||||
.cache
|
||||
nosetests.xml
|
||||
coverage.xml
|
||||
*.cover
|
||||
*.py,cover
|
||||
.hypothesis/
|
||||
.pytest_cache/
|
||||
cover/
|
||||
|
||||
# Translations
|
||||
*.mo
|
||||
*.pot
|
||||
|
||||
# Django stuff:
|
||||
*.log
|
||||
local_settings.py
|
||||
db.sqlite3
|
||||
db.sqlite3-journal
|
||||
|
||||
# Flask stuff:
|
||||
instance/
|
||||
.webassets-cache
|
||||
|
||||
# Scrapy stuff:
|
||||
.scrapy
|
||||
|
||||
# Sphinx documentation
|
||||
docs/_build/
|
||||
|
||||
# PyBuilder
|
||||
.pybuilder/
|
||||
target/
|
||||
|
||||
# Jupyter Notebook
|
||||
.ipynb_checkpoints
|
||||
|
||||
# IPython
|
||||
profile_default/
|
||||
ipython_config.py
|
||||
|
||||
# pyenv
|
||||
# For a library or package, you might want to ignore these files since the code is
|
||||
# intended to run in multiple environments; otherwise, check them in:
|
||||
# .python-version
|
||||
|
||||
# pipenv
|
||||
# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
|
||||
# However, in case of collaboration, if having platform-specific dependencies or dependencies
|
||||
# having no cross-platform support, pipenv may install dependencies that don't work, or not
|
||||
# install all needed dependencies.
|
||||
#Pipfile.lock
|
||||
|
||||
# poetry
|
||||
# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
|
||||
# This is especially recommended for binary packages to ensure reproducibility, and is more
|
||||
# commonly ignored for libraries.
|
||||
# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
|
||||
#poetry.lock
|
||||
|
||||
# pdm
|
||||
# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
|
||||
#pdm.lock
|
||||
# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
|
||||
# in version control.
|
||||
# https://pdm.fming.dev/#use-with-ide
|
||||
.pdm.toml
|
||||
|
||||
# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
|
||||
__pypackages__/
|
||||
|
||||
# Celery stuff
|
||||
celerybeat-schedule
|
||||
celerybeat.pid
|
||||
|
||||
# SageMath parsed files
|
||||
*.sage.py
|
||||
|
||||
# Environments
|
||||
.env
|
||||
.venv
|
||||
env/
|
||||
venv/
|
||||
ENV/
|
||||
env.bak/
|
||||
venv.bak/
|
||||
|
||||
# Spyder project settings
|
||||
.spyderproject
|
||||
.spyproject
|
||||
|
||||
# Rope project settings
|
||||
.ropeproject
|
||||
|
||||
# mkdocs documentation
|
||||
/site
|
||||
|
||||
# mypy
|
||||
.mypy_cache/
|
||||
.dmypy.json
|
||||
dmypy.json
|
||||
|
||||
# Pyre type checker
|
||||
.pyre/
|
||||
|
||||
# pytype static type analyzer
|
||||
.pytype/
|
||||
|
||||
# Cython debug symbols
|
||||
cython_debug/
|
||||
|
||||
# PyCharm
|
||||
# JetBrains specific template is maintained in a separate JetBrains.gitignore that can
|
||||
# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
|
||||
# and can be added to the global gitignore or merged into this file. For a more nuclear
|
||||
# option (not recommended) you can uncomment the following to ignore the entire idea folder.
|
||||
#.idea/
|
||||
|
||||
# End of https://www.toptal.com/developers/gitignore/api/macos,python,pycharm
|
||||
312
client_s2/compLib/.gitignore
vendored
Normal file
312
client_s2/compLib/.gitignore
vendored
Normal file
|
|
@ -0,0 +1,312 @@
|
|||
# Created by https://www.toptal.com/developers/gitignore/api/macos,python,pycharm
|
||||
# Edit at https://www.toptal.com/developers/gitignore?templates=macos,python,pycharm
|
||||
|
||||
### macOS ###
|
||||
# General
|
||||
.DS_Store
|
||||
.AppleDouble
|
||||
.LSOverride
|
||||
|
||||
# Icon must end with two \r
|
||||
Icon
|
||||
|
||||
|
||||
# Thumbnails
|
||||
._*
|
||||
|
||||
# Files that might appear in the root of a volume
|
||||
.DocumentRevisions-V100
|
||||
.fseventsd
|
||||
.Spotlight-V100
|
||||
.TemporaryItems
|
||||
.Trashes
|
||||
.VolumeIcon.icns
|
||||
.com.apple.timemachine.donotpresent
|
||||
|
||||
# Directories potentially created on remote AFP share
|
||||
.AppleDB
|
||||
.AppleDesktop
|
||||
Network Trash Folder
|
||||
Temporary Items
|
||||
.apdisk
|
||||
|
||||
### macOS Patch ###
|
||||
# iCloud generated files
|
||||
*.icloud
|
||||
|
||||
### PyCharm ###
|
||||
# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider
|
||||
# Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839
|
||||
|
||||
# User-specific stuff
|
||||
.idea/**/workspace.xml
|
||||
.idea/**/tasks.xml
|
||||
.idea/**/usage.statistics.xml
|
||||
.idea/**/dictionaries
|
||||
.idea/**/shelf
|
||||
|
||||
# AWS User-specific
|
||||
.idea/**/aws.xml
|
||||
|
||||
# Generated files
|
||||
.idea/**/contentModel.xml
|
||||
|
||||
# Sensitive or high-churn files
|
||||
.idea/**/dataSources/
|
||||
.idea/**/dataSources.ids
|
||||
.idea/**/dataSources.local.xml
|
||||
.idea/**/sqlDataSources.xml
|
||||
.idea/**/dynamic.xml
|
||||
.idea/**/uiDesigner.xml
|
||||
.idea/**/dbnavigator.xml
|
||||
|
||||
# Gradle
|
||||
.idea/**/gradle.xml
|
||||
.idea/**/libraries
|
||||
|
||||
# Gradle and Maven with auto-import
|
||||
# When using Gradle or Maven with auto-import, you should exclude module files,
|
||||
# since they will be recreated, and may cause churn. Uncomment if using
|
||||
# auto-import.
|
||||
# .idea/artifacts
|
||||
# .idea/compiler.xml
|
||||
# .idea/jarRepositories.xml
|
||||
# .idea/modules.xml
|
||||
# .idea/*.iml
|
||||
# .idea/modules
|
||||
# *.iml
|
||||
# *.ipr
|
||||
|
||||
# CMake
|
||||
cmake-build-*/
|
||||
|
||||
# Mongo Explorer plugin
|
||||
.idea/**/mongoSettings.xml
|
||||
|
||||
# File-based project format
|
||||
*.iws
|
||||
|
||||
# IntelliJ
|
||||
out/
|
||||
|
||||
# mpeltonen/sbt-idea plugin
|
||||
.idea_modules/
|
||||
|
||||
# JIRA plugin
|
||||
atlassian-ide-plugin.xml
|
||||
|
||||
# Cursive Clojure plugin
|
||||
.idea/replstate.xml
|
||||
|
||||
# SonarLint plugin
|
||||
.idea/sonarlint/
|
||||
|
||||
# Crashlytics plugin (for Android Studio and IntelliJ)
|
||||
com_crashlytics_export_strings.xml
|
||||
crashlytics.properties
|
||||
crashlytics-build.properties
|
||||
fabric.properties
|
||||
|
||||
# Editor-based Rest Client
|
||||
.idea/httpRequests
|
||||
|
||||
# Android studio 3.1+ serialized cache file
|
||||
.idea/caches/build_file_checksums.ser
|
||||
|
||||
### PyCharm Patch ###
|
||||
# Comment Reason: https://github.com/joeblau/gitignore.io/issues/186#issuecomment-215987721
|
||||
|
||||
# *.iml
|
||||
# modules.xml
|
||||
# .idea/misc.xml
|
||||
# *.ipr
|
||||
|
||||
# Sonarlint plugin
|
||||
# https://plugins.jetbrains.com/plugin/7973-sonarlint
|
||||
.idea/**/sonarlint/
|
||||
|
||||
# SonarQube Plugin
|
||||
# https://plugins.jetbrains.com/plugin/7238-sonarqube-community-plugin
|
||||
.idea/**/sonarIssues.xml
|
||||
|
||||
# Markdown Navigator plugin
|
||||
# https://plugins.jetbrains.com/plugin/7896-markdown-navigator-enhanced
|
||||
.idea/**/markdown-navigator.xml
|
||||
.idea/**/markdown-navigator-enh.xml
|
||||
.idea/**/markdown-navigator/
|
||||
|
||||
# Cache file creation bug
|
||||
# See https://youtrack.jetbrains.com/issue/JBR-2257
|
||||
.idea/$CACHE_FILE$
|
||||
|
||||
# CodeStream plugin
|
||||
# https://plugins.jetbrains.com/plugin/12206-codestream
|
||||
.idea/codestream.xml
|
||||
|
||||
# Azure Toolkit for IntelliJ plugin
|
||||
# https://plugins.jetbrains.com/plugin/8053-azure-toolkit-for-intellij
|
||||
.idea/**/azureSettings.xml
|
||||
|
||||
### Python ###
|
||||
# Byte-compiled / optimized / DLL files
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
*$py.class
|
||||
|
||||
# C extensions
|
||||
*.so
|
||||
|
||||
# Distribution / packaging
|
||||
.Python
|
||||
build/
|
||||
develop-eggs/
|
||||
dist/
|
||||
downloads/
|
||||
eggs/
|
||||
.eggs/
|
||||
lib/
|
||||
lib64/
|
||||
parts/
|
||||
sdist/
|
||||
var/
|
||||
wheels/
|
||||
share/python-wheels/
|
||||
*.egg-info/
|
||||
.installed.cfg
|
||||
*.egg
|
||||
MANIFEST
|
||||
|
||||
# PyInstaller
|
||||
# Usually these files are written by a python script from a template
|
||||
# before PyInstaller builds the exe, so as to inject date/other infos into it.
|
||||
*.manifest
|
||||
*.spec
|
||||
|
||||
# Installer logs
|
||||
pip-log.txt
|
||||
pip-delete-this-directory.txt
|
||||
|
||||
# Unit test / coverage reports
|
||||
htmlcov/
|
||||
.tox/
|
||||
.nox/
|
||||
.coverage
|
||||
.coverage.*
|
||||
.cache
|
||||
nosetests.xml
|
||||
coverage.xml
|
||||
*.cover
|
||||
*.py,cover
|
||||
.hypothesis/
|
||||
.pytest_cache/
|
||||
cover/
|
||||
|
||||
# Translations
|
||||
*.mo
|
||||
*.pot
|
||||
|
||||
# Django stuff:
|
||||
*.log
|
||||
local_settings.py
|
||||
db.sqlite3
|
||||
db.sqlite3-journal
|
||||
|
||||
# Flask stuff:
|
||||
instance/
|
||||
.webassets-cache
|
||||
|
||||
# Scrapy stuff:
|
||||
.scrapy
|
||||
|
||||
# Sphinx documentation
|
||||
docs/_build/
|
||||
|
||||
# PyBuilder
|
||||
.pybuilder/
|
||||
target/
|
||||
|
||||
# Jupyter Notebook
|
||||
.ipynb_checkpoints
|
||||
|
||||
# IPython
|
||||
profile_default/
|
||||
ipython_config.py
|
||||
|
||||
# pyenv
|
||||
# For a library or package, you might want to ignore these files since the code is
|
||||
# intended to run in multiple environments; otherwise, check them in:
|
||||
# .python-version
|
||||
|
||||
# pipenv
|
||||
# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
|
||||
# However, in case of collaboration, if having platform-specific dependencies or dependencies
|
||||
# having no cross-platform support, pipenv may install dependencies that don't work, or not
|
||||
# install all needed dependencies.
|
||||
#Pipfile.lock
|
||||
|
||||
# poetry
|
||||
# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
|
||||
# This is especially recommended for binary packages to ensure reproducibility, and is more
|
||||
# commonly ignored for libraries.
|
||||
# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
|
||||
#poetry.lock
|
||||
|
||||
# pdm
|
||||
# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
|
||||
#pdm.lock
|
||||
# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
|
||||
# in version control.
|
||||
# https://pdm.fming.dev/#use-with-ide
|
||||
.pdm.toml
|
||||
|
||||
# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
|
||||
__pypackages__/
|
||||
|
||||
# Celery stuff
|
||||
celerybeat-schedule
|
||||
celerybeat.pid
|
||||
|
||||
# SageMath parsed files
|
||||
*.sage.py
|
||||
|
||||
# Environments
|
||||
.env
|
||||
.venv
|
||||
env/
|
||||
venv/
|
||||
ENV/
|
||||
env.bak/
|
||||
venv.bak/
|
||||
|
||||
# Spyder project settings
|
||||
.spyderproject
|
||||
.spyproject
|
||||
|
||||
# Rope project settings
|
||||
.ropeproject
|
||||
|
||||
# mkdocs documentation
|
||||
/site
|
||||
|
||||
# mypy
|
||||
.mypy_cache/
|
||||
.dmypy.json
|
||||
dmypy.json
|
||||
|
||||
# Pyre type checker
|
||||
.pyre/
|
||||
|
||||
# pytype static type analyzer
|
||||
.pytype/
|
||||
|
||||
# Cython debug symbols
|
||||
cython_debug/
|
||||
|
||||
# PyCharm
|
||||
# JetBrains specific template is maintained in a separate JetBrains.gitignore that can
|
||||
# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
|
||||
# and can be added to the global gitignore or merged into this file. For a more nuclear
|
||||
# option (not recommended) you can uncomment the following to ignore the entire idea folder.
|
||||
#.idea/
|
||||
|
||||
# End of https://www.toptal.com/developers/gitignore/api/macos,python,pycharm
|
||||
|
|
@ -58,16 +58,24 @@ message IRSensorsReadAllResponse {
|
|||
repeated uint32 data = 3 [packed = true];
|
||||
}
|
||||
|
||||
message MotorSetPowerRequest {
|
||||
uint32 port = 1;
|
||||
double power = 2;
|
||||
}
|
||||
|
||||
message MotorsSetPowerRequest {
|
||||
Header header = 1;
|
||||
uint32 port = 2;
|
||||
double power = 3;
|
||||
repeated MotorSetPowerRequest requests = 2;
|
||||
}
|
||||
|
||||
message MotorSetSpeedRequest {
|
||||
uint32 port = 1;
|
||||
double speed = 2;
|
||||
}
|
||||
|
||||
message MotorsSetSpeedRequest {
|
||||
Header header = 1;
|
||||
uint32 port = 2;
|
||||
double speed = 3;
|
||||
repeated MotorSetSpeedRequest requests = 2;
|
||||
}
|
||||
|
||||
message OdometryReadRequest {
|
||||
|
|
@ -98,4 +106,8 @@ message DriveRequest {
|
|||
Header header = 1;
|
||||
double linear_velocity_m_s = 2;
|
||||
double angular_velocity_rad_s = 3;
|
||||
}
|
||||
|
||||
message HealthUpdateRequest {
|
||||
Header header = 1;
|
||||
}
|
||||
|
|
@ -1,30 +1,65 @@
|
|||
import socket
|
||||
from threading import Lock
|
||||
|
||||
import compLib.CompLib_pb2 as CompLib_pb2
|
||||
|
||||
UNIX_SOCKET_PATH = "/tmp/compLib"
|
||||
TCP_SOCKET_HOST = "192.168.0.151"
|
||||
TCP_SOCKET_PORT = 9090
|
||||
|
||||
|
||||
class CompLibClient(object):
|
||||
USE_UNIX_SOCKET = False
|
||||
UNIX_SOCKET_PATH = "/tmp/compLib"
|
||||
USE_TCP_SOCKET = False
|
||||
TCP_SOCKET_HOST = "10.20.5.1"
|
||||
TCP_SOCKET_PORT = 9090
|
||||
SOCKET = None
|
||||
LOCK = Lock()
|
||||
|
||||
@staticmethod
|
||||
def use_unix_socket(socket_path="/tmp/compLib"):
|
||||
CompLibClient.UNIX_SOCKET_PATH = socket_path
|
||||
CompLibClient.USE_UNIX_SOCKET = True
|
||||
CompLibClient.USE_TCP_SOCKET = False
|
||||
|
||||
CompLibClient.SOCKET = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
|
||||
CompLibClient.SOCKET.connect(CompLibClient.UNIX_SOCKET_PATH)
|
||||
|
||||
from compLib.HealthCheck import HealthUpdater
|
||||
HealthUpdater.start()
|
||||
|
||||
@staticmethod
|
||||
def use_tcp_socket(socket_host, socket_port=TCP_SOCKET_PORT):
|
||||
CompLibClient.TCP_SOCKET_HOST = socket_host
|
||||
CompLibClient.TCP_SOCKET_PORT = socket_port
|
||||
CompLibClient.USE_UNIX_SOCKET = False
|
||||
CompLibClient.USE_TCP_SOCKET = True
|
||||
|
||||
CompLibClient.SOCKET = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
CompLibClient.SOCKET.connect((CompLibClient.TCP_SOCKET_HOST, CompLibClient.TCP_SOCKET_PORT))
|
||||
|
||||
from compLib.HealthCheck import HealthUpdater
|
||||
HealthUpdater.start()
|
||||
|
||||
@staticmethod
|
||||
def send(data: bytes, size: int) -> bytes:
|
||||
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
|
||||
sock.connect((TCP_SOCKET_HOST, TCP_SOCKET_PORT))
|
||||
with CompLibClient.LOCK:
|
||||
# if CompLibClient.USE_TCP_SOCKET:
|
||||
# sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
# sock.connect((CompLibClient.TCP_SOCKET_HOST, CompLibClient.TCP_SOCKET_PORT))
|
||||
#
|
||||
# elif CompLibClient.USE_UNIX_SOCKET:
|
||||
# sock = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
|
||||
# sock.connect(CompLibClient.UNIX_SOCKET_PATH)
|
||||
#
|
||||
# else:
|
||||
# return bytes(0)
|
||||
|
||||
# with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock:
|
||||
# sock.connect(UNIX_SOCKET_PATH)
|
||||
CompLibClient.SOCKET.sendall(size.to_bytes(1, byteorder='big'))
|
||||
CompLibClient.SOCKET.sendall(data)
|
||||
|
||||
sock.sendall(size.to_bytes(1, byteorder='big'))
|
||||
sock.sendall(data)
|
||||
|
||||
response_size_bytes = sock.recv(1)
|
||||
response_size_bytes = CompLibClient.SOCKET.recv(1)
|
||||
response_size = int.from_bytes(response_size_bytes, byteorder="big")
|
||||
# print(response_size)
|
||||
|
||||
response_bytes = sock.recv(response_size)
|
||||
response_bytes = CompLibClient.SOCKET.recv(response_size)
|
||||
# print(response_bytes.hex())
|
||||
# print(len(response_bytes))
|
||||
|
||||
|
|
@ -34,7 +69,7 @@ class CompLibClient(object):
|
|||
|
||||
@staticmethod
|
||||
def check_response(response_bytes: bytes) -> bool:
|
||||
print(f"{response_bytes}")
|
||||
# print(f"{response_bytes}")
|
||||
res = CompLib_pb2.GenericResponse()
|
||||
res.ParseFromString(response_bytes)
|
||||
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load diff
23
client_s2/compLib/HealthCheck.py
Normal file
23
client_s2/compLib/HealthCheck.py
Normal file
|
|
@ -0,0 +1,23 @@
|
|||
import threading
|
||||
import time
|
||||
|
||||
import compLib.CompLib_pb2 as CompLib_pb2
|
||||
from compLib.CompLibClient import CompLibClient
|
||||
|
||||
|
||||
class HealthUpdater(object):
|
||||
started = False
|
||||
|
||||
@staticmethod
|
||||
def start():
|
||||
if not HealthUpdater.started:
|
||||
threading.Thread(target=HealthUpdater.loop, daemon=True).start()
|
||||
HealthUpdater.started = True
|
||||
|
||||
@staticmethod
|
||||
def loop():
|
||||
while True:
|
||||
request = CompLib_pb2.HealthUpdateRequest()
|
||||
request.header.message_type = request.DESCRIPTOR.full_name
|
||||
CompLibClient.send(request.SerializeToString(), request.ByteSize())
|
||||
time.sleep(0.25)
|
||||
|
|
@ -1,4 +1,5 @@
|
|||
import compLib.CompLib_pb2 as CompLib_pb2
|
||||
|
||||
from compLib.CompLibClient import CompLibClient
|
||||
|
||||
|
||||
|
|
@ -10,7 +11,7 @@ class IRSensor(object):
|
|||
def read_all():
|
||||
"""Read all IR sensors at once.
|
||||
|
||||
:return: Tuple of all current ir sensors
|
||||
:return: Array of all current ir sensors
|
||||
"""
|
||||
request = CompLib_pb2.IRSensorsReadAllRequest()
|
||||
request.header.message_type = request.DESCRIPTOR.full_name
|
||||
|
|
@ -18,7 +19,7 @@ class IRSensor(object):
|
|||
response = CompLib_pb2.IRSensorsReadAllResponse()
|
||||
response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize()))
|
||||
|
||||
return tuple(i for i in response.data)
|
||||
return [i for i in response.data]
|
||||
|
||||
@staticmethod
|
||||
def enable():
|
||||
|
|
|
|||
|
|
@ -30,6 +30,31 @@ class Motor(object):
|
|||
|
||||
CompLibClient.send(request.SerializeToString(), request.ByteSize())
|
||||
|
||||
@staticmethod
|
||||
def multiple_power(*arguments: tuple[int, float]):
|
||||
"""Set specified motors to percentage power
|
||||
|
||||
:param arguments: tuples of port, percentage
|
||||
:raises: IndexError
|
||||
"""
|
||||
request = CompLib_pb2.MotorsSetPowerRequest()
|
||||
request.header.message_type = request.DESCRIPTOR.full_name
|
||||
|
||||
for port, percent in arguments:
|
||||
if port < 0 or port >= MOTOR_COUNT:
|
||||
raise IndexError("Invalid Motor port specified!")
|
||||
|
||||
if percent < -100 or percent > 100:
|
||||
raise IndexError("Invalid Motor speed specified! Speed is between -100 and 100 percent!")
|
||||
|
||||
inner_request = CompLib_pb2.MotorSetPowerRequest()
|
||||
inner_request.port = port
|
||||
inner_request.power = percent
|
||||
|
||||
request.requests.append(inner_request)
|
||||
|
||||
CompLibClient.send(request.SerializeToString(), request.ByteSize())
|
||||
|
||||
@staticmethod
|
||||
def speed(port: int, speed: float):
|
||||
"""Set specified motor to percentage power
|
||||
|
|
@ -49,6 +74,29 @@ class Motor(object):
|
|||
|
||||
CompLibClient.send(request.SerializeToString(), request.ByteSize())
|
||||
|
||||
@staticmethod
|
||||
def multiple_speed(*arguments: tuple[int, float]):
|
||||
"""Set specified motor to percentage power
|
||||
|
||||
:param arguments: tuples of port, speed in rpm
|
||||
:raises: IndexError
|
||||
"""
|
||||
|
||||
request = CompLib_pb2.MotorsSetSpeedRequest()
|
||||
request.header.message_type = request.DESCRIPTOR.full_name
|
||||
|
||||
for port, speed in arguments:
|
||||
if port < 0 or port >= MOTOR_COUNT:
|
||||
raise IndexError("Invalid Motor port specified!")
|
||||
|
||||
inner_request = CompLib_pb2.MotorSetSpeedRequest()
|
||||
inner_request.port = port
|
||||
inner_request.speed = speed
|
||||
|
||||
request.requests.append(inner_request)
|
||||
|
||||
CompLibClient.send(request.SerializeToString(), request.ByteSize())
|
||||
|
||||
# @staticmethod
|
||||
# def all_off():
|
||||
# """
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
24
client_s2/dev01.py
Normal file
24
client_s2/dev01.py
Normal file
|
|
@ -0,0 +1,24 @@
|
|||
import time
|
||||
|
||||
from compLib.CompLibClient import CompLibClient
|
||||
|
||||
|
||||
def main():
|
||||
from compLib.Motor import Motor
|
||||
|
||||
# Motor.speed(0, -50)
|
||||
# Motor.speed(3, 50)
|
||||
|
||||
Motor.power(0, 50)
|
||||
Motor.power(3, -50)
|
||||
|
||||
time.sleep(2)
|
||||
|
||||
Motor.power(0, 0)
|
||||
Motor.power(3, -0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
CompLibClient.use_tcp_socket("dev01.local")
|
||||
# follow()
|
||||
main()
|
||||
34
client_s2/dev03.py
Normal file
34
client_s2/dev03.py
Normal file
|
|
@ -0,0 +1,34 @@
|
|||
import time
|
||||
|
||||
from compLib.CompLibClient import CompLibClient
|
||||
from compLib.IRSensor import IRSensor
|
||||
|
||||
|
||||
def main():
|
||||
# Motor.speed(0, -50)
|
||||
# Motor.speed(3, 50)
|
||||
|
||||
# Motor.power(0, 50)
|
||||
# Motor.power(3, -50)
|
||||
#
|
||||
# time.sleep(2)
|
||||
#
|
||||
# Motor.power(0, 0)
|
||||
# Motor.power(3, -0)
|
||||
|
||||
start_time = time.time()
|
||||
for i in range(0, 1000):
|
||||
IRSensor.read_all()
|
||||
# Motor.multiple_power((0, 1), (3, 1))
|
||||
# Motor.speed(0, 1)
|
||||
# Motor.speed(3, 1)
|
||||
|
||||
print(1000.0 / (time.time() - start_time))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# CompLibClient.use_tcp_socket("dev03.local")
|
||||
CompLibClient.use_unix_socket()
|
||||
# follow()
|
||||
# cProfile.run("main()")
|
||||
main()
|
||||
109
client_s2/lf.py
Normal file
109
client_s2/lf.py
Normal file
|
|
@ -0,0 +1,109 @@
|
|||
import time
|
||||
|
||||
from compLib.CompLibClient import CompLibClient
|
||||
from compLib.IRSensor import IRSensor
|
||||
from compLib.Motor import Motor
|
||||
|
||||
DRIVE_SPEED = 5.0
|
||||
COLOR_BREAK = 1500.0
|
||||
KP = 2.0
|
||||
KD = 0.0
|
||||
|
||||
|
||||
def drive(left_speed, right_speed):
|
||||
print(left_speed, right_speed)
|
||||
|
||||
right_speed *= -1.0
|
||||
|
||||
Motor.speed(0, right_speed)
|
||||
Motor.speed(3, left_speed)
|
||||
|
||||
|
||||
def follow(sleep_time=0.1):
|
||||
last_error = 0
|
||||
sensors_black = 0
|
||||
while sensors_black <= 3:
|
||||
sensor_values = IRSensor.read_all()
|
||||
|
||||
sensors_black = 0
|
||||
for sensor in sensor_values:
|
||||
if sensor > COLOR_BREAK:
|
||||
sensors_black += 1
|
||||
|
||||
error = last_error
|
||||
if sensor_values[2] > COLOR_BREAK:
|
||||
error = 0
|
||||
elif sensor_values[0] > COLOR_BREAK:
|
||||
error = -1.5
|
||||
elif sensor_values[4] > COLOR_BREAK:
|
||||
error = 1.5
|
||||
elif sensor_values[1] > COLOR_BREAK:
|
||||
error = -1
|
||||
elif sensor_values[3] > COLOR_BREAK:
|
||||
error = 1
|
||||
elif error == 1.5:
|
||||
error = 3.5
|
||||
elif error == -1.5:
|
||||
error = -3.5
|
||||
|
||||
last_error = error
|
||||
|
||||
adjustment = KP * error + KD * (error - last_error)
|
||||
left_speed = DRIVE_SPEED + adjustment
|
||||
right_speed = DRIVE_SPEED - adjustment
|
||||
|
||||
print(sensor_values)
|
||||
print(f"{left_speed} {right_speed} {adjustment} {error}")
|
||||
drive(left_speed, right_speed)
|
||||
|
||||
drive(0, 0)
|
||||
time.sleep(sleep_time)
|
||||
|
||||
|
||||
def follow_simple():
|
||||
left_speed = DRIVE_SPEED
|
||||
right_speed = DRIVE_SPEED
|
||||
sensor_values = IRSensor.read_all()
|
||||
while True:
|
||||
sensor_values = IRSensor.read_all()
|
||||
# for i in range(len(sensor_values)):
|
||||
# sensor_values[i] = (sensor_values[i] + new_sensor_values[i]) / 2.0
|
||||
|
||||
print(sensor_values)
|
||||
|
||||
if sensor_values[0] > COLOR_BREAK and sensor_values[4] > COLOR_BREAK:
|
||||
break
|
||||
|
||||
if sensor_values[0] > COLOR_BREAK:
|
||||
left_speed = -DRIVE_SPEED / 2
|
||||
right_speed = DRIVE_SPEED
|
||||
elif sensor_values[4] > COLOR_BREAK:
|
||||
left_speed = DRIVE_SPEED
|
||||
right_speed = -DRIVE_SPEED / 2
|
||||
elif sensor_values[2] > COLOR_BREAK:
|
||||
left_speed = DRIVE_SPEED
|
||||
right_speed = DRIVE_SPEED
|
||||
|
||||
drive(left_speed, right_speed)
|
||||
|
||||
|
||||
def main():
|
||||
CompLibClient.use_unix_socket()
|
||||
|
||||
IRSensor.enable()
|
||||
time.sleep(0.1)
|
||||
|
||||
# while True:
|
||||
# print(IRSensor.read_all())
|
||||
|
||||
# follow_simple()
|
||||
# drive(5, 5)
|
||||
# time.sleep(5)
|
||||
# follow()
|
||||
# follow()
|
||||
# follow()
|
||||
# follow()
|
||||
# follow(0.2)
|
||||
|
||||
|
||||
main()
|
||||
|
|
@ -1,4 +1,6 @@
|
|||
SOCKET_PATH = "/tmp/compLib"
|
||||
import time
|
||||
|
||||
from compLib.CompLibClient import CompLibClient
|
||||
|
||||
|
||||
# def send(data, size):
|
||||
|
|
@ -30,61 +32,143 @@ def main():
|
|||
# send(encoder_read_positions_request.SerializeToString(), encoder_read_positions_request.ByteSize())
|
||||
# print("--- %s seconds ---" % (time.time() - start_time))
|
||||
|
||||
# from compLib.IRSensor import IRSensor
|
||||
# IRSensor.read_all()
|
||||
#
|
||||
from compLib.IRSensor import IRSensor
|
||||
IRSensor.enable()
|
||||
|
||||
startTime = time.time()
|
||||
while time.time() - startTime < 10:
|
||||
print(IRSensor.read_all())
|
||||
time.sleep(0.01)
|
||||
|
||||
# from compLib.Encoder import Encoder
|
||||
# print(Encoder.read_all_positions())
|
||||
# print(Encoder.read_all_velocities())
|
||||
|
||||
# from compLib.Motor import Motor
|
||||
# Motor.speed(0, 50)
|
||||
# Motor.speed(3, -50)
|
||||
#
|
||||
# import time
|
||||
# time.sleep(2)
|
||||
#
|
||||
# Motor.speed(0, 0)
|
||||
# Motor.speed(3, -0)
|
||||
|
||||
# Motor.power(0, 0)
|
||||
# Motor.power(3, 0)
|
||||
# Motor.speed(0, -50)
|
||||
# Motor.speed(3, 50)
|
||||
|
||||
# import math
|
||||
# from compLib.Movement import Movement
|
||||
# Movement.turn_degrees(90, math.pi * 2)
|
||||
# Movement.turn_degrees(-90, math.pi * 2)
|
||||
#
|
||||
# Movement.turn_degrees(90, math.pi * 2)
|
||||
# Movement.turn_degrees(90, -math.pi * 2)
|
||||
#
|
||||
# Movement.turn_degrees(90, math.pi * 2)
|
||||
# Movement.turn_degrees(-90, -math.pi * 2)
|
||||
# Motor.power(0, -50)
|
||||
# Motor.power(3, 50)
|
||||
|
||||
# from compLib.Movement import Movement
|
||||
# Movement.drive_distance(0.1, 0.5)
|
||||
# Movement.drive_distance(-0.1, 0.5)
|
||||
#
|
||||
# Movement.drive_distance(0.1, 0.5)
|
||||
# Movement.drive_distance(0.1, -0.5)
|
||||
#
|
||||
# Movement.drive_distance(0.1, 0.5)
|
||||
# Movement.drive_distance(-0.1, -0.5)
|
||||
# time.sleep(5)
|
||||
|
||||
from compLib.Movement import Movement
|
||||
import math
|
||||
Movement.drive_distance(0.5, 0.5)
|
||||
Movement.turn_degrees(90, math.pi * 2)
|
||||
|
||||
Movement.drive_distance(0.5, 0.5)
|
||||
Movement.turn_degrees(90, math.pi * 2)
|
||||
#
|
||||
# import time
|
||||
# time.sleep(2)
|
||||
#
|
||||
# Motor.speed(0, 0)
|
||||
# Motor.speed(3, -0)
|
||||
|
||||
Movement.drive_distance(0.5, 0.5)
|
||||
Movement.turn_degrees(90, math.pi * 2)
|
||||
# Motor.power(0, 0)
|
||||
# Motor.power(3, 0)
|
||||
|
||||
Movement.drive_distance(0.5, 0.5)
|
||||
Movement.turn_degrees(90, math.pi * 2)
|
||||
# import math
|
||||
# from compLib.Movement import Movement
|
||||
# Movement.turn_degrees(90, math.pi * 2)
|
||||
# Movement.turn_degrees(-90, math.pi * 2)
|
||||
#
|
||||
# Movement.turn_degrees(90, math.pi * 2)
|
||||
# Movement.turn_degrees(90, -math.pi * 2)
|
||||
#
|
||||
# Movement.turn_degrees(90, math.pi * 2)
|
||||
# Movement.turn_degrees(-90, -math.pi * 2)
|
||||
|
||||
# from compLib.Movement import Movement
|
||||
# Movement.drive_distance(0.1, 0.5)
|
||||
# Movement.drive_distance(-0.1, 0.5)
|
||||
#
|
||||
# Movement.drive_distance(0.1, 0.5)
|
||||
# Movement.drive_distance(0.1, -0.5)
|
||||
#
|
||||
# Movement.drive_distance(0.1, 0.5)
|
||||
# Movement.drive_distance(-0.1, -0.5)
|
||||
|
||||
# from compLib.Movement import Movement
|
||||
# import math
|
||||
# import time
|
||||
# Movement.drive_distance(0.5, 0.5)
|
||||
# time.sleep(1)
|
||||
# Movement.turn_degrees(90, math.pi * 2)
|
||||
# time.sleep(1)
|
||||
#
|
||||
# Movement.drive_distance(0.5, 0.5)
|
||||
# time.sleep(1)
|
||||
# Movement.turn_degrees(90, math.pi * 2)
|
||||
# time.sleep(1)
|
||||
#
|
||||
# Movement.drive_distance(0.5, 0.5)
|
||||
# time.sleep(1)
|
||||
# Movement.turn_degrees(90, math.pi * 2)
|
||||
# time.sleep(1)
|
||||
#
|
||||
# Movement.drive_distance(0.5, 0.5)
|
||||
# time.sleep(1)
|
||||
# Movement.turn_degrees(90, math.pi * 2)
|
||||
# time.sleep(1)
|
||||
|
||||
# import time
|
||||
#
|
||||
# from compLib.IRSensor import IRSensor
|
||||
# from compLib.Motor import Motor
|
||||
#
|
||||
# IRSensor.enable()
|
||||
#
|
||||
# DRIVE_SPEED = 2.0
|
||||
# COLOR_BREAK = 900
|
||||
# KP = 0.25
|
||||
# KD = 0.0
|
||||
#
|
||||
#
|
||||
# def drive(leftSpeed, rightSpeed):
|
||||
# Motor.speed(0, -rightSpeed)
|
||||
# Motor.power(3, leftSpeed)
|
||||
#
|
||||
#
|
||||
# def follow(sleepTime=0.1):
|
||||
# lastError = 0
|
||||
# sensorsBlack = 0
|
||||
#
|
||||
# while sensorsBlack < 3:
|
||||
# data = IRSensor.read_all()
|
||||
#
|
||||
# sensorsBlack = 0
|
||||
# for i in range(len(data)):
|
||||
# if data[i] > COLOR_BREAK:
|
||||
# sensorsBlack += 1
|
||||
#
|
||||
# error = lastError
|
||||
# if data[2] > COLOR_BREAK:
|
||||
# error = 0
|
||||
# elif data[0] > COLOR_BREAK:
|
||||
# error = -1.5
|
||||
# elif data[4] > COLOR_BREAK:
|
||||
# error = 1.5
|
||||
# elif data[1] > COLOR_BREAK:
|
||||
# error = -1
|
||||
# elif data[3] > COLOR_BREAK:
|
||||
# error = 1
|
||||
# elif error == 1.5:
|
||||
# error = 3
|
||||
# elif error == -1.5:
|
||||
# error = -3
|
||||
#
|
||||
# lastError = error
|
||||
#
|
||||
# adjustment = KP * error + KD * (error - lastError)
|
||||
# leftSpeed = DRIVE_SPEED + adjustment
|
||||
# rightSpeed = DRIVE_SPEED - adjustment
|
||||
#
|
||||
# print(f"{leftSpeed} {rightSpeed} {adjustment} {error}")
|
||||
# drive(leftSpeed, rightSpeed)
|
||||
#
|
||||
# drive(0, 0)
|
||||
# time.sleep(sleepTime)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
CompLibClient.use_tcp_socket("dev03.local")
|
||||
# follow()
|
||||
main()
|
||||
|
|
|
|||
16
server_v2/.idea/deployment.xml
generated
16
server_v2/.idea/deployment.xml
generated
|
|
@ -2,7 +2,19 @@
|
|||
<project version="4">
|
||||
<component name="PublishConfigData" remoteFilesAllowedToDisappearOnAutoupload="false">
|
||||
<serverData>
|
||||
<paths name="Judge01 (9c7ab821-1501-483e-beda-814d54656ca2)">
|
||||
<paths name="dev01 (e8239813-a9bc-43df-830c-e157833a3358)">
|
||||
<serverdata>
|
||||
<mappings>
|
||||
<mapping deploy="/tmp/tmp.c1a2b48Z1L" local="$PROJECT_DIR$" />
|
||||
</mappings>
|
||||
<excludedPaths>
|
||||
<excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debug" />
|
||||
<excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debugdev03" />
|
||||
<excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debugdev01" />
|
||||
</excludedPaths>
|
||||
</serverdata>
|
||||
</paths>
|
||||
<paths name="dev03 (9c7ab821-1501-483e-beda-814d54656ca2)">
|
||||
<serverdata>
|
||||
<mappings>
|
||||
<mapping deploy="/tmp/tmp.vU8xaArBs9" local="$PROJECT_DIR$" />
|
||||
|
|
@ -10,6 +22,8 @@
|
|||
<excludedPaths>
|
||||
<excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debugremote" />
|
||||
<excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debug" />
|
||||
<excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debugdev03" />
|
||||
<excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debugdev01" />
|
||||
</excludedPaths>
|
||||
</serverdata>
|
||||
</paths>
|
||||
|
|
|
|||
|
|
@ -55,12 +55,14 @@ set(SRC_FILES
|
|||
src/communication/MessageBuilder.cpp
|
||||
src/communication/UnixSocketServer.cpp
|
||||
src/communication/TCPSocketServer.cpp
|
||||
src/GoToController.cpp)
|
||||
src/GoToController.cpp
|
||||
src/HealthChecker.cpp)
|
||||
|
||||
set(HDR_FILES
|
||||
include/spi.hpp
|
||||
include/reset.hpp
|
||||
include/mathUtils.hpp
|
||||
include/networkUtils.hpp
|
||||
include/IRSensors.hpp
|
||||
include/Robot.hpp
|
||||
include/Cache.hpp
|
||||
|
|
@ -74,7 +76,8 @@ set(HDR_FILES
|
|||
include/communication/MessageBuilder.hpp
|
||||
include/communication/UnixSocketServer.hpp
|
||||
include/communication/TCPSocketServer.hpp
|
||||
include/GoToController.hpp)
|
||||
include/GoToController.hpp
|
||||
include/HealthChecker.hpp)
|
||||
|
||||
include_directories(third_party/asio)
|
||||
|
||||
|
|
|
|||
|
|
@ -25,6 +25,8 @@ public:
|
|||
|
||||
void set_speed(uint8_t port, double speed_rad_s);
|
||||
|
||||
void stop_all();
|
||||
|
||||
void generate_step_response_data();
|
||||
|
||||
void generate_tuned_step_response_data();
|
||||
|
|
|
|||
35
server_v2/include/HealthChecker.hpp
Normal file
35
server_v2/include/HealthChecker.hpp
Normal file
|
|
@ -0,0 +1,35 @@
|
|||
#ifndef COMPLIB_SERVER_HEALTHCHECKER_HPP
|
||||
#define COMPLIB_SERVER_HEALTHCHECKER_HPP
|
||||
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
|
||||
#define HEALTH_CHECK_TIMEOUT_MS 500
|
||||
|
||||
class HealthChecker {
|
||||
public:
|
||||
static HealthChecker &getInstance() {
|
||||
static HealthChecker instance;
|
||||
return instance;
|
||||
}
|
||||
|
||||
HealthChecker(HealthChecker const &) = delete;
|
||||
|
||||
void operator=(HealthChecker const &) = delete;
|
||||
|
||||
void update();
|
||||
|
||||
private:
|
||||
HealthChecker();
|
||||
|
||||
[[noreturn]] void check_loop();
|
||||
|
||||
typedef std::chrono::steady_clock clock;
|
||||
|
||||
std::thread check_thread;
|
||||
std::chrono::time_point<clock> last_update;
|
||||
bool stop_sent = false;
|
||||
};
|
||||
|
||||
|
||||
#endif //COMPLIB_SERVER_HEALTHCHECKER_HPP
|
||||
|
|
@ -3,7 +3,6 @@
|
|||
|
||||
#include <thread>
|
||||
|
||||
#define SOCKET_PATH "/tmp/compLib"
|
||||
#define SOCKET_BUFFER_SIZE 128
|
||||
|
||||
class TCPSocketServer {
|
||||
|
|
|
|||
49
server_v2/include/networkUtils.hpp
Normal file
49
server_v2/include/networkUtils.hpp
Normal file
|
|
@ -0,0 +1,49 @@
|
|||
#ifndef COMPLIB_SERVER_NETWORKUTILS_H
|
||||
#define COMPLIB_SERVER_NETWORKUTILS_H
|
||||
|
||||
#include <unistd.h>
|
||||
#include <string>
|
||||
#include <netdb.h>
|
||||
#include "spdlog/spdlog.h"
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <ifaddrs.h>
|
||||
|
||||
namespace networkUtils {
|
||||
inline std::tuple<std::string, std::string> get_current_host_and_ip() {
|
||||
struct ifaddrs *ifaddr, *ifa;
|
||||
int family, s;
|
||||
char host[NI_MAXHOST];
|
||||
|
||||
if (getifaddrs(&ifaddr) == -1) {
|
||||
spdlog::error("getifaddrs failed");
|
||||
freeifaddrs(ifaddr);
|
||||
return std::tuple("", "");
|
||||
}
|
||||
|
||||
for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) {
|
||||
if (ifa->ifa_addr == NULL)
|
||||
continue;
|
||||
|
||||
family = ifa->ifa_addr->sa_family;
|
||||
|
||||
if (family == AF_INET) {
|
||||
s = getnameinfo(ifa->ifa_addr, sizeof(struct sockaddr_in), host, NI_MAXHOST, NULL, 0, NI_NUMERICHOST);
|
||||
if (s != 0) {
|
||||
spdlog::error("getnameinfo() failed: {}", gai_strerror(s));
|
||||
freeifaddrs(ifaddr);
|
||||
return std::tuple("", "");
|
||||
}
|
||||
|
||||
if (std::string(ifa->ifa_name) != "lo") {
|
||||
freeifaddrs(ifaddr);
|
||||
return std::tuple(ifa->ifa_name, std::string(host));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return std::tuple("", "");
|
||||
}
|
||||
}
|
||||
|
||||
#endif //COMPLIB_SERVER_NETWORKUTILS_H
|
||||
|
|
@ -31,6 +31,8 @@ public:
|
|||
|
||||
void write_array(uint8_t reg, uint8_t length, const uint8_t *data);
|
||||
|
||||
std::array<uint8_t, 4> read_version();
|
||||
|
||||
enum Register : uint8_t {
|
||||
IDENTIFICATION_MODEL_ID = 1,
|
||||
IDENTIFICATION_MODEL_REV_MAJOR = 2,
|
||||
|
|
|
|||
|
|
@ -58,16 +58,24 @@ message IRSensorsReadAllResponse {
|
|||
repeated uint32 data = 3 [packed = true];
|
||||
}
|
||||
|
||||
message MotorSetPowerRequest {
|
||||
uint32 port = 1;
|
||||
double power = 2;
|
||||
}
|
||||
|
||||
message MotorsSetPowerRequest {
|
||||
Header header = 1;
|
||||
uint32 port = 2;
|
||||
double power = 3;
|
||||
repeated MotorSetPowerRequest requests = 2;
|
||||
}
|
||||
|
||||
message MotorSetSpeedRequest {
|
||||
uint32 port = 1;
|
||||
double speed = 2;
|
||||
}
|
||||
|
||||
message MotorsSetSpeedRequest {
|
||||
Header header = 1;
|
||||
uint32 port = 2;
|
||||
double speed = 3;
|
||||
repeated MotorSetSpeedRequest requests = 2;
|
||||
}
|
||||
|
||||
message OdometryReadRequest {
|
||||
|
|
@ -98,4 +106,8 @@ message DriveRequest {
|
|||
Header header = 1;
|
||||
double linear_velocity_m_s = 2;
|
||||
double angular_velocity_rad_s = 3;
|
||||
}
|
||||
|
||||
message HealthUpdateRequest {
|
||||
Header header = 1;
|
||||
}
|
||||
|
|
@ -184,3 +184,9 @@ void ClosedLoopMotorController::calibrate_wheel_ticks(double turns, double ticks
|
|||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
|
||||
}
|
||||
|
||||
void ClosedLoopMotorController::stop_all() {
|
||||
for (int i = 0; i < ROBOT_MOTOR_COUNT; i++) {
|
||||
set_power(i, 0);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
28
server_v2/src/HealthChecker.cpp
Normal file
28
server_v2/src/HealthChecker.cpp
Normal file
|
|
@ -0,0 +1,28 @@
|
|||
#include "include/HealthChecker.hpp"
|
||||
#include "include/ClosedLoopMotorController.hpp"
|
||||
#include "include/spi.hpp"
|
||||
#include "include/IRSensors.hpp"
|
||||
|
||||
HealthChecker::HealthChecker() {
|
||||
last_update = clock::now();
|
||||
check_thread = std::thread(&HealthChecker::check_loop, this);
|
||||
check_thread.detach();
|
||||
}
|
||||
|
||||
void HealthChecker::update() {
|
||||
last_update = clock::now();
|
||||
}
|
||||
|
||||
void HealthChecker::check_loop() {
|
||||
while (true) {
|
||||
if (clock::now() - last_update < std::chrono::milliseconds(HEALTH_CHECK_TIMEOUT_MS)) {
|
||||
Spi::getInstance().read_version();
|
||||
stop_sent = false;
|
||||
} else if (!stop_sent) {
|
||||
ClosedLoopMotorController::getInstance().stop_all();
|
||||
IRSensors::disable();
|
||||
stop_sent = true;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(HEALTH_CHECK_TIMEOUT_MS / 2));
|
||||
}
|
||||
}
|
||||
|
|
@ -2,6 +2,7 @@
|
|||
#include "include/spi.hpp"
|
||||
#include "include/Robot.hpp"
|
||||
#include "include/mathUtils.hpp"
|
||||
#include "spdlog/spdlog.h"
|
||||
|
||||
void IRSensors::enable() {
|
||||
uint8_t data[ROBOT_IR_SENSOR_COUNT] = {};
|
||||
|
|
|
|||
|
|
@ -7,6 +7,7 @@
|
|||
#include "include/IRSensors.hpp"
|
||||
#include "include/ClosedLoopMotorController.hpp"
|
||||
#include "include/GoToController.hpp"
|
||||
#include "include/HealthChecker.hpp"
|
||||
|
||||
using namespace CompLib;
|
||||
|
||||
|
|
@ -14,7 +15,7 @@ google::protobuf::Message *MessageProcessor::process_message(const std::string &
|
|||
CompLib::GenericRequest message;
|
||||
message.ParseFromString(serializedMessage);
|
||||
auto messageTypeName = message.header().message_type();
|
||||
spdlog::debug("Got request with type {}", messageTypeName);
|
||||
// spdlog::debug("Got request with type {}", messageTypeName);
|
||||
|
||||
try {
|
||||
// Encoder
|
||||
|
|
@ -39,12 +40,16 @@ google::protobuf::Message *MessageProcessor::process_message(const std::string &
|
|||
if (messageTypeName == MotorsSetPowerRequest::GetDescriptor()->full_name()) {
|
||||
MotorsSetPowerRequest request;
|
||||
request.ParseFromString(serializedMessage);
|
||||
ClosedLoopMotorController::getInstance().set_power(request.port(), request.power());
|
||||
for (const auto &innerRequest: request.requests()) {
|
||||
ClosedLoopMotorController::getInstance().set_power(innerRequest.port(), innerRequest.power());
|
||||
}
|
||||
return MessageBuilder::default_successful_generic_response();
|
||||
} else if (messageTypeName == MotorsSetSpeedRequest::GetDescriptor()->full_name()) {
|
||||
MotorsSetSpeedRequest request;
|
||||
request.ParseFromString(serializedMessage);
|
||||
ClosedLoopMotorController::getInstance().set_speed(request.port(), request.speed());
|
||||
for (const auto &innerRequest: request.requests()) {
|
||||
ClosedLoopMotorController::getInstance().set_speed(innerRequest.port(), innerRequest.speed());
|
||||
}
|
||||
return MessageBuilder::default_successful_generic_response();
|
||||
}
|
||||
|
||||
|
|
@ -65,6 +70,15 @@ google::protobuf::Message *MessageProcessor::process_message(const std::string &
|
|||
GoToController::diff_drive_inverse_kinematics(request.linear_velocity_m_s(), request.angular_velocity_rad_s());
|
||||
return MessageBuilder::default_successful_generic_response();
|
||||
}
|
||||
|
||||
// Health
|
||||
if (messageTypeName == HealthUpdateRequest::GetDescriptor()->full_name()) {
|
||||
HealthUpdateRequest request;
|
||||
request.ParseFromString(serializedMessage);
|
||||
HealthChecker::getInstance().update();
|
||||
return MessageBuilder::default_successful_generic_response();
|
||||
}
|
||||
|
||||
} catch (const std::exception &ex) {
|
||||
spdlog::error("Error when processing message with header {}: {}", messageTypeName, ex.what());
|
||||
google::protobuf::Message *returnMessage = MessageBuilder::generic_response(false, ex.what());
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
#include <sys/un.h>
|
||||
#include <sys/socket.h>
|
||||
#include <zconf.h>
|
||||
#include <sys/stat.h>
|
||||
|
||||
#include "include/communication/UnixSocketServer.hpp"
|
||||
#include "include/communication/MessageProcessor.hpp"
|
||||
|
|
@ -23,6 +24,8 @@ UnixSocketServer::UnixSocketServer() {
|
|||
exit(-1);
|
||||
}
|
||||
|
||||
chmod(SOCKET_PATH, 0777);
|
||||
|
||||
if (listen(socket_file_descriptor, 1) == -1) {
|
||||
exit(-2);
|
||||
}
|
||||
|
|
@ -32,23 +35,31 @@ UnixSocketServer::UnixSocketServer() {
|
|||
for (;;) {
|
||||
int client_file_descriptor = accept(socket_file_descriptor, NULL, NULL);
|
||||
|
||||
read(client_file_descriptor, read_buffer, 1);
|
||||
uint8_t message_size = read_buffer[0];
|
||||
while (true) {
|
||||
int bytes_received = read(client_file_descriptor, read_buffer, 1);
|
||||
if (bytes_received <= 0) {
|
||||
break;
|
||||
}
|
||||
uint8_t message_size = read_buffer[0];
|
||||
|
||||
read(client_file_descriptor, read_buffer, read_buffer[0]);
|
||||
std::string string_message;
|
||||
for (int i{}; i < message_size; i++) {
|
||||
string_message += read_buffer[i];
|
||||
bytes_received = read(client_file_descriptor, read_buffer, read_buffer[0]);
|
||||
if (bytes_received <= 0) {
|
||||
break;
|
||||
}
|
||||
std::string string_message;
|
||||
for (int i{}; i < message_size; i++) {
|
||||
string_message += read_buffer[i];
|
||||
}
|
||||
|
||||
auto response = MessageProcessor::process_message(string_message);
|
||||
uint8_t response_size = response->ByteSizeLong();
|
||||
write_buffer[0] = response_size;
|
||||
write(client_file_descriptor, write_buffer, 1);
|
||||
|
||||
response->SerializeToArray(write_buffer, SOCKET_BUFFER_SIZE);
|
||||
write(client_file_descriptor, write_buffer, response_size);
|
||||
}
|
||||
|
||||
auto response = MessageProcessor::process_message(string_message);
|
||||
uint8_t response_size = response->ByteSizeLong();
|
||||
write_buffer[0] = response_size;
|
||||
write(client_file_descriptor, write_buffer, 1);
|
||||
|
||||
response->SerializeToArray(write_buffer, SOCKET_BUFFER_SIZE);
|
||||
write(client_file_descriptor, write_buffer, response_size);
|
||||
|
||||
close(client_file_descriptor);
|
||||
}
|
||||
}
|
||||
|
|
@ -6,18 +6,22 @@
|
|||
#include "include/ClosedLoopMotorController.hpp"
|
||||
#include "include/communication/UnixSocketServer.hpp"
|
||||
#include "include/communication/TCPSocketServer.hpp"
|
||||
#include "include/HealthChecker.hpp"
|
||||
#include "include/GoToController.hpp"
|
||||
#include "include/Encoders.hpp"
|
||||
#include "include/networkUtils.hpp"
|
||||
|
||||
using namespace std;
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
Reset::reset_robot();
|
||||
spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v");
|
||||
spdlog::set_level(spdlog::level::info);
|
||||
spdlog::set_level(spdlog::level::debug);
|
||||
spdlog::info("Starting complib_server...");
|
||||
|
||||
UnixSocketServer unixSocketServer;
|
||||
TCPSocketServer tcpSocketServer;
|
||||
HealthChecker::getInstance();
|
||||
|
||||
// ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, M_PI * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT);
|
||||
// ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, M_PI_4 * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT);
|
||||
|
|
|
|||
|
|
@ -1,5 +1,6 @@
|
|||
#include <string.h>
|
||||
#include <spdlog/spdlog.h>
|
||||
#include <array>
|
||||
|
||||
#include "include/spi.hpp"
|
||||
#include "include/mathUtils.hpp"
|
||||
|
|
@ -114,6 +115,16 @@ void Spi::transfer() {
|
|||
}
|
||||
}
|
||||
|
||||
std::array<uint8_t, 4> Spi::read_version() {
|
||||
uint8_t data[4] = {0};
|
||||
read_array(IDENTIFICATION_MODEL_ID, 4, data);
|
||||
auto result = std::array<uint8_t, 4>();
|
||||
for (int i = 0; i < 4; i++) {
|
||||
result.at(i) = data[i];
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
Spi::Spi() {}
|
||||
|
|
@ -147,4 +158,8 @@ void Spi::transfer() {
|
|||
spdlog::warn("Calling SPI without actual interface.");
|
||||
}
|
||||
|
||||
std::array<uint8_t, 4> Spi::read_version() {
|
||||
spdlog::warn("Calling SPI without actual interface.");
|
||||
}
|
||||
|
||||
#endif
|
||||
BIN
server_v2/test_bin/compsrv
Normal file → Executable file
BIN
server_v2/test_bin/compsrv
Normal file → Executable file
Binary file not shown.
Reference in a new issue