proto-files for communication, ability for sending robot-telemetry over mqtt

CMakeLists.txt

-#cmake_minimum_required(VERSION 3.10)
-#project(MY_PROJECT)
-#set(CMAKE_CXX_STANDARD 14)
-
-#add_executable(MY_PROJECT main.cpp)
-# Perform linking of the paho lib
-#target_link_libraries(MY_PROJECT LINK_PUBLIC ${PahoMqtt} ${PahoMqttCpp})
-# LINK_PUBLIC vs LINK_PRIVATE: https://cmake.org/cmake/help/v3.10/command/target_link_libraries.html
-
-
-
 cmake_minimum_required(VERSION 2.8.3)
 project(panda_simulation)
 
-# Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++14)
 set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}")
@@ -41,6 +29,7 @@ find_package(catkin REQUIRED
 
 # System dependencies are found with CMake's conventions
 find_package(Boost REQUIRED COMPONENTS filesystem)
+find_package(Protobuf 3 REQUIRED)
 
 find_library(PahoMqtt3c libpaho-mqtt3c.so REQUIRED)
 find_library(PahoMqtt3cs libpaho-mqtt3cs.so REQUIRED)
@@ -70,7 +59,6 @@ catkin_package(CATKIN_DEPENDS
                hardware_interface
                pluginlib
                DEPENDS
-               #libpaho-mqtt3
               # system_lib
         )
 
@@ -83,6 +71,7 @@ include_directories(${catkin_INCLUDE_DIRS})
 
 # add custom controller as library
 add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
+add_library(linkstate src/linkstate.pb.cc src/linkstate.pb.h)
 
 # Specify libraries to link a library or executable target against
 target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
@@ -92,7 +81,7 @@ add_executable(box_publisher_node src/box_publisher.cpp)
 add_executable(robot_state_initializer_node src/robot_state_initializer.cpp)
 add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp src/SafetyEnvironmentCreator.cpp)
 add_executable(SampleConstraintPlanner src/SampleConstraintPlanner.cpp)
-add_executable(RobotStateProvider src/RobotStateProvider.cpp)
+add_executable(RobotStateProvider src/RobotStateProvider.cpp src/mem_persistence.cpp)
 add_executable(TimedCartesianPlanner src/TimedCartesianPlanner.cpp)
 add_executable(MqttToRosNode src/MqttToRosNode.cpp)
 
