Skip to content
Snippets Groups Projects
Select Git revision
  • e756e65e747d1915f3e38cab60c2e37d87407304
  • main default protected
  • devel2
  • release
  • devel
5 results

WorkflowNet.pnml

Blame
  • dummy_pick_place.cpp 7.33 KiB
    /*! \file dummy_pick_place.cpp
    
        A ROS node that simulates the CGV framework
    
        \author Johannes Mey
        \date 07.05.21
    */
    
    #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 <optional>
    
    #include "connector.pb.h"
    
    #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;
    
    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));
        }
    }
    
    int main(int argc, char **argv) {
    
        GOOGLE_PROTOBUF_VERIFY_VERSION;
    
        ros::init(argc, argv, "dummy_pick_place");
    
        ros::NodeHandle n("ccf");
    
        if ((rv = nng_pair1_open(&sock)) != 0) {
            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(
                    "[" << NODE_NAME << "] nng_dial returned: " << nng_strerror(rv)
                        << ". Trying to connect again in one second...");
            connection_retry_rate.sleep();
        }
        ROS_INFO_STREAM(
                "[" << 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
    
        // initialize random number generator
        std::random_device rd;
        std::mt19937 rng(rd());
    
        ros::Timer timer = n.createTimer(ros::Duration(10), [&rng, &scene, &pause_rate](ros::TimerEvent timerEvent) {
    
            if (!scene.has_value()) {
                ROS_INFO_STREAM("[" << NODE_NAME << "] not doing anything since scene is not initialized yet.");
                return;
            }
    
            // 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);
                }
            }
    
            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++;
                        }
                    }
    
                }
            }
    
            ROS_INFO_STREAM("[" << NODE_NAME << "] There are " << correctlyPlacedObjects
                                << " correctly placed objects in the scene.");
    
            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());
    
                // 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};
    
    
                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("[" << NODE_NAME << "] nng_recv returned: " << nng_strerror(rv));
            }
    
            ros::spinOnce();
    
            loop_rate.sleep();
        }
    
        return 0;
    }