Skip to content
Snippets Groups Projects
Commit 5de0a664 authored by Johannes Mey's avatar Johannes Mey
Browse files

improved logic in dummy selector

parent 46e920e1
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
src/dummy_pick_place.cpp
+ 115
− 57
View file @ 5de0a664
......@@ -20,7 +20,10 @@
#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;
......@@ -37,12 +40,6 @@ void sendSelection(const std::string &object) {
}
}
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;
......@@ -52,18 +49,22 @@ int main(int argc, char **argv) {
ros::NodeHandle n("cgv_connector");
if ((rv = nng_pair1_open(&sock)) != 0) {
ROS_ERROR_STREAM("[dummyCgv] nng_pair1_open returned: " << nng_strerror(rv));
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(
"[dummyCgv] nng_dial returned: " << nng_strerror(rv) << ". Trying to connect again in one second...");
"[" << NODE_NAME << "] 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
"[" << 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
......@@ -72,53 +73,69 @@ int main(int argc, char **argv) {
std::random_device rd;
std::mt19937 rng(rd());
while (ros::ok()) {
ros::Timer timer = n.createTimer(ros::Duration(10), [&rng, &scene, &pause_rate](ros::TimerEvent timerEvent) {
char *buf = nullptr;
size_t sz;
if ((rv = nng_recv(sock, &buf, &sz, NNG_FLAG_ALLOC | NNG_FLAG_NONBLOCK)) == 0) {
if (!scene.has_value()) {
ROS_INFO_STREAM("[" << NODE_NAME << "] not doing anything since scene is not initialized yet.");
return;
}
// store the result in a protobuf object and free the buffer
Scene scene;
scene.ParseFromArray(buf, sz);
nng_free(buf, sz);
// 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);
}
}
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);
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 {
ROS_WARN_STREAM("[dummyCgv] Received an invalid scene!" << std::endl
<< "[dummyCgv] Partial content:" << std::endl << scene.ShortDebugString());
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++;
}
}
// collect all object names
std::vector<std::string> objects{};
std::vector<std::string> locations{};
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_DROP_OFF_LOCATION) {
locations.push_back(object.id());
}
}
ROS_INFO_STREAM("[" << NODE_NAME << "] There are " << correctlyPlacedObjects
<< " correctly placed objects in the scene.");
if (objects.empty()) {
ROS_INFO("[dummyCgv] No objects to select in a scene without boxes!");
ROS_INFO_STREAM("[" << NODE_NAME << "] No objects to select that need to be placed!");
} else if (locations.empty()) {
ROS_INFO("[dummyCgv] No location to place the object at available!");
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();
std::string object{objects[distribution(rng)]};
auto object{objects[distribution(rng)]};
ROS_INFO_STREAM("[dummyCgv] Selecting random object: " << object);
sendSelection(object);
ROS_INFO_STREAM("[" << NODE_NAME << "] Selecting random object: " << object.id());
sendSelection(object.id());
// wait again, then send the bin object
pause_rate.sleep();
......@@ -127,15 +144,56 @@ int main(int argc, char **argv) {
// select a random object
std::uniform_int_distribution<u_long> locationDistribution{0, locations.size() - 1};
std::string location{locations[locationDistribution(rng)]};
ROS_INFO_STREAM("[dummyCgv] Selecting random location: " << location);
sendSelection(location);
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("[dummyCgv] nng_recv returned: " << nng_strerror(rv));
ROS_ERROR_STREAM("[" << NODE_NAME << "] nng_recv returned: " << nng_strerror(rv));
}
ros::spinOnce();
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment