diff --git a/src/dummy_pick_place.cpp b/src/dummy_pick_place.cpp index 98c7e232f830c352e9ae99fa54fc1974e7411036..b4ada058b880dcf13b14a76386bb4d6de4a33da5 100644 --- a/src/dummy_pick_place.cpp +++ b/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,19 +49,23 @@ 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 - << "). Connection established!"); + "[" << 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,70 +73,127 @@ int main(int argc, char **argv) { std::random_device rd; std::mt19937 rng(rd()); - while (ros::ok()) { - - char *buf = nullptr; - size_t sz; - if ((rv = nng_recv(sock, &buf, &sz, NNG_FLAG_ALLOC | NNG_FLAG_NONBLOCK)) == 0) { + ros::Timer timer = n.createTimer(ros::Duration(10), [&rng, &scene, &pause_rate](ros::TimerEvent timerEvent) { - // store the result in a protobuf object and free the buffer - Scene scene; - scene.ParseFromArray(buf, sz); - nng_free(buf, sz); + if (!scene.has_value()) { + ROS_INFO_STREAM("[" << NODE_NAME << "] not doing anything since scene is not initialized yet."); + return; + } - 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); - } else { - ROS_WARN_STREAM("[dummyCgv] Received an invalid scene!" << std::endl - << "[dummyCgv] Partial content:" << std::endl << scene.ShortDebugString()); + // 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); } + } - // 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()); + 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 { + 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++; + } } + } + } - if (objects.empty()) { - ROS_INFO("[dummyCgv] No objects to select in a scene without boxes!"); - } else if (locations.empty()) { - ROS_INFO("[dummyCgv] No location to place the object at available!"); - } else { - // select a random object - std::uniform_int_distribution<u_long> distribution{0, objects.size() - 1}; + ROS_INFO_STREAM("[" << NODE_NAME << "] There are " << correctlyPlacedObjects + << " correctly placed objects in the scene."); - // wait some time, then send a selected object - pause_rate.sleep(); - std::string object{objects[distribution(rng)]}; + if (objects.empty()) { + ROS_INFO_STREAM("[" << NODE_NAME << "] No objects to select that need to be placed!"); + } else if (locations.empty()) { + 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(); + auto object{objects[distribution(rng)]}; + + ROS_INFO_STREAM("[" << NODE_NAME << "] Selecting random object: " << object.id()); + sendSelection(object.id()); - ROS_INFO_STREAM("[dummyCgv] Selecting random object: " << object); - sendSelection(object); + // wait again, then send the bin object + pause_rate.sleep(); - // 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> locationDistribution{0, locations.size() - 1}; - // we expect an object named "bin" to be there - // 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();