rename main controller and add launch file

CMakeLists.txt

@@ -136,7 +136,7 @@ include_directories(
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(${PROJECT_NAME}_dummy_selection_provider src/dummy_selection_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(${PROJECT_NAME}_dummy_sorting_controller src/dummy_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} )
-add_executable(${PROJECT_NAME}_dummy_main_controller src/dummy_main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} )
+add_executable(${PROJECT_NAME}_main_controller src/main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} )
 add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 
 
@@ -146,21 +146,21 @@ add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_cont
 ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
 set_target_properties(${PROJECT_NAME}_dummy_selection_provider PROPERTIES OUTPUT_NAME dummy_selection_provider PREFIX "")
 set_target_properties(${PROJECT_NAME}_dummy_sorting_controller PROPERTIES OUTPUT_NAME dummy_sorting_controller PREFIX "")
-set_target_properties(${PROJECT_NAME}_dummy_main_controller PROPERTIES OUTPUT_NAME dummy_main_controller PREFIX "")
+set_target_properties(${PROJECT_NAME}_main_controller PROPERTIES OUTPUT_NAME main_controller PREFIX "")
 set_target_properties(${PROJECT_NAME}_moveit_sorting_controller PROPERTIES OUTPUT_NAME moveit_sorting_controller PREFIX "")
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
 add_dependencies(${PROJECT_NAME}_dummy_selection_provider ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(${PROJECT_NAME}_dummy_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(${PROJECT_NAME}_dummy_main_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_main_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(${PROJECT_NAME}_moveit_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(${PROJECT_NAME}_dummy_sorting_controller
         ${catkin_LIBRARIES}
         )
-target_link_libraries(${PROJECT_NAME}_dummy_main_controller
+target_link_libraries(${PROJECT_NAME}_main_controller
         ${catkin_LIBRARIES}
         )
 target_link_libraries(${PROJECT_NAME}_moveit_sorting_controller

launch/virtual-table_dummy_main.launch → launch/virtual-table_main_controller.launch

 <launch>
 
     <arg name="connection_address" default="tcp://*:6576" />
-    <arg name="client_controllers" default="[]" />
+    <arg name="client_controllers" default="['ads', 'st', 'ceti']" />
 
 
-    <node pkg="ccf_immersive_sorting" type="dummy_main_controller" name="dummy_main_controller_instance" output="screen" >
+    <node pkg="ccf_immersive_sorting" type="main_controller" name="main_controller" output="screen" >
         <param name="connection_address" type="string" value="$(arg connection_address)" />
         <param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
         <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene_virtual-table.json" />

src/dummy_main_controller.cpp → src/main_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";
@@ -47,6 +48,9 @@ int main(int argc, char **argv) {
 
     DummyRobotArmController connector{n, NODE_NAME};
 
+    std::map<std::string, Scene> clients;
+    std::vector<std::pair<std::string, std::string>> actions;
+
     // add an NNG connection
     std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(connectionAddress);
     connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
@@ -54,14 +58,16 @@ int main(int argc, char **argv) {
     connector.addConnection(std::move(connection));
 
     auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
+    auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
 
     ROS_INFO_STREAM("Connecting to " << clientControllers.size() << " client controllers.");
+    std::shared_ptr<MqttConnection> client_connection = std::make_shared<MqttConnection>(NODE_NAME, mqttServer);
+    connector.addConnection(client_connection);
     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 scene_update_topic = "/" + client + getParameter(n, "topics/scene", NODE_NAME + "/scene/update");
+        client_connection->listen(scene_update_topic);
+        connector.addCallback(scene_update_topic, [client, &clients](auto msg) { clients[client].ParseFromString(msg);});
     }
 
     connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
@@ -77,6 +83,16 @@ int main(int argc, char **argv) {
     ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
             const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
 
+
+    ros::Timer timer_scene_log = n.createTimer(ros::Duration(10), [&clients](
+            const ros::TimerEvent &event) {
+
+        for (const auto &[name, scene] : clients) {
+            ROS_WARN_STREAM("CLIENT " << name << " has a scene with " << scene.objects_size() << "objects");
+        }
+
+    });
+
     auto selectionMessageCallback = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin, &n](
             const Selection &selection) {