From a484bc213792dd8e98a8c277b812086a255a2e18 Mon Sep 17 00:00:00 2001 From: Konstantin Lampalzer Date: Sun, 22 May 2022 23:00:57 +0200 Subject: [PATCH] Add complib client and TCP communication --- client_s2/.idea/.gitignore | 3 + client_s2/.idea/client_s2.iml | 8 + .../inspectionProfiles/Project_Default.xml | 12 ++ .../inspectionProfiles/profiles_settings.xml | 6 + client_s2/.idea/misc.xml | 4 + client_s2/.idea/modules.xml | 8 + client_s2/.idea/saveactions_settings.xml | 15 ++ client_s2/.idea/vcs.xml | 6 + client_s2/CompLib.proto | 62 +++++- client_s2/CompLib_pb2.py | 84 -------- .../__pycache__/CompLib_pb2.cpython-39.pyc | Bin 1923 -> 0 bytes client_s2/compLib/CompLibClient.py | 45 +++++ client_s2/compLib/CompLib_pb2.py | 190 +++++++++++++++++ client_s2/compLib/Encoder.py | 35 ++++ client_s2/compLib/IRSensor.py | 39 ++++ client_s2/compLib/Motor.py | 69 +++++++ client_s2/compLib/__init__.py | 0 .../__pycache__/CompLibClient.cpython-39.pyc | Bin 0 -> 1302 bytes .../__pycache__/CompLib_pb2.cpython-39.pyc | Bin 0 -> 4335 bytes .../__pycache__/Encoder.cpython-39.pyc | Bin 0 -> 1751 bytes .../__pycache__/IRSensor.cpython-39.pyc | Bin 0 -> 1656 bytes .../compLib/__pycache__/Motor.cpython-39.pyc | Bin 0 -> 1805 bytes .../__pycache__/__init__.cpython-39.pyc | Bin 0 -> 163 bytes client_s2/main.py | 68 ++++--- server_v2/CMakeLists.txt | 31 ++- .../include/communication/MessageBuilder.hpp | 2 + .../include/communication/TCPSocketServer.hpp | 20 ++ .../communication/UnixSocketServer.hpp | 2 +- server_v2/include/reset.hpp | 35 ++-- server_v2/include/spi.hpp | 2 +- .../src/communication/MessageBuilder.cpp | 10 +- .../src/communication/MessageProcessor.cpp | 1 + .../src/communication/TCPSocketServer.cpp | 74 +++++++ server_v2/src/main.cpp | 25 ++- server_v2/src/spi.cpp | 191 +++++++++++------- 35 files changed, 815 insertions(+), 232 deletions(-) create mode 100644 client_s2/.idea/.gitignore create mode 100644 client_s2/.idea/client_s2.iml create mode 100644 client_s2/.idea/inspectionProfiles/Project_Default.xml create mode 100644 client_s2/.idea/inspectionProfiles/profiles_settings.xml create mode 100644 client_s2/.idea/misc.xml create mode 100644 client_s2/.idea/modules.xml create mode 100644 client_s2/.idea/saveactions_settings.xml create mode 100644 client_s2/.idea/vcs.xml delete mode 100644 client_s2/CompLib_pb2.py delete mode 100644 client_s2/__pycache__/CompLib_pb2.cpython-39.pyc create mode 100644 client_s2/compLib/CompLibClient.py create mode 100644 client_s2/compLib/CompLib_pb2.py create mode 100644 client_s2/compLib/Encoder.py create mode 100644 client_s2/compLib/IRSensor.py create mode 100644 client_s2/compLib/Motor.py create mode 100644 client_s2/compLib/__init__.py create mode 100644 client_s2/compLib/__pycache__/CompLibClient.cpython-39.pyc create mode 100644 client_s2/compLib/__pycache__/CompLib_pb2.cpython-39.pyc create mode 100644 client_s2/compLib/__pycache__/Encoder.cpython-39.pyc create mode 100644 client_s2/compLib/__pycache__/IRSensor.cpython-39.pyc create mode 100644 client_s2/compLib/__pycache__/Motor.cpython-39.pyc create mode 100644 client_s2/compLib/__pycache__/__init__.cpython-39.pyc create mode 100644 server_v2/include/communication/TCPSocketServer.hpp create mode 100644 server_v2/src/communication/TCPSocketServer.cpp diff --git a/client_s2/.idea/.gitignore b/client_s2/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/client_s2/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/client_s2/.idea/client_s2.iml b/client_s2/.idea/client_s2.iml new file mode 100644 index 0000000..d0876a7 --- /dev/null +++ b/client_s2/.idea/client_s2.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/client_s2/.idea/inspectionProfiles/Project_Default.xml b/client_s2/.idea/inspectionProfiles/Project_Default.xml new file mode 100644 index 0000000..9277599 --- /dev/null +++ b/client_s2/.idea/inspectionProfiles/Project_Default.xml @@ -0,0 +1,12 @@ + + + + \ No newline at end of file diff --git a/client_s2/.idea/inspectionProfiles/profiles_settings.xml b/client_s2/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/client_s2/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/client_s2/.idea/misc.xml b/client_s2/.idea/misc.xml new file mode 100644 index 0000000..d56657a --- /dev/null +++ b/client_s2/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/client_s2/.idea/modules.xml b/client_s2/.idea/modules.xml new file mode 100644 index 0000000..8417644 --- /dev/null +++ b/client_s2/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/client_s2/.idea/saveactions_settings.xml b/client_s2/.idea/saveactions_settings.xml new file mode 100644 index 0000000..b7b3f34 --- /dev/null +++ b/client_s2/.idea/saveactions_settings.xml @@ -0,0 +1,15 @@ + + + + + + \ No newline at end of file diff --git a/client_s2/.idea/vcs.xml b/client_s2/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/client_s2/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/client_s2/CompLib.proto b/client_s2/CompLib.proto index 77a202d..1bb1bd0 100644 --- a/client_s2/CompLib.proto +++ b/client_s2/CompLib.proto @@ -20,17 +20,65 @@ message GenericResponse { Status status = 2; } -message ReadSensorsRequest { +message EncoderReadPositionsRequest { Header header = 1; } -message ReadSensorsResponse { +message EncoderReadPositionsResponse { Header header = 1; Status status = 2; - uint32 ir_1 = 3; - uint32 ir_2 = 4; - uint32 ir_3 = 5; - uint32 ir_4 = 6; - uint32 ir_5 = 7; + repeated int32 positions = 3 [packed = true]; +} + +message EncoderReadVelocitiesRequest { + Header header = 1; +} + +message EncoderReadVelocitiesResponse { + Header header = 1; + Status status = 2; + repeated double velocities = 3 [packed = true]; +} + +message IRSensorsEnableRequest { + Header header = 1; +} + +message IRSensorsDisableRequest { + Header header = 1; +} + +message IRSensorsReadAllRequest { + Header header = 1; +} + +message IRSensorsReadAllResponse { + Header header = 1; + Status status = 2; + repeated uint32 data = 3 [packed = true]; +} + +message MotorsSetPowerRequest { + Header header = 1; + uint32 port = 2; + double power = 3; +} + +message MotorsSetSpeedRequest { + Header header = 1; + uint32 port = 2; + double speed = 3; +} + +message OdometryReadRequest { + Header header = 1; +} + +message OdometryReadResponse { + Header header = 1; + Status status = 2; + double x_position = 3; + double y_position = 4; + double orientation = 5; } diff --git a/client_s2/CompLib_pb2.py b/client_s2/CompLib_pb2.py deleted file mode 100644 index ea548fd..0000000 --- a/client_s2/CompLib_pb2.py +++ /dev/null @@ -1,84 +0,0 @@ -# -*- coding: utf-8 -*- -# Generated by the protocol buffer compiler. DO NOT EDIT! -# source: CompLib.proto -"""Generated protocol buffer code.""" -from google.protobuf import descriptor as _descriptor -from google.protobuf import descriptor_pool as _descriptor_pool -from google.protobuf import message as _message -from google.protobuf import reflection as _reflection -from google.protobuf import symbol_database as _symbol_database -# @@protoc_insertion_point(imports) - -_sym_db = _symbol_database.Default() - - - - -DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\rCompLib.proto\x12\x07\x43ompLib\"\x1e\n\x06Header\x12\x14\n\x0cmessage_type\x18\x01 \x01(\t\"3\n\x06Status\x12\x12\n\nsuccessful\x18\x01 \x01(\x08\x12\x15\n\rerror_message\x18\x02 \x01(\t\"1\n\x0eGenericRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"S\n\x0fGenericResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\"5\n\x12ReadSensorsRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"\x9d\x01\n\x13ReadSensorsResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x0c\n\x04ir_1\x18\x03 \x01(\r\x12\x0c\n\x04ir_2\x18\x04 \x01(\r\x12\x0c\n\x04ir_3\x18\x05 \x01(\r\x12\x0c\n\x04ir_4\x18\x06 \x01(\r\x12\x0c\n\x04ir_5\x18\x07 \x01(\rb\x06proto3') - - - -_HEADER = DESCRIPTOR.message_types_by_name['Header'] -_STATUS = DESCRIPTOR.message_types_by_name['Status'] -_GENERICREQUEST = DESCRIPTOR.message_types_by_name['GenericRequest'] -_GENERICRESPONSE = DESCRIPTOR.message_types_by_name['GenericResponse'] -_READSENSORSREQUEST = DESCRIPTOR.message_types_by_name['ReadSensorsRequest'] -_READSENSORSRESPONSE = DESCRIPTOR.message_types_by_name['ReadSensorsResponse'] -Header = _reflection.GeneratedProtocolMessageType('Header', (_message.Message,), { - 'DESCRIPTOR' : _HEADER, - '__module__' : 'CompLib_pb2' - # @@protoc_insertion_point(class_scope:CompLib.Header) - }) -_sym_db.RegisterMessage(Header) - -Status = _reflection.GeneratedProtocolMessageType('Status', (_message.Message,), { - 'DESCRIPTOR' : _STATUS, - '__module__' : 'CompLib_pb2' - # @@protoc_insertion_point(class_scope:CompLib.Status) - }) -_sym_db.RegisterMessage(Status) - -GenericRequest = _reflection.GeneratedProtocolMessageType('GenericRequest', (_message.Message,), { - 'DESCRIPTOR' : _GENERICREQUEST, - '__module__' : 'CompLib_pb2' - # @@protoc_insertion_point(class_scope:CompLib.GenericRequest) - }) -_sym_db.RegisterMessage(GenericRequest) - -GenericResponse = _reflection.GeneratedProtocolMessageType('GenericResponse', (_message.Message,), { - 'DESCRIPTOR' : _GENERICRESPONSE, - '__module__' : 'CompLib_pb2' - # @@protoc_insertion_point(class_scope:CompLib.GenericResponse) - }) -_sym_db.RegisterMessage(GenericResponse) - -ReadSensorsRequest = _reflection.GeneratedProtocolMessageType('ReadSensorsRequest', (_message.Message,), { - 'DESCRIPTOR' : _READSENSORSREQUEST, - '__module__' : 'CompLib_pb2' - # @@protoc_insertion_point(class_scope:CompLib.ReadSensorsRequest) - }) -_sym_db.RegisterMessage(ReadSensorsRequest) - -ReadSensorsResponse = _reflection.GeneratedProtocolMessageType('ReadSensorsResponse', (_message.Message,), { - 'DESCRIPTOR' : _READSENSORSRESPONSE, - '__module__' : 'CompLib_pb2' - # @@protoc_insertion_point(class_scope:CompLib.ReadSensorsResponse) - }) -_sym_db.RegisterMessage(ReadSensorsResponse) - -if _descriptor._USE_C_DESCRIPTORS == False: - - DESCRIPTOR._options = None - _HEADER._serialized_start=26 - _HEADER._serialized_end=56 - _STATUS._serialized_start=58 - _STATUS._serialized_end=109 - _GENERICREQUEST._serialized_start=111 - _GENERICREQUEST._serialized_end=160 - _GENERICRESPONSE._serialized_start=162 - _GENERICRESPONSE._serialized_end=245 - _READSENSORSREQUEST._serialized_start=247 - _READSENSORSREQUEST._serialized_end=300 - _READSENSORSRESPONSE._serialized_start=303 - _READSENSORSRESPONSE._serialized_end=460 -# @@protoc_insertion_point(module_scope) diff --git a/client_s2/__pycache__/CompLib_pb2.cpython-39.pyc b/client_s2/__pycache__/CompLib_pb2.cpython-39.pyc deleted file mode 100644 index 53c4d1a54b4e7261d327b9088945377da263d826..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1923 zcmb_d&2H;N6tsCt$%Nuw>;et3CiiVu3g_aoyai5(r@{U%qqZJD%~JbH;9^LNvTS$-dS9 zv8rjmNn`dh(f9~|;%8mcnC56qXGWk84JV^Zo=N(h=9P)xwR6LKII3-wzc+Z{RNDGyAiOBF%P5A3xkdRWH11} z;e`ygZ)M`Q^q4RZ9{0yl$kD3aTXYN+mRbhgk=#T zCZ3104km#bEm%!b0nS5ClgO5#f}WB&^_srdgD)o_B5R%GzfjYtz|u~2`))nc**9%c zO+&;u90~Zx7%32HZED)thfbHg`HWaSoYjVr2)X#zYxzzm_1EJ5uLW6UlJz-#+RETI zRT68bm7TG6Te%tQ-By0ade$W z+=HfY%mWy>lOS?TibIt3o!TD5O5wg6_^*IH_XFT_GPXI}6@vD!=*S%c#=Y$B9dvt0 z?9;u|Gux@s7u}<7?{L4@{rs$JpWcNF=fuY3>_4xO( bytes: + # with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock: + # sock.connect((TCP_SOCKET_HOST, TCP_SOCKET_PORT)) + + with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock: + sock.connect(UNIX_SOCKET_PATH) + + sock.sendall(size.to_bytes(1, byteorder='big')) + sock.sendall(data) + + response_size_bytes = sock.recv(1) + response_size = int.from_bytes(response_size_bytes, byteorder="big") + # print(response_size) + + response_bytes = sock.recv(response_size) + # print(response_bytes.hex()) + # print(len(response_bytes)) + + CompLibClient.check_response(response_bytes) + + return response_bytes + + @staticmethod + def check_response(response_bytes: bytes) -> bool: + print(f"{response_bytes}") + res = CompLib_pb2.GenericResponse() + res.ParseFromString(response_bytes) + + if res.status.successful: + return True + + # TODO: Log error message if unsuccessful + return False diff --git a/client_s2/compLib/CompLib_pb2.py b/client_s2/compLib/CompLib_pb2.py new file mode 100644 index 0000000..eaa5fe8 --- /dev/null +++ b/client_s2/compLib/CompLib_pb2.py @@ -0,0 +1,190 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: CompLib.proto +"""Generated protocol buffer code.""" +from google.protobuf import descriptor as _descriptor +from google.protobuf import descriptor_pool as _descriptor_pool +from google.protobuf import message as _message +from google.protobuf import reflection as _reflection +from google.protobuf import symbol_database as _symbol_database +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\rCompLib.proto\x12\x07\x43ompLib\"\x1e\n\x06Header\x12\x14\n\x0cmessage_type\x18\x01 \x01(\t\"3\n\x06Status\x12\x12\n\nsuccessful\x18\x01 \x01(\x08\x12\x15\n\rerror_message\x18\x02 \x01(\t\"1\n\x0eGenericRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"S\n\x0fGenericResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\">\n\x1b\x45ncoderReadPositionsRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"w\n\x1c\x45ncoderReadPositionsResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x15\n\tpositions\x18\x03 \x03(\x05\x42\x02\x10\x01\"?\n\x1c\x45ncoderReadVelocitiesRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"y\n\x1d\x45ncoderReadVelocitiesResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x16\n\nvelocities\x18\x03 \x03(\x01\x42\x02\x10\x01\"9\n\x16IRSensorsEnableRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\":\n\x17IRSensorsDisableRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\":\n\x17IRSensorsReadAllRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"n\n\x18IRSensorsReadAllResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x10\n\x04\x64\x61ta\x18\x03 \x03(\rB\x02\x10\x01\"U\n\x15MotorsSetPowerRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x0c\n\x04port\x18\x02 \x01(\r\x12\r\n\x05power\x18\x03 \x01(\x01\"U\n\x15MotorsSetSpeedRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x0c\n\x04port\x18\x02 \x01(\r\x12\r\n\x05speed\x18\x03 \x01(\x01\"6\n\x13OdometryReadRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"\x95\x01\n\x14OdometryReadResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x12\n\nx_position\x18\x03 \x01(\x01\x12\x12\n\ny_position\x18\x04 \x01(\x01\x12\x13\n\x0borientation\x18\x05 \x01(\x01\x62\x06proto3') + + + +_HEADER = DESCRIPTOR.message_types_by_name['Header'] +_STATUS = DESCRIPTOR.message_types_by_name['Status'] +_GENERICREQUEST = DESCRIPTOR.message_types_by_name['GenericRequest'] +_GENERICRESPONSE = DESCRIPTOR.message_types_by_name['GenericResponse'] +_ENCODERREADPOSITIONSREQUEST = DESCRIPTOR.message_types_by_name['EncoderReadPositionsRequest'] +_ENCODERREADPOSITIONSRESPONSE = DESCRIPTOR.message_types_by_name['EncoderReadPositionsResponse'] +_ENCODERREADVELOCITIESREQUEST = DESCRIPTOR.message_types_by_name['EncoderReadVelocitiesRequest'] +_ENCODERREADVELOCITIESRESPONSE = DESCRIPTOR.message_types_by_name['EncoderReadVelocitiesResponse'] +_IRSENSORSENABLEREQUEST = DESCRIPTOR.message_types_by_name['IRSensorsEnableRequest'] +_IRSENSORSDISABLEREQUEST = DESCRIPTOR.message_types_by_name['IRSensorsDisableRequest'] +_IRSENSORSREADALLREQUEST = DESCRIPTOR.message_types_by_name['IRSensorsReadAllRequest'] +_IRSENSORSREADALLRESPONSE = DESCRIPTOR.message_types_by_name['IRSensorsReadAllResponse'] +_MOTORSSETPOWERREQUEST = DESCRIPTOR.message_types_by_name['MotorsSetPowerRequest'] +_MOTORSSETSPEEDREQUEST = DESCRIPTOR.message_types_by_name['MotorsSetSpeedRequest'] +_ODOMETRYREADREQUEST = DESCRIPTOR.message_types_by_name['OdometryReadRequest'] +_ODOMETRYREADRESPONSE = DESCRIPTOR.message_types_by_name['OdometryReadResponse'] +Header = _reflection.GeneratedProtocolMessageType('Header', (_message.Message,), { + 'DESCRIPTOR' : _HEADER, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.Header) + }) +_sym_db.RegisterMessage(Header) + +Status = _reflection.GeneratedProtocolMessageType('Status', (_message.Message,), { + 'DESCRIPTOR' : _STATUS, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.Status) + }) +_sym_db.RegisterMessage(Status) + +GenericRequest = _reflection.GeneratedProtocolMessageType('GenericRequest', (_message.Message,), { + 'DESCRIPTOR' : _GENERICREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.GenericRequest) + }) +_sym_db.RegisterMessage(GenericRequest) + +GenericResponse = _reflection.GeneratedProtocolMessageType('GenericResponse', (_message.Message,), { + 'DESCRIPTOR' : _GENERICRESPONSE, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.GenericResponse) + }) +_sym_db.RegisterMessage(GenericResponse) + +EncoderReadPositionsRequest = _reflection.GeneratedProtocolMessageType('EncoderReadPositionsRequest', (_message.Message,), { + 'DESCRIPTOR' : _ENCODERREADPOSITIONSREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.EncoderReadPositionsRequest) + }) +_sym_db.RegisterMessage(EncoderReadPositionsRequest) + +EncoderReadPositionsResponse = _reflection.GeneratedProtocolMessageType('EncoderReadPositionsResponse', (_message.Message,), { + 'DESCRIPTOR' : _ENCODERREADPOSITIONSRESPONSE, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.EncoderReadPositionsResponse) + }) +_sym_db.RegisterMessage(EncoderReadPositionsResponse) + +EncoderReadVelocitiesRequest = _reflection.GeneratedProtocolMessageType('EncoderReadVelocitiesRequest', (_message.Message,), { + 'DESCRIPTOR' : _ENCODERREADVELOCITIESREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.EncoderReadVelocitiesRequest) + }) +_sym_db.RegisterMessage(EncoderReadVelocitiesRequest) + +EncoderReadVelocitiesResponse = _reflection.GeneratedProtocolMessageType('EncoderReadVelocitiesResponse', (_message.Message,), { + 'DESCRIPTOR' : _ENCODERREADVELOCITIESRESPONSE, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.EncoderReadVelocitiesResponse) + }) +_sym_db.RegisterMessage(EncoderReadVelocitiesResponse) + +IRSensorsEnableRequest = _reflection.GeneratedProtocolMessageType('IRSensorsEnableRequest', (_message.Message,), { + 'DESCRIPTOR' : _IRSENSORSENABLEREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.IRSensorsEnableRequest) + }) +_sym_db.RegisterMessage(IRSensorsEnableRequest) + +IRSensorsDisableRequest = _reflection.GeneratedProtocolMessageType('IRSensorsDisableRequest', (_message.Message,), { + 'DESCRIPTOR' : _IRSENSORSDISABLEREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.IRSensorsDisableRequest) + }) +_sym_db.RegisterMessage(IRSensorsDisableRequest) + +IRSensorsReadAllRequest = _reflection.GeneratedProtocolMessageType('IRSensorsReadAllRequest', (_message.Message,), { + 'DESCRIPTOR' : _IRSENSORSREADALLREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.IRSensorsReadAllRequest) + }) +_sym_db.RegisterMessage(IRSensorsReadAllRequest) + +IRSensorsReadAllResponse = _reflection.GeneratedProtocolMessageType('IRSensorsReadAllResponse', (_message.Message,), { + 'DESCRIPTOR' : _IRSENSORSREADALLRESPONSE, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.IRSensorsReadAllResponse) + }) +_sym_db.RegisterMessage(IRSensorsReadAllResponse) + +MotorsSetPowerRequest = _reflection.GeneratedProtocolMessageType('MotorsSetPowerRequest', (_message.Message,), { + 'DESCRIPTOR' : _MOTORSSETPOWERREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.MotorsSetPowerRequest) + }) +_sym_db.RegisterMessage(MotorsSetPowerRequest) + +MotorsSetSpeedRequest = _reflection.GeneratedProtocolMessageType('MotorsSetSpeedRequest', (_message.Message,), { + 'DESCRIPTOR' : _MOTORSSETSPEEDREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.MotorsSetSpeedRequest) + }) +_sym_db.RegisterMessage(MotorsSetSpeedRequest) + +OdometryReadRequest = _reflection.GeneratedProtocolMessageType('OdometryReadRequest', (_message.Message,), { + 'DESCRIPTOR' : _ODOMETRYREADREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.OdometryReadRequest) + }) +_sym_db.RegisterMessage(OdometryReadRequest) + +OdometryReadResponse = _reflection.GeneratedProtocolMessageType('OdometryReadResponse', (_message.Message,), { + 'DESCRIPTOR' : _ODOMETRYREADRESPONSE, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.OdometryReadResponse) + }) +_sym_db.RegisterMessage(OdometryReadResponse) + +if _descriptor._USE_C_DESCRIPTORS == False: + + DESCRIPTOR._options = None + _ENCODERREADPOSITIONSRESPONSE.fields_by_name['positions']._options = None + _ENCODERREADPOSITIONSRESPONSE.fields_by_name['positions']._serialized_options = b'\020\001' + _ENCODERREADVELOCITIESRESPONSE.fields_by_name['velocities']._options = None + _ENCODERREADVELOCITIESRESPONSE.fields_by_name['velocities']._serialized_options = b'\020\001' + _IRSENSORSREADALLRESPONSE.fields_by_name['data']._options = None + _IRSENSORSREADALLRESPONSE.fields_by_name['data']._serialized_options = b'\020\001' + _HEADER._serialized_start=26 + _HEADER._serialized_end=56 + _STATUS._serialized_start=58 + _STATUS._serialized_end=109 + _GENERICREQUEST._serialized_start=111 + _GENERICREQUEST._serialized_end=160 + _GENERICRESPONSE._serialized_start=162 + _GENERICRESPONSE._serialized_end=245 + _ENCODERREADPOSITIONSREQUEST._serialized_start=247 + _ENCODERREADPOSITIONSREQUEST._serialized_end=309 + _ENCODERREADPOSITIONSRESPONSE._serialized_start=311 + _ENCODERREADPOSITIONSRESPONSE._serialized_end=430 + _ENCODERREADVELOCITIESREQUEST._serialized_start=432 + _ENCODERREADVELOCITIESREQUEST._serialized_end=495 + _ENCODERREADVELOCITIESRESPONSE._serialized_start=497 + _ENCODERREADVELOCITIESRESPONSE._serialized_end=618 + _IRSENSORSENABLEREQUEST._serialized_start=620 + _IRSENSORSENABLEREQUEST._serialized_end=677 + _IRSENSORSDISABLEREQUEST._serialized_start=679 + _IRSENSORSDISABLEREQUEST._serialized_end=737 + _IRSENSORSREADALLREQUEST._serialized_start=739 + _IRSENSORSREADALLREQUEST._serialized_end=797 + _IRSENSORSREADALLRESPONSE._serialized_start=799 + _IRSENSORSREADALLRESPONSE._serialized_end=909 + _MOTORSSETPOWERREQUEST._serialized_start=911 + _MOTORSSETPOWERREQUEST._serialized_end=996 + _MOTORSSETSPEEDREQUEST._serialized_start=998 + _MOTORSSETSPEEDREQUEST._serialized_end=1083 + _ODOMETRYREADREQUEST._serialized_start=1085 + _ODOMETRYREADREQUEST._serialized_end=1139 + _ODOMETRYREADRESPONSE._serialized_start=1142 + _ODOMETRYREADRESPONSE._serialized_end=1291 +# @@protoc_insertion_point(module_scope) diff --git a/client_s2/compLib/Encoder.py b/client_s2/compLib/Encoder.py new file mode 100644 index 0000000..d1cf404 --- /dev/null +++ b/client_s2/compLib/Encoder.py @@ -0,0 +1,35 @@ +import compLib.CompLib_pb2 as CompLib_pb2 +from compLib.CompLibClient import CompLibClient + + +class Encoder(object): + """Class used to read the encoders + """ + + @staticmethod + def read_all_positions(): + """Read all encoder positions. + + :return: Tuple of all current encoder positions + """ + request = CompLib_pb2.EncoderReadPositionsRequest() + request.header.message_type = request.DESCRIPTOR.full_name + + response = CompLib_pb2.EncoderReadPositionsResponse() + response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize())) + + return tuple(i for i in response.positions) + + @staticmethod + def read_all_velocities(): + """Read the velocity of all motors connected. + + :return: Tuple of all current motor velocities + """ + request = CompLib_pb2.EncoderReadVelocitiesRequest() + request.header.message_type = request.DESCRIPTOR.full_name + + response = CompLib_pb2.EncoderReadVelocitiesResponse() + response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize())) + + return tuple(i for i in response.velocities) diff --git a/client_s2/compLib/IRSensor.py b/client_s2/compLib/IRSensor.py new file mode 100644 index 0000000..840c839 --- /dev/null +++ b/client_s2/compLib/IRSensor.py @@ -0,0 +1,39 @@ +import compLib.CompLib_pb2 as CompLib_pb2 +from compLib.CompLibClient import CompLibClient + + +class IRSensor(object): + """Access the different IR Sensors of the robot + """ + + @staticmethod + def read_all(): + """Read all IR sensors at once. + + :return: Tuple of all current ir sensors + """ + request = CompLib_pb2.IRSensorsReadAllRequest() + request.header.message_type = request.DESCRIPTOR.full_name + + response = CompLib_pb2.IRSensorsReadAllResponse() + response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize())) + + return tuple(i for i in response.data) + + @staticmethod + def enable(): + """Turn on all IR emitters + """ + request = CompLib_pb2.IRSensorsEnableRequest() + request.header.message_type = request.DESCRIPTOR.full_name + + CompLibClient.send(request.SerializeToString(), request.ByteSize()) + + @staticmethod + def disable(): + """Turn off all IR emitters + """ + request = CompLib_pb2.IRSensorsDisableRequest() + request.header.message_type = request.DESCRIPTOR.full_name + + CompLibClient.send(request.SerializeToString(), request.ByteSize()) diff --git a/client_s2/compLib/Motor.py b/client_s2/compLib/Motor.py new file mode 100644 index 0000000..27ce586 --- /dev/null +++ b/client_s2/compLib/Motor.py @@ -0,0 +1,69 @@ +import compLib.CompLib_pb2 as CompLib_pb2 +from compLib.CompLibClient import CompLibClient + +MOTOR_COUNT = 4 + + +class Motor(object): + """Class used to control the motors + """ + + @staticmethod + def power(port: int, percent: float): + """Set specified motor to percentage power + + :param port: Port, which the motor is connected to. 0-3 + :param percent: Percentage of max speed. between -100 and 100 + :raises: IndexError + """ + + 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!") + + request = CompLib_pb2.MotorsSetPowerRequest() + request.header.message_type = request.DESCRIPTOR.full_name + request.port = port + request.power = percent + + CompLibClient.send(request.SerializeToString(), request.ByteSize()) + + @staticmethod + def speed(port: int, speed: float): + """Set specified motor to percentage power + + :param port: Port, which the motor is connected to. 0-3 + :param speed: Speed at which a motor should turn in RPM + :raises: IndexError + """ + + if port < 0 or port >= MOTOR_COUNT: + raise IndexError("Invalid Motor port specified!") + + request = CompLib_pb2.MotorsSetSpeedRequest() + request.header.message_type = request.DESCRIPTOR.full_name + request.port = port + request.speed = speed + + CompLibClient.send(request.SerializeToString(), request.ByteSize()) + + # @staticmethod + # def all_off(): + # """ + # Turns of all motors + # """ + # Logging.get_logger().debug(f"Motor.all_off") + # + # for i in range(1, MOTOR_COUNT + 1): + # Motor.active_break(i) + + # @staticmethod + # def active_break(port: int): + # """ + # Actively break with a specific motor + # + # :param port: Port, which the motor is connected to. 1-4 + # """ + # Motor.pwm(port, 0, MotorMode.BREAK) diff --git a/client_s2/compLib/__init__.py b/client_s2/compLib/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/client_s2/compLib/__pycache__/CompLibClient.cpython-39.pyc b/client_s2/compLib/__pycache__/CompLibClient.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e506fdc9ae9afd48ffded9fd59becc80c966070e GIT binary patch literal 1302 zcmZux&2Aev5a!>m)@#X5?4YNfnoAcbq{v8u1PFpSLYxF`WW!R>mY2n9$;2y5y9!Aq zh#>V)=TtQ412hL2=%ug3YfpKFo;uu>0|_XJ8F7c?%s1Z*l{6Y5f-(H-x4}0aLjSmr z^}*od5s1DGN+5wph*!UOOgO}MZy#Q2PAyyUr=5XxQC+JXSj*FvwF*n zC#|#?PkzV-vqt;=L9>1LUh`J7eW#uMdK014om(G(r;2?S77}qR5GRsKCOydn{zL^4 z^aH!V+%K&)!9I}^fwKjo2GIlC=u{y)qfF)|qLzuoB$nxHCatfenW{V`YjJ&82Ou2~ z{W~awmLNk6LN3W4XA3eXOKR99#t2s491neg9hi^giw&(-K*A+nP!kNd<|W{dWWnH8 zZ4UPro(Y%RbFVz>{fG!q%V1tmjC!bz^e?v%F6Ymn1=u3i{fq$moguhSv^{;+Ng$Kk@C}ap(KE*FSx7_=4BcBF|-N>@~L+yR9ev_~fwvjMucx zMKT)Mz!b6bs5w(IeQRl!8ymh-#kevnN0i-3&t!TYE2$?%u4Uva3TEYcg)@hN?_cf3 zj^>ri#)i@EeEBRR{*BZwIQaKP>!p@Tx6Z+}Nph3rZ!@WReBfR90Q|{BbKS zYZ~i=tG;S&6uUWj4|Rsr>fl)KfkNIM4#^FW5bw~Cg=Dt3dBq)%h}wa($~9o>*uEMr zdzWh28sEXfpgH;zT|grkyd;-sf#-Nh=T-6XkPSWct$PK6Kn COGRM- literal 0 HcmV?d00001 diff --git a/client_s2/compLib/__pycache__/CompLib_pb2.cpython-39.pyc b/client_s2/compLib/__pycache__/CompLib_pb2.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..5e710a8bba5d7be76f183f6169ecda4a99769213 GIT binary patch literal 4335 zcmd6q%WoS+9LIMZzcP;F#IHDS*QIUJN8BcTZChH>WD5~#9J~pLy)3q8Qa0Uq&F-cp z{0T@1F5EbzNCrN5RMS1otuUNqvy} zIC!F`WQnB?hn`4!n#J(_aF`~Z$a;olc1LJ(T+&Cm8w8i-j)2Q?mjajPZU|g~yEM2W zcf;VuxXXYmaW?{PoV!tQ6WnFNO>&n5H^p5Z+%$Iua5LN$!Oe0v2CmEo4^Pk}OYBy5 zWHw4mbo{HxlL7rCOCP?*PCk+0b0IMp5LrQ-;>0OIkbuYu;xs2t3nCQ|c|n}v#2G;h z1w=s*bDWqHL^>dff|%#TydZ`HVoVTcIdN7HnSdw>;v6T=31TE5#szVn6Xyjn8W0nL zxWI`Eg2)EMq#zbJu^@M)KD2PHp%n0HVCoT!17!b39SmeZ_ zAjSfsEQlpeEOBCB_cEP$82KXVT%<6+j4PnU-K%tJTmnBW{56P|AU+esdmiIc5T6a= zJ%5ds*{E@yo&Z&7g=N8?1Xbv3EC>D+s6tP(Joq!93Y}vG@bjPwJrvGGgY}w9-Enb?;7D$>oj_nvhW+}pxE|FpWG0j$;eYDS9FFQ-(AM$2+Vrnt7(CbgP^@>I^ zBE{|4ZI}IH4gxOk8*esTX}8Oj+hkI0Ptm{QW9*K_ezRSGt3MsDDHP&`* z$5q>nrp1mu-Z#jYNL@AEf1Qip_OfLiyWKV^iFQTqD;ua062-O1hbYa5Xqy!7!`gRT zjd`2)W1ODHTPE2d67ATI$0vL`nGz;8eWb3YxiHiR{bN^lYi7=2^EOh+->N{Y3{#x zLWaWO3FW_NLijKIvi(%B(HouS;(Wv>tE#qAU)$WO)qP?Zdp6y-m|@)8(EA&q8-X)$ zMAk$5i_e$P(+MbedL88k${vc1a)|OZ%I_$DfS6ZgkZUO4qx^vKE6Q&uyU>Vfp?rh# zEy|B5KcP$yfXt$NhVnVe&nUm3+=TnT`8tY=@(3jbr|kR}5 z;e>afT1ihC{gd2*sw90R6yayP1GPzdvUj99P%q~7&v+-^&3o>g*AuI3r?GE&dcuGr zgEsZiWlA;Xm<`MPgwcDZ1zQE?7QzR;Ycvmxc4Lo0-dpPOs#*u5Z7pwYYkJ0bQ{7PO zYb$m2oo!Xyg5HXNwprWIRDH@&H&$wpRfpWonzpvJ2Js$0eMD)H{7mt?>UwPj(p8Z? z6V4772kB!5Y}@LFRzqFByRP<{E%ggl*R&%_u*1vi>%E-uevT*!+9?|MYcOFoP2Jk8 zy@wyQI~t+5wyCPCy?EZJt=8_VTlM#`re3u0T(mcU;zqa`TnO)XZ+2U73)2(9gM&Xl z^-P^TFkO#1y=W4m&L9f@bahfFLnvvKVU!HY2+Al*7A1$0M=78bLG-M#t*OR}(O;k% z%tE`vuT1*zj>#5q~`kmyQ!X{Dve7m|0rEc{hT*|yg z+cVpb;F7cNu;zwwPoQxZn%rxh}o=$HkO6XfuFq9-Ar!d`f+Dq ojYs97-fJKk8;%nB<*y+*B1is8#z^FU>LT&p=i%>h`M62LZuiwVc>%@5>}N ztp)XDpZY7LqyG}GJ@wpMPo3G-%dra-J(K`P+#zT9@#8mC(CN4gTleC(@yucDcly|D z=wr{YtFO@rCV0#eKH{9lR%|8q$mZ-76SijrCUZFsEI`bq1_vofio6g3fJE-X0KwAdZ&^ygXBpjvoMI&Zf_OWy<@f( zBJuO_6Z82Znuy$Aa(4h{1t@LY7cYl{gV+0qKOUIYy8=_EK?0_JjEg~KtHYHT~*JR5UCBA=?4Dze81nW)SwwnB*HF@~Y-2IO#J}p&Db@U$a!XkaX z)M%3FOe!zT(iB1s;$Mrp3|yu|t^H?BOL~?ncI&HAschm?5}#kvU<=KpcZ1Hwfe=CO zHZ96FhFw$6n~DJpScp*Gq4amjd_?9RMSQWLwatiy7`AC42~2)WhH6EALgrI6#$AJw zpOJS}sFilMSK3)Gq`(iwvn-w7E$jF2)ol7@x%j?ueLu-W5d)6fzJFE(F%=SiHGE%W zA-VBk&{3FxKF)-EM6kT;*w&=mI6+?}peA>0i@P0*OY(Q?X1jb!qH%Ad&MTrHd@xh} hO;n{=&&DUHu`ayzs+`+^BuI2TMf;GElGL+~N%Hh#ww?4Zj{XSd8J zU#FwpEQLZRw0wQo`0*S~Jw*pBcPuuRY-ueqJGw2nurAn~pLF(+z&T~a zo_#-nSOjsF5g=6oRza+TvPhukS7->)GYNVsi)X>%bdmufrX$HzR$NlKI=ViPa74Nq zM1f3^-9a~JKllXpJI2|mB^W~C2k%+eGFI=A;i;q|SMJX*``;>%s{f`el#UCX7C)w- zrV{!;mGX_6#0m5hs^lS{gs_$Hnw=Iu< zY|XY-0DG%lALx<_+(4^zNxgWZF5N{FRp?b(Y>^hRPf2;!45cIs)Y(dWkf&N>rPmTV z==zc%h4)uL)T<&MWpJ5pxq(61Cf6Z%iyWm&Zj!rAZVTOeE zm_y8Go2$oqb^L$j{7KDuT(=K)+`p5)64_p={sU`y{|3%YQQs9{>^kY&Ytok+4!WN4 zqDsC|Wc(<~OEJv==j|vuoyHjn8^4-SB+7)Au{m{`_`^XXL=x8M)tE%w^tdi+S9;a7I2OL#ypy!&lZp zo9I?QPiH^G<9-tJOkHEx2oyV?Ou>krSy5= z^I66Cs;mlo*0i^ut6VHT?Zx{mQ;{uB;#J zrG4t2dSLyavb2_VP+bzRHC4ZCEbYtY75FY&SJsscbov0JviN<-6_qm{#V0X9FgYWT zGSBcir?dQwi@L#qSbDQigeiE1>d^t%2lQ+bN0Sw18cQT19~?9lGR$AA02AOb=Z z1KcS;p=tOE)8TA~j=4JHJfjc4>~?9GF$&LB6cNUf%O2g!7=N`ZL@sXU1OX1;ezupr z43n5qb5JJV70~VY&CSitHG_qV-M=n0yRWUJLre!T{Zm5kh}u4AZ|KJU;ql>7&_8^3 zaIEXA%ygp^W-uE+(px4W3B(VO*(3g9&ZW}z?{Z&RnjCr@$E&UT#2~bc8S6e{^5ccK%@^9#D5aL1HyL~zx`jr zH*kBUszOy3Pgs_ooaFNau9CUPXq?fb!TznRd;cbRQ30wzhoS~gfmg1;!&noXk#Z?G z(ZU5AXg)%-i3W8g?xR6f2|Pw|56#D5-YLk;Z=n~pQ)7P(mQ4iu+ZBD!NID+xtd(a;*?Tj^AEh7#+0}kNK85?uL|UWa Wc57YzssQyhUPcm&u1T7<>--6MFxq7R literal 0 HcmV?d00001 diff --git a/client_s2/compLib/__pycache__/__init__.cpython-39.pyc b/client_s2/compLib/__pycache__/__init__.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f848f0aac5d443685015ee817a9d23e92f6fb880 GIT binary patch literal 163 zcmYe~<>g`kg81sLBoO@=L?8o3AjbiSi&=m~3PUi1CZpdU!Gr-U0jfuoT{IkpIhML>7<{WlbM=V5?^ct5y?!_kB`sH c%PfhH*DI*J#bJ}1pHiBWY6r66GY~TX09KMHyZ`_I literal 0 HcmV?d00001 diff --git a/client_s2/main.py b/client_s2/main.py index 5bbd783..dd3e732 100644 --- a/client_s2/main.py +++ b/client_s2/main.py @@ -1,36 +1,48 @@ -import socket -import sys -import os -import CompLib_pb2 - - SOCKET_PATH = "/tmp/compLib" -def send(data, size): - with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock: - sock.connect(SOCKET_PATH) - sock.sendall(size.to_bytes(1, byteorder='big')) - sock.sendall(data) - - responseSizeBytes = sock.recv(1) - responseSize = int.from_bytes(responseSizeBytes, byteorder="big") - print(responseSize) - - responseBytes = sock.recv(responseSize) - genericResponse = CompLib_pb2.GenericResponse() - - genericResponse.ParseFromString(responseBytes) - print(genericResponse) - - # reponseBytes = +# def send(data, size): +# with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock: +# sock.connect(SOCKET_PATH) +# sock.sendall(size.to_bytes(1, byteorder='big')) +# sock.sendall(data) +# +# response_size_bytes = sock.recv(1) +# response_size = int.from_bytes(response_size_bytes, byteorder="big") +# # print(f"Response size: {response_size}") +# +# response_bytes = sock.recv(response_size) +# generic_response = CompLib_pb2.GenericResponse() +# +# generic_response.ParseFromString(response_bytes) +# # print(f"Response: {generic_response}") +# +# # reponseBytes = def main(): - readSensorsRequest = CompLib_pb2.ReadSensorsRequest() - # readSensorsRequest.header = CompLib_pb2.Header() - readSensorsRequest.header.message_type = readSensorsRequest.DESCRIPTOR.full_name - send(readSensorsRequest.SerializeToString(), readSensorsRequest.ByteSize()) + # encoder_read_positions_request = CompLib_pb2.EncoderReadPositionsRequest() + # # readSensorsRequest.header = CompLib_pb2.Header() + # encoder_read_positions_request.header.message_type = encoder_read_positions_request.DESCRIPTOR.full_name + # + # start_time = time.time() + # for i in range(100000): + # 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.Encoder import Encoder + Encoder.read_all_positions() + Encoder.read_all_velocities() + + # from compLib.Motor import Motor + # Motor.speed(0, 50) + # Motor.speed(3, -50) + + # time.sleep(10) -main() +if __name__ == '__main__': + main() diff --git a/server_v2/CMakeLists.txt b/server_v2/CMakeLists.txt index 629923a..a5ab68b 100644 --- a/server_v2/CMakeLists.txt +++ b/server_v2/CMakeLists.txt @@ -10,13 +10,26 @@ if (NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif () -set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wno-psabi") -set(CMAKE_CXX_FLAGS_DEBUG "-g") -set(CMAKE_CXX_FLAGS_RELEASE "-O3") - find_package(Protobuf REQUIRED) find_package(spdlog REQUIRED) -find_package(pigpio REQUIRED) + +if (APPLE) + set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") + set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -gdwarf-3") + set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -Wall -Wextra -gdwarf-3") + + set(LIBRARIES "spdlog::spdlog") +elseif (UNIX) + message("Running on Raspberry Pi") + add_definitions(-DIS_RASPI) + set(CMAKE_CXX_FLAGS_RELEASE "-O3") + + find_package(pigpio REQUIRED) + set(LIBRARIES "pigpio" "spdlog::spdlog") + set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wno-psabi") +endif () + +message("CMAKE_SYSTEM_PROCESSOR = ${CMAKE_SYSTEM_PROCESSOR}") set(PROTO_FILES protos/CompLib.proto @@ -35,7 +48,8 @@ set(SRC_FILES src/Odometry.cpp src/communication/MessageProcessor.cpp src/communication/MessageBuilder.cpp - src/communication/UnixSocketServer.cpp) + src/communication/UnixSocketServer.cpp + src/communication/TCPSocketServer.cpp) set(HDR_FILES include/spi.hpp @@ -52,7 +66,8 @@ set(HDR_FILES include/Odometry.hpp include/communication/MessageProcessor.hpp include/communication/MessageBuilder.hpp - include/communication/UnixSocketServer.hpp) + include/communication/UnixSocketServer.hpp + include/communication/TCPSocketServer.hpp) include_directories(third_party/asio) @@ -63,4 +78,4 @@ target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} PUBLIC ${PROTOBUF_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME} ${PROTOBUF_LIBRARIES} pigpio spdlog::spdlog) +target_link_libraries(${PROJECT_NAME} ${PROTOBUF_LIBRARIES} ${LIBRARIES}) diff --git a/server_v2/include/communication/MessageBuilder.hpp b/server_v2/include/communication/MessageBuilder.hpp index 93912f4..37106bc 100644 --- a/server_v2/include/communication/MessageBuilder.hpp +++ b/server_v2/include/communication/MessageBuilder.hpp @@ -1,6 +1,8 @@ #ifndef COMPLIB_SERVER_MESSAGEBUILDER_HPP #define COMPLIB_SERVER_MESSAGEBUILDER_HPP +#include +#include #include "CompLib.pb.h" #include "include/Robot.hpp" diff --git a/server_v2/include/communication/TCPSocketServer.hpp b/server_v2/include/communication/TCPSocketServer.hpp new file mode 100644 index 0000000..a1c0384 --- /dev/null +++ b/server_v2/include/communication/TCPSocketServer.hpp @@ -0,0 +1,20 @@ +#ifndef COMPLIB_SERVER_TCPSOCKETSERVER_HPP +#define COMPLIB_SERVER_TCPSOCKETSERVER_HPP + +#include + +#define SOCKET_PATH "/tmp/compLib" +#define SOCKET_BUFFER_SIZE 128 + +class TCPSocketServer { +public: + TCPSocketServer(); + +private: + [[noreturn]] void server_loop(); + + std::thread server_thread; +}; + + +#endif //COMPLIB_SERVER_TCPSOCKETSERVER_HPP diff --git a/server_v2/include/communication/UnixSocketServer.hpp b/server_v2/include/communication/UnixSocketServer.hpp index c734db9..bae3642 100644 --- a/server_v2/include/communication/UnixSocketServer.hpp +++ b/server_v2/include/communication/UnixSocketServer.hpp @@ -4,7 +4,7 @@ #include #define SOCKET_PATH "/tmp/compLib" -#define SOCKET_BUFFER_SIZE 64 +#define SOCKET_BUFFER_SIZE 128 class UnixSocketServer { public: diff --git a/server_v2/include/reset.hpp b/server_v2/include/reset.hpp index 07f1580..b8e9cc3 100644 --- a/server_v2/include/reset.hpp +++ b/server_v2/include/reset.hpp @@ -1,9 +1,6 @@ #ifndef COMPLIB_SERVER_RESET_HPP #define COMPLIB_SERVER_RESET_HPP -#include -#include - #define RESET_PIN 23 #define BOOT_PIN 17 @@ -11,21 +8,33 @@ #define RESET_STARTUP_SLEEP_TIME_US 1000 * 500 namespace Reset { - void reset_robot() { - gpioInitialise(); +#ifdef IS_RASPI - gpioSetMode(BOOT_PIN, PI_OUTPUT); - gpioSetMode(RESET_PIN, PI_OUTPUT); +#include +#include - gpioWrite(BOOT_PIN, 0); - gpioWrite(RESET_PIN, 0); + void reset_robot() { + gpioInitialise(); - usleep(RESET_SLEEP_TIME_US); + gpioSetMode(BOOT_PIN, PI_OUTPUT); + gpioSetMode(RESET_PIN, PI_OUTPUT); - gpioWrite(RESET_PIN, 1); + gpioWrite(BOOT_PIN, 0); + gpioWrite(RESET_PIN, 0); - usleep(RESET_STARTUP_SLEEP_TIME_US); - } + usleep(RESET_SLEEP_TIME_US); + + gpioWrite(RESET_PIN, 1); + + usleep(RESET_STARTUP_SLEEP_TIME_US); + } + +#else + + void reset_robot() { + } + +#endif } diff --git a/server_v2/include/spi.hpp b/server_v2/include/spi.hpp index 376b222..856fff0 100644 --- a/server_v2/include/spi.hpp +++ b/server_v2/include/spi.hpp @@ -136,7 +136,7 @@ private: void clear_buffers(); - uint8_t calculate_checksum(uint8_t *data, uint8_t length); + static uint8_t calculate_checksum(uint8_t *data, uint8_t length); }; diff --git a/server_v2/src/communication/MessageBuilder.cpp b/server_v2/src/communication/MessageBuilder.cpp index da69270..1fd1df5 100644 --- a/server_v2/src/communication/MessageBuilder.cpp +++ b/server_v2/src/communication/MessageBuilder.cpp @@ -1,3 +1,5 @@ +#include + #include "include/communication/MessageBuilder.hpp" using namespace CompLib; @@ -36,17 +38,17 @@ CompLib::EncoderReadPositionsResponse *MessageBuilder::encoder_read_positions_re response->set_allocated_header(header(EncoderReadPositionsResponse::descriptor()->full_name())); response->set_allocated_status(default_successful_status()); for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) { - response->set_positions(i, positions.at(i)); + response->add_positions(positions.at(i)); } return response; } CompLib::EncoderReadVelocitiesResponse *MessageBuilder::encoder_read_velocities_response(std::array velocities) { auto response = new EncoderReadVelocitiesResponse(); - response->set_allocated_header(header(EncoderReadPositionsResponse::descriptor()->full_name())); + response->set_allocated_header(header(EncoderReadVelocitiesResponse::descriptor()->full_name())); response->set_allocated_status(default_successful_status()); for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) { - response->set_velocities(i, velocities.at(i)); + response->add_velocities(velocities.at(i)); } return response; } @@ -56,7 +58,7 @@ CompLib::IRSensorsReadAllResponse *MessageBuilder::ir_sensors_read_all_response( response->set_allocated_header(header(IRSensorsReadAllResponse::descriptor()->full_name())); response->set_allocated_status(default_successful_status()); for (int i = 0; i < ROBOT_IR_SENSOR_COUNT; ++i) { - response->set_data(i, data.at(i)); + response->add_data(data.at(i)); } return response; } diff --git a/server_v2/src/communication/MessageProcessor.cpp b/server_v2/src/communication/MessageProcessor.cpp index bb5fb03..f5e7898 100644 --- a/server_v2/src/communication/MessageProcessor.cpp +++ b/server_v2/src/communication/MessageProcessor.cpp @@ -13,6 +13,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); try { // Encoder diff --git a/server_v2/src/communication/TCPSocketServer.cpp b/server_v2/src/communication/TCPSocketServer.cpp new file mode 100644 index 0000000..405123d --- /dev/null +++ b/server_v2/src/communication/TCPSocketServer.cpp @@ -0,0 +1,74 @@ +#include +#include +#include +#include + +#include + +#include "include/communication/TCPSocketServer.hpp" +#include "include/communication/MessageProcessor.hpp" + +#define SERVER_PORT 9090 +#define TCP_LISTEN_BACKLOG 5 + +TCPSocketServer::TCPSocketServer() { + server_thread = std::thread(&TCPSocketServer::server_loop, this); + server_thread.detach(); +} + +[[noreturn]] void TCPSocketServer::server_loop() { + struct sockaddr_in address; + int socket_file_descriptor = socket(AF_INET, SOCK_STREAM, 0); + if (socket_file_descriptor == 0) { + spdlog::error("TCP socket initialization failed!"); + exit(EXIT_FAILURE); + } + + int opt = 1; + if (int err = setsockopt(socket_file_descriptor, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) { + spdlog::error("TCP setsockopt failed! Err: {}", err); + exit(EXIT_FAILURE); + } + address.sin_family = AF_INET; + address.sin_addr.s_addr = INADDR_ANY; + address.sin_port = htons(SERVER_PORT); + + if (bind(socket_file_descriptor, (struct sockaddr *) &address, sizeof(address)) < 0) { + spdlog::error("TCP bind failed!"); + exit(EXIT_FAILURE); + } + + if (listen(socket_file_descriptor, TCP_LISTEN_BACKLOG) < 0) { + spdlog::error("TCP listen failed!"); + exit(EXIT_FAILURE); + } + + char read_buffer[SOCKET_BUFFER_SIZE]; + char write_buffer[SOCKET_BUFFER_SIZE]; + for (;;) { + int client_file_descriptor = accept(socket_file_descriptor, NULL, NULL); + if (client_file_descriptor < 0) { + spdlog::error("TCP accept failed!"); + continue; + } + + read(client_file_descriptor, read_buffer, 1); + 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]; + } + + 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); + } +} \ No newline at end of file diff --git a/server_v2/src/main.cpp b/server_v2/src/main.cpp index 0dddd15..157dda9 100644 --- a/server_v2/src/main.cpp +++ b/server_v2/src/main.cpp @@ -1,24 +1,31 @@ #include +#include #include "include/reset.hpp" -#include "include/Encoders.hpp" #include "include/OdometryController.hpp" #include "include/ClosedLoopMotorController.hpp" +#include "include/communication/UnixSocketServer.hpp" +#include "include/communication/TCPSocketServer.hpp" using namespace std; int main() { Reset::reset_robot(); spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v"); + spdlog::set_level(spdlog::level::debug); - ClosedLoopMotorController::getInstance().set_speed(0, -125); - ClosedLoopMotorController::getInstance().set_speed(3, 125); - OdometryController::getInstance().enable(); + UnixSocketServer unixSocketServer; + TCPSocketServer tcpSocketServer; - for (int i = 0; i < 10000; ++i) { - auto odom = OdometryController::getInstance().get(); - spdlog::info("X {} Y {} W {}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation()); - usleep(1000); - } +// ClosedLoopMotorController::getInstance().set_speed(0, -125); +// ClosedLoopMotorController::getInstance().set_speed(3, 125); +// OdometryController::getInstance().enable(); + +// for (int i = 0; i < 10000; ++i) { +// auto odom = OdometryController::getInstance().get(); +// spdlog::info("X {} Y {} W {}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation()); +// usleep(1000); +// } + std::this_thread::sleep_for(std::chrono::hours(12)); return 0; } diff --git a/server_v2/src/spi.cpp b/server_v2/src/spi.cpp index 9314905..1374925 100644 --- a/server_v2/src/spi.cpp +++ b/server_v2/src/spi.cpp @@ -1,8 +1,3 @@ -#include //Needed for SPI port -#include //Needed for SPI port -#include //Needed for SPI port -#include //Needed for SPI port -#include #include #include @@ -10,104 +5,146 @@ #include "include/mathUtils.hpp" void check_for_error(int error_code, std::string error_message) { - if (error_code < 0) { - spdlog::error(error_message); - exit(1); - } -} - -Spi::Spi() { - int spi_mode = SPI_MODE_0; - int spi_bits_per_workd = SPI_BITS_PER_WORD; - int spi_speed = SPI_SPEED; - - check_for_error(spi_file_descriptor = open("/dev/spidev1.2", O_RDWR), "Could not open SPI device"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MODE, &spi_mode), "Could not set SPI_IOC_WR_MODE"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MODE, &spi_mode), "Could not set SPI_IOC_RD_MODE"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_WR_BITS_PER_WORD"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_RD_BITS_PER_WORD"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed), "Could not set SPI_IOC_WR_MAX_SPEED_HZ"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MAX_SPEED_HZ, &spi_mode), "Could not set SPI_IOC_RD_MAX_SPEED_HZ"); + if (error_code < 0) { + spdlog::error(error_message); + exit(1); + } } void Spi::clear_buffers() { - memset(tx_buffer, 0, SPI_BUFFER_SIZE); - memset(rx_buffer, 0, SPI_BUFFER_SIZE); + memset(tx_buffer, 0, SPI_BUFFER_SIZE); + memset(rx_buffer, 0, SPI_BUFFER_SIZE); } -uint8_t Spi::calculate_checksum(uint8_t* data, uint8_t length) { - int sum = 0; - for (int i = 0; i < length; i++) { - sum += data[i]; - } - return sum % 256; +uint8_t Spi::calculate_checksum(uint8_t *data, uint8_t length) { + int sum = 0; + for (int i = 0; i < length; i++) { + sum += data[i]; + } + return sum % 256; +} + +#ifdef IS_RASPI + +#include //Needed for SPI port +#include //Needed for SPI port +#include //Needed for SPI port +#include //Needed for SPI port + +Spi::Spi() { + int spi_mode = SPI_MODE_0; + int spi_bits_per_workd = SPI_BITS_PER_WORD; + int spi_speed = SPI_SPEED; + + check_for_error(spi_file_descriptor = open("/dev/spidev1.2", O_RDWR), "Could not open SPI device"); + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MODE, &spi_mode), "Could not set SPI_IOC_WR_MODE"); + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MODE, &spi_mode), "Could not set SPI_IOC_RD_MODE"); + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_WR_BITS_PER_WORD"); + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_RD_BITS_PER_WORD"); + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed), "Could not set SPI_IOC_WR_MAX_SPEED_HZ"); + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MAX_SPEED_HZ, &spi_mode), "Could not set SPI_IOC_RD_MAX_SPEED_HZ"); } int Spi::read(uint8_t reg, uint8_t length) { - std::lock_guard lock(spi_mutex); + std::lock_guard lock(spi_mutex); - uint8_t read_buffer[SPI_BUFFER_SIZE] = {0}; - read_array(reg, length, read_buffer); + uint8_t read_buffer[SPI_BUFFER_SIZE] = {0}; + read_array(reg, length, read_buffer); - return mathUtils::int_from_bytes(read_buffer, length); + return mathUtils::int_from_bytes(read_buffer, length); } -void Spi::read_array(uint8_t reg, uint8_t length, uint8_t* data) { - std::lock_guard lock(spi_mutex); +void Spi::read_array(uint8_t reg, uint8_t length, uint8_t *data) { + std::lock_guard lock(spi_mutex); - clear_buffers(); - tx_buffer[0] = 0; - tx_buffer[1] = reg; - tx_buffer[2] = length; + clear_buffers(); + tx_buffer[0] = 0; + tx_buffer[1] = reg; + tx_buffer[2] = length; - transfer(); + transfer(); - uint8_t checksum = calculate_checksum(rx_buffer, length + 3); - if (checksum != rx_buffer[length + 3]) { - spdlog::error("Received invalid checksum {}. Should be {}", rx_buffer[length +3], checksum); - } + uint8_t checksum = calculate_checksum(rx_buffer, length + 3); + if (checksum != rx_buffer[length + 3]) { + spdlog::error("Received invalid checksum {}. Should be {}", rx_buffer[length + 3], checksum); + } - memcpy(data, rx_buffer + 2, length); + memcpy(data, rx_buffer + 2, length); } void Spi::write(uint8_t reg, uint8_t length, int value) { - std::lock_guard lock(spi_mutex); + std::lock_guard lock(spi_mutex); - uint8_t write_buffer[SPI_BUFFER_SIZE] = {0}; - mathUtils::bytes_from_int(value, length, write_buffer); - write_array(reg, length, write_buffer); + uint8_t write_buffer[SPI_BUFFER_SIZE] = {0}; + mathUtils::bytes_from_int(value, length, write_buffer); + write_array(reg, length, write_buffer); } -void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t* data) { - std::lock_guard lock(spi_mutex); +void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t *data) { + std::lock_guard lock(spi_mutex); - clear_buffers(); - tx_buffer[0] = 1; - tx_buffer[1] = reg; - tx_buffer[2] = length; - memcpy(tx_buffer + 3, data, length); + clear_buffers(); + tx_buffer[0] = 1; + tx_buffer[1] = reg; + tx_buffer[2] = length; + memcpy(tx_buffer + 3, data, length); - transfer(); + transfer(); } void Spi::transfer() { - std::lock_guard lock(spi_mutex); - - struct spi_ioc_transfer spi; - memset(&spi, 0, sizeof(spi)); - - spi.tx_buf = (unsigned long) tx_buffer; - spi.rx_buf = (unsigned long) rx_buffer; - spi.len = SPI_BUFFER_SIZE; - spi.delay_usecs = 0; - spi.speed_hz = SPI_SPEED; - spi.bits_per_word = SPI_BITS_PER_WORD; - spi.cs_change = 0; - - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data"); + std::lock_guard lock(spi_mutex); - if (tx_buffer[1] != rx_buffer[1]) { - spdlog::error("SPI error during read/write of register {}. Got reg {} instead!", tx_buffer[1], rx_buffer[1]); - } + struct spi_ioc_transfer spi; + memset(&spi, 0, sizeof(spi)); + + spi.tx_buf = (unsigned long) tx_buffer; + spi.rx_buf = (unsigned long) rx_buffer; + spi.len = SPI_BUFFER_SIZE; + spi.delay_usecs = 0; + spi.speed_hz = SPI_SPEED; + spi.bits_per_word = SPI_BITS_PER_WORD; + spi.cs_change = 0; + + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data"); + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data"); + + if (tx_buffer[1] != rx_buffer[1]) { + spdlog::error("SPI error during read/write of register {}. Got reg {} instead!", tx_buffer[1], rx_buffer[1]); + } } + +#else + +Spi::Spi() {} + +int Spi::read(uint8_t reg, uint8_t length) { + std::lock_guard lock(spi_mutex); + spdlog::warn("Calling SPI without actual interface."); + + return 0; +} + +void Spi::read_array(uint8_t reg, uint8_t length, uint8_t *data) { + std::lock_guard lock(spi_mutex); + spdlog::warn("Calling SPI without actual interface."); + clear_buffers(); + memcpy(data, rx_buffer, length); +} + +void Spi::write(uint8_t reg, uint8_t length, int value) { + std::lock_guard lock(spi_mutex); + spdlog::warn("Calling SPI without actual interface."); +} + +void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t *data) { + std::lock_guard lock(spi_mutex); + spdlog::warn("Calling SPI without actual interface."); +} + +void Spi::transfer() { + std::lock_guard lock(spi_mutex); + spdlog::warn("Calling SPI without actual interface."); +} + +#endif \ No newline at end of file