switch to mqtt connection for communication with main controller

src/dummy_sorting_controller.cpp

@@ -15,6 +15,7 @@
 
 #include "ccf/controller/DummyRobotArmController.h"
 #include "ccf/connection/NngConnection.h"
+#include "ccf/connection/MqttConnection.h"
 #include "ccf/util/NodeUtil.h"
 
 std::string NODE_NAME = "dummy_sorting_controller";
@@ -43,14 +44,12 @@ int main(int argc, char **argv) {
 
     auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
 
-    ROS_INFO_STREAM("Connecting to " << clientControllers.size() << " client controllers.");
-    for (const auto &client: clientControllers) {
-        ROS_INFO_STREAM("Connecting to client at " << client << ".");
-        std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false);
-        client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection"));
-        client_connection->setReceiveTopic("client_scene");
-        connector.addConnection(std::move(client_connection));
-    }
+    auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
+    std::unique_ptr<MqttConnection> mqtt_connection = std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());
+    mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection"));
+    connector.addConnection(std::move(mqtt_connection));
+
+    ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
 
     connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
                                                                   "/config/config_scene.yaml"));

src/moveit_sorting_controller.cpp

@@ -15,6 +15,7 @@
 
 #include "ccf/controller/MoveItRobotArmController.h"
 #include "ccf/connection/NngConnection.h"
+#include "ccf/connection/MqttConnection.h"
 #include "ccf/util/NodeUtil.h"
 
 std::string NODE_NAME = "moveit_sorting_controller";
@@ -43,14 +44,8 @@ int main(int argc, char **argv) {
 
     auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
 
-    ROS_INFO_STREAM("Connecting to " << clientControllers.size() << " client controllers.");
-    for (const auto &client: clientControllers) {
-        ROS_INFO_STREAM("Connecting to client at " << client << ".");
-        std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false);
-        client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection"));
-        client_connection->setReceiveTopic("client_scene");
-        connector.addConnection(std::move(client_connection));
-    }
+    auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
+    std::unique_ptr<MqttConnection> mqtt_connection = std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());
 
     ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)