extract demos, fix nng dependency

.gitmodules

-[submodule "lib/nng"]
-	path = lib/nng
-	url = https://github.com/nanomsg/nng.git
-	ignore = dirty
 [submodule "lib/paho.mqtt.c"]
 	path = lib/paho.mqtt.c
 	url = https://github.com/eclipse/paho.mqtt.c.git

CMakeLists.txt

@@ -19,18 +19,12 @@ find_package(catkin REQUIRED COMPONENTS
         )
 
 ## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
+find_package(nng REQUIRED)
 
 find_package(Protobuf 3 REQUIRED)
 
 include_directories(${CMAKE_CURRENT_BINARY_DIR})
 
-
-# make sure NNG is updated in the submodule
-execute_process(COMMAND git submodule update --init -- lib/nng
-        WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR})
-add_subdirectory(lib/nng)
-
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
@@ -121,18 +115,23 @@ generate_messages(
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-        CATKIN_DEPENDS message_runtime
-        INCLUDE_DIRS include
+        INCLUDE_DIRS
+            include
+            ${Protobuf_INCLUDE_DIRS}
+            ${CMAKE_CURRENT_BINARY_DIR}
+            ${CMAKE_CURRENT_SOURCE_DIR}
+            lib/paho.mqtt.c/src
+            lib/paho.mqtt.cpp/src
         LIBRARIES
             ${PROJECT_NAME}_scene_constructor_util
-            ${PROJECT_NAME}_grasp_util
             ${PROJECT_NAME}_scene_collision_object
-            ${PROJECT_NAME}_dummy_controller
-            ${PROJECT_NAME}_moveit_controller
-            ${PROJECT_NAME}_nng_connection
-            ${PROJECT_NAME}_mqtt_connection
-        #  CATKIN_DEPENDS other_catkin_pkg
-        #  DEPENDS system_lib
+            ${PROJECT_NAME}_base
+            ${PROJECT_NAME}_moveit
+            ${PROJECT_NAME}_nng
+            ${PROJECT_NAME}_mqtt
+            ${Protobuf_LIBRARIES}
+        CATKIN_DEPENDS message_runtime
+        DEPENDS nng
 )
 
 ###########
@@ -168,110 +167,56 @@ add_subdirectory(lib/paho.mqtt.cpp)
 add_dependencies(paho-mqttpp3 paho-mqtt3a)
 
 ## Declare a C++ library
-add_library(${PROJECT_NAME}_dummy_controller ${PROTO_SRCS} ${PROTO_HDRS} src/ccf/controller/DummyRobotArmController.cpp src/ccf/controller/RobotArmController.cpp src/ccf/controller/Controller.cpp)
-add_library(${PROJECT_NAME}_moveit_controller ${PROTO_SRCS} ${PROTO_HDRS} src/ccf/controller/MoveItRobotArmController.cpp src/ccf/controller/RobotArmController.cpp src/ccf/controller/Controller.cpp)
-add_library(${PROJECT_NAME}_nng_connection ${PROTO_SRCS} ${PROTO_HDRS} src/ccf/connection/NngConnection.cpp)
-add_library(${PROJECT_NAME}_mqtt_connection ${PROTO_SRCS} ${PROTO_HDRS} src/ccf/connection/MqttConnection.cpp)
-add_library(${PROJECT_NAME}_scene_constructor_util ${PROTO_SRCS} ${PROTO_HDRS} src/ccf/util/scene_constructor_util.cpp)
-add_library(${PROJECT_NAME}_scene_collision_object src/ccf/util/scene_collision_object.cpp)
+add_library(${PROJECT_NAME}_base ${PROTO_SRCS} ${PROTO_HDRS} src/controller/DummyRobotArmController.cpp src/controller/RobotArmController.cpp src/controller/Controller.cpp)
+add_library(${PROJECT_NAME}_moveit ${PROTO_SRCS} ${PROTO_HDRS} src/controller/MoveItRobotArmController.cpp)
+add_library(${PROJECT_NAME}_nng ${PROTO_SRCS} ${PROTO_HDRS} src/connection/NngConnection.cpp)
+add_library(${PROJECT_NAME}_mqtt ${PROTO_SRCS} ${PROTO_HDRS} src/connection/MqttConnection.cpp)
+add_library(${PROJECT_NAME}_scene_constructor_util ${PROTO_SRCS} ${PROTO_HDRS} src/util/scene_constructor_util.cpp)
+add_library(${PROJECT_NAME}_scene_collision_object src/util/scene_collision_object.cpp)
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
 # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(${PROJECT_NAME}_dummy_controller  ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(${PROJECT_NAME}_moveit_controller  ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(${PROJECT_NAME}_nng_connection  ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(${PROJECT_NAME}_mqtt_connection  ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_base  ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_moveit  ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_nng  ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_mqtt  ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(${PROJECT_NAME}_scene_constructor_util  ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(${PROJECT_NAME}_scene_collision_object  ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-add_executable(${PROJECT_NAME}_dummy_cgv src/dummies/dummy_cgv.cpp ${PROTO_SRCS} ${PROTO_HDRS})
