add settings and ros message interface to dummy selection provider

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 {