Add complib client and TCP communication
This commit is contained in:
Konstantin Lampalzer
parent
1a033c8b03
commit
a484bc2137
35 changed files with 815 additions and 232 deletions
3
client_s2/.idea/.gitignore
generated
vendored
Normal file
3
client_s2/.idea/.gitignore
generated
vendored
Normal file
|
|
@ -0,0 +1,3 @@
|
|||
# Default ignored files
|
||||
/shelf/
|
||||
/workspace.xml
|
||||
8
client_s2/.idea/client_s2.iml
generated
Normal file
8
client_s2/.idea/client_s2.iml
generated
Normal file
|
|
@ -0,0 +1,8 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<module type="PYTHON_MODULE" version="4">
|
||||
<component name="NewModuleRootManager">
|
||||
<content url="file://$MODULE_DIR$" />
|
||||
<orderEntry type="inheritedJdk" />
|
||||
<orderEntry type="sourceFolder" forTests="false" />
|
||||
</component>
|
||||
</module>
|
||||
12
client_s2/.idea/inspectionProfiles/Project_Default.xml
generated
Normal file
12
client_s2/.idea/inspectionProfiles/Project_Default.xml
generated
Normal file
|
|
@ -0,0 +1,12 @@
|
|||
<component name="InspectionProjectProfileManager">
|
||||
<profile version="1.0">
|
||||
<option name="myName" value="Project Default" />
|
||||
<inspection_tool class="PyUnresolvedReferencesInspection" enabled="true" level="WARNING" enabled_by_default="true">
|
||||
<option name="ignoredIdentifiers">
|
||||
<list>
|
||||
<option value="list.__getitem__" />
|
||||
</list>
|
||||
</option>
|
||||
</inspection_tool>
|
||||
</profile>
|
||||
</component>
|
||||
6
client_s2/.idea/inspectionProfiles/profiles_settings.xml
generated
Normal file
6
client_s2/.idea/inspectionProfiles/profiles_settings.xml
generated
Normal file
|
|
@ -0,0 +1,6 @@
|
|||
<component name="InspectionProjectProfileManager">
|
||||
<settings>
|
||||
<option name="USE_PROJECT_PROFILE" value="false" />
|
||||
<version value="1.0" />
|
||||
</settings>
|
||||
</component>
|
||||
4
client_s2/.idea/misc.xml
generated
Normal file
4
client_s2/.idea/misc.xml
generated
Normal file
|
|
@ -0,0 +1,4 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.9" project-jdk-type="Python SDK" />
|
||||
</project>
|
||||
8
client_s2/.idea/modules.xml
generated
Normal file
8
client_s2/.idea/modules.xml
generated
Normal file
|
|
@ -0,0 +1,8 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="ProjectModuleManager">
|
||||
<modules>
|
||||
<module fileurl="file://$PROJECT_DIR$/.idea/client_s2.iml" filepath="$PROJECT_DIR$/.idea/client_s2.iml" />
|
||||
</modules>
|
||||
</component>
|
||||
</project>
|
||||
15
client_s2/.idea/saveactions_settings.xml
generated
Normal file
15
client_s2/.idea/saveactions_settings.xml
generated
Normal file
|
|
@ -0,0 +1,15 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="SaveActionSettings">
|
||||
<option name="actions">
|
||||
<set>
|
||||
<option value="activate" />
|
||||
<option value="activateOnShortcut" />
|
||||
<option value="activateOnBatch" />
|
||||
<option value="organizeImports" />
|
||||
<option value="reformat" />
|
||||
<option value="rearrange" />
|
||||
</set>
|
||||
</option>
|
||||
</component>
|
||||
</project>
|
||||
6
client_s2/.idea/vcs.xml
generated
Normal file
6
client_s2/.idea/vcs.xml
generated
Normal file
|
|
@ -0,0 +1,6 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="VcsDirectoryMappings">
|
||||
<mapping directory="$PROJECT_DIR$/.." vcs="Git" />
|
||||
</component>
|
||||
</project>
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
Binary file not shown.
45
client_s2/compLib/CompLibClient.py
Normal file
45
client_s2/compLib/CompLibClient.py
Normal file
|
|
@ -0,0 +1,45 @@
|
|||
import socket
|
||||
|
||||
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):
|
||||
|
||||
@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 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
|
||||
190
client_s2/compLib/CompLib_pb2.py
Normal file
190
client_s2/compLib/CompLib_pb2.py
Normal file
|
|
@ -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)
|
||||
35
client_s2/compLib/Encoder.py
Normal file
35
client_s2/compLib/Encoder.py
Normal file
|
|
@ -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)
|
||||
39
client_s2/compLib/IRSensor.py
Normal file
39
client_s2/compLib/IRSensor.py
Normal file
|
|
@ -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())
|
||||
69
client_s2/compLib/Motor.py
Normal file
69
client_s2/compLib/Motor.py
Normal file
|
|
@ -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)
|
||||
0
client_s2/compLib/__init__.py
Normal file
0
client_s2/compLib/__init__.py
Normal file
BIN
client_s2/compLib/__pycache__/CompLibClient.cpython-39.pyc
Normal file
BIN
client_s2/compLib/__pycache__/CompLibClient.cpython-39.pyc
Normal file
Binary file not shown.
BIN
client_s2/compLib/__pycache__/CompLib_pb2.cpython-39.pyc
Normal file
BIN
client_s2/compLib/__pycache__/CompLib_pb2.cpython-39.pyc
Normal file
Binary file not shown.
BIN
client_s2/compLib/__pycache__/Encoder.cpython-39.pyc
Normal file
BIN
client_s2/compLib/__pycache__/Encoder.cpython-39.pyc
Normal file
Binary file not shown.
BIN
client_s2/compLib/__pycache__/IRSensor.cpython-39.pyc
Normal file
BIN
client_s2/compLib/__pycache__/IRSensor.cpython-39.pyc
Normal file
Binary file not shown.
BIN
client_s2/compLib/__pycache__/Motor.cpython-39.pyc
Normal file
BIN
client_s2/compLib/__pycache__/Motor.cpython-39.pyc
Normal file
Binary file not shown.
BIN
client_s2/compLib/__pycache__/__init__.cpython-39.pyc
Normal file
BIN
client_s2/compLib/__pycache__/__init__.cpython-39.pyc
Normal file
Binary file not shown.
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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})
|
||||
|
|
|
|||
|
|
@ -1,6 +1,8 @@
|
|||
#ifndef COMPLIB_SERVER_MESSAGEBUILDER_HPP
|
||||
#define COMPLIB_SERVER_MESSAGEBUILDER_HPP
|
||||
|
||||
#include <cstdint>
|
||||
#include <array>
|
||||
|
||||
#include "CompLib.pb.h"
|
||||
#include "include/Robot.hpp"
|
||||
|
|
|
|||
20
server_v2/include/communication/TCPSocketServer.hpp
Normal file
20
server_v2/include/communication/TCPSocketServer.hpp
Normal file
|
|
@ -0,0 +1,20 @@
|
|||
#ifndef COMPLIB_SERVER_TCPSOCKETSERVER_HPP
|
||||
#define COMPLIB_SERVER_TCPSOCKETSERVER_HPP
|
||||
|
||||
#include <thread>
|
||||
|
||||
#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
|
||||
|
|
@ -4,7 +4,7 @@
|
|||
#include <thread>
|
||||
|
||||
#define SOCKET_PATH "/tmp/compLib"
|
||||
#define SOCKET_BUFFER_SIZE 64
|
||||
#define SOCKET_BUFFER_SIZE 128
|
||||
|
||||
class UnixSocketServer {
|
||||
public:
|
||||
|
|
|
|||
|
|
@ -1,9 +1,6 @@
|
|||
#ifndef COMPLIB_SERVER_RESET_HPP
|
||||
#define COMPLIB_SERVER_RESET_HPP
|
||||
|
||||
#include <pigpio.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#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 <pigpio.h>
|
||||
#include <unistd.h>
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,5 @@
|
|||
#include <cstdint>
|
||||
|
||||
#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<double, ROBOT_MOTOR_COUNT> 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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
74
server_v2/src/communication/TCPSocketServer.cpp
Normal file
74
server_v2/src/communication/TCPSocketServer.cpp
Normal file
|
|
@ -0,0 +1,74 @@
|
|||
#include <sys/un.h>
|
||||
#include <sys/socket.h>
|
||||
#include <zconf.h>
|
||||
#include <netinet/in.h>
|
||||
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,24 +1,31 @@
|
|||
#include <spdlog/spdlog.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,8 +1,3 @@
|
|||
#include <fcntl.h> //Needed for SPI port
|
||||
#include <sys/ioctl.h> //Needed for SPI port
|
||||
#include <linux/spi/spidev.h> //Needed for SPI port
|
||||
#include <unistd.h> //Needed for SPI port
|
||||
#include <iostream>
|
||||
#include <string.h>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
|
|
@ -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 <fcntl.h> //Needed for SPI port
|
||||
#include <sys/ioctl.h> //Needed for SPI port
|
||||
#include <linux/spi/spidev.h> //Needed for SPI port
|
||||
#include <unistd.h> //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<std::recursive_mutex> lock(spi_mutex);
|
||||
std::lock_guard<std::recursive_mutex> 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<std::recursive_mutex> lock(spi_mutex);
|
||||
void Spi::read_array(uint8_t reg, uint8_t length, uint8_t *data) {
|
||||
std::lock_guard<std::recursive_mutex> 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<std::recursive_mutex> lock(spi_mutex);
|
||||
std::lock_guard<std::recursive_mutex> 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<std::recursive_mutex> lock(spi_mutex);
|
||||
void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t *data) {
|
||||
std::lock_guard<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> 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<std::recursive_mutex> lock(spi_mutex);
|
||||
spdlog::warn("Calling SPI without actual interface.");
|
||||
}
|
||||
|
||||
void Spi::transfer() {
|
||||
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
|
||||
spdlog::warn("Calling SPI without actual interface.");
|
||||
}
|
||||
|
||||
#endif
|
||||
Reference in a new issue