dummy_cgv.cpp

/*! \file dummy_cgv.cpp

    A ROS node that simulates the CGV framework

    \author Johannes Mey
    \date 07.07.20
*/

#include "ros/ros.h"
#include "std_msgs/String.h"

#include <nng/nng.h>
#include <nng/protocol/pair1/pair.h>

#include <google/protobuf/text_format.h>
#include <random>

#include "connector.pb.h"

#include <google/protobuf/util/json_util.h>
#include <google/protobuf/message.h>

const std::string URL = "tcp://127.0.0.1:6576";

nng_socket sock;
int rv;

void sendSelection(const std::string &object) {
    Selection selection;
    selection.set_id(object);
    int length = selection.ByteSize();
    void *data = nng_alloc(length);
    selection.SerializeToArray(data, length);

    if ((rv = nng_send(sock, data, length, NNG_FLAG_ALLOC)) != 0) {
        ROS_ERROR_STREAM("nng_send returned: " << nng_strerror(rv));
    }
}

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;

    ros::init(argc, argv, "dummy_cgv");

    ros::NodeHandle n("ccf");

    if ((rv = nng_pair1_open(&sock)) != 0) {
        ROS_ERROR_STREAM("[dummyCgv] nng_pair1_open returned: " << nng_strerror(rv));
    }

    ros::Rate connection_retry_rate(1);

    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...");
        connection_retry_rate.sleep();
    }
    ROS_INFO_STREAM(
            "[dummyCgv] 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

    // initialize random number generator
    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) {

            // store the result in a protobuf object and free the buffer
            Scene scene;
            scene.ParseFromArray(buf, sz);
            nng_free(buf, sz);

            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 object names
            std::vector<std::string> objects{};
            std::vector<std::string> bins{};
            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_BIN) {
                    bins.push_back(object.id());
                }
            }

            if (objects.empty()) {
                ROS_INFO("[dummyCgv] No objects to select in a scene without boxes!");
            } else if (bins.empty()) {
                ROS_INFO("[dummyCgv] 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("[dummyCgv] 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("[dummyCgv] Selecting random bin: " << bin);
                sendSelection(bin);
            }

        } else if (rv == NNG_EAGAIN) {
            // no message received in current spin
        } else {
            ROS_ERROR_STREAM("[dummyCgv] nng_recv returned: " << nng_strerror(rv));
        }

        ros::spinOnce();

        loop_rate.sleep();
    }

    return 0;
}