From 32e67627f1e63ae0799eec50dbf0d41601ef4e20 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Thu, 24 Jun 2021 15:33:47 +0200
Subject: [PATCH] make controllers more defensive

---
 .../ccf/controller/MoveItRobotArmController.h |  4 +-
 .../controller/MoveItRobotArmController.cpp   |  8 ++-
 src/ccf/controller/RobotArmController.cpp     |  2 -
 src/dummy_cgv_controller.cpp                  | 32 +++++++++--
 src/dummy_rag_controller_a.cpp                |  8 ++-
 src/dummy_rag_controller_b.cpp                | 55 ++++++++++---------
 src/moveit_cgv_controller.cpp                 | 21 +++++--
 7 files changed, 88 insertions(+), 42 deletions(-)

diff --git a/include/ccf/controller/MoveItRobotArmController.h b/include/ccf/controller/MoveItRobotArmController.h
index 3c8a3b3..a748bc5 100644
--- a/include/ccf/controller/MoveItRobotArmController.h
+++ b/include/ccf/controller/MoveItRobotArmController.h
@@ -21,7 +21,9 @@ private:
 public:
     explicit MoveItRobotArmController(ros::NodeHandle &nodeHandle, const std::string &cellName);
 
-    void updateScene(ccf::SceneUpdateRequest &req, ros::ServiceClient &bin_check_client);
+    virtual ~MoveItRobotArmController();
+
+    void updateScene(ccf::SceneUpdateRequest &req);
 
     bool pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) override;
     bool pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) override;
diff --git a/src/ccf/controller/MoveItRobotArmController.cpp b/src/ccf/controller/MoveItRobotArmController.cpp
index 73fcf7c..64530a6 100644
--- a/src/ccf/controller/MoveItRobotArmController.cpp
+++ b/src/ccf/controller/MoveItRobotArmController.cpp
@@ -17,7 +17,7 @@
 
 #include "ccf/controller/MoveItRobotArmController.h"
 
