diff --git a/include/ccf/controller/MoveItRobotArmController.h b/include/ccf/controller/MoveItRobotArmController.h
index 3c8a3b3f0141d761da5c0d424601d6020ec82d60..a748bc555339c27ac60e100e8ddac1055d1f50d2 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 73fcf7ce7c9ec9574fb35d465ad4837fec0e20c7..64530a67792af5519156daf09fccf517aa29437a 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 75335acd7637b62f674d4cd9dcce4a53b3eeab14..2f4008bb685cf32e0046d12d53cb6e430fb480ab 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 d89c9c25015776a1f332bdbbca07877a78ac57ae..b07d6d64d62a1ce0ab4f5e178116ef257d4a27d2 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 0ad7b0b36b0f4677b24dd7184179e37b25b64865..b471b3fb9794952e52377517635bc2a49ee451fe 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 5a439f55a10a2406f1c3b232de29edbb2129a7b1..8fe25f58654a3ffd6baed391b06399081e4b07db 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 c8884b928ddf5062d8e04f8faa9361e4ce9fbd59..cc1da1befcad4790a607adcaa39f332085278481 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);