From b8f4c2fa9bf3172de90bdd2808aac894bf71b942 Mon Sep 17 00:00:00 2001 From: Johannes Mey <johannes.mey@tu-dresden.de> Date: Fri, 17 Dec 2021 09:28:18 +0100 Subject: [PATCH] add settings and ros message interface to dummy selection provider --- src/dummy_selection_provider.cpp | 81 ++++++++++++++++++++++---------- 1 file changed, 55 insertions(+), 26 deletions(-) diff --git a/src/dummy_selection_provider.cpp b/src/dummy_selection_provider.cpp index 85efe4b..3725ed3 100644 --- a/src/dummy_selection_provider.cpp +++ b/src/dummy_selection_provider.cpp @@ -9,6 +9,8 @@ #include "ros/ros.h" #include "std_msgs/String.h" +#include "ccf/util/NodeUtil.h" + #include <nng/nng.h> #include <nng/protocol/pair1/pair.h> @@ -51,6 +53,9 @@ int main(int argc, char **argv) { ros::NodeHandle n("ccf"); + auto autoselect = CetiRosToolbox::getPrivateParameter<bool>("autoselect", "false"); + auto prefix = CetiRosToolbox::getPrivateParameter<std::string>("prefix", "sub_select"); + if ((rv = nng_pair1_open(&sock)) != 0) { ROS_ERROR_STREAM("nng_pair1_open returned: " << nng_strerror(rv)); } @@ -70,6 +75,11 @@ int main(int argc, char **argv) { std::random_device rd; std::mt19937 rng(rd()); + ros::Subscriber sub_select = + n.subscribe<std_msgs::String>(prefix + "/select", 1000, [&](auto &m) { sendSelection(m->data); }); + ros::Publisher pub_objects = n.advertise<std_msgs::String>(prefix + "/objects", 1); + ros::Publisher pub_bins = n.advertise<std_msgs::String>(prefix + "/bins", 1); + while (ros::ok()) { char *buf = nullptr; @@ -104,33 +114,52 @@ int main(int argc, char **argv) { } } - if (objects.empty()) { - ROS_INFO("No objects to select in a scene without boxes!"); - } else if (bins.empty()) { - 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}; - - // wait some time, then send a selected object - pause_rate.sleep(); - std::string object{objects[distribution(rng)]}; - - ROS_INFO_STREAM("Selecting random object: " << object); - sendSelection(object); - - // wait again, then send the bin object - pause_rate.sleep(); - - // we expect an object named "bin" to be there - // select a random object - std::uniform_int_distribution<u_long> binDistribution{0, bins.size() - 1}; - - std::string bin{bins[binDistribution(rng)]}; - ROS_INFO_STREAM("Selecting random bin: " << bin); - sendSelection(bin); + std_msgs::String objects_msg; + objects_msg.data = objects.empty() ? "" : + std::accumulate( + ++objects.begin(), objects.end(), + *objects.begin(), + [](auto &a, auto &b) { return a + "," + b; }); + + pub_objects.publish(objects_msg); + + std_msgs::String bins_msg; + bins_msg.data = bins.empty() ? "" : + std::accumulate( + ++bins.begin(), bins.end(), + *bins.begin(), + [](auto &a, auto &b) { return a + "," + b; }); + + pub_bins.publish(bins_msg); + + if (autoselect) { + if (objects.empty()) { + ROS_INFO("No objects to select in a scene without boxes!"); + } else if (bins.empty()) { + 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}; + + // wait some time, then send a selected object + pause_rate.sleep(); + std::string object{objects[distribution(rng)]}; + + ROS_INFO_STREAM("Selecting random object: " << object); + sendSelection(object); + + // wait again, then send the bin object + pause_rate.sleep(); + + // we expect an object named "bin" to be there + // select a random object + std::uniform_int_distribution<u_long> binDistribution{0, bins.size() - 1}; + + std::string bin{bins[binDistribution(rng)]}; + ROS_INFO_STREAM("Selecting random bin: " << bin); + sendSelection(bin); + } } - } else if (rv == NNG_EAGAIN) { // no message received in current spin } else { -- GitLab