Update

server_v2/.idea/deployment.xml

@@ -2,7 +2,19 @@
 <project version="4">
   <component name="PublishConfigData" remoteFilesAllowedToDisappearOnAutoupload="false">
     <serverData>
-      <paths name="Judge01 (9c7ab821-1501-483e-beda-814d54656ca2)">
+      <paths name="dev01 (e8239813-a9bc-43df-830c-e157833a3358)">
+        <serverdata>
+          <mappings>
+            <mapping deploy="/tmp/tmp.c1a2b48Z1L" local="$PROJECT_DIR$" />
+          </mappings>
+          <excludedPaths>
+            <excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debug" />
+            <excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debugdev03" />
+            <excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debugdev01" />
+          </excludedPaths>
+        </serverdata>
+      </paths>
+      <paths name="dev03 (9c7ab821-1501-483e-beda-814d54656ca2)">
         <serverdata>
           <mappings>
             <mapping deploy="/tmp/tmp.vU8xaArBs9" local="$PROJECT_DIR$" />
@@ -10,6 +22,8 @@
           <excludedPaths>
             <excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debugremote" />
             <excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debug" />
+            <excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debugdev03" />
+            <excludedPath local="true" path="$PROJECT_DIR$/cmake-build-debugdev01" />
           </excludedPaths>
         </serverdata>
       </paths>

server_v2/CMakeLists.txt

@@ -55,12 +55,14 @@ set(SRC_FILES
         src/communication/MessageBuilder.cpp
         src/communication/UnixSocketServer.cpp
         src/communication/TCPSocketServer.cpp
-        src/GoToController.cpp)
+        src/GoToController.cpp
+        src/HealthChecker.cpp)
 
 set(HDR_FILES
         include/spi.hpp
         include/reset.hpp
         include/mathUtils.hpp
+        include/networkUtils.hpp
         include/IRSensors.hpp
         include/Robot.hpp
         include/Cache.hpp
@@ -74,7 +76,8 @@ set(HDR_FILES
         include/communication/MessageBuilder.hpp
         include/communication/UnixSocketServer.hpp
         include/communication/TCPSocketServer.hpp
-        include/GoToController.hpp)
+        include/GoToController.hpp
+        include/HealthChecker.hpp)
 
 include_directories(third_party/asio)
 

server_v2/include/ClosedLoopMotorController.hpp

@@ -25,6 +25,8 @@ public:
 
   void set_speed(uint8_t port, double speed_rad_s);
 
+  void stop_all();
+
   void generate_step_response_data();
 
   void generate_tuned_step_response_data();

server_v2/include/HealthChecker.hpp

@@ -0,0 +1,35 @@
+#ifndef COMPLIB_SERVER_HEALTHCHECKER_HPP
+#define COMPLIB_SERVER_HEALTHCHECKER_HPP
+
+#include <thread>
+#include <chrono>
+
+#define HEALTH_CHECK_TIMEOUT_MS 500
+
+class HealthChecker {
+public:
+  static HealthChecker &getInstance() {
+    static HealthChecker instance;
+    return instance;
+  }
+
+  HealthChecker(HealthChecker const &) = delete;
+
+  void operator=(HealthChecker const &) = delete;
+
+  void update();
+
+private:
+  HealthChecker();
+
+  [[noreturn]] void check_loop();
+
+  typedef std::chrono::steady_clock clock;
+
+  std::thread check_thread;
+  std::chrono::time_point<clock> last_update;
+  bool stop_sent = false;
+};
+
+
+#endif //COMPLIB_SERVER_HEALTHCHECKER_HPP

server_v2/include/communication/TCPSocketServer.hpp

@@ -3,7 +3,6 @@
 
 #include <thread>
 
