Add MQTT interface

.gitmodules

 [submodule "cmake"]
 	path = cmake
 	url = https://github.com/jrl-umi3218/jrl-cmakemodules.git
+[submodule "paho.mqtt.c"]
+	path = paho.mqtt.c
+	url = https://github.com/eclipse/paho.mqtt.c.git

CMakeLists.txt

@@ -139,10 +139,17 @@ target_compile_options(${PROJECT_NAME} PUBLIC -DNDEBUG -O3)
 target_compile_definitions(${PROJECT_NAME} PUBLIC CONFIG_SOLO12_YAML="${PROJECT_SOURCE_DIR}/scripts/config_solo12.yaml")
 target_compile_definitions(${PROJECT_NAME} PUBLIC WALK_PARAMETERS_YAML="${PROJECT_SOURCE_DIR}/src/walk_parameters.yaml")
 
+
+# MQTT Interface
+add_subdirectory (paho.mqtt.c)
+add_library(mqtt-interface SHARED include/qrw/mqtt-interface.hpp src/mqtt-interface.cpp)
+target_link_libraries(mqtt-interface PRIVATE paho-mqtt3c paho-mqtt3a)
+target_include_directories(mqtt-interface PUBLIC $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>)
+
 # Main Executable
 # add_executable(${PROJECT_NAMESPACE}-${PROJECT_NAME} src/main.cpp)
 add_executable(${PROJECT_NAMESPACE}-${PROJECT_NAME} src/control_solo12.cpp)
-target_link_libraries(${PROJECT_NAMESPACE}-${PROJECT_NAME} ${PROJECT_NAME})
+target_link_libraries(${PROJECT_NAMESPACE}-${PROJECT_NAME} ${PROJECT_NAME} mqtt-interface)
 
 # Python Bindings
 if(BUILD_PYTHON_INTERFACE)

include/qrw/mqtt-interface.hpp

+#ifndef MQTT_INTERFACE_HPP
+#define MQTT_INTERFACE_HPP
+
+#include <mutex>
+
+
+class MqttInterface {
+ public:
+  MqttInterface();
+  void set(double current, double voltage, double energy, std::string & joystick);
+  void setStatus(std::string & status);
+  void start();
+  void stop();
+  bool getStop();
+  bool getCalibrate();
+
+ private:
+  bool run_;
+  double current_;
+  double voltage_;
+  double energy_;
+  std::string status_;
+  std::string joystick_;
+  std::mutex run_m;
+  std::mutex data_m;
+
+  bool getRun();
+  std::string getCurrent();
+  std::string getVoltage();
+  std::string getEnergy();
+  std::string getStatus();
+  std::string getJoystick();
+};
+#endif /* !MQTT_INTERFACE_HPP */

paho.mqtt.c @ 3b7ae634

+Subproject commit 3b7ae6348bc917d42c04efa962e4868c09bbde9f

src/control_solo12.cpp

@@ -6,6 +6,7 @@
 #include "qrw/Params.hpp"
 #include "qrw/Controller.hpp"
 #include "qrw/FakeRobot.hpp"
+#include "qrw/mqtt-interface.hpp"
 
 using namespace odri_control_interface;
 
@@ -83,13 +84,26 @@ int main()
     robot->joints->SetZeroCommands();
     robot->ParseSensorData();
 
+    // Initialize MQTT
+    MqttInterface mqtt_interface;
+    std::thread mqtt_thread(&MqttInterface::start, &mqtt_interface);
+    std::string mqtt_ready = "ready";
+    std::string mqtt_running = "running";
+    std::string mqtt_stopping = "stopping";
+    std::string mqtt_done = "done";
+    std::string mqtt_placeholder = "TODO";
+    mqtt_interface.setStatus(mqtt_ready);
+    mqtt_interface.set(robot->powerboard->GetCurrent(), robot->powerboard->GetVoltage(), 
+		    robot->powerboard->GetEnergy(), mqtt_placeholder);
     // Wait for Enter input before starting the control loop
     put_on_the_floor(robot, q_init, params, controller);
 
+    mqtt_interface.setStatus(mqtt_running);
     std::chrono::time_point<std::chrono::steady_clock> t_log [params.N_SIMULATION-2];
     // Main loop
     while ((!robot->IsTimeout()) && (k_loop < params.N_SIMULATION-2) && (!controller.error))
     {
+	if (mqtt_interface.getStop()) break;
         t_log[k_loop] = std::chrono::steady_clock::now();
 
         // Parse sensor data from the robot
@@ -127,6 +141,8 @@ int main()
         k_loop++;
         if (k_loop % 1000 == 0)
         {
+	    mqtt_interface.set(robot->powerboard->GetCurrent(), robot->powerboard->GetVoltage(), 
+			    robot->powerboard->GetEnergy(), mqtt_placeholder);
             std::cout << "Joints: ";
             robot->joints->PrintVector(robot->joints->GetPositions());
             std::cout << std::endl;
@@ -134,6 +150,7 @@ int main()
     }
 
     // DAMPING TO GET ON THE GROUND PROGRESSIVELY *********************
+    mqtt_interface.setStatus(mqtt_stopping);
     double t = 0.0;
     double t_max = 2.5;
     while ((!robot->IsTimeout()) && (t < t_max))
@@ -157,6 +174,8 @@ int main()
     }
     // FINAL SHUTDOWN *************************************************
 
+    mqtt_interface.setStatus(mqtt_done);
+
     // Whatever happened we send 0 torques to the motors.
     robot->joints->SetZeroCommands();
     robot->SendCommandAndWaitEndOfCycle(params.dt_wbc);
@@ -172,6 +191,8 @@ int main()
     parallel_thread.join();
     std::cout << "Parallel thread closed" << std::endl;
 
