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,6 +114,25 @@ int main(int argc, char **argv) {
                 }
             }
 
+            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()) {
@@ -130,7 +159,7 @@ int main(int argc, char **argv) {
                     ROS_INFO_STREAM("Selecting random bin: " << bin);
                     sendSelection(bin);
                 }
-
+            }
         } else if (rv == NNG_EAGAIN) {
             // no message received in current spin
         } else {