update demo

include/cell_controller.h

@@ -14,6 +14,7 @@ class CellController
   Cell cell_;
   std::shared_ptr<Action> current_action_;
   std::shared_ptr<Task> current_task_;
+  std::function<void(const Action&)> action_callback_;
 
 public:
   CellController()
@@ -26,6 +27,11 @@ public:
     cell_.id = id;
   }
 
+  void setActionCallback(const std::function<void(const Action &)> &action_callback)
+  {
+    action_callback_ = action_callback;
+  }
+
   const Cell& cell() const
   {
     return cell_;
@@ -46,6 +52,14 @@ public:
     return current_task_;
   }
 
+  void setCurrentTask(const std::shared_ptr<Task> &new_task)
+  {
+    if (!currentActionCompleted()) {
+      ROS_WARN_STREAM("Cell " << cell_.id << " got a new task although an action of the previous task is still ongoing.");
+    }
+    current_task_.reset(new_task.get());
+  }
+
   void proceedWithTask()
   {
     if (cell_.state == Cell::IDLE)
@@ -108,19 +122,19 @@ public:
   std::optional<Action> triggerNextAction()
   {
     // TODO this assumes each action can be performed only once, which holds for pick-and-drop tasks but not in general
-    for (Action action : current_task_->GetActions())
+    for (Action action : current_task_->getActions())
     {
       if (!action.isCompleted(cell_.scene) && action.canBePerformed(cell_.scene))
       {
         cell_.state = Cell::WORKING;
-        // TODO send selections to controller
+        ROS_INFO_STREAM("Triggering action " << action);
+        action_callback_(action);
         return action;
       }
     }
     return std::nullopt;
   }
 
-  friend class CellController;
 };
 
 #endif  // CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_

include/task.h

@@ -9,12 +9,12 @@ class Task
   std::vector<Action> actions_;
 
 public:
-  void AddAction(const Action& action)
+  void addAction(const Action& action)
   {
     actions_.emplace_back(action);
   }
 
-  [[nodiscard]] const std::vector<Action>& GetActions() const
+  [[nodiscard]] const std::vector<Action>& getActions() const
   {
     return actions_;
   }

src/dummy_sorting_controller.cpp

@@ -23,103 +23,93 @@ std::string NODE_NAME = "dummy_sorting_controller";
 using CetiRosToolbox::getParameter;
 using CetiRosToolbox::getPrivateParameter;
 
