Start on unix socket server on c++ side

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)
 

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<int32_t, ROBOT_MOTOR_COUNT> positions);
+
+  static CompLib::EncoderReadVelocitiesResponse *encoder_read_velocities_response(std::array<double, ROBOT_MOTOR_COUNT> velocities);
+
+  static CompLib::IRSensorsReadAllResponse *ir_sensors_read_all_response(std::array<uint16_t, ROBOT_IR_SENSOR_COUNT> data);
+
+};
+
+
+#endif //COMPLIB_SERVER_MESSAGEBUILDER_HPP

server_v2/include/communication/MessageProcessor.hpp

@@ -0,0 +1,15 @@
+#ifndef COMPLIB_SERVER_MESSAGEPROCESSOR_HPP
+#define COMPLIB_SERVER_MESSAGEPROCESSOR_HPP
+
+#include <CompLib.pb.h>
+
+#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

server_v2/include/communication/UnixSocketServer.hpp

@@ -0,0 +1,20 @@
+#ifndef COMPLIB_SERVER_UNIXSOCKETSERVER_HPP
+#define COMPLIB_SERVER_UNIXSOCKETSERVER_HPP
+
+#include <thread>
+
+#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

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;
+  repeated int32 positions = 3 [packed = true];
-  uint32 ir_2 = 4;
+}
-  uint32 ir_3 = 5;
+
-  uint32 ir_4 = 6;
+message EncoderReadVelocitiesRequest {
-  uint32 ir_5 = 7;
+  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;
 }
 

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);
+  }
+}