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
Hide 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,19 +49,23 @@ 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
<< "). Connection established!");
"[" << 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,70 +73,127 @@ int main(int argc, char **argv) {
std::random_device rd;
std::mt19937 rng(rd());
while (ros::ok()) {
char *buf = nullptr;
size_t sz;
if ((rv = nng_recv(sock, &buf, &sz, NNG_FLAG_ALLOC | NNG_FLAG_NONBLOCK)) == 0) {
ros::Timer timer = n.createTimer(ros::Duration(10), [&rng, &scene, &pause_rate](ros::TimerEvent timerEvent) {
// store the result in a protobuf object and free the buffer
Scene scene;
scene.ParseFromArray(buf, sz);
nng_free(buf, sz);
if (!scene.has_value()) {
ROS_INFO_STREAM("[" << NODE_NAME << "] not doing anything since scene is not initialized yet.");
return;
}
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);
} else {
ROS_WARN_STREAM("[dummyCgv] Received an invalid scene!" << std::endl
<< "[dummyCgv] Partial content:" << std::endl << scene.ShortDebugString());
// 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);
}
}
// 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());
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++;
}
}
}
}
if (objects.empty()) {
ROS_INFO("[dummyCgv] No objects to select in a scene without boxes!");
} else if (locations.empty()) {
ROS_INFO("[dummyCgv] No location to place the object at available!");
} else {
// select a random object
std::uniform_int_distribution<u_long> distribution{0, objects.size() - 1};
ROS_INFO_STREAM("[" << NODE_NAME << "] There are " << correctlyPlacedObjects
<< " correctly placed objects in the scene.");
// wait some time, then send a selected object
pause_rate.sleep();
std::string object{objects[distribution(rng)]};
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());
ROS_INFO_STREAM("[dummyCgv] Selecting random object: " << object);
sendSelection(object);
// wait again, then send the bin object
pause_rate.sleep();
// 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};
// we expect an object named "bin" to be there
// 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