@@ -107,7 +96,9 @@ target_link_libraries(RobotStateProvider LINK_PUBLIC
         ${PahoMqtt3cs}
         ${PahoMqtt3a}
         ${PahoMqtt3as}
-        ${PahoMqttCpp})
+        ${PahoMqttCpp}
+        ${Protobuf_LIBRARIES}
+        linkstate)
 target_link_libraries(MqttToRosNode LINK_PUBLIC
         ${catkin_LIBRARIES}
         ${PahoMqtt3c}

proto/dataconfig.proto

+syntax = "proto2";
+
+package config;
+
+message DataConfig {
+
+  required bool enablePosition = 1;
+  required bool enableOrientation = 2;
+  required bool enableTwistLinear = 3;
+  required bool enableTwistAngular = 4;
+
+  required int32 publishRate = 5;
+}
\ No newline at end of file

proto/linkstate.proto

+syntax = "proto2";
+
+package panda;
+
+message PandaLinkState {
+
+  required string name = 1;
+
+  message Position {
+    required float positionX = 1;
+    required float positionY = 2;
+    required float positionZ = 3;
+  }
+
+  message Orientation {
+    required float orientationX = 1;
+    required float orientationY = 2;
+    required float orientationZ = 3;
+    required float orientationW = 4;
+  }
+
+  message TwistLinear {
+    required float twistLinearX = 1;
+    required float twistLinearY = 2;
+    required float twistLinearZ = 3;
+  }
+
+  message TwistAngular {
+    required float twistAngularX = 1;
+    required float twistAngularY = 2;
+    required float twistAngularZ = 3;
+  }
+
+  optional Position pos = 2;
+  optional Orientation orient = 3;
+  optional TwistLinear tl = 4;
+  optional TwistAngular ta = 5;
+}
\ No newline at end of file

proto/robotconfig.proto

+syntax = "proto2";
+
+package config;
+
+message RobotConfig {
+
+  required double speed = 1;
+}
\ No newline at end of file

src/RobotStateProvider.cpp

@@ -5,6 +5,8 @@
 #include <gazebo_msgs/LinkStates.h>
 #include <mqtt/client.h>
 #include "string.h"
+#include "linkstate.pb.h"
+#include "mem_persistence.cpp"
 
 // CONFIGURATION START
 // ^^^^^^^^^^^^^^^^^^^
@@ -17,40 +19,44 @@ bool export_position = true;
 bool export_orientation = true;
 bool export_twist_linear = true;
 bool export_twist_angular = true;
-bool export_names = true;
+
+bool export_to_log = false;
 
 // CONFIGURATION END
 // ^^^^^^^^^^^^^^^^^
 
-const std::string SERVER_ADDRESS { "tcp://localhost:1883" };
-const std::string CLIENT_ID { "ros_mqtt_client" };
-const std::string TOPIC { "robot_data_feed" };
-const int QOS = 1;
+/*
+ * mqtt-topics for all links
+ * ground, link0, link1, link2, link3, link4, link5, link6, link7 (end-effector), left finger, right finder
+ */
+std::string topics [11] = {"panda_link_0","panda_link_1","panda_link_2" ,"panda_link_3",
+                           "panda_link_4","panda_link_5","panda_link_6","panda_link_7",
+                           "panda_link_8","panda_link_9","panda_link_10"};
 
-int current_redirect = 0;
+bool mqttSetup = false;
 bool isInitialized = false;
 
+const int QOS = 1;
+int current_redirect = 0;
+
+mem_persistence persist;
+mqtt::client client("tcp://localhost:1883", "robot_state_provider_mqtt", &persist);
+
 /*
  * logs to its own file in /.ros/logs (configured in launch-file)
  */
-void exportMessageToLog(const gazebo_msgs::LinkStates & msg)
-{
-    if(export_names){
-
+void exportMessageToLog(const gazebo_msgs::LinkStates &msg) {
     ROS_INFO_STREAM("<<< LINK NAMES >>>");
 
-        for(int i = 0; i < msg.name.size(); i++)
-        {
+    for (int i = 0; i < msg.name.size(); i++) {
         ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i));
     }
-    }
 
     if (export_position) {
 
         ROS_INFO_STREAM("<<< POSITIONS BY LINK >>>");
 
-        for(int i = 0; i < msg.pose.size(); i++)
-        {
+        for (int i = 0; i < msg.pose.size(); i++) {
             ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).position.x);
             ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).position.y);
             ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).position.z);
@@ -61,8 +67,7 @@ void exportMessageToLog(const gazebo_msgs::LinkStates & msg)
 
         ROS_INFO_STREAM("<<< ORIENTATIONS BY LINK >>>");
 
-        for(int i = 0; i < msg.pose.size(); i++)
-        {
+        for (int i = 0; i < msg.pose.size(); i++) {
             ROS_INFO_STREAM("[" << i << "] [w] " << msg.pose.at(i).orientation.w);
             ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).orientation.x);
             ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).orientation.y);
@@ -72,8 +77,7 @@ void exportMessageToLog(const gazebo_msgs::LinkStates & msg)
 
     if (export_twist_linear) {
         ROS_INFO_STREAM("<<< LINEAR TWISTS BY LINK >>>");
-        for(int i = 0; i < msg.twist.size(); i++)
-        {
+        for (int i = 0; i < msg.twist.size(); i++) {
             ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).linear.x);
             ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).linear.y);
             ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).linear.z);
