Skip to content
Snippets Groups Projects
Commit 17da7f3b authored by Johannes Mey's avatar Johannes Mey
Browse files

clean up log messages

parent eac195a6
No related branches found
No related tags found
1 merge request!1Noetic/feature/simplification
Show whitespace changes
Inline Side-by-side
src/dummy_selection_provider.cpp
+ 17
− 24
View file @ 17da7f3b
...@@ -52,20 +52,16 @@ int main(int argc, char **argv) { ...@@ -52,20 +52,16 @@ int main(int argc, char **argv) {
ros::NodeHandle n("ccf"); ros::NodeHandle n("ccf");
if ((rv = nng_pair1_open(&sock)) != 0) { 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); ros::Rate connection_retry_rate(1);
while ((rv = nng_dial(sock, URL.c_str(), nullptr, 0)) != 0) { while ((rv = nng_dial(sock, URL.c_str(), nullptr, 0)) != 0) {
ROS_WARN_STREAM( ROS_WARN_STREAM("nng_dial returned: " << nng_strerror(rv) << ". Trying to connect again in one second...");
"[dummy_selection_provider] nng_dial returned: " << nng_strerror(rv)
<< ". Trying to connect again in one second...");
connection_retry_rate.sleep(); connection_retry_rate.sleep();
} }
ROS_INFO_STREAM( ROS_INFO_STREAM("nng_dial returned: " << nng_strerror(rv) << " (which is the translation of error code " << rv
"[dummy_selection_provider] nng_dial returned: " << nng_strerror(rv)
<< " (which is the translation of error code " << rv
<< "). Connection established!"); << "). Connection established!");
ros::Rate loop_rate(200); ros::Rate loop_rate(200);
ros::Rate pause_rate(ros::Duration(2)); // seconds ros::Rate pause_rate(ros::Duration(2)); // seconds
...@@ -87,11 +83,10 @@ int main(int argc, char **argv) { ...@@ -87,11 +83,10 @@ int main(int argc, char **argv) {
std::string s; std::string s;
if (google::protobuf::TextFormat::PrintToString(scene, &s)) { if (google::protobuf::TextFormat::PrintToString(scene, &s)) {
ROS_INFO_STREAM("[dummy_selection_provider] Received a valid scene with " << scene.objects().size() ROS_INFO_STREAM("Received a valid scene with " << scene.objects().size() << " objects.");
<< " objects."); ROS_DEBUG_STREAM("Content:" << std::endl << s);
ROS_DEBUG_STREAM("[dummy_selection_provider] Content:" << std::endl << s);
} else { } else {
ROS_WARN_STREAM("[dummy_selection_provider] Received an invalid scene!" << std::endl ROS_WARN_STREAM("Received an invalid scene!" << std::endl
<< "[dummy_selection_provider] Partial content:" << "[dummy_selection_provider] Partial content:"
<< std::endl << std::endl
<< scene.ShortDebugString()); << scene.ShortDebugString());
...@@ -101,9 +96,7 @@ int main(int argc, char **argv) { ...@@ -101,9 +96,7 @@ int main(int argc, char **argv) {
std::vector<std::string> objects{}; std::vector<std::string> objects{};
std::vector<std::string> bins{}; std::vector<std::string> bins{};
for (const auto &object: scene.objects()) { for (const auto &object: scene.objects()) {
ROS_INFO_STREAM( ROS_DEBUG_STREAM("found object " << object.id() << " of type " << Object_Type_Name(object.type()));
"[dummy_selection_provider] found object " << object.id() << " of type "
<< Object_Type_Name(object.type()));
if (object.type() == Object_Type_BOX) { if (object.type() == Object_Type_BOX) {
objects.push_back(object.id()); objects.push_back(object.id());
} else if (object.type() == Object_Type_BIN) { } else if (object.type() == Object_Type_BIN) {
...@@ -112,9 +105,9 @@ int main(int argc, char **argv) { ...@@ -112,9 +105,9 @@ int main(int argc, char **argv) {
} }
if (objects.empty()) { 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()) { } 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 { } else {
// select a random object // select a random object
std::uniform_int_distribution<u_long> distribution{0, objects.size() - 1}; std::uniform_int_distribution<u_long> distribution{0, objects.size() - 1};
...@@ -123,7 +116,7 @@ int main(int argc, char **argv) { ...@@ -123,7 +116,7 @@ int main(int argc, char **argv) {
pause_rate.sleep(); pause_rate.sleep();
std::string object{objects[distribution(rng)]}; 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); sendSelection(object);
// wait again, then send the bin object // wait again, then send the bin object
...@@ -134,14 +127,14 @@ int main(int argc, char **argv) { ...@@ -134,14 +127,14 @@ int main(int argc, char **argv) {
std::uniform_int_distribution<u_long> binDistribution{0, bins.size() - 1}; std::uniform_int_distribution<u_long> binDistribution{0, bins.size() - 1};
std::string bin{bins[binDistribution(rng)]}; 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); sendSelection(bin);
} }
} else if (rv == NNG_EAGAIN) { } else if (rv == NNG_EAGAIN) {
// no message received in current spin // no message received in current spin
} else { } else {
ROS_ERROR_STREAM("[dummy_selection_provider] nng_recv returned: " << nng_strerror(rv)); ROS_ERROR_STREAM("nng_recv returned: " << nng_strerror(rv));
} }
ros::spinOnce(); ros::spinOnce();
......
src/dummy_sorting_controller.cpp
+ 18
− 24
View file @ 17da7f3b
...@@ -7,8 +7,6 @@ ...@@ -7,8 +7,6 @@
\date 07.07.20 \date 07.07.20
*/ */
#define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost
#include <ros/ros.h> #include <ros/ros.h>
#include <ros/package.h> #include <ros/package.h>
#include <std_msgs/Empty.h> #include <std_msgs/Empty.h>
...@@ -45,16 +43,17 @@ int main(int argc, char **argv) { ...@@ -45,16 +43,17 @@ int main(int argc, char **argv) {
auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers"); auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to " << clientControllers.size() << " client controllers."); ROS_INFO_STREAM("Connecting to " << clientControllers.size() << " client controllers.");
for (const auto &client: clientControllers) { for (const auto &client: clientControllers) {
ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to client at " << client << "."); ROS_INFO_STREAM("Connecting to client at " << client << ".");
std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false); std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false);
client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection")); client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection"));
client_connection->setReceiveTopic("client_scene"); client_connection->setReceiveTopic("client_scene");
connector.addConnection(std::move(client_connection)); 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 *robot = nullptr;
Object *selectedBox = nullptr; Object *selectedBox = nullptr;
...@@ -74,38 +73,36 @@ int main(int argc, char **argv) { ...@@ -74,38 +73,36 @@ int main(int argc, char **argv) {
selection.SerializeAsString()); selection.SerializeAsString());
if (currentlyPickedBox.has_value()) { 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; return;
} }
Object *object; Object *object = connector.resolveObject(selection.id());
try { if (!object) {
object = connector.resolveObject(selection.id()); ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
} catch (std::out_of_range &e) {
ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
return; return;
} }
auto type = Object::Type_Name(object->type()); auto type = Object::Type_Name(object->type());
if (object->type() == Object::BOX) { if (object->type() == Object::BOX) {
selectedBox = object; selectedBox = object;
ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'"); ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
} else if (object->type() == Object::BIN) { } else if (object->type() == Object::BIN) {
selectedBin = object; selectedBin = object;
ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'"); ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
} else { } else {
ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'"); ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
} }
if (selectedBin && selectedBox) { if (selectedBin && selectedBox) {
auto boxId = selectedBox->id(); 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; currentlyPickedBox = boxId;
if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) { 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; selectedBox = nullptr;
selectedBin = nullptr; selectedBin = nullptr;
} else { } 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; selectedBox = nullptr;
selectedBin = nullptr; selectedBin = nullptr;
connector.sendScene(); connector.sendScene();
...@@ -116,12 +113,9 @@ int main(int argc, char **argv) { ...@@ -116,12 +113,9 @@ int main(int argc, char **argv) {
auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() { auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
if (currentlyPickedBox.has_value()) { if (currentlyPickedBox.has_value()) {
try { auto resolved = connector.resolveObject(currentlyPickedBox.value());
connector.resolveObject(currentlyPickedBox.value()); if (!resolved) {
} catch (std::out_of_range &e) { ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
ROS_INFO_STREAM(
"[" << NODE_NAME << "] box " << currentlyPickedBox.value()
<< " has been removed from the scene!");
currentlyPickedBox.reset(); currentlyPickedBox.reset();
} }
} }
......
src/moveit_sorting_controller.cpp
+ 18
− 24
View file @ 17da7f3b
...@@ -7,8 +7,6 @@ ...@@ -7,8 +7,6 @@
\date 07.07.20 \date 07.07.20
*/ */
#define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost
#include <ros/ros.h> #include <ros/ros.h>
#include <ros/package.h> #include <ros/package.h>
#include <std_msgs/Empty.h> #include <std_msgs/Empty.h>
...@@ -45,9 +43,9 @@ int main(int argc, char **argv) { ...@@ -45,9 +43,9 @@ int main(int argc, char **argv) {
auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers"); auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to " << clientControllers.size() << " client controllers."); ROS_INFO_STREAM("Connecting to " << clientControllers.size() << " client controllers.");
for (const auto &client: clientControllers) { for (const auto &client: clientControllers) {
ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to client at " << client << "."); ROS_INFO_STREAM("Connecting to client at " << client << ".");
std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false); std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false);
client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection")); client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection"));
client_connection->setReceiveTopic("client_scene"); client_connection->setReceiveTopic("client_scene");
...@@ -56,7 +54,8 @@ int main(int argc, char **argv) { ...@@ -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) 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 *robot = nullptr;
Object *selectedBox = nullptr; Object *selectedBox = nullptr;
...@@ -76,38 +75,36 @@ int main(int argc, char **argv) { ...@@ -76,38 +75,36 @@ int main(int argc, char **argv) {
selection.SerializeAsString()); selection.SerializeAsString());
if (currentlyPickedBox.has_value()) { 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; return;
} }
Object *object; Object *object = connector.resolveObject(selection.id());
try { if (!object) {
object = connector.resolveObject(selection.id()); ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
} catch (std::out_of_range &e) {
ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
return; return;
} }
auto type = Object::Type_Name(object->type()); auto type = Object::Type_Name(object->type());
if (object->type() == Object::BOX) { if (object->type() == Object::BOX) {
selectedBox = object; selectedBox = object;
ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'"); ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
} else if (object->type() == Object::BIN) { } else if (object->type() == Object::BIN) {
selectedBin = object; selectedBin = object;
ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'"); ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
} else { } else {
ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'"); ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
} }
if (selectedBin && selectedBox) { if (selectedBin && selectedBox) {
auto boxId = selectedBox->id(); 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; currentlyPickedBox = boxId;
if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) { 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; selectedBox = nullptr;
selectedBin = nullptr; selectedBin = nullptr;
} else { } 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; selectedBox = nullptr;
selectedBin = nullptr; selectedBin = nullptr;
connector.sendScene(); connector.sendScene();
...@@ -118,12 +115,9 @@ int main(int argc, char **argv) { ...@@ -118,12 +115,9 @@ int main(int argc, char **argv) {
auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() { auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
if (currentlyPickedBox.has_value()) { if (currentlyPickedBox.has_value()) {
try { auto resolved = connector.resolveObject(currentlyPickedBox.value());
connector.resolveObject(currentlyPickedBox.value()); if (!resolved) {
} catch (std::out_of_range &e) { ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
ROS_INFO_STREAM(
"[" << NODE_NAME << "] box " << currentlyPickedBox.value()
<< " has been removed from the scene!");
currentlyPickedBox.reset(); currentlyPickedBox.reset();
} }
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment