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 = [¤tlyPickedBox, &connector, &robot, &selectedBox, &selectedBin](const Selection &selection) { + auto selectionMessageCallback = [¤tlyPickedBox, &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 = [¤tlyPickedBox, &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 = [¤tlyPickedBox, &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