update logic for immersive sorting demo (selection by name prefix)

include/action.h

@@ -53,11 +53,17 @@ private:
     }
   }
 
+public:
+  /// Lookup for an object starting with a prefix
+  /// \param scene
+  /// \param id the prefix the object starts with
+  /// \return
   [[nodiscard]] static std::optional<Object> lookupObject(const Scene& scene, const std::string& id)
   {
     for (Object object : scene.objects())
     {
-      if (object.id() == id)
+      // if the id can be found at position 0
+      if (object.id().rfind(id, 0) == 0)
       {
         return object;
       }
@@ -65,7 +71,6 @@ private:
     return std::nullopt;
   }
 
-public:
   [[nodiscard]] Type getType() const
   {
     return type_;
@@ -121,7 +126,6 @@ public:
   }
 
   friend std::ostream& operator<<(std::ostream& os, const Action& action);
-
 };
 
 std::ostream& operator<<(std::ostream& os, const Action& action);

include/cell.h

@@ -15,8 +15,10 @@ struct Cell
     WORKING
   };
 
-  std::string stateString() const {
-    switch (state) {
+  std::string stateString() const
+  {
+    switch (state)
+    {
       case DISCONNECTED:
         return "DISCONNECTED";
       case IDLE:
@@ -31,10 +33,21 @@ struct Cell
   std::string id;
   Scene scene;
   State state = UNDEFINED;
-  std::optional<std::string> processed_object;
 
   Cell() = default;
 
+  std::string robot() const
+  {
+    for (const auto& object : scene.objects())
+    {
+      if (object.type() == Object_Type_ARM)
+      {
+        return object.id();
+      }
+    }
+    return "<no robot found>";
+  }
+
   // one does not copy cells
   Cell(const Cell&) = delete;
   Cell& operator=(const Cell&) = delete;

include/cell_controller.h

@@ -56,8 +56,8 @@ public:
   {
     if (!currentActionCompleted())
     {
-      ROS_WARN_STREAM(
-          "Cell " << cell_.id << " got a new task although an action of the previous task is still ongoing.");
+      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());
   }
@@ -73,25 +73,16 @@ public:
   void updateModel(const std::string& scene_string)
   {
     ROS_INFO_STREAM("Getting a scene update for cell " << cell_.id);
-    if (cell_.state == Cell::DISCONNECTED)
-    {
-      cell_.state = Cell::IDLE;
-    }
 
     cell_.scene.ParseFromString(scene_string);
-    proceedWithTask();
-  }
-
-  // TODO do we need this method?
-  void updateModel(const Scene &scene)
+    for (const auto& object : cell_.scene.objects())
     {
-    ROS_INFO_STREAM("Getting a scene update for cell " << cell_.id);
-    if (cell_.state == Cell::DISCONNECTED)
+      if (object.type() == Object_Type_ARM)
       {
-      cell_.state = Cell::IDLE;
+        cell_.state = object.state() == Object_State_STATE_IDLE ? Cell::IDLE : Cell::WORKING;
+        break;
+      }
     }
-
-    cell_.scene.CopyFrom(scene);
     proceedWithTask();
   }
 
@@ -120,20 +111,31 @@ public:
 
   std::optional<Action> triggerNextAction()
   {
+    if (current_action_ != nullptr && !current_action_->isCompleted(cell_.scene))
+    {
+      return std::nullopt;
+    }
     // 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())
     {
-      if (!action.isCompleted(cell_.scene) && action.canBePerformed(cell_.scene))
+      ROS_WARN_STREAM("Action completed: " << !action.isCompleted(cell_.scene));
+      if (cell_.state != Cell::WORKING && !action.isCompleted(cell_.scene) && action.canBePerformed(cell_.scene))
       {
         cell_.state = Cell::WORKING;
         ROS_INFO_STREAM("Triggering action " << action);
-        action_callback_(action);
+        auto source = Action::lookupObject(cell_.scene, action.getSource());
+        auto target = Action::lookupObject(cell_.scene, action.getTarget());
+        if (!source || !target)
+        {
+          return std::nullopt;
+        }
+        current_action_ = std::make_shared<Action>(action.getType(), source->id(), target->id());
+        action_callback_(*current_action_);
         return action;
       }
     }
     return std::nullopt;
   }
-
 };
 
 #endif  // CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_

src/main_controller.cpp

@@ -18,8 +18,8 @@ std::string NODE_NAME;
 using CetiRosToolbox::getParameter;
 using CetiRosToolbox::getPrivateParameter;
 
-const float SELECT = 2.0;
-const float UNSELECT = 0.5;
+const float UNSELECT = 0.8;
+const float SELECT = 1/UNSELECT;
 const float DELETING = 0.08;
 
 void highlight(Object *selected_bin, float factor)
@@ -43,7 +43,10 @@ int main(int argc, char **argv)
 
   auto connection_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
 
-  DummyRobotArmController controller{n, NODE_NAME, "arm"};
+  auto robotName = getPrivateParameter<std::string>("arm", "virtual-arm");
+  ROS_INFO_STREAM("This cell controls arm " << robotName);
+
+  DummyRobotArmController controller{n, NODE_NAME, robotName};
 
   std::map<std::string, CellController> clients;
   std::vector<std::pair<std::string, std::string>> actions;
@@ -54,7 +57,7 @@ int main(int argc, char **argv)
   connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
   controller.addConnection(std::move(connection));
 
-  auto client_controllers = getPrivateParameter<std::vector<std::string>>("client_controllers", {"ceti_cell_1", "ceti_cell_2", "ads-cell", "st-cell"});
+  auto client_controllers = getPrivateParameter<std::vector<std::string>>("client_controllers", {"ceti_cell_1", "ceti_cell_2", "ads-cell", "st-cell", "ceti_cell_empty"});
   auto mqtt_server = getPrivateParameter<std::string>("mqtt_server", "tcp://127.0.0.1:1883");
 
   // create an empty task
@@ -68,10 +71,11 @@ int main(int argc, char **argv)
     ROS_INFO_STREAM("Connecting to client at " << client << ".");
     clients[client].setCellId(client);
     clients[client].setCurrentTask(task);
-    clients[client].setActionCallback([client, &controller](const Action &action)
+    clients[client].setActionCallback([client, &controller, &clients](const Action &action)
                                       {
                                         ROS_INFO_STREAM("Action Callback Called for " << action);
                                         Command command;
+                                        command.mutable_pickandplace()->set_idrobot(clients[client].cell().robot());
                                         command.mutable_pickandplace()->set_idpick(action.getSource());
                                         command.mutable_pickandplace()->set_idplace(action.getTarget());
                                         controller.sendToAll("/" + client + "/command", command.SerializeAsString());
@@ -87,7 +91,7 @@ int main(int argc, char **argv)
       // uncomment in case things fail. clients[client].proceedWithTask();
     });
   }
-  auto fallback_path = ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene_virtual-table.json";
+  auto fallback_path = ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene_virtual-tag-table.json";
   controller.loadScene(getPrivateParameter<std::string>("scene", fallback_path));
 
   Object *robot = controller.resolveObject(controller.getRobotName());