From 60a5832c94b067c365490205b59a42d605496eb7 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Mon, 3 Jan 2022 09:54:55 +0100
Subject: [PATCH] work on cell controller
---
src/main_controller.cpp | 342 +++++++++++++++++++++++++++++++---------
1 file changed, 269 insertions(+), 73 deletions(-)
diff --git a/src/main_controller.cpp b/src/main_controller.cpp
index 37a30db..892fafd 100644
--- a/src/main_controller.cpp
+++ b/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,11 +29,204 @@ 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(¤t_action);
+ }
+ const std::shared_ptr<Task> &GetCurrentTask() const {
+ return currentTask;
+ }
+ void SetCurrentTask(const std::shared_ptr<Task> ¤t_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);
- selectedBin->mutable_color()->set_g(selectedBin->color().g()*factor);
- selectedBin->mutable_color()->set_b(selectedBin->color().b()*factor);
+ 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);
}
}
@@ -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;
@@ -87,83 +283,83 @@ int main(int argc, char **argv) {
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");
- }
+ for (const auto &[name, scene]: clients) {
+ ROS_WARN_STREAM("CLIENT " << name << " has a scene with " << scene.objects_size() << "objects");
+ }
});
auto selectionMessageCallback = [¤tlyPickedBox, &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) {
- highlight(selectedBox, UNSELECT);
- if (selectedBox == object) {
- selectedBox = nullptr;
- } else {
- selectedBox = object;
- highlight(selectedBox, SELECT);
- }
- connector.sendScene();
- ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
- } else if (object->type() == Object::BIN) {
- highlight(selectedBin, UNSELECT);
- if (selectedBin == object) {
- 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 && selectedBox) {
- auto boxId = selectedBox->id();
- ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selectedBin->id());
- currentlyPickedBox = boxId;
- highlight(selectedBin, UNSELECT);
- highlight(selectedBox, DELETING);
- connector.sendScene();
- if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
- ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
- } else {
- ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
- }
- selectedBox = nullptr;
- selectedBin = nullptr;
- connector.sendScene();
- } else {
-
- }
+ // 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) {
+ highlight(selectedBox, UNSELECT);
+ if (selectedBox == object) {
+ selectedBox = nullptr;
+ } else {
+ selectedBox = object;
+ highlight(selectedBox, SELECT);
+ }
+ connector.sendScene();
+ ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
+ } else if (object->type() == Object::BIN) {
+ highlight(selectedBin, UNSELECT);
+ if (selectedBin == object) {
+ 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 && selectedBox) {
+ auto boxId = selectedBox->id();
+ ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selectedBin->id());
+ currentlyPickedBox = boxId;
+ highlight(selectedBin, UNSELECT);
+ highlight(selectedBox, DELETING);
+ connector.sendScene();
+ if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
+ ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
+ } else {
+ ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
+ }
+ selectedBox = nullptr;
+ selectedBin = nullptr;
+ connector.sendScene();
+ } else {
+
+ }
};
connector.reactToSelectionMessage(selectionMessageCallback);
auto sceneUpdateCallback = [¤tlyPickedBox, &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();
- }
- }
+ 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);
--
GitLab