-int main(int argc, char **argv) {
-
-    GOOGLE_PROTOBUF_VERIFY_VERSION;
-
-    ros::init(argc, argv, NODE_NAME);
-    NODE_NAME = ros::this_node::getName();
-
-    ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
-
-    auto connectionAddress = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
-
-    DummyRobotArmController connector{n, NODE_NAME};
-
-    // add an NNG connection
-    std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(connectionAddress);
-    connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
-    connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
-    connector.addConnection(std::move(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_st-table.json"));
-
-    Object *robot = nullptr;
-    Object *selectedBox = nullptr;
-    Object *selectedBin = nullptr;
-    std::optional<std::string> currentlyPickedBox{};
-
-    ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&connector](
-            const std_msgs::EmptyConstPtr &msg) { connector.sendScene(); });
-    ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
-            const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
-
-    auto selectionMessageCallback = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin, &n](
-            const Selection &selection) {
-
-        // forward the selection to the clients
-        connector.sendToAll(getParameter<std::string>(n, "topics/selection", "selection"),
-                            selection.SerializeAsString());
-
-        if (currentlyPickedBox.has_value()) {
-            ROS_WARN_STREAM("Unable to accept selections while picking is in progress.");
-            return;
-        }
-
-        Object *object = connector.resolveObject(selection.id());
-        if (!object) {
-            ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
-            return;
-        }
-        auto type = Object::Type_Name(object->type());
-        if (object->type() == Object::BOX) {
-            selectedBox = object;
-            ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
-        } else if (object->type() == Object::BIN) {
-            selectedBin = object;
-            ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
-        } else {
-            ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
-        }
-
-        if (selectedBin && selectedBox) {
-            auto boxId = selectedBox->id();
-            ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selectedBin->id());
-            currentlyPickedBox = boxId;
-            if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
-                ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
-                selectedBox = nullptr;
-                selectedBin = nullptr;
-            } else {
-                ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
-                selectedBox = nullptr;
-                selectedBin = nullptr;
-                connector.sendScene();
-            }
-        }
-    };
-    connector.reactToSelectionMessage(selectionMessageCallback);
-
-    auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
-        if (currentlyPickedBox.has_value()) {
-            auto resolved = connector.resolveObject(currentlyPickedBox.value());
-            if (!resolved) {
-                ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
-                currentlyPickedBox.reset();
-            }
-        }
-    };
-    connector.reactToSceneUpdateMessage(sceneUpdateCallback);
-
-    ros::spin();
-
-    return 0;
+int main(int argc, char **argv)
+{
+
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+  ros::init(argc, argv, NODE_NAME);
+  NODE_NAME = ros::this_node::getName();
+
+  ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
+
+  auto connectionAddress = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
+
+  DummyRobotArmController controller{n, NODE_NAME};
+
+  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"));
+  mqtt_connection->listen(getParameter(n, "topics/command", "/" + ros::this_node::getName() + "/command"));
+  controller.addConnection(std::move(mqtt_connection));
+
+  ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
+
+  controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
+      "/config/config_scene_st-table.json"));
+
+  std::optional<std::string> currentlyPickedBox{};
+
+  ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller](
+      const std_msgs::EmptyConstPtr &msg)
+  { controller.sendScene(); });
+  ros::Timer timer = n.createTimer(ros::Duration(10), [&controller](
+      const ros::TimerEvent &event)
+  { controller.sendScene(); }); // send a scene every ten seconds
+
+
+  auto pick_place_callback = [&controller](const PickPlace& pick_place)
+  {
+    Object *pick_object = controller.resolveObject(pick_place.idpick());
+    if (!pick_object)
+    {
+      ROS_ERROR_STREAM(
+          "Selected unknown pick object '" << pick_place.idpick() << "'");
+      return;
+    }
+    Object *place_object = controller.resolveObject(pick_place.idplace());
+    if (!place_object)
+    {
+      ROS_ERROR_STREAM(
+          "Selected unknown place object '" << pick_place.idplace() << "'");
+      return;
+    }
+    if (pick_object->type() != Object::BOX)
+    {
+      ROS_WARN_STREAM(
+          "Selected unsupported pick object of type " << pick_object->type());
+    }
+    if (place_object->type() != Object::BIN)
+    {
+      ROS_WARN_STREAM(
+          "Selected unsupported place object of type " << place_object->type());
+    }
+    Object *robot = nullptr; // TODO
+    if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false)) {
+      ROS_WARN_STREAM("Unable to remove box '" << pick_object->id() << "'!");
+    } else {
+      ROS_INFO_STREAM("Job is done! '" << pick_object->id() << "' is no more.");
+      controller.sendScene();
+    }
+  };
+  controller.reactToPickAndPlaceMessage(pick_place_callback);
+
+  auto sceneUpdateCallback = [&currentlyPickedBox, &controller]()
+  {
+    if (currentlyPickedBox.has_value())
+    {
+      auto resolved = controller.resolveObject(currentlyPickedBox.value());
+      if (!resolved)
+      {
+        ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
+        currentlyPickedBox.reset();
+      }
+    }
+  };
+  controller.reactToSceneUpdateMessage(sceneUpdateCallback);
+
+  ros::spin();
+
+  return 0;
 }
\ No newline at end of file

src/main_controller.cpp

@@ -12,7 +12,7 @@
 #include "ccf/connection/MqttConnection.h"
 #include "ccf/util/NodeUtil.h"
 