+    mqtt_interface.stop();
+    mqtt_thread.join();
     /*int duration_log [params.N_SIMULATION-2];
     for (int i = 0; i < params.N_SIMULATION-3; i++)
     {

src/mqtt-interface.cpp

+#include "qrw/mqtt-interface.hpp"
+
+#include <iostream>
+#include <thread>
+#include <cstring>
+
+#include "MQTTClient.h"
+
+#define ADDRESS "tcp://localhost:1883"
+#define CLIENTID "Gepetto QRW MQTT Interface"
+
+bool stop_ = false;
+bool calibrate_ = false;
+std::mutex commands_m;
+
+void delivered(void * /*context*/, MQTTClient_deliveryToken /*dt*/) {}
+
+int msgarrvd(void *context, char *topicName, int topicLen, MQTTClient_message *message) {
+  std::string payload((char *)message->payload, message->payloadlen);
+  if (payload == "stop") {
+    std::lock_guard<std::mutex> guard(commands_m);
+    stop_ = true;
+  } else if (payload == "calibrate") {
+    std::lock_guard<std::mutex> guard(commands_m);
+    calibrate_ = true;
+  } else {
+    std::cerr << "Unknown Message on " << topicName << ": " << payload << std::endl;
+  }
+
+  MQTTClient_freeMessage(&message);
+  MQTTClient_free(topicName);
+  return 1;
+}
+
+void connlost(void *context, char *cause) {
+  std::cerr << "Connection lost" << std::endl;
+  std::cerr << "cause: " << cause << std::endl;
+}
+
+void MqttInterface::set(double current, double voltage, double energy, std::string & joystick) {
+  std::lock_guard<std::mutex> guard(data_m);
+  current_ = current;
+  voltage_ = voltage;
+  energy_ = energy;
+  joystick_ = joystick;
+}
+
+void MqttInterface::setStatus(std::string & status) {
+  std::lock_guard<std::mutex> guard(data_m);
+  status_ = status;
+}
+
+MqttInterface::MqttInterface() : run_(true), current_(0), voltage_(0), energy_(0) {}
+
+void MqttInterface::start() {
+  int rc;
+
+  MQTTClient client;
+  MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer;
+  MQTTClient_create(&client, ADDRESS, CLIENTID, MQTTCLIENT_PERSISTENCE_NONE, NULL);
+  conn_opts.keepAliveInterval = 20;
+  conn_opts.cleansession = 1;
+  MQTTClient_setCallbacks(client, NULL, connlost, msgarrvd, delivered);
+  if ((rc = MQTTClient_connect(client, &conn_opts)) != MQTTCLIENT_SUCCESS) {
+    printf("Failed to connect, return code %d\n", rc);
+    exit(EXIT_FAILURE);
+  }
+  MQTTClient_subscribe(client, "/odri/commands", 0);
+  MQTTClient_deliveryToken token;
+
+  std::string current_s;
+  std::string voltage_s;
+  std::string energy_s;
+  std::string status_s;
+  std::string joystick_s;
+  const char* current_c;
+  const char* voltage_c;
+  const char* energy_c;
+  const char* status_c;
+  const char* joystick_c;
+
+  while (getRun()) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(330));
+
+    current_s = getCurrent();
+    voltage_s = getVoltage();
+    energy_s = getEnergy();
+    status_s = getStatus();
+    joystick_s = getJoystick();
+    current_c = current_s.data();
+    voltage_c = voltage_s.data();
+    energy_c = energy_s.data();
+    status_c = status_s.data();
+    joystick_c = joystick_s.data();
+
+    MQTTClient_publish(client, "/odri/current", std::strlen(current_c), current_c, 0, 0, &token);
+    MQTTClient_publish(client, "/odri/voltage", std::strlen(voltage_c), voltage_c, 0, 0, &token);
+    MQTTClient_publish(client, "/odri/energy", std::strlen(energy_c), energy_c, 0, 0, &token);
+    MQTTClient_publish(client, "/odri/status", std::strlen(status_c), status_c, 0, 0, &token);
+    MQTTClient_publish(client, "/odri/joystick", std::strlen(joystick_c), joystick_c, 0, 0, &token);
+  }
+
+  MQTTClient_disconnect(client, 10000);
+  MQTTClient_destroy(&client);
+}
+
+void MqttInterface::stop() {
+  std::lock_guard<std::mutex> guard(run_m);
+  run_ = false;
+}
+
+bool MqttInterface::getStop() {
+  std::lock_guard<std::mutex> guard(commands_m);
+  if (stop_) {
+    stop_ = false;
+    return true;
+  }
+  return false;
+}
+
+bool MqttInterface::getCalibrate() {
+  std::lock_guard<std::mutex> guard(commands_m);
+  if (calibrate_) {
+    calibrate_ = false;
+    return true;
+  }
+  return false;
+}
+
+bool MqttInterface::getRun() {
+  std::lock_guard<std::mutex> guard(run_m);
+  return run_;
+}
+
+std::string MqttInterface::getStatus() {
+  std::lock_guard<std::mutex> guard(data_m);
+  return status_;
+}
+
+std::string MqttInterface::getJoystick() {
+  std::lock_guard<std::mutex> guard(data_m);
+  return joystick_;
+}
+
+std::string MqttInterface::getCurrent() {
+  std::lock_guard<std::mutex> guard(data_m);
+  std::string ret = std::to_string(current_);
+  return ret;
+}
+
+std::string MqttInterface::getVoltage() {
+  std::lock_guard<std::mutex> guard(data_m);
+  std::string ret = std::to_string(voltage_);
+  return ret;
+}
+
+std::string MqttInterface::getEnergy() {
+  std::lock_guard<std::mutex> guard(data_m);
+  std::string ret = std::to_string(energy_);
+  return ret;
+}