-add_executable(${PROJECT_NAME}_dummy_pick_place src/dummies/dummy_pick_place.cpp ${PROTO_SRCS} ${PROTO_HDRS})
-add_executable(${PROJECT_NAME}_dummy_cgv_controller src/dummy_cgv_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} )
-add_executable(${PROJECT_NAME}_dummy_rag_controller_a src/dummy_rag_controller_a.cpp ${PROTO_SRCS} ${PROTO_HDRS} )
-add_executable(${PROJECT_NAME}_dummy_rag_controller_b src/dummy_rag_controller_b.cpp ${PROTO_SRCS} ${PROTO_HDRS} )
-add_executable(${PROJECT_NAME}_moveit_cgv_controller src/moveit_cgv_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
-add_executable(${PROJECT_NAME}_scene_controller src/ccf/util/scene_controller_node.cpp ${PROTO_SRCS} ${PROTO_HDRS})
+add_executable(${PROJECT_NAME}_scene_controller src/util/scene_controller_node.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
 ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-set_target_properties(${PROJECT_NAME}_dummy_cgv PROPERTIES OUTPUT_NAME dummy_cgv PREFIX "")
-set_target_properties(${PROJECT_NAME}_dummy_pick_place PROPERTIES OUTPUT_NAME dummy_pick_place PREFIX "")
-set_target_properties(${PROJECT_NAME}_dummy_cgv_controller PROPERTIES OUTPUT_NAME dummy_cgv_controller PREFIX "")
-set_target_properties(${PROJECT_NAME}_dummy_rag_controller_a PROPERTIES OUTPUT_NAME dummy_rag_controller_a PREFIX "")
-set_target_properties(${PROJECT_NAME}_dummy_rag_controller_b PROPERTIES OUTPUT_NAME dummy_rag_controller_b PREFIX "")
-set_target_properties(${PROJECT_NAME}_moveit_cgv_controller PROPERTIES OUTPUT_NAME moveit_cgv_controller PREFIX "")
 set_target_properties(${PROJECT_NAME}_scene_controller PROPERTIES OUTPUT_NAME scene_controller PREFIX "")
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(${PROJECT_NAME}_dummy_cgv ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(${PROJECT_NAME}_dummy_pick_place ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(${PROJECT_NAME}_dummy_cgv_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(${PROJECT_NAME}_dummy_rag_controller_a ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(${PROJECT_NAME}_dummy_rag_controller_b ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(${PROJECT_NAME}_moveit_cgv_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(${PROJECT_NAME}_scene_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-
-target_link_libraries(${PROJECT_NAME}_nng_connection
+target_link_libraries(${PROJECT_NAME}_nng
         ${catkin_LIBRARIES}
         nng
         )
-target_link_libraries(${PROJECT_NAME}_mqtt_connection
+target_link_libraries(${PROJECT_NAME}_mqtt
         ${catkin_LIBRARIES}
         paho-mqttpp3
         )
-target_link_libraries(${PROJECT_NAME}_dummy_controller
-        ${catkin_LIBRARIES}
-        ${Protobuf_LIBRARIES}
-        )
-target_link_libraries(${PROJECT_NAME}_moveit_controller
-        ${catkin_LIBRARIES}
-        ${Protobuf_LIBRARIES}
-        )
-target_link_libraries(${PROJECT_NAME}_dummy_cgv
-        ${catkin_LIBRARIES}
-        nng
-        ${Protobuf_LIBRARIES}
-        )
-target_link_libraries(${PROJECT_NAME}_dummy_pick_place
-        ${catkin_LIBRARIES}
-        nng
-        ${Protobuf_LIBRARIES}
-        )
-target_link_libraries(${PROJECT_NAME}_dummy_cgv_controller
-        ${catkin_LIBRARIES}
-        ${Protobuf_LIBRARIES}
-        ${PROJECT_NAME}_dummy_controller
-        ${PROJECT_NAME}_nng_connection
-        )
-target_link_libraries(${PROJECT_NAME}_dummy_rag_controller_a
-        ${catkin_LIBRARIES}
-        ${Protobuf_LIBRARIES}
-        ${PROJECT_NAME}_dummy_controller
-        ${PROJECT_NAME}_nng_connection
-        ${PROJECT_NAME}_mqtt_connection
-        )
-target_link_libraries(${PROJECT_NAME}_dummy_rag_controller_b
+target_link_libraries(${PROJECT_NAME}_base
         ${catkin_LIBRARIES}
         ${Protobuf_LIBRARIES}
-        ${PROJECT_NAME}_dummy_controller
-        ${PROJECT_NAME}_mqtt_connection
         )
-target_link_libraries(${PROJECT_NAME}_moveit_cgv_controller
+target_link_libraries(${PROJECT_NAME}_moveit
         ${catkin_LIBRARIES}
         ${Protobuf_LIBRARIES}
-        ${PROJECT_NAME}_moveit_controller
-        ${PROJECT_NAME}_nng_connection
+        ${PROJECT_NAME}_base
         )
 target_link_libraries(${PROJECT_NAME}_scene_collision_object
         ${catkin_LIBRARIES}

nng @ 8d3b5387

-Subproject commit 8d3b53879bdaf1b1d617a40633a66eea9eb345ef

package.xml

@@ -34,6 +34,7 @@
   <depend>geometry_msgs</depend>
   <depend>franka_gripper</depend>
   <build_depend>tf2</build_depend>
+  <build_depend>nng</build_depend>
   <build_depend>protobuf</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>panda_grasping</build_depend>

src/dummies/dummy_cgv.cpp

-/*! \file dummy_cgv.cpp
-
-    A ROS node that simulates the CGV framework
-
-    \author Johannes Mey
-    \date 07.07.20
-*/
-
-#include "ros/ros.h"
-#include "std_msgs/String.h"
-
-#include <nng/nng.h>
-#include <nng/protocol/pair1/pair.h>
-
-#include <google/protobuf/text_format.h>
-#include <random>
-
-#include "connector.pb.h"
-
-#include <google/protobuf/util/json_util.h>
-#include <google/protobuf/message.h>
-
-const std::string URL = "tcp://127.0.0.1:6576";
-
-nng_socket sock;
-int rv;
-
-void sendSelection(const std::string &object) {
-    Selection selection;
-    selection.set_id(object);
-    int length = selection.ByteSize();
-    void *data = nng_alloc(length);
-    selection.SerializeToArray(data, length);
-
-    if ((rv = nng_send(sock, data, length, NNG_FLAG_ALLOC)) != 0) {
-        ROS_ERROR_STREAM("nng_send returned: " << nng_strerror(rv));
-    }
-}
-
-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;
-
-    ros::init(argc, argv, "dummy_cgv");
-
-    ros::NodeHandle n("ccf");
-
-    if ((rv = nng_pair1_open(&sock)) != 0) {
-        ROS_ERROR_STREAM("[dummyCgv] nng_pair1_open returned: " << nng_strerror(rv));
-    }
-
-    ros::Rate connection_retry_rate(1);
-
-    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...");
-        connection_retry_rate.sleep();
-    }
-    ROS_INFO_STREAM(
-            "[dummyCgv] 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
-
-    // initialize random number generator
-    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) {
-
-            // store the result in a protobuf object and free the buffer
-            Scene scene;
-            scene.ParseFromArray(buf, sz);
-            nng_free(buf, sz);
-
-            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 object names
-            std::vector<std::string> objects{};
-            std::vector<std::string> bins{};
-            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_BIN) {
-                    bins.push_back(object.id());
-                }
-            }
-
-            if (objects.empty()) {
-                ROS_INFO("[dummyCgv] No objects to select in a scene without boxes!");
-            } else if (bins.empty()) {
-                ROS_INFO("[dummyCgv] No bin to put the object in 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)]};
-
-                ROS_INFO_STREAM("[dummyCgv] Selecting random object: " << object);
-                sendSelection(object);
-
-                // 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> binDistribution{0, bins.size() - 1};
-
-                std::string bin{bins[binDistribution(rng)]};
-                ROS_INFO_STREAM("[dummyCgv] Selecting random bin: " << bin);
-                sendSelection(bin);
-            }
-
-        } else if (rv == NNG_EAGAIN) {
-            // no message received in current spin
-        } else {
-            ROS_ERROR_STREAM("[dummyCgv] nng_recv returned: " << nng_strerror(rv));
-        }
-
-        ros::spinOnce();
-
-        loop_rate.sleep();
-    }
-
-    return 0;
-}
\ No newline at end of file

src/dummies/dummy_pick_place.cpp

-/*! \file dummy_pick_place.cpp
-
-    A ROS node that simulates the CGV framework
-
-    \author Johannes Mey
-    \date 07.05.21
-*/
-
-#include "ros/ros.h"
-#include "std_msgs/String.h"
-
-#include <nng/nng.h>
-#include <nng/protocol/pair1/pair.h>
-
-#include <google/protobuf/text_format.h>
-#include <random>
-#include <optional>
-
-#include "connector.pb.h"
-
-#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;
-
-void sendSelection(const std::string &object) {
-    Selection selection;
-    selection.set_id(object);
-    int length = selection.ByteSize();
-    void *data = nng_alloc(length);
-    selection.SerializeToArray(data, length);
-
-    if ((rv = nng_send(sock, data, length, NNG_FLAG_ALLOC)) != 0) {
-        ROS_ERROR_STREAM("nng_send returned: " << nng_strerror(rv));
-    }
-}
-
-int main(int argc, char **argv) {
-
-    GOOGLE_PROTOBUF_VERIFY_VERSION;
-
-    ros::init(argc, argv, "dummy_pick_place");
-
-    ros::NodeHandle n("ccf");
-
-    if ((rv = nng_pair1_open(&sock)) != 0) {
-        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(
-                "[" << NODE_NAME << "] nng_dial returned: " << nng_strerror(rv)
-                    << ". Trying to connect again in one second...");
-        connection_retry_rate.sleep();
-    }
-    ROS_INFO_STREAM(
-            "[" << 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
-
-    // initialize random number generator
-    std::random_device rd;
-    std::mt19937 rng(rd());
-
-    ros::Timer timer = n.createTimer(ros::Duration(10), [&rng, &scene, &pause_rate](ros::TimerEvent timerEvent) {
-
-        if (!scene.has_value()) {
-            ROS_INFO_STREAM("[" << NODE_NAME << "] not doing anything since scene is not initialized yet.");
-            return;
-        }
-
-        // 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);
-            }
-        }
-
-        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++;
-                    }
-                }
-
-            }
-        }
-
-        ROS_INFO_STREAM("[" << NODE_NAME << "] There are " << correctlyPlacedObjects
-                            << " correctly placed objects in the scene.");
-
-        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());
-
-            // 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};
-
-
-            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("[" << NODE_NAME << "] nng_recv returned: " << nng_strerror(rv));
-        }
-
-        ros::spinOnce();
-
-        loop_rate.sleep();
-    }
-
-    return 0;
-}

src/dummy_cgv_controller.cpp

-/*! \file dummy_cgv_controller.cpp
-
-    A ROS node that simulates a node connected to a model-based cobotic application
-
-    \author Johannes Mey
-    \author Sebastian Ebert
-    \date 07.07.20
-*/
-
-#define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost
-
-#include <ros/ros.h>
-#include <ros/package.h>
-#include <std_msgs/Empty.h>
-
-#include "connector.pb.h"
-
-#include "ccf/controller/DummyRobotArmController.h"
-#include "ccf/connection/NngConnection.h"
-#include "ccf/util/NodeUtil.h"
-
-std::string NODE_NAME = "dummy_cgv_controller";
-
-using CetiRosToolbox::getParameter;
-using CetiRosToolbox::getPrivateParameter;
-
-int main(int argc, char **argv) {
-
-    GOOGLE_PROTOBUF_VERIFY_VERSION;
-
-    ros::init(argc, argv, NODE_NAME);
-    NODE_NAME = ros::this_node::getName();
-
-    ros::NodeHandle n("connector_node_ros_cgv"); // namespace where the connection_address is
-
-    auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
-
-    DummyRobotArmController connector{n, NODE_NAME};
-
-    // add an NNG connection
-    std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address);
-    connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
-    connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
-    connector.addConnection(std::move(connection));
-
-    auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
-
-    ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to " << clientControllers.size() << " client controllers.");
-    for (const auto &client : clientControllers) {
-        ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to client at " << client << ".");
-        std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false);
-        client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection"));
-        client_connection->setReceiveTopic("client_scene");
-        connector.addConnection(std::move(client_connection));
-    }
-
-    connector.loadScene(ros::package::getPath("ccf") + "/config/config_scene.yaml");
-
-    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](
-            const std_msgs::EmptyConstPtr &msg) { connector.sendScene(); });
-    ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
-            const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
-
-    auto selectionMessageCallback = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin, &n](
-            const Selection &selection) {
-
-        // forward the selection to the clients
-        connector.sendToAll(getParameter<std::string>(n, "topics/selection", "selection"),
-                            selection.SerializeAsString());
-
-        if (currentlyPickedBox.has_value()) {
-            ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress.");
-            return;
-        }
-
-        Object *object;
-        try {
-            object = connector.resolveObject(selection.id());
-        } catch (std::out_of_range &e) {
-            ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
-            return;
-        }
-        auto type = Object::Type_Name(object->type());
-        if (object->type() == Object::BOX) {
-            selectedBox = object;
-            ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
-        } else if (object->type() == Object::BIN) {
-            selectedBin = object;
-            ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
-        } else {
-            ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'");
-        }
-
-        if (selectedBin && selectedBox) {
-            auto boxId = selectedBox->id();
-            ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id());
-            currentlyPickedBox = boxId;
-            if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
-                ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!");
-                selectedBox = nullptr;
-                selectedBin = nullptr;
-            } else {
-                ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more.");
-                selectedBox = nullptr;
-                selectedBin = nullptr;
-                connector.sendScene();
-            }
-        }
-    };
-    connector.reactToSelectionMessage(selectionMessageCallback);
-
-    auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
-        if (currentlyPickedBox.has_value()) {
-            try {
-                connector.resolveObject(currentlyPickedBox.value());
-            } catch (std::out_of_range &e) {
-                ROS_INFO_STREAM(
-                        "[" << NODE_NAME << "] box " << currentlyPickedBox.value()
-                            << " has been removed from the scene!");
-                currentlyPickedBox.reset();
-            }
-        }
-    };
-    connector.reactToSceneUpdateMessage(sceneUpdateCallback);
-
-    ros::spin();
-
-    return 0;
-}

