diff --git a/config/config_pick_place_scene.json b/config/config_pick_place_scene.json new file mode 100644 index 0000000000000000000000000000000000000000..ddefea7c5a933d27f7028a8cba1a4ce8b336de09 --- /dev/null +++ b/config/config_pick_place_scene.json @@ -0,0 +1,20 @@ +{ "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 } } +] } diff --git a/include/Connector.h b/include/Connector.h index 128b719ea21936de0da38391049392c76b89ac73..c482125e678a517d3238995000c3f9d87e3729ca 100644 --- a/include/Connector.h +++ b/include/Connector.h @@ -43,6 +43,7 @@ public: // common functionality void addConnection(std::unique_ptr<Connection> &&connection); + void loadScene(const std::string& sceneFile); std::shared_ptr<Scene> getScene(); void sendToAll(const std::string &channel, const std::string &message); @@ -74,7 +75,7 @@ public: void reactToSceneUpdateMessage(std::function<void()> lambda); // helper methods - std::optional<Object> resolveObject(const std::string &id); + Object* resolveObject(const std::string &id); }; #endif //CGV_CONNECTOR_WORKSPACE_CONNECTOR_H diff --git a/src/Connector.cpp b/src/Connector.cpp index 0cdd3f00c3d7db408d5e0765310972f72cb43522..af22209d75c589a2c906ef981e6927ca89d3af22 100644 --- a/src/Connector.cpp +++ b/src/Connector.cpp @@ -3,9 +3,11 @@ // #include <fstream> +#include <stdexcept> #include <memory> #include <google/protobuf/text_format.h> #include <ros/package.h> +#include <google/protobuf/util/json_util.h> #include "Connector.h" @@ -36,13 +38,15 @@ void Connector::receive(const std::string &channel, const std::string &data) { } } -std::optional<Object> Connector::resolveObject(const std::string &id) { - for (Object o : scene->objects()) { - if (o.id() == id) { - return o; +Object* Connector::resolveObject(const std::string &id) { + ROS_ERROR_STREAM("resolving object with id " << id); + for (int i = 0; i<scene->objects_size(); i++) { + 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() { @@ -93,8 +97,8 @@ void Connector::reactToSelectionMessage(std::function<void(Selection)> lambda) { Connector::Connector(const ros::NodeHandle &nodeHandle, const std::string &cellName) : nodeHandle(nodeHandle), scene(nullptr), sceneUpdateAction([]() {}), - pickPlaceAction([](PickPlace p) {}), - selectionAction([](Selection s) {}), + pickPlaceAction([](const PickPlace& p) {}), + selectionAction([](const Selection& s) {}), cellName(cellName) { selectionTopic = "selection"; @@ -147,3 +151,37 @@ void Connector::sendToAll(const std::string &channel, const std::string &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()); + } +} diff --git a/src/DummyConnector.cpp b/src/DummyConnector.cpp index b86d76e8dc704cd32b517f969d5c1f6da3f025d2..ae51c2dc1186503106815d2e18ec4583aa6618ac 100644 --- a/src/DummyConnector.cpp +++ b/src/DummyConnector.cpp @@ -21,7 +21,6 @@ bool DummyConnector::pickAndPlace(Object &robot, Object &object, Object &locatio ROS_INFO_STREAM("[ros2cgv-dummy] \"Picking and placing\" for 3 seconds..."); bool result = Connector::pickAndPlace(robot, object, location, simulateOnly); ros::Rate(ros::Duration(3)).sleep(); - sendScene(); return result; } diff --git a/src/dummy_cgv_connector.cpp b/src/dummy_cgv_connector.cpp index ff7aa1ba70e39b603a22d24a93fa582601a722b4..692970823eb564ef3ea18621e923cc4ebb0ba42e 100644 --- a/src/dummy_cgv_connector.cpp +++ b/src/dummy_cgv_connector.cpp @@ -94,9 +94,9 @@ int main(int argc, char **argv) { loadScene(connector); - std::shared_ptr<Object> robot{}; - std::optional<Object> selectedBox{}; - std::optional<Object> selectedBin{}; + Object* robot = nullptr; + Object* selectedBox = nullptr; + Object* selectedBin = nullptr; std::optional<std::string> currentlyPickedBox{}; ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&connector]( @@ -123,17 +123,17 @@ int main(int argc, char **argv) { ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'"); } - if (selectedBin.has_value() && selectedBox.has_value()) { + if (selectedBin && selectedBox) { auto boxId = selectedBox->id(); ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id()); if (!connector.Connector::pickAndDrop(*robot, *selectedBox, *selectedBin, false)) { ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!"); - selectedBox.reset(); - selectedBin.reset(); + selectedBox = nullptr; + selectedBin = nullptr; } else { ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more."); - selectedBox.reset(); - selectedBin.reset(); + selectedBox = nullptr; + selectedBin = nullptr; connector.sendScene(); } } diff --git a/src/dummy_rag_connector.cpp b/src/dummy_rag_connector.cpp index b38a8d21cb9f52ff6da3b830e902e271952650ca..0d4d56d30231c9c5868926bb115ca93311df9102 100644 --- a/src/dummy_rag_connector.cpp +++ b/src/dummy_rag_connector.cpp @@ -41,6 +41,9 @@ int main(int argc, char **argv) { 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 std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address); connection->setReceiveTopic(getParameter(n, "topics/selection", "selection")); @@ -59,8 +62,8 @@ int main(int argc, char **argv) { ros::Timer timer = n.createTimer(ros::Duration(10), [&connector]( const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds - std::optional<Object> selectedBox{}; - std::optional<Object> selectedLocation{}; + Object* selectedBox{nullptr}; + Object* selectedLocation{nullptr}; std::optional<std::string> currentlyPickedBox{}; auto selectionLambda = [¤tlyPickedBox, &connector, &selectedBox, &selectedLocation]( const Selection &selection) { @@ -82,7 +85,7 @@ int main(int argc, char **argv) { 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(); ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " at " << selectedLocation->id()); @@ -99,9 +102,9 @@ int main(int argc, char **argv) { return; } - bool rv = connector.Connector::pickAndPlace(*robot, *selectedBox, *selectedLocation, false); - selectedBox.reset(); - selectedLocation.reset(); + bool rv = connector.pickAndPlace(*robot, *selectedBox, *selectedLocation, false); + selectedBox = nullptr; + selectedLocation = nullptr; if (rv) { ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! moved '" << boxId << "'."); connector.sendScene(); diff --git a/src/dummy_rbg_connector.cpp b/src/dummy_rbg_connector.cpp index 8c391177defd31fdc37eec2783bc6b974b6318dd..dc906cde6d76168c2ccc1ea5c412ab10f1e7d5d6 100644 --- a/src/dummy_rbg_connector.cpp +++ b/src/dummy_rbg_connector.cpp @@ -58,12 +58,12 @@ int main(int argc, char **argv) { << " and place it at " << command.idplace() << " using robot " << command.idrobot() << "."); - std::optional<Object> robot = connector.resolveObject(command.idrobot()); - std::optional<Object> object = connector.resolveObject(command.idpick()); - std::optional<Object> location = connector.resolveObject(command.idplace()); + auto robot = connector.resolveObject(command.idrobot()); + auto object = connector.resolveObject(command.idpick()); + auto location = connector.resolveObject(command.idplace()); // check the parts of the command - if (!robot.has_value()) { + if (!robot) { ROS_ERROR_STREAM("Unknown robot " << command.idrobot() << ". Aborting PickPlace command."); return; } else if (robot->type() != Object::ARM) { @@ -71,7 +71,7 @@ int main(int argc, char **argv) { << ". Aborting PickPlace command."); } - if (!object.has_value()) { + if (!object) { ROS_ERROR_STREAM("Unknown object " << command.idpick() << ". Aborting PickPlace command."); return; } else if (object->type() != Object::BOX) { @@ -79,7 +79,7 @@ int main(int argc, char **argv) { << ". Aborting PickPlace command."); } - if (!location.has_value()) { + if (!location) { ROS_ERROR_STREAM("Unknown place location " << command.idplace() << ". Aborting PickPlace command."); return; } else if (location->type() != Object::DROP_OFF_LOCATION) { @@ -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 - if (connector.pickAndPlace(robot.value(), object.value(), location.value(), false)) { + if (connector.pickAndPlace(*robot, *object, *location, false)) { ROS_INFO_STREAM("[" << NODE_NAME << "] Command successful."); } else { ROS_INFO_STREAM("[" << NODE_NAME << "] Command failed."); diff --git a/src/moveit_cgv_connector.cpp b/src/moveit_cgv_connector.cpp index 7e53d2b1d6d3306f4f6d135b8a7501bce31d7a10..792845747b19f70005af93e0e292f9b4d8a2ffd9 100644 --- a/src/moveit_cgv_connector.cpp +++ b/src/moveit_cgv_connector.cpp @@ -56,8 +56,8 @@ int main(int argc, char **argv) { connector.addConnection(std::move(connection)); std::shared_ptr<Object> robot{}; - std::optional<Object> selectedBox{}; - std::optional<Object> selectedBin{}; + Object* selectedBox = nullptr; + Object* selectedBin = nullptr; std::optional<std::string> currentlyPickedBox{}; ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&connector]( @@ -84,7 +84,7 @@ int main(int argc, char **argv) { ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'"); } - if (selectedBin.has_value() && selectedBox.has_value()) { + if (selectedBin && selectedBox) { auto boxId = selectedBox->id(); ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id()); if (!connector.Connector::pickAndDrop(*robot, *selectedBox, *selectedBin, false)) { @@ -93,8 +93,8 @@ int main(int argc, char **argv) { ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more."); connector.sendScene(); } - selectedBox.reset(); - selectedBin.reset(); + selectedBox = nullptr; + selectedBin = nullptr; } }; @@ -107,7 +107,7 @@ int main(int argc, char **argv) { ROS_INFO_STREAM("[" << NODE_NAME << "] Received a scene update from the controller."); connector.updateScene(req, bin_check_client); if (currentlyPickedBox.has_value() && - !connector.resolveObject(currentlyPickedBox.value()).has_value()) { + !connector.resolveObject(currentlyPickedBox.value())) { ROS_INFO_STREAM("[" << NODE_NAME << "] box " << currentlyPickedBox.value() << " has been removed from the scene!"); currentlyPickedBox.reset();