fix update bug, add (commented) scene loading

config/config_pick_place_scene.json

+{ "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 } }
+] }

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

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());
+    }
+}

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;
 }
 

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();
             }
         }

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 = [&currentlyPickedBox, &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();

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.");

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();