move files, apply formatting, work on demo

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}_main_controller src/main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} )
+add_executable(${PROJECT_NAME}_main_controller src/main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/cell.cpp src/action.cpp src/task.cpp src/cell_controller.cpp include/cell_controller.h)
 add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 
 

include/action.h

+#ifndef CCF_IMMERSIVE_SORTING_ACTION_H_
+#define CCF_IMMERSIVE_SORTING_ACTION_H_
+
+#include <string>
+
+#include "connector.pb.h"
+
+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 PICK_DROP:
+        return Object::BOX;
+      case UNDEFINED:
+      default:
+        return Object::UNKNOWN;
+    }
+  }
+  static constexpr Object::Type requiredTargetType(Type type)
+  {
+    switch (type)
+    {
+      case PICK_DROP:
+        return Object::BIN;
+      case UNDEFINED:
+      default:
+        return Object::UNKNOWN;
+    }
+  }
+
+  [[nodiscard]] static std::optional<Object> lookupObject(const Scene& scene, const std::string& id)
+  {
+    for (Object object : scene.objects())
+    {
+      if (object.id() == id)
+      {
+        return object;
+      }
+    }
+    return std::nullopt;
+  }
+
+public:
+  [[nodiscard]] Type getType() const
+  {
+    return type_;
+  }
+
+  [[nodiscard]] const std::string& getSource() const
+  {
+    return source_;
+  }
+
+  [[nodiscard]] 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_);
+  }
+
+  [[nodiscard]] 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
+      default:
+        return true;
+    }
+  }
+};
+
+#endif  // CCF_IMMERSIVE_SORTING_ACTION_H_

include/cell.h

+#ifndef CCF_IMMERSIVE_SORTING_CELL_H_
+#define CCF_IMMERSIVE_SORTING_CELL_H_
+
+#include <connector.pb.h>
+#include <optional>
+#include <string>
+
+struct Cell
+{
+  enum State
+  {
+    UNDEFINED,
+    DISCONNECTED,
+    IDLE,
+    WORKING
+  };
+
+  std::string id;
+  Scene scene;
+  State state = UNDEFINED;
+  std::optional<std::string> processed_object;
+
+  Cell() = default;
+
+  // one does not copy cells
+  Cell(const Cell&) = delete;
+  Cell& operator=(const Cell&) = delete;
+};
+
+#endif  // CCF_IMMERSIVE_SORTING_CELL_H_

include/cell_controller.h

+#ifndef CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_
+#define CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_
+
+#include <utility>
+
+#include "cell.h"
+#include "action.h"
+#include "task.h"
+
+#include "ros/ros.h"
+
+class CellController
+{
+  Cell cell_;
+  std::shared_ptr<Action> current_action_;
+  std::shared_ptr<Task> current_task_;
+
+public:
+  CellController()
+  {
+    cell_.state = Cell::DISCONNECTED;
+  }
+
+  void setCellId(const std::string& id)
+  {
+    cell_.id = id;
+  }
+
+  const Cell& cell() const
+  {
+    return cell_;
+  }
+
+  bool hasCurrentAction() const
+  {
+    return current_action_ == nullptr;
+  }
+
+  const Action& getCurrentAction() const
+  {
+    return *current_action_;
+  }
+
+  const std::shared_ptr<Task>& getCurrentTask() const
+  {
+    return current_task_;
+  }
+
+  void proceedWithTask()
+  {
+    if (cell_.state == Cell::IDLE)
+    {
+      if (currentActionCompleted())
+      {
+        auto action = triggerNextAction();
+      }
+    }
+  }
+
+  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)
+  {
+    ROS_INFO_STREAM("Getting a scene update for cell " << cell_.id);
+    if (cell_.state == Cell::DISCONNECTED)
+    {
+      cell_.state = Cell::IDLE;
+    }
+
+    cell_.scene.CopyFrom(scene);
+    proceedWithTask();
+  }
+
+  /**
+   * A current action is completed if there is no current action or the current action can no longer be performed
+   * @return
+   */
+  bool currentActionCompleted()
+  {
+    if (current_action_ == nullptr)
+    {
+      return true;
+    }
+    else if (current_action_->isCompleted(cell_.scene))
+    {
+      // the current action is completed, but this was not registered before, so we have to set the cell state
+      cell_.state = Cell::IDLE;
+      current_action_ = nullptr;
+      return true;
+    }
+    else
+    {
+      return false;
+    }
+  }
+
+  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())
+    {
+      if (!action.isCompleted(cell_.scene) && action.canBePerformed(cell_.scene))
+      {
+        cell_.state = Cell::WORKING;
+        // TODO send selections to controller
+        return action;
+      }
+    }
+    return std::nullopt;
+  }
+
+  friend class CellController;
+};
+
+#endif  // CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_

include/task.h

+#ifndef CCF_IMMERSIVE_SORTING_TASK_H_
+#define CCF_IMMERSIVE_SORTING_TASK_H_
+
+#include <vector>
+#include "action.h"
+
+class Task
+{
+  std::vector<Action> actions_;
+
+public:
+  void AddAction(const Action& action)
+  {
+    actions_.emplace_back(action);
+  }
+
+  [[nodiscard]] const std::vector<Action>& GetActions() const
+  {
+    return actions_;
+  }
+};
+
+#endif  // CCF_IMMERSIVE_SORTING_TASK_H_