-#define SOCKET_PATH "/tmp/compLib"
 #define SOCKET_BUFFER_SIZE 128
 
 class TCPSocketServer {

server_v2/include/networkUtils.hpp

@@ -0,0 +1,49 @@
+#ifndef COMPLIB_SERVER_NETWORKUTILS_H
+#define COMPLIB_SERVER_NETWORKUTILS_H
+
+#include <unistd.h>
+#include <string>
+#include <netdb.h>
+#include "spdlog/spdlog.h"
+
+#include <sys/types.h>
+#include <ifaddrs.h>
+
+namespace networkUtils {
+  inline std::tuple<std::string, std::string> get_current_host_and_ip() {
+    struct ifaddrs *ifaddr, *ifa;
+    int family, s;
+    char host[NI_MAXHOST];
+
+    if (getifaddrs(&ifaddr) == -1) {
+      spdlog::error("getifaddrs failed");
+      freeifaddrs(ifaddr);
+      return std::tuple("", "");
+    }
+
+    for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) {
+      if (ifa->ifa_addr == NULL)
+        continue;
+
+      family = ifa->ifa_addr->sa_family;
+
+      if (family == AF_INET) {
+        s = getnameinfo(ifa->ifa_addr, sizeof(struct sockaddr_in), host, NI_MAXHOST, NULL, 0, NI_NUMERICHOST);
+        if (s != 0) {
+          spdlog::error("getnameinfo() failed: {}", gai_strerror(s));
+          freeifaddrs(ifaddr);
+          return std::tuple("", "");
+        }
+
+        if (std::string(ifa->ifa_name) != "lo") {
+          freeifaddrs(ifaddr);
+          return std::tuple(ifa->ifa_name, std::string(host));
+        }
+      }
+    }
+
+    return std::tuple("", "");
+  }
+}
+
+#endif //COMPLIB_SERVER_NETWORKUTILS_H

server_v2/include/spi.hpp

@@ -31,6 +31,8 @@ public:
 
   void write_array(uint8_t reg, uint8_t length, const uint8_t *data);
 
+  std::array<uint8_t, 4> read_version();
+
   enum Register : uint8_t {
     IDENTIFICATION_MODEL_ID = 1,
     IDENTIFICATION_MODEL_REV_MAJOR = 2,

server_v2/protos/CompLib.proto

@@ -58,16 +58,24 @@ message IRSensorsReadAllResponse {
   repeated uint32 data = 3 [packed = true];
 }
 
+message MotorSetPowerRequest {
+  uint32 port = 1;
+  double power = 2;
+}
+
 message MotorsSetPowerRequest {
   Header header = 1;
-  uint32 port = 2;
-  double power = 3;
+  repeated MotorSetPowerRequest requests = 2;
+}
+
+message MotorSetSpeedRequest {
+  uint32 port = 1;
+  double speed = 2;
 }
 
 message MotorsSetSpeedRequest {
   Header header = 1;
-  uint32 port = 2;
-  double speed = 3;
+  repeated MotorSetSpeedRequest requests = 2;
 }
 
 message OdometryReadRequest {
@@ -98,4 +106,8 @@ message DriveRequest {
   Header header = 1;
   double linear_velocity_m_s = 2;
   double angular_velocity_rad_s = 3;
+}
+
+message HealthUpdateRequest {
+  Header header = 1;
 }

server_v2/src/ClosedLoopMotorController.cpp

@@ -184,3 +184,9 @@ void ClosedLoopMotorController::calibrate_wheel_ticks(double turns, double ticks
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
 }
+
+void ClosedLoopMotorController::stop_all() {
+  for (int i = 0; i < ROBOT_MOTOR_COUNT; i++) {
+    set_power(i, 0);
+  }
+}

server_v2/src/HealthChecker.cpp

@@ -0,0 +1,28 @@
+#include "include/HealthChecker.hpp"
+#include "include/ClosedLoopMotorController.hpp"
+#include "include/spi.hpp"
+#include "include/IRSensors.hpp"
+
+HealthChecker::HealthChecker() {
+  last_update = clock::now();
+  check_thread = std::thread(&HealthChecker::check_loop, this);
+  check_thread.detach();
+}
+
+void HealthChecker::update() {
+  last_update = clock::now();
+}
+
+void HealthChecker::check_loop() {
+  while (true) {
+    if (clock::now() - last_update < std::chrono::milliseconds(HEALTH_CHECK_TIMEOUT_MS)) {
+      Spi::getInstance().read_version();
+      stop_sent = false;
+    } else if (!stop_sent) {
+      ClosedLoopMotorController::getInstance().stop_all();
+      IRSensors::disable();
+      stop_sent = true;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(HEALTH_CHECK_TIMEOUT_MS / 2));
+  }
+}

server_v2/src/IRSensors.cpp

@@ -2,6 +2,7 @@
 #include "include/spi.hpp"
 #include "include/Robot.hpp"
 #include "include/mathUtils.hpp"
+#include "spdlog/spdlog.h"
 
 void IRSensors::enable() {
   uint8_t data[ROBOT_IR_SENSOR_COUNT] = {};

server_v2/src/communication/MessageProcessor.cpp

@@ -7,6 +7,7 @@
 #include "include/IRSensors.hpp"
 #include "include/ClosedLoopMotorController.hpp"
 #include "include/GoToController.hpp"
+#include "include/HealthChecker.hpp"
 
 using namespace CompLib;
 
@@ -14,7 +15,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);
+//  spdlog::debug("Got request with type {}", messageTypeName);
 
   try {
     // Encoder
@@ -39,12 +40,16 @@ google::protobuf::Message *MessageProcessor::process_message(const std::string &
     if (messageTypeName == MotorsSetPowerRequest::GetDescriptor()->full_name()) {
       MotorsSetPowerRequest request;
       request.ParseFromString(serializedMessage);
-      ClosedLoopMotorController::getInstance().set_power(request.port(), request.power());
+      for (const auto &innerRequest: request.requests()) {
+        ClosedLoopMotorController::getInstance().set_power(innerRequest.port(), innerRequest.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());
+      for (const auto &innerRequest: request.requests()) {
+        ClosedLoopMotorController::getInstance().set_speed(innerRequest.port(), innerRequest.speed());
+      }
       return MessageBuilder::default_successful_generic_response();
     }
 
@@ -65,6 +70,15 @@ google::protobuf::Message *MessageProcessor::process_message(const std::string &
       GoToController::diff_drive_inverse_kinematics(request.linear_velocity_m_s(), request.angular_velocity_rad_s());
       return MessageBuilder::default_successful_generic_response();
     }
+
+    // Health
+    if (messageTypeName == HealthUpdateRequest::GetDescriptor()->full_name()) {
+      HealthUpdateRequest request;
+      request.ParseFromString(serializedMessage);
+      HealthChecker::getInstance().update();
+      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());

server_v2/src/communication/UnixSocketServer.cpp

@@ -1,6 +1,7 @@
 #include <sys/un.h>
 #include <sys/socket.h>
 #include <zconf.h>
+#include <sys/stat.h>
 
 #include "include/communication/UnixSocketServer.hpp"
 #include "include/communication/MessageProcessor.hpp"
@@ -23,6 +24,8 @@ UnixSocketServer::UnixSocketServer() {
     exit(-1);
   }
 
+  chmod(SOCKET_PATH, 0777);
+
   if (listen(socket_file_descriptor, 1) == -1) {
     exit(-2);
   }
@@ -32,23 +35,31 @@ UnixSocketServer::UnixSocketServer() {
   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];
+    while (true) {
+      int bytes_received = read(client_file_descriptor, read_buffer, 1);
+      if (bytes_received <= 0) {
+        break;
+      }
+      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];
+      bytes_received = read(client_file_descriptor, read_buffer, read_buffer[0]);
+      if (bytes_received <= 0) {
+        break;
+      }
+      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);
     }
 
-    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);
   }
 }

