improved logic in dummy selector

src/dummy_pick_place.cpp

@@ -20,7 +20,10 @@
 #include <google/protobuf/util/json_util.h>
 #include <google/protobuf/message.h>
 
+const bool RANDOM_LOCATION_SELECTION = false;
+
 const std::string URL = "tcp://127.0.0.1:6576";
+const std::string NODE_NAME = "pick_place_dummy";
 
 nng_socket sock;
 int rv;
@@ -37,12 +40,6 @@ void sendSelection(const std::string &object) {
     }
 }
 
-std::string ProtoToJson(const google::protobuf::Message &proto) {
-    std::string json;
-    google::protobuf::util::MessageToJsonString(proto, &json);
-    return json;
-}
-
 int main(int argc, char **argv) {
 
     GOOGLE_PROTOBUF_VERIFY_VERSION;
@@ -52,18 +49,22 @@ int main(int argc, char **argv) {
     ros::NodeHandle n("cgv_connector");
 
     if ((rv = nng_pair1_open(&sock)) != 0) {
-        ROS_ERROR_STREAM("[dummyCgv] nng_pair1_open returned: " << nng_strerror(rv));
+        ROS_ERROR_STREAM("[" << NODE_NAME << "] nng_pair1_open returned: " << nng_strerror(rv));
     }
 
     ros::Rate connection_retry_rate(1);
 
+    std::optional<Scene> scene = std::optional<Scene>();
+
     while ((rv = nng_dial(sock, URL.c_str(), nullptr, 0)) != 0) {
         ROS_WARN_STREAM(
-                "[dummyCgv] nng_dial returned: " << nng_strerror(rv) << ". Trying to connect again in one second...");
+                "[" << NODE_NAME << "] nng_dial returned: " << nng_strerror(rv)
+                    << ". Trying to connect again in one second...");
         connection_retry_rate.sleep();
     }
     ROS_INFO_STREAM(
-            "[dummyCgv] nng_dial returned: " << nng_strerror(rv) << " (which is the translation of error code " << rv
+            "[" << NODE_NAME << "] nng_dial returned: " << nng_strerror(rv)
+                << " (which is the translation of error code " << rv
                 << "). Connection established!");
     ros::Rate loop_rate(200);
     ros::Rate pause_rate(ros::Duration(2)); // seconds
@@ -72,53 +73,69 @@ int main(int argc, char **argv) {
     std::random_device rd;
     std::mt19937 rng(rd());
 
-    while (ros::ok()) {
+    ros::Timer timer = n.createTimer(ros::Duration(10), [&rng, &scene, &pause_rate](ros::TimerEvent timerEvent) {
 
-        char *buf = nullptr;
-        size_t sz;
-        if ((rv = nng_recv(sock, &buf, &sz, NNG_FLAG_ALLOC | NNG_FLAG_NONBLOCK)) == 0) {
+        if (!scene.has_value()) {
+            ROS_INFO_STREAM("[" << NODE_NAME << "] not doing anything since scene is not initialized yet.");
+            return;
+        }
 
-            // store the result in a protobuf object and free the buffer
-            Scene scene;
-            scene.ParseFromArray(buf, sz);
-            nng_free(buf, sz);
+        // collect all locations
+        std::vector<Object> locations{};
+        for (const auto &object : scene->objects()) {
+            if (object.type() == Object_Type_DROP_OFF_LOCATION) {
+                locations.push_back(object);
+            }
+        }
 
-            std::string s;
-            if (google::protobuf::TextFormat::PrintToString(scene, &s)) {
-                ROS_INFO_STREAM("[dummyCgv] Received a valid scene with " << scene.objects().size() << " objects.");
-                ROS_DEBUG_STREAM("[dummyCgv] Content:" << std::endl << s);
+        int correctlyPlacedObjects = 0;
+
+        // collect all (misplaced) objects
+        std::vector<Object> objects{};
+        for (const auto &object : scene->objects()) {
+            if (object.type() == Object_Type_BOX) {
+                if (RANDOM_LOCATION_SELECTION) {
+                    objects.push_back(object);
                 } else {
-                ROS_WARN_STREAM("[dummyCgv] Received an invalid scene!" << std::endl
-                << "[dummyCgv] Partial content:" << std::endl << scene.ShortDebugString());
+                    Object targetLocationForObject;
+                    bool objectIsAlreadyPlaced = false;
+                    for (const Object &other : scene->objects()) {
+                        if (other.type() == Object::DROP_OFF_LOCATION &&
+                            other.color().r() == object.color().r() &&
+                            other.color().g() == object.color().g() &&
+                            other.color().b() == object.color().b() &&
+                            other.pos().x() == object.pos().x() &&
+                            other.pos().y() == object.pos().y()) {
+                            objectIsAlreadyPlaced = true;
+                        }
+                    }
+                    if (!objectIsAlreadyPlaced) {
+                        objects.push_back(object);
+                    } else {
+                        correctlyPlacedObjects++;
+                    }
                 }
 
-            // collect all object names
-            std::vector<std::string> objects{};
-            std::vector<std::string> locations{};
-            for (const auto &object : scene.objects()) {
-                ROS_INFO_STREAM(
-                        "[dummyCgv] found object " << object.id() << " of type " << Object_Type_Name(object.type()));
-                if (object.type() == Object_Type_BOX) {
-                    objects.push_back(object.id());
-                } else if (object.type() == Object_Type_DROP_OFF_LOCATION) {
-                    locations.push_back(object.id());
             }
         }
 
+        ROS_INFO_STREAM("[" << NODE_NAME << "] There are " << correctlyPlacedObjects
+                            << " correctly placed objects in the scene.");
+
         if (objects.empty()) {
-                ROS_INFO("[dummyCgv] No objects to select in a scene without boxes!");
+            ROS_INFO_STREAM("[" << NODE_NAME << "] No objects to select that need to be placed!");
         } else if (locations.empty()) {
-                ROS_INFO("[dummyCgv] No location to place the object at available!");
+            ROS_INFO_STREAM("[" << NODE_NAME << "] No location to place the object at 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)]};
+            auto object{objects[distribution(rng)]};
 
-                ROS_INFO_STREAM("[dummyCgv] Selecting random object: " << object);
-                sendSelection(object);
+            ROS_INFO_STREAM("[" << NODE_NAME << "] Selecting random object: " << object.id());
+            sendSelection(object.id());
 
             // wait again, then send the bin object
             pause_rate.sleep();
@@ -127,15 +144,56 @@ int main(int argc, char **argv) {
             // select a random object
             std::uniform_int_distribution<u_long> locationDistribution{0, locations.size() - 1};
 
-                std::string location{locations[locationDistribution(rng)]};
-                ROS_INFO_STREAM("[dummyCgv] Selecting random location: " << location);
-                sendSelection(location);
+
+            auto location{locations[locationDistribution(rng)]};
+            if (RANDOM_LOCATION_SELECTION) {
+                ROS_INFO_STREAM("[" << NODE_NAME << "] Selecting random location: " << location.id());
+                sendSelection(location.id());
+            } else {
+                for (const auto &colorLocation : locations) {
+                    if (colorLocation.color().r() == object.color().r() &&
+                        colorLocation.color().g() == object.color().g() &&
+                        colorLocation.color().b() == object.color().b()) {
+                        ROS_INFO_STREAM(
+                                "[" << NODE_NAME << "] Selecting location with same color: " << colorLocation.id());
+                        sendSelection(colorLocation.id());
+                        return;
+                    }
                 }
+                ROS_ERROR_STREAM("[" << NODE_NAME
+                                     << "] Unable to find location with the correct color, selecting random location: "
+                                     << location.id());
+                sendSelection(location.id());
+            }
+        }
+    });
 
+    while (ros::ok()) {
+
+        char *buf = nullptr;
+        size_t sz;
+        if ((rv = nng_recv(sock, &buf, &sz, NNG_FLAG_ALLOC | NNG_FLAG_NONBLOCK)) == 0) {
+
+            // store the result in a protobuf object and free the buffer
+            Scene newScene;
+            newScene.ParseFromArray(buf, sz);
+            nng_free(buf, sz);
+
+            std::string s;
+            if (google::protobuf::TextFormat::PrintToString(newScene, &s)) {
+                ROS_INFO_STREAM(
+                        "[" << NODE_NAME << "] Received a valid scene with " << scene->objects().size() << " objects.");
+                ROS_DEBUG_STREAM("[" << NODE_NAME << "] Content:" << std::endl << s);
+                scene = newScene;
+            } else {
+                ROS_WARN_STREAM("[" << NODE_NAME << "] Received an invalid scene!" << std::endl
+                                    << "[" << NODE_NAME << "] Partial content:" << std::endl
+                                    << scene->ShortDebugString());
+            }
         } else if (rv == NNG_EAGAIN) {
             // no message received in current spin
         } else {
-            ROS_ERROR_STREAM("[dummyCgv] nng_recv returned: " << nng_strerror(rv));
+            ROS_ERROR_STREAM("[" << NODE_NAME << "] nng_recv returned: " << nng_strerror(rv));
         }
 
         ros::spinOnce();