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

fix update bug, add (commented) scene loading

parent d9f1a7c2
No related branches found
No related tags found
No related merge requests found
{ "objects": [
{ "id": "tablePillar1","pos": { "x": 0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
{ "id": "tablePillar2","pos": { "x": -0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
{ "id": "tablePillar3","pos": { "x": -0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
{ "id": "tablePillar4","pos": { "x": 0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
{ "id": "table","pos": { "z": 0.7 },"size": { "length": 1.6,"width": 1.6,"height": 0.1 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
{ "id": "binBlue","type": "DROP_OFF_LOCATION","pos": { "x": -0.34,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "b": 1 } },
{ "id": "binRed","type": "DROP_OFF_LOCATION","pos": { "x": 0.06,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } },
{ "id": "binGreen","type": "DROP_OFF_LOCATION","pos": { "x": 0.46,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "g": 1 } },
{ "id": "objectRed1","type": "BOX","pos": { "x": 0.5,"y": -0.1,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } },
{ "id": "objectRed2","type": "BOX","pos": { "x": 0.25,"y": -0.2,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "w": 1 },"color": { "r": 1 } },
{ "id": "objectRed3","type": "BOX","pos": { "x": -0.4,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } },
{ "id": "objectGreen1","type": "BOX","pos": { "x": 0.45,"y": -0.3,"z": 0.8105 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } },
{ "id": "objectGreen2","type": "BOX","pos": { "x": -0.45,"y": -0.2,"z": 0.8105 },"size": {"length":0.031,"width":0.031,"height":0.138},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } },
{ "id": "objectGreen3","type": "BOX","pos": { "x": 0.1,"y": -0.3,"z": 0.819 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } },
{ "id": "objectBlue1","type": "BOX","pos": { "x": 0.25,"y": -0.4,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
{ "id": "objectBlue2","type": "BOX","pos": { "x": -0.3,"y": -0.3,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
{ "id": "objectBlue3","type": "BOX","pos": { "x": 0.4,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
{ "id": "arm","type": "ARM","pos": { "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } }
] }
...@@ -43,6 +43,7 @@ public: ...@@ -43,6 +43,7 @@ public:
// common functionality // common functionality
void addConnection(std::unique_ptr<Connection> &&connection); void addConnection(std::unique_ptr<Connection> &&connection);
void loadScene(const std::string& sceneFile);
std::shared_ptr<Scene> getScene(); std::shared_ptr<Scene> getScene();
void sendToAll(const std::string &channel, const std::string &message); void sendToAll(const std::string &channel, const std::string &message);
...@@ -74,7 +75,7 @@ public: ...@@ -74,7 +75,7 @@ public:
void reactToSceneUpdateMessage(std::function<void()> lambda); void reactToSceneUpdateMessage(std::function<void()> lambda);
// helper methods // helper methods
std::optional<Object> resolveObject(const std::string &id); Object* resolveObject(const std::string &id);
}; };
#endif //CGV_CONNECTOR_WORKSPACE_CONNECTOR_H #endif //CGV_CONNECTOR_WORKSPACE_CONNECTOR_H
...@@ -3,9 +3,11 @@ ...@@ -3,9 +3,11 @@
// //
#include <fstream> #include <fstream>
#include <stdexcept>
#include <memory> #include <memory>
#include <google/protobuf/text_format.h> #include <google/protobuf/text_format.h>
#include <ros/package.h> #include <ros/package.h>
#include <google/protobuf/util/json_util.h>
#include "Connector.h" #include "Connector.h"
...@@ -36,13 +38,15 @@ void Connector::receive(const std::string &channel, const std::string &data) { ...@@ -36,13 +38,15 @@ void Connector::receive(const std::string &channel, const std::string &data) {
} }
} }
std::optional<Object> Connector::resolveObject(const std::string &id) { Object* Connector::resolveObject(const std::string &id) {
for (Object o : scene->objects()) { ROS_ERROR_STREAM("resolving object with id " << id);
if (o.id() == id) { for (int i = 0; i<scene->objects_size(); i++) {
return o; if (scene->objects(i).id() == id) {
ROS_ERROR_STREAM("... found it: " << scene->objects(i).id());
return scene->mutable_objects(i);
} }
} }
return std::optional<Object>(); throw std::out_of_range("... did not find it. ");
} }
void Connector::sendScene() { void Connector::sendScene() {
...@@ -93,8 +97,8 @@ void Connector::reactToSelectionMessage(std::function<void(Selection)> lambda) { ...@@ -93,8 +97,8 @@ void Connector::reactToSelectionMessage(std::function<void(Selection)> lambda) {
Connector::Connector(const ros::NodeHandle &nodeHandle, const std::string &cellName) Connector::Connector(const ros::NodeHandle &nodeHandle, const std::string &cellName)
: nodeHandle(nodeHandle), scene(nullptr), : nodeHandle(nodeHandle), scene(nullptr),
sceneUpdateAction([]() {}), sceneUpdateAction([]() {}),
pickPlaceAction([](PickPlace p) {}), pickPlaceAction([](const PickPlace& p) {}),
selectionAction([](Selection s) {}), selectionAction([](const Selection& s) {}),
cellName(cellName) { cellName(cellName) {
selectionTopic = "selection"; selectionTopic = "selection";
...@@ -147,3 +151,37 @@ void Connector::sendToAll(const std::string &channel, const std::string &message ...@@ -147,3 +151,37 @@ void Connector::sendToAll(const std::string &channel, const std::string &message
connection->send(channel, message); connection->send(channel, message);
} }
} }
void Connector::loadScene(const std::string &sceneFile) {
// read file into string: https://stackoverflow.com/questions/2602013/read-whole-ascii-file-into-c-stdstring
std::ifstream t(sceneFile);
if (!t.is_open()) {
ROS_ERROR_STREAM("[ros2cgv-dummy] Unable to open scene config file " << sceneFile);
}
std::string str;
t.seekg(0, std::ios::end); // skip to end of file
str.reserve(t.tellg()); // reserve memory in the string of the size of the file
t.seekg(0, std::ios::beg); // go back to the beginning
// Note the double parentheses! F**k C++! https://en.wikipedia.org/wiki/Most_vexing_parse
str.assign((std::istreambuf_iterator<char>(t)), std::istreambuf_iterator<char>());
Scene newScene;
google::protobuf::util::Status status = google::protobuf::util::JsonStringToMessage(str, &newScene);
if (!status.ok()) {
ROS_ERROR_STREAM("[ros2cgv-dummy] Unable to parse Json String: " << status.ToString());
} else {
ROS_INFO_STREAM("[ros2cgv-dummy] Parsed a scene with " << newScene.objects().size() << " objects.");
initScene(newScene);
}
std::string s;
if (google::protobuf::TextFormat::PrintToString(newScene, &s)) {
ROS_DEBUG_STREAM("[ros2cgv-dummy] Received scene" << std::endl << s);
} else {
ROS_WARN_STREAM("[ros2cgv-dummy] Scene invalid! partial content: " << newScene.ShortDebugString());
}
}
...@@ -21,7 +21,6 @@ bool DummyConnector::pickAndPlace(Object &robot, Object &object, Object &locatio ...@@ -21,7 +21,6 @@ bool DummyConnector::pickAndPlace(Object &robot, Object &object, Object &locatio
ROS_INFO_STREAM("[ros2cgv-dummy] \"Picking and placing\" for 3 seconds..."); ROS_INFO_STREAM("[ros2cgv-dummy] \"Picking and placing\" for 3 seconds...");
bool result = Connector::pickAndPlace(robot, object, location, simulateOnly); bool result = Connector::pickAndPlace(robot, object, location, simulateOnly);
ros::Rate(ros::Duration(3)).sleep(); ros::Rate(ros::Duration(3)).sleep();
sendScene();
return result; return result;
} }
......
...@@ -94,9 +94,9 @@ int main(int argc, char **argv) { ...@@ -94,9 +94,9 @@ int main(int argc, char **argv) {
loadScene(connector); loadScene(connector);
std::shared_ptr<Object> robot{}; Object* robot = nullptr;
std::optional<Object> selectedBox{}; Object* selectedBox = nullptr;
std::optional<Object> selectedBin{}; Object* selectedBin = nullptr;
std::optional<std::string> currentlyPickedBox{}; std::optional<std::string> currentlyPickedBox{};
ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&connector]( ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&connector](
...@@ -123,17 +123,17 @@ int main(int argc, char **argv) { ...@@ -123,17 +123,17 @@ int main(int argc, char **argv) {
ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'"); ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'");
} }
if (selectedBin.has_value() && selectedBox.has_value()) { if (selectedBin && selectedBox) {
auto boxId = selectedBox->id(); auto boxId = selectedBox->id();
ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id()); ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id());
if (!connector.Connector::pickAndDrop(*robot, *selectedBox, *selectedBin, false)) { if (!connector.Connector::pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!"); ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!");
selectedBox.reset(); selectedBox = nullptr;
selectedBin.reset(); selectedBin = nullptr;
} else { } else {
ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more."); ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more.");
selectedBox.reset(); selectedBox = nullptr;
selectedBin.reset(); selectedBin = nullptr;
connector.sendScene(); connector.sendScene();
} }
} }
......
...@@ -41,6 +41,9 @@ int main(int argc, char **argv) { ...@@ -41,6 +41,9 @@ int main(int argc, char **argv) {
DummyConnector connector{n, CELL_NAME}; DummyConnector connector{n, CELL_NAME};
// only for debugging (uncomment when running without the RAG part)
// connector.loadScene("/home/jm/work/git/ceti/models2021/src/cgv_connector/config/config_pick_place_scene.json");
// add an NNG connection // add an NNG connection
std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address); std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address);
connection->setReceiveTopic(getParameter(n, "topics/selection", "selection")); connection->setReceiveTopic(getParameter(n, "topics/selection", "selection"));
...@@ -59,8 +62,8 @@ int main(int argc, char **argv) { ...@@ -59,8 +62,8 @@ int main(int argc, char **argv) {
ros::Timer timer = n.createTimer(ros::Duration(10), [&connector]( ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
std::optional<Object> selectedBox{}; Object* selectedBox{nullptr};
std::optional<Object> selectedLocation{}; Object* selectedLocation{nullptr};
std::optional<std::string> currentlyPickedBox{}; std::optional<std::string> currentlyPickedBox{};
auto selectionLambda = [&currentlyPickedBox, &connector, &selectedBox, &selectedLocation]( auto selectionLambda = [&currentlyPickedBox, &connector, &selectedBox, &selectedLocation](
const Selection &selection) { const Selection &selection) {
...@@ -82,7 +85,7 @@ int main(int argc, char **argv) { ...@@ -82,7 +85,7 @@ int main(int argc, char **argv) {
ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'"); ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'");
} }
if (selectedLocation.has_value() && selectedBox.has_value()) { if (selectedLocation != nullptr && selectedBox != nullptr) {
auto boxId = selectedBox->id(); auto boxId = selectedBox->id();
ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " at " << selectedLocation->id()); ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " at " << selectedLocation->id());
...@@ -99,9 +102,9 @@ int main(int argc, char **argv) { ...@@ -99,9 +102,9 @@ int main(int argc, char **argv) {
return; return;
} }
bool rv = connector.Connector::pickAndPlace(*robot, *selectedBox, *selectedLocation, false); bool rv = connector.pickAndPlace(*robot, *selectedBox, *selectedLocation, false);
selectedBox.reset(); selectedBox = nullptr;
selectedLocation.reset(); selectedLocation = nullptr;
if (rv) { if (rv) {
ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! moved '" << boxId << "'."); ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! moved '" << boxId << "'.");
connector.sendScene(); connector.sendScene();
......
...@@ -58,12 +58,12 @@ int main(int argc, char **argv) { ...@@ -58,12 +58,12 @@ int main(int argc, char **argv) {
<< " and place it at " << command.idplace() << " and place it at " << command.idplace()
<< " using robot " << command.idrobot() << "."); << " using robot " << command.idrobot() << ".");
std::optional<Object> robot = connector.resolveObject(command.idrobot()); auto robot = connector.resolveObject(command.idrobot());
std::optional<Object> object = connector.resolveObject(command.idpick()); auto object = connector.resolveObject(command.idpick());
std::optional<Object> location = connector.resolveObject(command.idplace()); auto location = connector.resolveObject(command.idplace());
// check the parts of the command // check the parts of the command
if (!robot.has_value()) { if (!robot) {
ROS_ERROR_STREAM("Unknown robot " << command.idrobot() << ". Aborting PickPlace command."); ROS_ERROR_STREAM("Unknown robot " << command.idrobot() << ". Aborting PickPlace command.");
return; return;
} else if (robot->type() != Object::ARM) { } else if (robot->type() != Object::ARM) {
...@@ -71,7 +71,7 @@ int main(int argc, char **argv) { ...@@ -71,7 +71,7 @@ int main(int argc, char **argv) {
<< ". Aborting PickPlace command."); << ". Aborting PickPlace command.");
} }
if (!object.has_value()) { if (!object) {
ROS_ERROR_STREAM("Unknown object " << command.idpick() << ". Aborting PickPlace command."); ROS_ERROR_STREAM("Unknown object " << command.idpick() << ". Aborting PickPlace command.");
return; return;
} else if (object->type() != Object::BOX) { } else if (object->type() != Object::BOX) {
...@@ -79,7 +79,7 @@ int main(int argc, char **argv) { ...@@ -79,7 +79,7 @@ int main(int argc, char **argv) {
<< ". Aborting PickPlace command."); << ". Aborting PickPlace command.");
} }
if (!location.has_value()) { if (!location) {
ROS_ERROR_STREAM("Unknown place location " << command.idplace() << ". Aborting PickPlace command."); ROS_ERROR_STREAM("Unknown place location " << command.idplace() << ". Aborting PickPlace command.");
return; return;
} else if (location->type() != Object::DROP_OFF_LOCATION) { } else if (location->type() != Object::DROP_OFF_LOCATION) {
...@@ -88,7 +88,7 @@ int main(int argc, char **argv) { ...@@ -88,7 +88,7 @@ int main(int argc, char **argv) {
} }
// by now, we know that the request is legit, so we call the pick and place action // by now, we know that the request is legit, so we call the pick and place action
if (connector.pickAndPlace(robot.value(), object.value(), location.value(), false)) { if (connector.pickAndPlace(*robot, *object, *location, false)) {
ROS_INFO_STREAM("[" << NODE_NAME << "] Command successful."); ROS_INFO_STREAM("[" << NODE_NAME << "] Command successful.");
} else { } else {
ROS_INFO_STREAM("[" << NODE_NAME << "] Command failed."); ROS_INFO_STREAM("[" << NODE_NAME << "] Command failed.");
......
...@@ -56,8 +56,8 @@ int main(int argc, char **argv) { ...@@ -56,8 +56,8 @@ int main(int argc, char **argv) {
connector.addConnection(std::move(connection)); connector.addConnection(std::move(connection));
std::shared_ptr<Object> robot{}; std::shared_ptr<Object> robot{};
std::optional<Object> selectedBox{}; Object* selectedBox = nullptr;
std::optional<Object> selectedBin{}; Object* selectedBin = nullptr;
std::optional<std::string> currentlyPickedBox{}; std::optional<std::string> currentlyPickedBox{};
ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&connector]( ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&connector](
...@@ -84,7 +84,7 @@ int main(int argc, char **argv) { ...@@ -84,7 +84,7 @@ int main(int argc, char **argv) {
ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'"); ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'");
} }
if (selectedBin.has_value() && selectedBox.has_value()) { if (selectedBin && selectedBox) {
auto boxId = selectedBox->id(); auto boxId = selectedBox->id();
ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id()); ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id());
if (!connector.Connector::pickAndDrop(*robot, *selectedBox, *selectedBin, false)) { if (!connector.Connector::pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
...@@ -93,8 +93,8 @@ int main(int argc, char **argv) { ...@@ -93,8 +93,8 @@ int main(int argc, char **argv) {
ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more."); ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more.");
connector.sendScene(); connector.sendScene();
} }
selectedBox.reset(); selectedBox = nullptr;
selectedBin.reset(); selectedBin = nullptr;
} }
}; };
...@@ -107,7 +107,7 @@ int main(int argc, char **argv) { ...@@ -107,7 +107,7 @@ int main(int argc, char **argv) {
ROS_INFO_STREAM("[" << NODE_NAME << "] Received a scene update from the controller."); ROS_INFO_STREAM("[" << NODE_NAME << "] Received a scene update from the controller.");
connector.updateScene(req, bin_check_client); connector.updateScene(req, bin_check_client);
if (currentlyPickedBox.has_value() && if (currentlyPickedBox.has_value() &&
!connector.resolveObject(currentlyPickedBox.value()).has_value()) { !connector.resolveObject(currentlyPickedBox.value())) {
ROS_INFO_STREAM("[" << NODE_NAME << "] box " << currentlyPickedBox.value() ROS_INFO_STREAM("[" << NODE_NAME << "] box " << currentlyPickedBox.value()
<< " has been removed from the scene!"); << " has been removed from the scene!");
currentlyPickedBox.reset(); currentlyPickedBox.reset();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment