diff --git a/server_v2/CMakeLists.txt b/server_v2/CMakeLists.txt index 8da8431..629923a 100644 --- a/server_v2/CMakeLists.txt +++ b/server_v2/CMakeLists.txt @@ -32,7 +32,10 @@ set(SRC_FILES src/ClosedLoopMotorController.cpp src/PID.cpp src/OdometryController.cpp - src/Odometry.cpp) + src/Odometry.cpp + src/communication/MessageProcessor.cpp + src/communication/MessageBuilder.cpp + src/communication/UnixSocketServer.cpp) set(HDR_FILES include/spi.hpp @@ -46,7 +49,10 @@ set(HDR_FILES include/ClosedLoopMotorController.hpp include/PID.hpp include/OdometryController.hpp - include/Odometry.hpp) + include/Odometry.hpp + include/communication/MessageProcessor.hpp + include/communication/MessageBuilder.hpp + include/communication/UnixSocketServer.hpp) include_directories(third_party/asio) diff --git a/server_v2/include/communication/MessageBuilder.hpp b/server_v2/include/communication/MessageBuilder.hpp new file mode 100644 index 0000000..93912f4 --- /dev/null +++ b/server_v2/include/communication/MessageBuilder.hpp @@ -0,0 +1,29 @@ +#ifndef COMPLIB_SERVER_MESSAGEBUILDER_HPP +#define COMPLIB_SERVER_MESSAGEBUILDER_HPP + + +#include "CompLib.pb.h" +#include "include/Robot.hpp" + +class MessageBuilder { +public: + static CompLib::Header *header(const std::string &message_type); + + static CompLib::Status *status(bool successful, const std::string &error_message); + + static CompLib::GenericResponse *generic_response(bool successful, const std::string &error_message); + + static CompLib::Status *default_successful_status(); + + static CompLib::GenericResponse *default_successful_generic_response(); + + static CompLib::EncoderReadPositionsResponse *encoder_read_positions_response(std::array positions); + + static CompLib::EncoderReadVelocitiesResponse *encoder_read_velocities_response(std::array velocities); + + static CompLib::IRSensorsReadAllResponse *ir_sensors_read_all_response(std::array data); + +}; + + +#endif //COMPLIB_SERVER_MESSAGEBUILDER_HPP diff --git a/server_v2/include/communication/MessageProcessor.hpp b/server_v2/include/communication/MessageProcessor.hpp new file mode 100644 index 0000000..6b02888 --- /dev/null +++ b/server_v2/include/communication/MessageProcessor.hpp @@ -0,0 +1,15 @@ +#ifndef COMPLIB_SERVER_MESSAGEPROCESSOR_HPP +#define COMPLIB_SERVER_MESSAGEPROCESSOR_HPP + +#include + +#define ERROR_MESSAGE_HEADER_UNKNOWN "Header.message_type unknown" + +class MessageProcessor { +public: + static google::protobuf::Message *process_message(const std::string &serializedMessage); + +}; + + +#endif //COMPLIB_SERVER_MESSAGEPROCESSOR_HPP \ No newline at end of file diff --git a/server_v2/include/communication/UnixSocketServer.hpp b/server_v2/include/communication/UnixSocketServer.hpp new file mode 100644 index 0000000..c734db9 --- /dev/null +++ b/server_v2/include/communication/UnixSocketServer.hpp @@ -0,0 +1,20 @@ +#ifndef COMPLIB_SERVER_UNIXSOCKETSERVER_HPP +#define COMPLIB_SERVER_UNIXSOCKETSERVER_HPP + +#include + +#define SOCKET_PATH "/tmp/compLib" +#define SOCKET_BUFFER_SIZE 64 + +class UnixSocketServer { +public: + UnixSocketServer(); + +private: + [[noreturn]] void server_loop(); + + std::thread server_thread; +}; + + +#endif //COMPLIB_SERVER_UNIXSOCKETSERVER_HPP diff --git a/server_v2/protos/CompLib.proto b/server_v2/protos/CompLib.proto index 77a202d..1bb1bd0 100644 --- a/server_v2/protos/CompLib.proto +++ b/server_v2/protos/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/server_v2/src/communication/MessageBuilder.cpp b/server_v2/src/communication/MessageBuilder.cpp new file mode 100644 index 0000000..da69270 --- /dev/null +++ b/server_v2/src/communication/MessageBuilder.cpp @@ -0,0 +1,62 @@ +#include "include/communication/MessageBuilder.hpp" + +using namespace CompLib; + +CompLib::Header *MessageBuilder::header(const std::string &message_type) { + auto header = new CompLib::Header(); + header->set_message_type(message_type); + return header; +} + +CompLib::Status *MessageBuilder::status(bool successful, const std::string &error_message) { + auto status = new CompLib::Status(); + status->set_successful(successful); + status->set_error_message(error_message); + return status; +} + +CompLib::GenericResponse *MessageBuilder::generic_response(bool successful, const std::string &error_message) { + auto genericResponse = new CompLib::GenericResponse(); + genericResponse->set_allocated_header(header(CompLib::GenericResponse::descriptor()->full_name())); + genericResponse->set_allocated_status(status(successful, error_message)); + return genericResponse; +} + +CompLib::Status *MessageBuilder::default_successful_status() { + return status(true, ""); +} + + +CompLib::GenericResponse *MessageBuilder::default_successful_generic_response() { + return generic_response(true, ""); +} + +CompLib::EncoderReadPositionsResponse *MessageBuilder::encoder_read_positions_response(std::array positions) { + auto response = new EncoderReadPositionsResponse(); + 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)); + } + 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_status(default_successful_status()); + for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) { + response->set_velocities(i, velocities.at(i)); + } + return response; +} + +CompLib::IRSensorsReadAllResponse *MessageBuilder::ir_sensors_read_all_response(std::array data) { + auto response = new IRSensorsReadAllResponse(); + 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)); + } + return response; +} diff --git a/server_v2/src/communication/MessageProcessor.cpp b/server_v2/src/communication/MessageProcessor.cpp new file mode 100644 index 0000000..bb5fb03 --- /dev/null +++ b/server_v2/src/communication/MessageProcessor.cpp @@ -0,0 +1,57 @@ +#include + +#include "include/communication/MessageProcessor.hpp" +#include "include/communication/MessageBuilder.hpp" + +#include "include/Encoders.hpp" +#include "include/IRSensors.hpp" +#include "include/ClosedLoopMotorController.hpp" + +using namespace CompLib; + +google::protobuf::Message *MessageProcessor::process_message(const std::string &serializedMessage) { + CompLib::GenericRequest message; + message.ParseFromString(serializedMessage); + auto messageTypeName = message.header().message_type(); + + try { + // Encoder + if (messageTypeName == EncoderReadPositionsRequest::GetDescriptor()->full_name()) { + return MessageBuilder::encoder_read_positions_response(Encoders::getInstance().get_positions()); + } else if (messageTypeName == EncoderReadVelocitiesRequest::GetDescriptor()->full_name()) { + return MessageBuilder::encoder_read_velocities_response(Encoders::getInstance().get_velocities_rpm()); + } + + // IR Sensors + if (messageTypeName == IRSensorsEnableRequest::GetDescriptor()->full_name()) { + IRSensors::enable(); + return MessageBuilder::default_successful_generic_response(); + } else if (messageTypeName == IRSensorsDisableRequest::GetDescriptor()->full_name()) { + IRSensors::disable(); + return MessageBuilder::default_successful_generic_response(); + } else if (messageTypeName == IRSensorsReadAllRequest::GetDescriptor()->full_name()) { + return MessageBuilder::ir_sensors_read_all_response(IRSensors::getInstance().read()); + } + + // Motors + if (messageTypeName == MotorsSetPowerRequest::GetDescriptor()->full_name()) { + MotorsSetPowerRequest request; + request.ParseFromString(serializedMessage); + ClosedLoopMotorController::getInstance().set_power(request.port(), request.power()); + return MessageBuilder::default_successful_generic_response(); + } else if (messageTypeName == MotorsSetSpeedRequest::GetDescriptor()->full_name()) { + MotorsSetSpeedRequest request; + request.ParseFromString(serializedMessage); + ClosedLoopMotorController::getInstance().set_speed(request.port(), request.speed()); + return MessageBuilder::default_successful_generic_response(); + } + } catch (const std::exception &ex) { + spdlog::error("Error when processing message with header {}: {}", messageTypeName, ex.what()); + google::protobuf::Message *returnMessage = MessageBuilder::generic_response(false, ex.what()); + return returnMessage; + } + + spdlog::error("Message {} unknown!", messageTypeName); + google::protobuf::Message *returnMessage = MessageBuilder::generic_response(false, ERROR_MESSAGE_HEADER_UNKNOWN); + return returnMessage; +} diff --git a/server_v2/src/communication/UnixSocketServer.cpp b/server_v2/src/communication/UnixSocketServer.cpp new file mode 100644 index 0000000..f9b336a --- /dev/null +++ b/server_v2/src/communication/UnixSocketServer.cpp @@ -0,0 +1,54 @@ +#include +#include +#include + +#include "include/communication/UnixSocketServer.hpp" +#include "include/communication/MessageProcessor.hpp" + +UnixSocketServer::UnixSocketServer() { + server_thread = std::thread(&UnixSocketServer::server_loop, this); + server_thread.detach(); +} + +[[noreturn]] void UnixSocketServer::server_loop() { + struct sockaddr_un socket_address; + int socket_file_descriptor = socket(AF_UNIX, SOCK_STREAM, 0); + remove(SOCKET_PATH); + + memset(&socket_address, 0, sizeof(struct sockaddr_un)); + socket_address.sun_family = AF_UNIX; + strncpy(socket_address.sun_path, SOCKET_PATH, sizeof(socket_address.sun_path) - 1); + + if (bind(socket_file_descriptor, (struct sockaddr *) &socket_address, sizeof(struct sockaddr_un)) == -1) { + exit(-1); + } + + if (listen(socket_file_descriptor, 1) == -1) { + exit(-2); + } + + char read_buffer[SOCKET_BUFFER_SIZE]; + char write_buffer[SOCKET_BUFFER_SIZE]; + for (;;) { + int client_file_descriptor = accept(socket_file_descriptor, NULL, NULL); + + read(client_file_descriptor, read_buffer, 1); + uint8_t message_size = read_buffer[0]; + + 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