@@ -82,8 +86,7 @@ void exportMessageToLog(const gazebo_msgs::LinkStates & msg)
 
     if (export_twist_angular) {
         ROS_INFO_STREAM("<<< ANGULAR TWISTS BY LINK >>>");
-        for(int i = 0; i < msg.twist.size(); i++)
-        {
+        for (int i = 0; i < msg.twist.size(); i++) {
             ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).angular.x);
             ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).angular.y);
             ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).angular.z);
@@ -91,8 +94,84 @@ void exportMessageToLog(const gazebo_msgs::LinkStates & msg)
     }
 }
 
-void providerCallback(const gazebo_msgs::LinkStates & msg)
+panda::PandaLinkState buildLinkStateMessage(const gazebo_msgs::LinkStates &msg, int link_number) {
+
+    panda::PandaLinkState pls_msg;
+
+    pls_msg.set_name(msg.name.at(link_number));
+
+    if (export_position) {
+        panda::PandaLinkState_Position *pos = new panda::PandaLinkState_Position();
+        pos->set_positionx(msg.pose.at(link_number).position.x);
+        pos->set_positiony(msg.pose.at(link_number).position.y);
+        pos->set_positionz(msg.pose.at(link_number).position.z);
+        pls_msg.set_allocated_pos(pos);
+    }
+
+    if (export_orientation) {
+        panda::PandaLinkState_Orientation *orient = new panda::PandaLinkState_Orientation();
+        orient->set_orientationw(msg.pose.at(link_number).orientation.w);
+        orient->set_orientationx(msg.pose.at(link_number).orientation.x);
+        orient->set_orientationy(msg.pose.at(link_number).orientation.y);
+        orient->set_orientationz(msg.pose.at(link_number).orientation.z);
+        pls_msg.set_allocated_orient(orient);
+    }
+
+    if (export_twist_linear) {
+        panda::PandaLinkState_TwistLinear *tl = new panda::PandaLinkState_TwistLinear();
+        tl->set_twistlinearx(msg.twist.at(link_number).linear.x);
+        tl->set_twistlineary(msg.twist.at(link_number).linear.y);
+        tl->set_twistlinearz(msg.twist.at(link_number).linear.z);
+        pls_msg.set_allocated_tl(tl);
+    }
+
+    if (export_twist_angular) {
+        panda::PandaLinkState_TwistAngular *ta = new panda::PandaLinkState_TwistAngular();
+        ta->set_twistangularx(msg.twist.at(link_number).angular.x);
+        ta->set_twistangulary(msg.twist.at(link_number).angular.y);
+        ta->set_twistangularz(msg.twist.at(link_number).angular.z);
+        pls_msg.set_allocated_ta(ta);
+    }
+
+    return pls_msg;
+}
+
+void setupMqtt() {
+
+    if (!mqttSetup) {
+        std::cout << "Initializing..." << std::endl;
+
+        mqtt::connect_options connOpts;
+        connOpts.set_keep_alive_interval(20);
+        connOpts.set_clean_session(true);
+        std::cout << "...OK" << std::endl;
+
+        std::cout << "\nConnecting..." << std::endl;
+        client.connect(connOpts);
+        std::cout << "...OK" << std::endl;
+        mqttSetup = true;
+    }
+}
+
+
+void sendMqttMessages(const gazebo_msgs::LinkStates &msg) {
+
+    setupMqtt();
+
+    for(int i = 0; i < msg.name.size(); i++)
     {
+        panda::PandaLinkState pls_msg = buildLinkStateMessage(msg, i);
+        std::string mqtt_msg;
+        pls_msg.SerializeToString(&mqtt_msg);
+
+        //std::cout << "SENDING MESSAGE" << std::endl;
+        auto pubmsg = mqtt::make_message(topics[i], mqtt_msg);
+        pubmsg->set_qos(QOS);
+        client.publish(pubmsg);
+    }
+}
+
+void providerCallback(const gazebo_msgs::LinkStates &msg) {
     if (current_redirect != 0 && current_redirect != message_redirect_rate) {
         // std::cout << "Ignoring message (redirect: " << current_redirect << " )" << std::endl;
         current_redirect++;
@@ -101,7 +180,13 @@ void providerCallback(const gazebo_msgs::LinkStates & msg)
 
     if (current_redirect == 0 || current_redirect == message_redirect_rate) {
         //std::cout << "Redirecting message:" << msg.pose.at(1).position << std::endl;
+
+        if(export_to_log){
             exportMessageToLog(msg);
+        }
+
+        sendMqttMessages(msg);
+
         if (current_redirect == message_redirect_rate) {
             current_redirect = 1;
         } else {
@@ -110,37 +195,23 @@ void providerCallback(const gazebo_msgs::LinkStates & msg)
     }
 }
 
-void sendMqttMessage(){
-    mqtt::client client(SERVER_ADDRESS, CLIENT_ID);
-
-    mqtt::connect_options connOpts;
-    connOpts.set_keep_alive_interval(20);
-    connOpts.set_clean_session(true);
-
-    client.connect(connOpts);
-    auto pubmsg = mqtt::make_message(TOPIC, "test");
-    pubmsg->set_qos(QOS);
-    client.publish(pubmsg);
-
-    client.disconnect();
-}
-
-int main(int argc, char **argv)
-{
+int main(int argc, char **argv) {
     ros::init(argc, argv, "listener");
     ros::NodeHandle n;
     n.getParam("ready", isInitialized);
 
+    while (ros::ok()) {
         // make sure the robot is initialized
         while (!isInitialized) {
             //std::cout << "Waiting" << std::endl;
             n.getParam("tud_planner_ready", isInitialized);
         }
 
-    //sendMqttMessage();
-
         ros::Subscriber sub = n.subscribe("/gazebo/link_states", 10000, providerCallback);
         ros::spin();
+    }
 
+    // disconnect from mqtt
+    client.disconnect();
     return 0;
 }
\ No newline at end of file

src/mem_persistence.cpp

+#include <string>
+#include <map>
+#include <vector>
+#include "mqtt/client.h"
+
+// Connection persistence class.
+class mem_persistence : virtual public mqtt::iclient_persistence {
+    // Whether the store is open
+    bool open_;
+
+    // Use an STL map to store shared persistence pointers
+    // against string keys.
+    std::map<std::string, std::string> store_;
+
+public:
+    mem_persistence() : open_(false) {}
+
+    // "Open" the store
+    void open(const std::string &clientId, const std::string &serverURI) override {
+        open_ = true;
+    }
+
+    // Close the persistent store that was previously opened.
+    void close() override {
+        open_ = false;
+    }
+
+    // Clears persistence, so that it no longer contains any persisted data.
+    void clear() override {
+        store_.clear();
+    }
+
+    // Returns whether or not data is persisted using the specified key.
+    bool contains_key(const std::string &key) override {
+        return store_.find(key) != store_.end();
+    }
+
+    // Returns the keys in this persistent data store.
+    const mqtt::string_collection &keys() const override {
+        static mqtt::string_collection ks;
+        ks.clear();
+        for (const auto &k : store_)
+            ks.push_back(k.first);
+        return ks;
+    }
+
+    // Puts the specified data into the persistent store.
+    void put(const std::string &key, const std::vector<mqtt::string_view> &bufs) override {
+        std::string str;
+        for (const auto &b : bufs)
+            str += b.str();
+        store_[key] = std::move(str);
+    }
+
+    // Gets the specified data out of the persistent store.
+    mqtt::string_view get(const std::string &key) const override {
+        auto p = store_.find(key);
+        if (p == store_.end())
+            throw mqtt::persistence_exception();
+
+        return mqtt::string_view(p->second);
+    }
+
+    // Remove the data for the specified key.
+    void remove(const std::string &key) override {
+        auto p = store_.find(key);
+        if (p == store_.end())
+            throw mqtt::persistence_exception();
+        store_.erase(p);;
+    }
+};
\ No newline at end of file