Select Git revision
dummy_pick_place.cpp
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);