Start on unix socket server on c++ side

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<int32_t, ROBOT_MOTOR_COUNT> 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<double, ROBOT_MOTOR_COUNT> 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<uint16_t, ROBOT_IR_SENSOR_COUNT> 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;
+}

server_v2/src/communication/MessageProcessor.cpp

@@ -0,0 +1,57 @@
+#include <spdlog/spdlog.h>
+
+#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;
+}

server_v2/src/communication/UnixSocketServer.cpp

@@ -0,0 +1,54 @@
+#include <sys/un.h>
+#include <sys/socket.h>
+#include <zconf.h>
+
+#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);
+  }
+}