src/action.cpp

+#include "action.h"

src/cell.cpp

+#include "cell.h"
+

src/cell_controller.cpp

+#include "cell_controller.h"

src/main_controller.cpp

-/*! \file dummy_sorting_controller.cpp
-
-    A ROS node that simulates a node connected to a model-based cobotic application
-
-    \author Johannes Mey
-    \author Sebastian Ebert
-    \date 07.07.20
-*/
-
 #include <ros/ros.h>
 #include <ros/package.h>
 #include <std_msgs/Empty.h>
 
 #include <utility>
+#include <cell_controller.h>
 
 #include "connector.pb.h"
 
@@ -20,7 +12,8 @@
 #include "ccf/connection/MqttConnection.h"
 #include "ccf/util/NodeUtil.h"
 
-std::string NODE_NAME = "main_controller";
+const char* DEFAULT_NODE_NAME = "main_controller";
+std::string NODE_NAME;
 
 using CetiRosToolbox::getParameter;
 using CetiRosToolbox::getPrivateParameter;
@@ -29,212 +22,21 @@ 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) {
+void highlight(Object* selectedBin, float factor)
+{
+  if (selectedBin)
+  {
     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);
   }
 }
 
-int main(int argc, char **argv) {
-
+int main(int argc, char** argv)
+{
   GOOGLE_PROTOBUF_VERIFY_VERSION;
 
-    ros::init(argc, argv, NODE_NAME);
+  ros::init(argc, argv, DEFAULT_NODE_NAME);
   NODE_NAME = ros::this_node::getName();
 
   ros::NodeHandle n("connector_node_ros_ccf");  // namespace where the connection_address is
@@ -243,7 +45,7 @@ int main(int argc, char **argv) {
 
   DummyRobotArmController connector{ n, NODE_NAME };
 
-    std::map<std::string, Cell> clients;
+  std::map<std::string, CellController> clients;
   std::vector<std::pair<std::string, std::string>> actions;
 
   // add an NNG connection
@@ -258,104 +60,116 @@ int main(int argc, char **argv) {
   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) {
+  for (const auto& client : clientControllers)
+  {
     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");
     client_connection->listen(scene_update_topic);
-        connector.addCallback(scene_update_topic,
-                              [client, &clients](auto msg) { clients[client].scene.ParseFromString(msg); });
+    connector.addCallback(scene_update_topic, [client, &clients](auto msg) { clients[client].updateModel(msg); });
   }
-
-    connector.loadScene(getPrivateParameter<std::string>("scene", 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-table.json";
+  connector.loadScene(getPrivateParameter<std::string>("scene", fallback_path));
 
   Object* robot = nullptr;
-    Object *selectedBox = nullptr;
+  Object* selected_box = 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
-
-
-    ros::Timer timer_scene_log = n.createTimer(ros::Duration(10), [&clients](
-            const ros::TimerEvent &event) {
+  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
 
-      for (const auto &[name, scene]: clients) {
-          ROS_WARN_STREAM("CLIENT " << name << " has a scene with " << scene.objects_size() << "objects");
+  //
+  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");
     }
-
   });
 
-    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()) {
+  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) {
+    if (!object)
+    {
       ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
       return;
     }
     auto type = Object::Type_Name(object->type());
-      if (object->type() == Object::BOX) {
-          highlight(selectedBox, UNSELECT);
-          if (selectedBox == object) {
-              selectedBox = nullptr;
-          } else {
-              selectedBox = object;
-              highlight(selectedBox, SELECT);
+    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) {
+    }
+    else if (object->type() == Object::BIN)
+    {
       highlight(selectedBin, UNSELECT);
-          if (selectedBin == object) {
+      if (selectedBin == object)
+      {
         selectedBin = nullptr;
-          } else {
+      }
+      else
+      {
         selectedBin = object;
         highlight(selectedBin, SELECT);
       }
       connector.sendScene();
       ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
-      } else {
+    }
+    else
+    {
       ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
     }
 
-      if (selectedBin && selectedBox) {
-          auto boxId = selectedBox->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(selectedBox, DELETING);
+      highlight(selected_box, DELETING);
       connector.sendScene();
-          if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
+      if (!connector.pickAndDrop(*robot, *selected_box, *selectedBin, false))
+      {
         ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
-          } else {
+      }
+      else
+      {
         ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
       }
-          selectedBox = nullptr;
+      selected_box = nullptr;
       selectedBin = nullptr;
       connector.sendScene();
-      } else {
-
     }
   };
   connector.reactToSelectionMessage(selectionMessageCallback);
 
   auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
-      if (currentlyPickedBox.has_value()) {
+    if (currentlyPickedBox.has_value())
+    {
       auto resolved = connector.resolveObject(currentlyPickedBox.value());
-          if (!resolved) {
+      if (!resolved)
+      {
         ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
         currentlyPickedBox.reset();
       }

src/task.cpp

+#include "task.h"