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

clean up log messages

parent eac195a6
Branches
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