-const char* DEFAULT_NODE_NAME = "main_controller";
+const char *DEFAULT_NODE_NAME = "main_controller";
 std::string NODE_NAME;
 
 using CetiRosToolbox::getParameter;
@@ -22,17 +22,17 @@ const float SELECT = 2.0;
 const float UNSELECT = 0.5;
 const float DELETING = 0.08;
 
-void highlight(Object* selectedBin, float factor)
+void highlight(Object *selected_bin, float factor)
 {
-  if (selectedBin)
+  if (selected_bin)
   {
-    selectedBin->mutable_color()->set_r(selectedBin->color().r() * factor);
-    selectedBin->mutable_color()->set_g(selectedBin->color().g() * factor);
-    selectedBin->mutable_color()->set_b(selectedBin->color().b() * factor);
+    selected_bin->mutable_color()->set_r(selected_bin->color().r() * factor);
+    selected_bin->mutable_color()->set_g(selected_bin->color().g() * factor);
+    selected_bin->mutable_color()->set_b(selected_bin->color().b() * factor);
   }
 }
 
-int main(int argc, char** argv)
+int main(int argc, char **argv)
 {
   GOOGLE_PROTOBUF_VERIFY_VERSION;
 
@@ -41,141 +41,170 @@ int main(int argc, char** argv)
 
   ros::NodeHandle n("connector_node_ros_ccf");  // namespace where the connection_address is
 
-  auto connectionAddress = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
+  auto connection_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
 
-  DummyRobotArmController connector{ n, NODE_NAME };
+  DummyRobotArmController controller{n, NODE_NAME};
 
   std::map<std::string, CellController> clients;
   std::vector<std::pair<std::string, std::string>> actions;
 
   // add an NNG connection
-  std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(connectionAddress);
+  std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(connection_address);
   connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
   connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
-  connector.addConnection(std::move(connection));
+  controller.addConnection(std::move(connection));
 
-  auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
-  auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
+  auto client_controllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
+  auto mqtt_server = getPrivateParameter<std::string>("mqtt_server", "tcp://127.0.0.1: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)
+  // create an empty task
+  auto task = std::make_shared<Task>();
+
+  ROS_INFO_STREAM("Connecting to " << client_controllers.size() << " client controllers.");
+  std::shared_ptr<MqttConnection> client_connection = std::make_shared<MqttConnection>(mqtt_server, NODE_NAME);
+  controller.addConnection(client_connection);
+  for (const auto &client: client_controllers)
   {
     ROS_INFO_STREAM("Connecting to client at " << client << ".");
     clients[client].setCellId(client);
-    auto scene_update_topic = "/" + client + getParameter(n, "topics/scene", NODE_NAME + "/scene/update");
+    clients[client].setCurrentTask(task);
+    clients[client].setActionCallback([client, &controller](const Action &action)
+                                      {
+                                        ROS_INFO_STREAM("Action Callback Called for " << action);
+                                        PickPlace command;
+                                        command.set_idpick(action.getSource());
+                                        command.set_idplace(action.getTarget());
+                                        controller.sendToAll(client + "/command", command.SerializeAsString());
+                                      });
+    auto scene_update_topic = "/" + client + getParameter<std::string>(n, "topics/scene", "/scene/update");
+    ROS_INFO_STREAM("Listening to MQTT topic " << scene_update_topic);
     client_connection->listen(scene_update_topic);
-    connector.addCallback(scene_update_topic, [client, &clients](auto msg) { clients[client].updateModel(msg); });
+    controller.addCallback(scene_update_topic, [client, &clients](auto msg)
+    { clients[client].updateModel(msg); });
   }
   auto fallback_path = ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene_virtual-table.json";
-  connector.loadScene(getPrivateParameter<std::string>("scene", fallback_path));
+  controller.loadScene(getPrivateParameter<std::string>("scene", fallback_path));
 
-  Object* robot = nullptr;
-  Object* selected_box = nullptr;
-  Object* selectedBin = nullptr;
-  std::optional<std::string> currentlyPickedBox{};
+  Object *robot = nullptr;
+  Object *selected_box = nullptr;
+  Object *selected_bin = nullptr;
+  std::optional<std::string> picked_box{};
 
   ros::Subscriber sub = n.subscribe<std_msgs::Empty>(
-      "send_scene", 1000, [&connector](const std_msgs::EmptyConstPtr& msg) { connector.sendScene(); });
-  ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](const ros::TimerEvent& event) {
-    connector.sendScene();
+      "send_scene", 1000, [&controller](const std_msgs::EmptyConstPtr &msg)
+      { controller.sendScene(); });
+  ros::Timer timer = n.createTimer(ros::Duration(10), [&controller](const ros::TimerEvent &event)
+  {
+    controller.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, cell_controller] : clients)
+  ros::Timer timer_scene_log = n.createTimer(ros::Duration(10), [&clients](const ros::TimerEvent &event)
+  {
+    for (const auto&[name, cell_controller]: clients)
     {
-      ROS_WARN_STREAM("CLIENT " << name << " has a cell_controller with " << cell_controller.cell().scene.objects_size()
-                                << "objects");
+      ROS_WARN_STREAM("CLIENT " << name << " has a cell controller with " << cell_controller.cell().scene.objects_size()
+                                << " objects which is " << cell_controller.cell().stateString());
     }
   });
 
-  auto selectionMessageCallback = [&currentlyPickedBox, &connector, &robot, &selected_box,
-                                   &selectedBin](const Selection& selection) {
-    if (currentlyPickedBox.has_value())
-    {
-      ROS_WARN_STREAM("Unable to accept selections while picking is in progress.");
-      return;
-    }
-
-    Object* object = connector.resolveObject(selection.id());
-    if (!object)
-    {
-      ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
-      return;
-    }
-    auto type = Object::Type_Name(object->type());
-    if (object->type() == Object::BOX)
-    {
-      highlight(selected_box, UNSELECT);
-      if (selected_box == object)
-      {
-        selected_box = nullptr;
-      }
-      else
-      {
-        selected_box = object;
-        highlight(selected_box, SELECT);
-      }
-      connector.sendScene();
-      ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
-    }
-    else if (object->type() == Object::BIN)
-    {
-      highlight(selectedBin, UNSELECT);
-      if (selectedBin == object)
+  auto selection_message_callback =
+      [&picked_box, &controller, &robot, &selected_box, &selected_bin, &task, &clients](const Selection &selection)
       {
-        selectedBin = nullptr;
-      }
-      else
-      {
-        selectedBin = object;
-        highlight(selectedBin, SELECT);
-      }
-      connector.sendScene();
-      ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
-    }
-    else
-    {
-      ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
-    }
-
-    if (selectedBin && selected_box)
-    {
-      auto boxId = selected_box->id();
-      ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selectedBin->id());
-      currentlyPickedBox = boxId;
-      highlight(selectedBin, UNSELECT);
-      highlight(selected_box, DELETING);
-      connector.sendScene();
-      if (!connector.pickAndDrop(*robot, *selected_box, *selectedBin, false))
-      {
-        ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
-      }
-      else
-      {
-        ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
-      }
-      selected_box = nullptr;
-      selectedBin = nullptr;
-      connector.sendScene();
-    }
-  };
-  connector.reactToSelectionMessage(selectionMessageCallback);
-
-  auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
-    if (currentlyPickedBox.has_value())
+        if (picked_box.has_value())
+        {
+          ROS_WARN_STREAM("Unable to accept selections while picking is in progress.");
+          return;
+        }
+
+        Object *object = controller.resolveObject(selection.id());
+        if (!object)
+        {
+          ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
+          return;
+        }
+        auto type = Object::Type_Name(object->type());
+        if (object->type() == Object::BOX)
+        {
+          highlight(selected_box, UNSELECT);
+          if (selected_box == object)
+          {
+            selected_box = nullptr;
+          }
+          else
+          {
+            selected_box = object;
+            highlight(selected_box, SELECT);
+          }
+          controller.sendScene();
+          ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
+        }
+        else if (object->type() == Object::BIN)
+        {
+          highlight(selected_bin, UNSELECT);
+          if (selected_bin == object)
+          {
+            selected_bin = nullptr;
+          }
+          else
+          {
+            selected_bin = object;
+            highlight(selected_bin, SELECT);
+          }
+          controller.sendScene();
+          ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
+        }
+        else
+        {
+          ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
+        }
+
+        if (selected_bin && selected_box)
+        {
+          auto boxId = selected_box->id();
+          ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selected_bin->id());
+          picked_box = boxId;
+          highlight(selected_bin, UNSELECT);
+          highlight(selected_box, DELETING);
+          controller.sendScene();
+
+          // create an action and add it to the current task
+          Action pick_drop_action(Action::PICK_DROP, selected_box->id(), selected_bin->id());
+          task->addAction(pick_drop_action);
+
+          for (auto &[name, client]: clients)
+          {
+            client.proceedWithTask();
+          }
+
+          if (!controller.pickAndDrop(*robot, *selected_box, *selected_bin, false))
+          {
+            ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
+          }
+          else
+          {
+            ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
+          }
+          selected_box = nullptr;
+          selected_bin = nullptr;
+          controller.sendScene();
+        }
+      };
+  controller.reactToSelectionMessage(selection_message_callback);
+
+  auto scene_update_callback = [&picked_box, &controller]()
+  {
+    if (picked_box.has_value())
     {
-      auto resolved = connector.resolveObject(currentlyPickedBox.value());
+      auto resolved = controller.resolveObject(picked_box.value());
       if (!resolved)
       {
-        ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
-        currentlyPickedBox.reset();
+        ROS_INFO_STREAM("box " << picked_box.value() << " has been removed from the scene!");
+        picked_box.reset();
       }
     }
   };
