make controllers more defensive

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;

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();
+}
+

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);
         }
     }

src/dummy_cgv_controller.cpp

@@ -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();
 

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;

src/dummy_rag_controller_b.cpp

@@ -60,11 +60,15 @@ 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
+        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;
@@ -88,6 +92,7 @@ int main(int argc, char **argv) {
                 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
         if (connector.pickAndPlace(*robot, *object, *location, false)) {

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,11 +101,15 @@ int main(int argc, char **argv) {
     connector.reactToSelectionMessage(selectionMessageCallback);
 
     auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
-        if (currentlyPickedBox.has_value() && !connector.resolveObject(currentlyPickedBox.value())) {
+        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);