src/dummy_rag_controller_a.cpp

-/*! \file dummy_rag_controller_a.cpp
-
-    A ROS node that simulates a node connected to a model-based cobotic application
-
-    \author Johannes Mey
-    \author Sebastian Ebert
-    \date 07.07.20
-*/
-
-#define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost
-
-#include <fstream>
-
-#include <ros/ros.h>
-#include <ros/package.h>
-#include <std_msgs/Empty.h>
-
-#include "connector.pb.h"
-
-#include "ccf/controller/DummyRobotArmController.h"
-#include "ccf/connection/NngConnection.h"
-#include "ccf/connection/MqttConnection.h"
-#include "ccf/util/NodeUtil.h"
-
-#include <google/protobuf/text_format.h>
-
-const std::string CELL_NAME = "place-a";
-const std::string NODE_NAME = "rag_connector_dummy_a";
-
-using CetiRosToolbox::getParameter;
-using CetiRosToolbox::getPrivateParameter;
-
-int main(int argc, char **argv) {
-
-    GOOGLE_PROTOBUF_VERIFY_VERSION;
-
-    ros::init(argc, argv, NODE_NAME);
-
-    ros::NodeHandle n("ceti_connector"); // namespace
-
-    auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
-
-    DummyRobotArmController connector{n, CELL_NAME};
-
-    // only for debugging (uncomment when running without the RAG part)
-    // connector.loadScene("/home/jm/work/git/ceti/models2021/src/ccf/config/config_pick_place_scene.json");
-
-    // add an NNG connection
-    std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address);
-    connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
-    connection->setSendTopic(getParameter(n, "topics/scene", CELL_NAME + "/scene/update"));
-    connector.addConnection(std::move(connection));
-
-    // add an MQTT connection
-    std::string mqtt_address = getParameter<std::string>(n, "mqtt/mqtt_address", "tcp://localhost:1883");
-    std::unique_ptr<MqttConnection> mqtt_connection = std::make_unique<MqttConnection>(mqtt_address, NODE_NAME);
-    mqtt_connection->addTopic(CELL_NAME + "/scene/init");
-    mqtt_connection->addTopic(CELL_NAME + "/command");
-    connector.addConnection(std::move(mqtt_connection));
-
-    ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000,
-                                                       [&connector](auto &msg) { connector.sendScene(); });
-    ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
-            const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
-
-    Object *selectedBox{nullptr};
-    Object *selectedLocation{nullptr};
-    std::optional<std::string> currentlyPickedBox{};
-    auto selectionLambda = [&currentlyPickedBox, &connector, &selectedBox, &selectedLocation](
-            const Selection &selection) {
-
-        if (currentlyPickedBox.has_value()) {
-            ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress.");
-            return;
-        }
-
-        Object *object;
-        try {
-            object = connector.resolveObject(selection.id());
-        } catch (std::out_of_range &e) {
-            ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
-            return;
-        }
-        auto type = Object::Type_Name(object->type());
-        if (object->type() == Object::BOX) {
-            selectedBox = object;
-            ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
-        } else if (object->type() == Object::DROP_OFF_LOCATION) {
-            selectedLocation = object;
-            ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
-        } else {
-            ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'");
-        }
-
-        if (selectedLocation != nullptr && selectedBox != nullptr) {
-            auto boxId = selectedBox->id();
-            ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " at " << selectedLocation->id());
-
-            // we use the first robot we can find for our picking and placing
-            std::optional<Object> robot;
-            for (auto &r : connector.getScene()->objects()) {
-                if (r.type() == Object::ARM) {
-                    robot = r;
-                    break;
-                }
-            }
-            if (!robot.has_value()) {
-                ROS_ERROR_STREAM("[" << NODE_NAME << "] Unable to find a robot to perform the task.");
-                return;
-            }
-
-            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();
-            } else {
-                ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to move box '" << boxId << "'!");
-            }
-        }
-    };
-    connector.reactToSelectionMessage(selectionLambda);
-
-    ros::spin();
-
-    return 0;
-}
\ No newline at end of file