-  connector.reactToSceneUpdateMessage(sceneUpdateCallback);
+  controller.reactToSceneUpdateMessage(scene_update_callback);
 
   ros::spin();
 

src/moveit_sorting_controller.cpp

@@ -23,103 +23,93 @@ std::string NODE_NAME = "moveit_sorting_controller";
 using CetiRosToolbox::getParameter;
 using CetiRosToolbox::getPrivateParameter;
 
-int main(int argc, char **argv) {
-
-    GOOGLE_PROTOBUF_VERIFY_VERSION;
-
-    ros::init(argc, argv, NODE_NAME);
-    NODE_NAME = ros::this_node::getName();
-
-    ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
-
-    auto connectionAddress = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
-
-    MoveItRobotArmController connector{n, NODE_NAME};
-
-    // add an NNG connection
-    std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(connectionAddress);
-    connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
-    connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
-    connector.addConnection(std::move(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_st-table.json"));
-
-    Object *robot = nullptr;
-    Object *selectedBox = nullptr;
-    Object *selectedBin = nullptr;
-    std::optional<std::string> currentlyPickedBox{};
-
-    ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&connector](
-            const std_msgs::EmptyConstPtr &msg) { connector.sendScene(); });
-    ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
-            const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
-
-    auto selectionMessageCallback = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin, &n](
-            const Selection &selection) {
-
-        // forward the selection to the clients
-        connector.sendToAll(getParameter<std::string>(n, "topics/selection", "selection"),
-                            selection.SerializeAsString());
-
-        if (currentlyPickedBox.has_value()) {
-            ROS_WARN_STREAM("Unable to accept selections while picking is in progress.");
-            return;
-        }
-
-        Object *object = connector.resolveObject(selection.id());
-        if (!object) {
-            ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
-            return;
-        }
-        auto type = Object::Type_Name(object->type());
-        if (object->type() == Object::BOX) {
-            selectedBox = object;
-            ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
-        } else if (object->type() == Object::BIN) {
-            selectedBin = object;
-            ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
-        } else {
-            ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
-        }
-
-        if (selectedBin && selectedBox) {
-            auto boxId = selectedBox->id();
-            ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selectedBin->id());
-            currentlyPickedBox = boxId;
-            if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
-                ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
-                selectedBox = nullptr;
-                selectedBin = nullptr;
-            } else {
-                ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
-                selectedBox = nullptr;
-                selectedBin = nullptr;
-                connector.sendScene();
-            }
-        }
-    };
-    connector.reactToSelectionMessage(selectionMessageCallback);
-
-    auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
-        if (currentlyPickedBox.has_value()) {
-            auto resolved = connector.resolveObject(currentlyPickedBox.value());
-            if (!resolved) {
-                ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
-                currentlyPickedBox.reset();
-            }
-        }
-    };
-    connector.reactToSceneUpdateMessage(sceneUpdateCallback);
-
-    ros::spin();
-
-    return 0;
+int main(int argc, char **argv)
+{
+
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+  ros::init(argc, argv, NODE_NAME);
+  NODE_NAME = ros::this_node::getName();
+
+  ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
+
+  auto connectionAddress = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
+
+  MoveItRobotArmController controller{n, NODE_NAME};
+
+  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"));
+  mqtt_connection->listen(getParameter(n, "topics/command", "/" + ros::this_node::getName() + "/command"));
+  controller.addConnection(std::move(mqtt_connection));
+
+  ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
+
+  controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
+      "/config/config_scene_st-table.json"));
+
+  std::optional<std::string> currentlyPickedBox{};
+
+  ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller](
+      const std_msgs::EmptyConstPtr &msg)
+  { controller.sendScene(); });
+  ros::Timer timer = n.createTimer(ros::Duration(10), [&controller](
+      const ros::TimerEvent &event)
+  { controller.sendScene(); }); // send a scene every ten seconds
+
+
+  auto pick_place_callback = [&controller](const PickPlace& pick_place)
+  {
+    Object *pick_object = controller.resolveObject(pick_place.idpick());
+    if (!pick_object)
+    {
+      ROS_ERROR_STREAM(
+          "Selected unknown pick object '" << pick_place.idpick() << "'");
+      return;
+    }
+    Object *place_object = controller.resolveObject(pick_place.idplace());
+    if (!place_object)
+    {
+      ROS_ERROR_STREAM(
+          "Selected unknown place object '" << pick_place.idplace() << "'");
+      return;
+    }
+    if (pick_object->type() != Object::BOX)
+    {
+      ROS_WARN_STREAM(
+          "Selected unsupported pick object of type " << pick_object->type());
+    }
+    if (place_object->type() != Object::BIN)
+    {
+      ROS_WARN_STREAM(
+          "Selected unsupported place object of type " << place_object->type());
+    }
+    Object *robot = nullptr; // TODO
+    if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false)) {
+      ROS_WARN_STREAM("Unable to remove box '" << pick_object->id() << "'!");
+    } else {
+      ROS_INFO_STREAM("Job is done! '" << pick_object->id() << "' is no more.");
+      controller.sendScene();
+    }
+  };
+  controller.reactToPickAndPlaceMessage(pick_place_callback);
+
+  auto sceneUpdateCallback = [&currentlyPickedBox, &controller]()
+  {
+    if (currentlyPickedBox.has_value())
+    {
+      auto resolved = controller.resolveObject(currentlyPickedBox.value());
+      if (!resolved)
+      {
+        ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
+        currentlyPickedBox.reset();
+      }
+    }
+  };
+  controller.reactToSceneUpdateMessage(sceneUpdateCallback);
+
+  ros::spin();
+
+  return 0;
 }
\ No newline at end of file