-void MoveItRobotArmController::updateScene(ccf::SceneUpdateRequest &req, ros::ServiceClient &bin_check_client) {
+void MoveItRobotArmController::updateScene(ccf::SceneUpdateRequest &req) {
 
     Scene newScene;
 
@@ -137,7 +137,7 @@ MoveItRobotArmController::MoveItRobotArmController(ros::NodeHandle &nodeHandle,
             "updateCgvScene",
             [this](auto &req, auto &res) {
                 ROS_INFO_STREAM("[MoveItRobotArmController] Received a scene update from the controller.");
-                updateScene(req, bin_check_client);
+                updateScene(req);
                 return true;
             }
     );
@@ -151,3 +151,7 @@ bool MoveItRobotArmController::reachableLocation(const Object &robot, const Obje
     return false;
 }
 
+MoveItRobotArmController::~MoveItRobotArmController() {
+    get_scene_service.shutdown();
+}
+
diff --git a/src/ccf/controller/RobotArmController.cpp b/src/ccf/controller/RobotArmController.cpp
index 75335ac..2f4008b 100644
--- a/src/ccf/controller/RobotArmController.cpp
+++ b/src/ccf/controller/RobotArmController.cpp
@@ -31,10 +31,8 @@ void RobotArmController::receive(const std::string &channel, const std::string &
 }
 
 Object* RobotArmController::resolveObject(const std::string &id) {
-    ROS_ERROR_STREAM("resolving object with id " << id);
     for (int i = 0; i<scene->objects_size(); i++) {
         if (scene->objects(i).id() == id) {
-            ROS_ERROR_STREAM("... found it: " << scene->objects(i).id());
             return scene->mutable_objects(i);
         }
     }
diff --git a/src/dummy_cgv_controller.cpp b/src/dummy_cgv_controller.cpp
index d89c9c2..b07d6d6 100644
--- a/src/dummy_cgv_controller.cpp
+++ b/src/dummy_cgv_controller.cpp
@@ -87,9 +87,9 @@ int main(int argc, char **argv) {
 
     loadScene(connector);
 
-    Object* robot = nullptr;
-    Object* selectedBox = nullptr;
-    Object* selectedBin = nullptr;
+    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](
@@ -97,14 +97,21 @@ int main(int argc, char **argv) {
     ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
             const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
 
-    auto lambda = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin](const Selection &selection) {
+    auto selectionMessageCallback = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin](
+            const Selection &selection) {
 
         if (currentlyPickedBox.has_value()) {
             ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress.");
             return;
         }
 
-        auto object = connector.resolveObject(selection.id());
+        Object *object;
+        try {
+            object = connector.resolveObject(selection.id());
+        } catch (std::out_of_range &e) {
+            ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
+            return;
+        }
         auto type = Object::Type_Name(object->type());
         if (object->type() == Object::BOX) {
             selectedBox = object;
@@ -119,6 +126,7 @@ int main(int argc, char **argv) {
         if (selectedBin && selectedBox) {
             auto boxId = selectedBox->id();
             ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id());
+            currentlyPickedBox = boxId;
             if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
                 ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!");
                 selectedBox = nullptr;
@@ -131,8 +139,20 @@ int main(int argc, char **argv) {
             }
         }
     };
+    connector.reactToSelectionMessage(selectionMessageCallback);
 
-    connector.reactToSelectionMessage(lambda);
+    auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
+        if (currentlyPickedBox.has_value()) {
+            try {
+                connector.resolveObject(currentlyPickedBox.value());
+            } catch (std::out_of_range &e) {
+                ROS_INFO_STREAM(
+                        "[" << NODE_NAME << "] box " << currentlyPickedBox.value() << " has been removed from the scene!");
+                currentlyPickedBox.reset();
+            }
+        }
+    };
+    connector.reactToSceneUpdateMessage(sceneUpdateCallback);
 
     ros::spin();
 
diff --git a/src/dummy_rag_controller_a.cpp b/src/dummy_rag_controller_a.cpp
index 0ad7b0b..b471b3f 100644
--- a/src/dummy_rag_controller_a.cpp
+++ b/src/dummy_rag_controller_a.cpp
@@ -73,7 +73,13 @@ int main(int argc, char **argv) {
             return;
         }
 
-        auto object = connector.resolveObject(selection.id());
+        Object *object;
+        try {
+            object = connector.resolveObject(selection.id());
+        } catch (std::out_of_range &e) {
+            ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
+            return;
+        }
         auto type = Object::Type_Name(object->type());
         if (object->type() == Object::BOX) {
             selectedBox = object;
diff --git a/src/dummy_rag_controller_b.cpp b/src/dummy_rag_controller_b.cpp
index 5a439f5..8fe25f5 100644
--- a/src/dummy_rag_controller_b.cpp
+++ b/src/dummy_rag_controller_b.cpp
@@ -60,33 +60,38 @@ int main(int argc, char **argv) {
                             << " and place it at " << command.idplace()
                             << " using robot " << command.idrobot() << ".");
 
-        auto robot = connector.resolveObject(command.idrobot());
-        auto object = connector.resolveObject(command.idpick());
-        auto location = connector.resolveObject(command.idplace());
-
-        // check the parts of the command
-        if (!robot) {
-            ROS_ERROR_STREAM("Unknown robot " << command.idrobot() << ". Aborting PickPlace command.");
-            return;
-        } else if (robot->type() != Object::ARM) {
-            ROS_ERROR_STREAM("Robot " << command.idrobot() << " has wrong type " << robot->type()
-                                      << ". Aborting PickPlace command.");
-        }
+        Object *robot = nullptr;
+        Object *object = nullptr;
+        Object *location = nullptr;
+
+        try {
+            robot = connector.resolveObject(command.idrobot());
+            object = connector.resolveObject(command.idpick());
+            location = connector.resolveObject(command.idplace());
+        } catch (std::out_of_range &e) {        // check the parts of the command
+            if (!robot) {
+                ROS_ERROR_STREAM("Unknown robot " << command.idrobot() << ". Aborting PickPlace command.");
+                return;
+            } else if (robot->type() != Object::ARM) {
+                ROS_ERROR_STREAM("Robot " << command.idrobot() << " has wrong type " << robot->type()
+                                          << ". Aborting PickPlace command.");
+            }
 
-        if (!object) {
-            ROS_ERROR_STREAM("Unknown object " << command.idpick() << ". Aborting PickPlace command.");
-            return;
-        } else if (object->type() != Object::BOX) {
-            ROS_ERROR_STREAM("Object " << command.idpick() << " has wrong type " << object->type()
-                                       << ". Aborting PickPlace command.");
-        }
+            if (!object) {
+                ROS_ERROR_STREAM("Unknown object " << command.idpick() << ". Aborting PickPlace command.");
+                return;
+            } else if (object->type() != Object::BOX) {
+                ROS_ERROR_STREAM("Object " << command.idpick() << " has wrong type " << object->type()
+                                           << ". Aborting PickPlace command.");
+            }
 
-        if (!location) {
-            ROS_ERROR_STREAM("Unknown place location " << command.idplace() << ". Aborting PickPlace command.");
-            return;
-        } else if (location->type() != Object::DROP_OFF_LOCATION) {
-            ROS_ERROR_STREAM("Location " << command.idplace() << " has wrong type " << location->type()
-                                         << ". Aborting PickPlace command.");
+            if (!location) {
+                ROS_ERROR_STREAM("Unknown place location " << command.idplace() << ". Aborting PickPlace command.");
+                return;
+            } else if (location->type() != Object::DROP_OFF_LOCATION) {
+                ROS_ERROR_STREAM("Location " << command.idplace() << " has wrong type " << location->type()
+                                             << ". Aborting PickPlace command.");
+            }
         }
 
         // by now, we know that the request is legit, so we call the pick and place action
diff --git a/src/moveit_cgv_controller.cpp b/src/moveit_cgv_controller.cpp
index c8884b9..cc1da1b 100644
--- a/src/moveit_cgv_controller.cpp
+++ b/src/moveit_cgv_controller.cpp
@@ -64,7 +64,13 @@ int main(int argc, char **argv) {
             return;
         }
 
-        auto object = connector.resolveObject(selection.id());
+        Object *object;
+        try {
+            object = connector.resolveObject(selection.id());
+        } catch (std::out_of_range &e) {
+            ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
+            return;
+        }
         auto type = Object::Type_Name(object->type());
         if (object->type() == Object::BOX) {
             selectedBox = object;
@@ -79,6 +85,7 @@ int main(int argc, char **argv) {
         if (selectedBin && selectedBox) {
             auto boxId = selectedBox->id();
             ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id());
+            currentlyPickedBox = boxId;
             if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
                 ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!");
                 selectedBox = nullptr;
@@ -94,10 +101,14 @@ int main(int argc, char **argv) {
     connector.reactToSelectionMessage(selectionMessageCallback);
 
     auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
-        if (currentlyPickedBox.has_value() && !connector.resolveObject(currentlyPickedBox.value())) {
-            ROS_INFO_STREAM(
-                    "[" << NODE_NAME << "] box " << currentlyPickedBox.value() << " has been removed from the scene!");
-            currentlyPickedBox.reset();
+        if (currentlyPickedBox.has_value()) {
+            try {
+                connector.resolveObject(currentlyPickedBox.value());
+            } catch (std::out_of_range &e) {
+                ROS_INFO_STREAM(
+                        "[" << NODE_NAME << "] box " << currentlyPickedBox.value() << " has been removed from the scene!");
+                currentlyPickedBox.reset();
+            }
         }
     };
     connector.reactToSceneUpdateMessage(sceneUpdateCallback);
-- 
GitLab