Select Git revision
RelAstBase.parser
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;
}