src/dummy_rag_controller_b.cpp

-/*! \file dummy_rag_controller_b.cpp
-
-    A ROS node that simulates a node connected to a model-based cobotic application
-
-    \author Johannes Mey
-    \author Sebastian Ebert
-    \date 07.07.20
-*/
-
-#define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost
-
-#include <fstream>
-
-#include <ros/ros.h>
-#include <ros/package.h>
-#include <std_msgs/Empty.h>
-
-#include "connector.pb.h"
-
-#include "ccf/controller/DummyRobotArmController.h"
-#include "ccf/connection/MqttConnection.h"
-#include "ccf/util/NodeUtil.h"
-
-#include <google/protobuf/text_format.h>
-
-const std::string CELL_NAME = "place-b";
-const std::string NODE_NAME = "rag_connector_dummy_b";
-
-using CetiRosToolbox::getParameter;
-using CetiRosToolbox::getPrivateParameter;
-
-int main(int argc, char **argv) {
-
-    GOOGLE_PROTOBUF_VERIFY_VERSION;
-
-    ros::init(argc, argv, NODE_NAME);
-
-    ros::NodeHandle n("ceti_connector"); // namespace
-
-    auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
-
-    std::string cell_prefix = getParameter(n, "place_b_prefix", CELL_NAME);
-
-    DummyRobotArmController connector{n, CELL_NAME};
-    connector.setSendSceneTopic(cell_prefix + "/scene");
-
-    // only for debugging (uncomment when running without the RAG part)
-    // connector.loadScene("/home/jm/work/git/ceti/models2021/src/ccf/config/config_pick_place_scene.json");
-
-    // add an MQTT connection
-    std::string mqtt_address = getParameter<std::string>(n, "mqtt/mqtt_address", "tcp://localhost:1883");
-    std::unique_ptr<MqttConnection> mqtt_connection = std::make_unique<MqttConnection>(mqtt_address, NODE_NAME);
-    mqtt_connection->addTopic(connector.getInitSceneTopic());
-    mqtt_connection->addTopic(connector.getCommandTopic());
-    connector.addConnection(std::move(mqtt_connection));
-
-    ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000,
-                                                       [&connector](auto &msg) { connector.sendScene(); });
-    ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
-            const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
-
-    auto pickPlaceLambda = [&connector](const PickPlace &command) {
-        ROS_INFO_STREAM("[" << NODE_NAME << "] Received a command to pick object " << command.idpick()
-                            << " and place it at " << command.idplace()
-                            << " using robot " << command.idrobot() << ".");
-
-        Object *robot = nullptr;
-        Object *object = nullptr;
-        Object *location = nullptr;
-
-        try {
-            robot = connector.resolveObject(command.idrobot());
-            object = connector.resolveObject(command.idpick());
-            location = connector.resolveObject(command.idplace());
-        } catch (std::out_of_range &e) {        // check the parts of the command
-            if (!robot) {
-                ROS_ERROR_STREAM("Unknown robot " << command.idrobot() << ". Aborting PickPlace command.");
-                return;
-            } else if (robot->type() != Object::ARM) {
-                ROS_ERROR_STREAM("Robot " << command.idrobot() << " has wrong type " << robot->type()
-                                          << ". Aborting PickPlace command.");
-            }
-
-            if (!object) {
-                ROS_ERROR_STREAM("Unknown object " << command.idpick() << ". Aborting PickPlace command.");
-                return;
-            } else if (object->type() != Object::BOX) {
-                ROS_ERROR_STREAM("Object " << command.idpick() << " has wrong type " << object->type()
-                                           << ". Aborting PickPlace command.");
-            }
-
-            if (!location) {
-                ROS_ERROR_STREAM("Unknown place location " << command.idplace() << ". Aborting PickPlace command.");
-                return;
-            } else if (location->type() != Object::DROP_OFF_LOCATION) {
-                ROS_ERROR_STREAM("Location " << command.idplace() << " has wrong type " << location->type()
-                                             << ". Aborting PickPlace command.");
-            }
-        }
-
-        // by now, we know that the request is legit, so we call the pick and place action
-        if (connector.pickAndPlace(*robot, *object, *location, false)) {
-            ROS_INFO_STREAM("[" << NODE_NAME << "] Command successful.");
-        } else {
-            ROS_INFO_STREAM("[" << NODE_NAME << "] Command failed.");
-        }
-
-    };
-    connector.reactToPickAndPlaceMessage(pickPlaceLambda);
-
-    std::string reachabilityTopicPrefix = cell_prefix + "/reachability/";
-
-    auto sceneUpdateLambda = [&reachabilityTopicPrefix, &connector]() {
-        ROS_INFO_STREAM("[" << NODE_NAME << "] Scene was updated, sending new reachability information.");
-
-        for (auto &robot : connector.getScene()->objects()) {
-            if (robot.type() == Object::ARM) {
-                Reachability reachability;
-                reachability.set_idrobot(robot.id());
-                for (auto &object : connector.getScene()->objects()) {
-                    if (object.type() == Object::DROP_OFF_LOCATION) {
-                        auto r = reachability.add_objects();
-                        r->set_idobject(object.id());
-                        // FIXME the second object is wrong here, but ignored in the implementation
-                        r->set_reachable(connector.reachableLocation(robot, object, object));
-                    } else if (object.type() == Object::BOX) {
-                        auto r = reachability.add_objects();
-                        r->set_idobject(object.id());
-                        r->set_reachable(connector.reachableObject(robot, object));
-                    }
-                    // otherwise skip the object
-                }
-                connector.sendToAll(reachabilityTopicPrefix + robot.id(), reachability.SerializeAsString());
-            }
-        }
-
-    };
-    connector.reactToSceneUpdateMessage(sceneUpdateLambda);
-
-    ros::spin();
-
-    return 0;
-}

