Skip to content
Snippets Groups Projects
Select Git revision
  • b1c3c740645fb867810ba4354586a1ce8a18fb1f
  • noetic/main default
2 results

dummy_pick_place.cpp

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);