cpp

CMakeLists.txt

+cmake_minimum_required (VERSION 3.5)
+
+project (mqtt-interface)
+
+find_package(Eigen3 REQUIRED)
+find_package(Threads REQUIRED)
+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 Eigen3::Eigen)
+target_include_directories(mqtt-interface PUBLIC $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>)
+
+add_executable(mock src/main.cpp)
+target_link_libraries(mock PUBLIC mqtt-interface Eigen3::Eigen Threads::Threads)

include/qrw/mqtt-interface.hpp

+#ifndef MQTT_INTERFACE_HPP
+#define MQTT_INTERFACE_HPP
+
+#include <Eigen/Dense>
+#include <mutex>
+
+typedef Eigen::Matrix<double, 4, 1> MyVector4d;
+
+class MqttInterface {
+ public:
+  MqttInterface();
+  void set(const MyVector4d& values, unsigned char battery);
+  void start();
+  void stop();
+
+ private:
+  bool run_;
+  MyVector4d values_;
+  unsigned char battery_;
+  std::mutex run_m;
+  std::mutex data_m;
+
+  bool getRun();
+  // std::string getValues();
+  std::string getBattery();
+};
+#endif /* !MQTT_INTERFACE_HPP */

src/main.cpp

+#include <iostream>
+#include <thread>
+
+#include "qrw/mqtt-interface.hpp"
+
+int main() {
+  MyVector4d values;
+  MqttInterface mqtt_interface;
+
+  std::thread mqtt_thread(&MqttInterface::start, &mqtt_interface);
+
+  for (unsigned char battery = 100; battery > 72; battery--) {
+    values.setRandom();
+    mqtt_interface.set(values, battery);
+    std::this_thread::sleep_for(std::chrono::milliseconds(250));
+  }
+
+  mqtt_interface.stop();
+  mqtt_thread.join();
+
+  return 0;
+}

src/mqtt-interface.cpp

+#include "qrw/mqtt-interface.hpp"
+
+#include <iostream>
+#include <thread>
+
+#include "MQTTClient.h"
+
+#define ADDRESS "tcp://localhost:1883"
+#define CLIENTID "Gepetto QRW MQTT Interface"
+
+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);
+  std::cout << "Message arrived 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(const MyVector4d &values, unsigned char battery) {
+  std::lock_guard<std::mutex> guard(data_m);
+  values_ = values;
+  battery_ = battery;
+}
+
+MqttInterface::MqttInterface() : run_(true), values_(MyVector4d::Zero()), battery_(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;
+
+  char *battery_c;
+  // char* values_c;
+
+  while (getRun()) {
+    battery_c = getBattery().data();
+
+    MQTTClient_publish(client, "/odri/battery", strlen(battery_c), battery_c, 0, 0, &token);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+
+  MQTTClient_disconnect(client, 10000);
+  MQTTClient_destroy(&client);
+}
+
+void MqttInterface::stop() {
+  std::lock_guard<std::mutex> guard(run_m);
+  run_ = false;
+}
+
+bool MqttInterface::getRun() {
+  std::lock_guard<std::mutex> guard(run_m);
+  return run_;
+}
+
+std::string MqttInterface::getBattery() {
+  std::lock_guard<std::mutex> guard(data_m);
+  std::string ret = std::to_string(battery_);
+  return ret;
+}