work on cell controller

src/main_controller.cpp

@@ -11,6 +11,8 @@
 #include <ros/package.h>
 #include <std_msgs/Empty.h>
 
+#include <utility>
+
 #include "connector.pb.h"
 
 #include "ccf/controller/DummyRobotArmController.h"
@@ -27,6 +29,199 @@ const float SELECT = 2.0;
 const float UNSELECT = 0.5;
 const float DELETING = 0.08;
 
+
+
+struct Cell {
+  enum CELL_STATE {
+    UNDEFINED,
+    DISCONNECTED,
+    IDLE,
+    WORKING
+  };
+
+  Scene scene;
+  CELL_STATE state = UNDEFINED;
+  std::optional<std::string> processedObject;
+};
+
+class Action {
+ public:
+  enum Type {
+    UNDEFINED,
+    PICK_DROP
+  };
+
+ private:
+  const Type type;
+  const std::string source;
+  const std::string target;
+
+ public:
+  bool operator==(const Action &rhs) const {
+      return type == rhs.type &&
+              source == rhs.source &&
+              target == rhs.target;
+  }
+  bool operator!=(const Action &rhs) const {
+      return !(rhs == *this);
+  }
+ private:
+
+  static constexpr Object::Type requiredSourceType(Type type) {
+      switch (type) {
+          case UNDEFINED:return Object::UNKNOWN;
+          case PICK_DROP:return Object::BOX;
+      }
+  }
+  static constexpr Object::Type requiredTargetType(Type type) {
+      switch (type) {
+          case UNDEFINED:return Object::UNKNOWN;
+          case PICK_DROP:return Object::BIN;
+      }
+  }
+
+  std::optional<Object> lookupObject(const Scene &scene, const std::string &id) const {
+      for (Object object: scene.objects()) {
+          if (object.id() == id) {
+              return object;
+          }
+      }
+      return std::nullopt;
+  }
+
+ public:
+  Type GetType() const {
+      return type;
+  }
+
+  const std::string &GetSource() const {
+      return source;
+  }
+
+  const std::string &GetTarget() const {
+      return target;
+  }
+
+ public:
+  /**
+   * Constructor
+   * @param type
+   * @param source
+   * @param target
+   */
+  Action(Type type, std::string source, std::string target)
+          : type(type), source(std::move(source)), target(std::move(target)) {}
+
+  /**
+   * an action can be performed in a scene if source and target exist in the scene and have the correct type for the action
+   * @param scene
+   * @return true if the action can be performed
+   */
+  bool canBePerformed(const Scene &scene) {
+      auto s = lookupObject(scene, source);
+      auto t = lookupObject(scene, target);
+      return s && t && s->type() == requiredSourceType(type) && t->type() == requiredTargetType(type);
+  }
+
+  bool isCompleted(const Scene &scene) const {
+      switch (type) {
+          case Action::Type::PICK_DROP:
+              // pick and drop is completed if the dropped element no longer exists
+              return !lookupObject(scene, source).has_value();
+          case Action::Type::UNDEFINED:
+              // undefined actions are defined to be completed
+              return true;
+      }
+  }
+
+};
+
+class Task {
+
+  std::vector<Action> actions;
+
+ public:
+
+  void addAction(const Action &action) {
+      actions.emplace_back(action);
+  }
+
+  const std::vector<Action> &GetActions() const {
+      return actions;
+  }
+
+};
+
+class CellController {
+  Cell cell;
+  std::shared_ptr<Action> currentAction;
+  std::shared_ptr<Task> currentTask;
+
+ public:
+  const Cell &GetCell() const {
+      return cell;
+  }
+  void SetCell(const Cell &cell) {
+      CellController::cell = cell;
+  }
+
+  bool HasCurrentAction() const {
+      return currentAction == nullptr;
+  }
+
+  const Action &GetCurrentAction() const {
+      return *currentAction;
+  }
+  void SetCurrentAction(Action current_action) {
+      currentAction.reset(&current_action);
+  }
+  const std::shared_ptr<Task> &GetCurrentTask() const {
+      return currentTask;
+  }
+  void SetCurrentTask(const std::shared_ptr<Task> &current_task) {
+      currentTask = current_task;
+  }
+
+  void proceedWithTask() {
+      if (cell.state == Cell::IDLE) {
+
+      }
+  }
+
+  void updateModel(Scene scene) {
+
+      if (cell.state == Cell::DISCONNECTED) {
+          cell.state == Cell::IDLE;
+      }
+
+      cell.scene = std::move(scene);
+      if (currentActionCompleted()) {
+          auto action = triggerNextAction();
+          // TODO send selections to controller
+      }
+  }
+
+  /**
+   * A current action is completed if there is no current action or the current action can no longer be performed
+   * @return
+   */
+  bool currentActionCompleted() {
+      return (currentAction == nullptr) || !currentAction->isCompleted(cell.scene);
+  }
+
+
+
+  std::optional<Action> triggerNextAction() {
+      for (Action action : currentTask->GetActions()) {
+          if (!action.isCompleted(cell.scene) && action.canBePerformed(cell.scene)) {
+              return action;
+          }
+      }
+      return std::nullopt;
+  }
+
+};
+
 void highlight(Object *selectedBin, float factor) {
     if (selectedBin) {
         selectedBin->mutable_color()->set_r(selectedBin->color().r() * factor);
@@ -48,7 +243,7 @@ int main(int argc, char **argv) {
 
     DummyRobotArmController connector{n, NODE_NAME};
 
-    std::map<std::string, Scene> clients;
+    std::map<std::string, Cell> clients;
     std::vector<std::pair<std::string, std::string>> actions;
 
     // add an NNG connection
@@ -67,11 +262,12 @@ int main(int argc, char **argv) {
         ROS_INFO_STREAM("Connecting to client at " << client << ".");
         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.addCallback(scene_update_topic,
+                              [client, &clients](auto msg) { clients[client].scene.ParseFromString(msg); });
     }
 
     connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
-                                                                  "/config/config_scene_st-table.json"));
+            "/config/config_scene_virtual-table.json"));
 
     Object *robot = nullptr;
     Object *selectedBox = nullptr;