src/moveit_cgv_controller.cpp

-/*! \file moveit_cgv_controller.cpp
-
-    A ROS node that controls a robot connected to a model-based cobotic application
-
-    \author Johannes Mey
-    \author Sebastian Ebert
-    \date 07.07.20
-*/
-
-#define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost
-
-#include <ros/ros.h>
-#include <std_msgs/Empty.h>
-
-#include "connector.pb.h"
-
-#include "ccf/controller/MoveItRobotArmController.h"
-#include "ccf/connection/NngConnection.h"
-#include "ccf/util/NodeUtil.h"
-
-std::string NODE_NAME = "moveit_cgv_controller";
-
-using CetiRosToolbox::getParameter;
-using CetiRosToolbox::getPrivateParameter;
-
-int main(int argc, char **argv) {
-
-    GOOGLE_PROTOBUF_VERIFY_VERSION;
-
-    ros::init(argc, argv, NODE_NAME);
-    NODE_NAME = ros::this_node::getName();
-
-    ros::NodeHandle n("connector_node_ros_cgv"); // namespace where the connection_address is
-
-    auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
-
-    MoveItRobotArmController connector{n, NODE_NAME};
-
-    // add an NNG connection
-    std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address);
-    connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
-    connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
-    connector.addConnection(std::move(connection));
-
-    auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
-
-    ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to " << clientControllers.size() << " client controllers.");
-    for (const auto &client : clientControllers) {
-        ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to client at " << client << ".");
-        std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false);
-        client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection"));
-        client_connection->setReceiveTopic("client_scene");
-        connector.addConnection(std::move(client_connection));
-    }
-
-    // scene loading is not required, the scene is updated by moveit
-
-    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](
-            const std_msgs::EmptyConstPtr &msg) { connector.sendScene(); });
-    ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
-            const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
-
-    auto selectionMessageCallback = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin, &n](
-            const Selection &selection) {
-
-        // forward the selection to the clients
-        connector.sendToAll(getParameter<std::string>(n, "topics/selection", "selection"),
-                            selection.SerializeAsString());
-
-        if (currentlyPickedBox.has_value()) {
-            ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress.");
-            return;
-        }
-
-        Object *object;
-        try {
-            object = connector.resolveObject(selection.id());
-        } catch (std::out_of_range &e) {
-            ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
-            return;
-        }
-        auto type = Object::Type_Name(object->type());
-        if (object->type() == Object::BOX) {
-            selectedBox = object;
-            ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
-        } else if (object->type() == Object::BIN) {
-            selectedBin = object;
-            ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
-        } else {
-            ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'");
-        }
-
-        if (selectedBin && selectedBox) {
-            auto boxId = selectedBox->id();
-            ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id());
-            currentlyPickedBox = boxId;
-            if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
-                ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!");
-                selectedBox = nullptr;
-                selectedBin = nullptr;
-            } else {
-                ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more.");
-                selectedBox = nullptr;
-                selectedBin = nullptr;
-                connector.sendScene();
-            }
-        }
-    };
-    connector.reactToSelectionMessage(selectionMessageCallback);
-
-    auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
-        if (currentlyPickedBox.has_value()) {
-            try {
-                connector.resolveObject(currentlyPickedBox.value());
-            } catch (std::out_of_range &e) {
-                ROS_INFO_STREAM(
-                        "[" << NODE_NAME << "] box " << currentlyPickedBox.value()
-                            << " has been removed from the scene!");
-                currentlyPickedBox.reset();
-            }
-        }
-    };
-    connector.reactToSceneUpdateMessage(sceneUpdateCallback);
-
-    ros::spin();
-
-    return 0;
-}