diff --git a/src/dummy_selection_provider.cpp b/src/dummy_selection_provider.cpp
index 5be7a623f5c01a8bb7eacf9bbc26bec69cce943a..85efe4b260fc406579deb2456980f701e83d2307 100644
--- a/src/dummy_selection_provider.cpp
+++ b/src/dummy_selection_provider.cpp
@@ -52,21 +52,17 @@ int main(int argc, char **argv) {
ros::NodeHandle n("ccf");
if ((rv = nng_pair1_open(&sock)) != 0) {
- ROS_ERROR_STREAM("[dummy_selection_provider] nng_pair1_open returned: " << nng_strerror(rv));
+ ROS_ERROR_STREAM("nng_pair1_open returned: " << nng_strerror(rv));
}
ros::Rate connection_retry_rate(1);
while ((rv = nng_dial(sock, URL.c_str(), nullptr, 0)) != 0) {
- ROS_WARN_STREAM(
- "[dummy_selection_provider] nng_dial returned: " << nng_strerror(rv)
- << ". Trying to connect again in one second...");
+ ROS_WARN_STREAM("nng_dial returned: " << nng_strerror(rv) << ". Trying to connect again in one second...");
connection_retry_rate.sleep();
}
- ROS_INFO_STREAM(
- "[dummy_selection_provider] nng_dial returned: " << nng_strerror(rv)
- << " (which is the translation of error code " << rv
- << "). Connection established!");
+ ROS_INFO_STREAM("nng_dial returned: " << nng_strerror(rv) << " (which is the translation of error code " << rv
+ << "). Connection established!");
ros::Rate loop_rate(200);
ros::Rate pause_rate(ros::Duration(2)); // seconds
@@ -87,23 +83,20 @@ int main(int argc, char **argv) {
std::string s;
if (google::protobuf::TextFormat::PrintToString(scene, &s)) {
- ROS_INFO_STREAM("[dummy_selection_provider] Received a valid scene with " << scene.objects().size()
- << " objects.");
- ROS_DEBUG_STREAM("[dummy_selection_provider] Content:" << std::endl << s);
+ ROS_INFO_STREAM("Received a valid scene with " << scene.objects().size() << " objects.");
+ ROS_DEBUG_STREAM("Content:" << std::endl << s);
} else {
- ROS_WARN_STREAM("[dummy_selection_provider] Received an invalid scene!" << std::endl
- << "[dummy_selection_provider] Partial content:"
- << std::endl
- << scene.ShortDebugString());
+ ROS_WARN_STREAM("Received an invalid scene!" << std::endl
+ << "[dummy_selection_provider] Partial content:"
+ << std::endl
+ << scene.ShortDebugString());
}
// collect all object names
std::vector<std::string> objects{};
std::vector<std::string> bins{};
- for (const auto &object : scene.objects()) {
- ROS_INFO_STREAM(
- "[dummy_selection_provider] found object " << object.id() << " of type "
- << Object_Type_Name(object.type()));
+ for (const auto &object: scene.objects()) {
+ ROS_DEBUG_STREAM("found object " << object.id() << " of type " << Object_Type_Name(object.type()));
if (object.type() == Object_Type_BOX) {
objects.push_back(object.id());
} else if (object.type() == Object_Type_BIN) {
@@ -112,9 +105,9 @@ int main(int argc, char **argv) {
}
if (objects.empty()) {
- ROS_INFO("[dummy_selection_provider] No objects to select in a scene without boxes!");
+ ROS_INFO("No objects to select in a scene without boxes!");
} else if (bins.empty()) {
- ROS_INFO("[dummy_selection_provider] No bin to put the object in available!");
+ ROS_INFO("No bin to put the object in available!");
} else {
// select a random object
std::uniform_int_distribution<u_long> distribution{0, objects.size() - 1};
@@ -123,7 +116,7 @@ int main(int argc, char **argv) {
pause_rate.sleep();
std::string object{objects[distribution(rng)]};
- ROS_INFO_STREAM("[dummy_selection_provider] Selecting random object: " << object);
+ ROS_INFO_STREAM("Selecting random object: " << object);
sendSelection(object);
// wait again, then send the bin object
@@ -134,14 +127,14 @@ int main(int argc, char **argv) {
std::uniform_int_distribution<u_long> binDistribution{0, bins.size() - 1};
std::string bin{bins[binDistribution(rng)]};
- ROS_INFO_STREAM("[dummy_selection_provider] Selecting random bin: " << bin);
+ ROS_INFO_STREAM("Selecting random bin: " << bin);
sendSelection(bin);
}
} else if (rv == NNG_EAGAIN) {
// no message received in current spin
} else {
- ROS_ERROR_STREAM("[dummy_selection_provider] nng_recv returned: " << nng_strerror(rv));
+ ROS_ERROR_STREAM("nng_recv returned: " << nng_strerror(rv));
}
ros::spinOnce();
diff --git a/src/dummy_sorting_controller.cpp b/src/dummy_sorting_controller.cpp
index c03e7d6d043c09da5bcae1b4a9b5fe3913fa4ac9..d1580aebb81fadc74332dc825eb70909f21f6a86 100644
--- a/src/dummy_sorting_controller.cpp
+++ b/src/dummy_sorting_controller.cpp
@@ -7,8 +7,6 @@
\date 07.07.20
*/
-#define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost
-
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/Empty.h>
@@ -45,16 +43,17 @@ int main(int argc, char **argv) {
auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
- ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to " << clientControllers.size() << " client controllers.");
- for (const auto &client : clientControllers) {
- ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to client at " << client << ".");
+ ROS_INFO_STREAM("Connecting to " << clientControllers.size() << " client controllers.");
+ for (const auto &client: clientControllers) {
+ ROS_INFO_STREAM("Connecting to client at " << client << ".");
std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false);
client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection"));
client_connection->setReceiveTopic("client_scene");
connector.addConnection(std::move(client_connection));
}
- connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene.yaml"));
+ connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
+ "/config/config_scene.yaml"));
Object *robot = nullptr;
Object *selectedBox = nullptr;
@@ -74,38 +73,36 @@ int main(int argc, char **argv) {
selection.SerializeAsString());
if (currentlyPickedBox.has_value()) {
- ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress.");
+ ROS_WARN_STREAM("Unable to accept selections while picking is in progress.");
return;
}
- Object *object;
- try {
- object = connector.resolveObject(selection.id());
- } catch (std::out_of_range &e) {
- ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
+ 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) {
selectedBox = object;
- ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
+ ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
} else if (object->type() == Object::BIN) {
selectedBin = object;
- ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
+ ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
} else {
- ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'");
+ ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
}
if (selectedBin && selectedBox) {
auto boxId = selectedBox->id();
- ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id());
+ ROS_INFO_STREAM("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 << "'!");
+ ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
selectedBox = nullptr;
selectedBin = nullptr;
} else {
- ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more.");
+ ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
selectedBox = nullptr;
selectedBin = nullptr;
connector.sendScene();
@@ -116,12 +113,9 @@ int main(int argc, char **argv) {
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!");
+ auto resolved = connector.resolveObject(currentlyPickedBox.value());
+ if (!resolved) {
+ ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
currentlyPickedBox.reset();
}
}
diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp
index 8c30c3b5aa21c2f71ee5d1966ae89eddaeabe8dc..8e5d94886d5f4a13c21626de56d93bf7b9a6b403 100644
--- a/src/moveit_sorting_controller.cpp
+++ b/src/moveit_sorting_controller.cpp
@@ -7,8 +7,6 @@
\date 07.07.20
*/
-#define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost
-
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/Empty.h>
@@ -45,9 +43,9 @@ int main(int argc, char **argv) {
auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
- ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to " << clientControllers.size() << " client controllers.");
- for (const auto &client : clientControllers) {
- ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to client at " << client << ".");
+ ROS_INFO_STREAM("Connecting to " << clientControllers.size() << " client controllers.");
+ for (const auto &client: clientControllers) {
+ ROS_INFO_STREAM("Connecting to client at " << client << ".");
std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false);
client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection"));
client_connection->setReceiveTopic("client_scene");
@@ -56,7 +54,8 @@ int main(int argc, char **argv) {
ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
- connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene.yaml"));
+ connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
+ "/config/config_scene.yaml"));
Object *robot = nullptr;
Object *selectedBox = nullptr;
@@ -76,38 +75,36 @@ int main(int argc, char **argv) {
selection.SerializeAsString());
if (currentlyPickedBox.has_value()) {
- ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress.");
+ ROS_WARN_STREAM("Unable to accept selections while picking is in progress.");
return;
}
- Object *object;
- try {
- object = connector.resolveObject(selection.id());
- } catch (std::out_of_range &e) {
- ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
+ 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) {
selectedBox = object;
- ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
+ ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
} else if (object->type() == Object::BIN) {
selectedBin = object;
- ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
+ ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
} else {
- ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'");
+ ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
}
if (selectedBin && selectedBox) {
auto boxId = selectedBox->id();
- ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id());
+ ROS_INFO_STREAM("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 << "'!");
+ ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
selectedBox = nullptr;
selectedBin = nullptr;
} else {
- ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more.");
+ ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
selectedBox = nullptr;
selectedBin = nullptr;
connector.sendScene();
@@ -118,12 +115,9 @@ int main(int argc, char **argv) {
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!");
+ auto resolved = connector.resolveObject(currentlyPickedBox.value());
+ if (!resolved) {
+ ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
currentlyPickedBox.reset();
}
}