server_v2/src/main.cpp

@@ -6,18 +6,22 @@
 #include "include/ClosedLoopMotorController.hpp"
 #include "include/communication/UnixSocketServer.hpp"
 #include "include/communication/TCPSocketServer.hpp"
+#include "include/HealthChecker.hpp"
 #include "include/GoToController.hpp"
 #include "include/Encoders.hpp"
+#include "include/networkUtils.hpp"
 
 using namespace std;
 
 int main(int argc, char *argv[]) {
   Reset::reset_robot();
   spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v");
-  spdlog::set_level(spdlog::level::info);
+  spdlog::set_level(spdlog::level::debug);
+  spdlog::info("Starting complib_server...");
 
   UnixSocketServer unixSocketServer;
   TCPSocketServer tcpSocketServer;
+  HealthChecker::getInstance();
 
 //  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, M_PI * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT);
 //  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, M_PI_4 * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT);

server_v2/src/spi.cpp

@@ -1,5 +1,6 @@
 #include <string.h>
 #include <spdlog/spdlog.h>
+#include <array>
 
 #include "include/spi.hpp"
 #include "include/mathUtils.hpp"
@@ -114,6 +115,16 @@ void Spi::transfer() {
   }
 }
 
+std::array<uint8_t, 4> Spi::read_version() {
+  uint8_t data[4] = {0};
+  read_array(IDENTIFICATION_MODEL_ID, 4, data);
+  auto result = std::array<uint8_t, 4>();
+  for (int i = 0; i < 4; i++) {
+    result.at(i) = data[i];
+  }
+  return result;
+}
+
 #else
 
 Spi::Spi() {}
@@ -147,4 +158,8 @@ void Spi::transfer() {
   spdlog::warn("Calling SPI without actual interface.");
 }
 
+std::array<uint8_t, 4> Spi::read_version() {
+  spdlog::warn("Calling SPI without actual interface.");
+}
+
 #endif