initial commit of extracted package

.gitignore

+.idea
+cmake-build-*
+doc

CMakeLists.txt

+cmake_minimum_required(VERSION 3.0.2)
+project(ccf_immersive_sorting)
+
+## compile as C++14
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17")
+set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+        roscpp
+        roslib
+        ccf
+        )
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## 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
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES ccf_immersive_sorting
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+        include
+        ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/ccf_immersive_sorting.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})
+
+## 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_selection_provider src/dummy_selection_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
+add_executable(${PROJECT_NAME}_dummy_sorting_controller src/dummy_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} )
+add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.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_selection_provider PROPERTIES OUTPUT_NAME dummy_selection_provider PREFIX "")
+set_target_properties(${PROJECT_NAME}_dummy_sorting_controller PROPERTIES OUTPUT_NAME dummy_sorting_controller PREFIX "")
+set_target_properties(${PROJECT_NAME}_moveit_sorting_controller PROPERTIES OUTPUT_NAME moveit_sorting_controller PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(${PROJECT_NAME}_dummy_selection_provider ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_dummy_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_moveit_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_dummy_sorting_controller
+        ${catkin_LIBRARIES}
+        )
+target_link_libraries(${PROJECT_NAME}_moveit_sorting_controller
+        ${catkin_LIBRARIES}
+        )
+target_link_libraries(${PROJECT_NAME}_dummy_selection_provider
+        ${catkin_LIBRARIES}
+        nng
+        )
+
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_ccf_immersive_sorting.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

LICENSE

+BSD 3-Clause License
+
+Copyright (c) 2020, TU Dresden, Software Technology Group
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

config/config_scene.yaml

+{ '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': 'BIN','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': 'BIN','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': 'BIN','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': 1,'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 } }
+] }

launch/dummy_main.launch

+<launch>
+
+    <arg name="connection_address" default="tcp://*:6576" />
+    <arg name="client_controllers" default="[]" />
+
+    <node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="my_dummy_sorting_controller" output="screen">
+        <param name="connection_address" type="string" value="$(arg connection_address)" />
+        <param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
+        <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
+    </node>
+
+</launch>

launch/dummy_main_dummy_client_dummy_selector.launch

+<launch>
+
+    <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
+    <node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="main_controller" output="screen">
+        <param name="connection_address" type="string" value="tcp://*:6576" />
+        <param name="client_controllers" type="yaml" value="['tcp://127.0.0.1:6586']" />
+        <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
+    </node>
+    <node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="client_controller" output="screen">
+        <param name="connection_address" type="string" value="tcp://*:6586" />
+        <param name="client_controllers" type="yaml" value="[]" />
+        <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
+    </node>
+</launch>

launch/dummy_main_dummy_selector.launch

+<launch>
+    <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
+    <include file="$(find ccf_immersive_sorting)/launch/dummy_main.launch" />
+</launch>

launch/dummy_main_simulated_client.launch

+<launch>
+    <include file="$(find ccf)/launch/simulation_setup.launch"/>
+
+    <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="main_controller" output="screen">
+        <param name="connection_address" type="string" value="tcp://*:6576" />
+        <param name="client_controllers" type="yaml" value="['tcp://127.0.0.1:6586']" />
+        <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
+    </node>
+    <node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="client_controller" output="screen">
+        <param name="connection_address" type="string" value="tcp://*:6586" />
+        <param name="client_controllers" type="yaml" value="[]" />
+        <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
+    </node>
+</launch>

launch/dummy_selector.launch

+<launch>
+    <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
+</launch>

launch/robot_main.launch

+<launch>
+
+    <arg name="robot_ip" default="172.31.1.13" />
+    <arg name="load_gripper" default="true" />
+    <arg name="connection_address" default="tcp://*:6576" />
+    <arg name="client_controllers" default="[]" />
+
+    <include file="$(find ccf)/launch/robot_setup.launch" >
+        <arg name="robot_ip" value="$(arg robot_ip)" />
+        <arg name="load_gripper" value="$(arg load_gripper)"/>
+    </include>
+
+    <node pkg="ccf" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" output="screen" >
+        <param name="connection_address" type="string" value="$(arg connection_address)" />
+        <param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
+    </node>
+
+</launch>
\ No newline at end of file

launch/robot_main_dummy_selector.launch

+<launch>
+    <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
+    <include file="robot_main.launch" />
+</launch>

launch/simulated_main.launch

+<launch>
+
+    <arg name="connection_address" default="tcp://*:6576" />
+    <arg name="client_controllers" default="[]" />
+
+    <include file="$(find ccf)/launch/simulation_setup.launch"/>
+
+    <node pkg="ccf" type="moveit_sorting_controller" name="moveit_sorting_controller_instance" output="screen" >
+        <param name="connection_address" type="string" value="$(arg connection_address)" />
+        <param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
+    </node>
+
+</launch>
\ No newline at end of file

launch/simulated_main_dummy_client.launch

+<launch>
+    <include file="$(find ccf)/launch/simulation_setup.launch"/>
+
+    <node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="main_controller" output="screen">
+        <param name="connection_address" type="string" value="tcp://*:6576" />
+        <param name="client_controllers" type="yaml" value="['tcp://127.0.0.1:6586']" />
+    </node>
+    <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="client_controller" output="screen">
+        <param name="connection_address" type="string" value="tcp://*:6586" />
+        <param name="client_controllers" type="yaml" value="[]" />
+    </node>
+</launch>

launch/simulated_main_dummy_selector.launch

+<launch>
+    <node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
+    <include file="simulated_main.launch" />
+</launch>

package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>ccf_immersive_sorting</name>
+  <version>1.0.0</version>
+  <description>Immersive Sorting Demo using the CeTI Component Framework</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <maintainer email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/ccf_immersive_sorting</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <author email="johannes.mey@tu-dresden.de">Johannes Mey</author>
+  <author email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</author>
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <depend>roscpp</depend>
+  <depend>ccf</depend>
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/dummy_selection_provider.cpp

+/*! \file dummy_selection_provider.cpp
+
+    A ROS node that simulates a selection provider like 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_selection_provider");
+
+    ros::NodeHandle n("ccf");
+
+    if ((rv = nng_pair1_open(&sock)) != 0) {
+        ROS_ERROR_STREAM("[dummy_selection_provider] 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(
+                "[dummy_selection_provider] nng_dial returned: " << nng_strerror(rv)
+                                                                 << ". Trying to connect again in one second...");
+        connection_retry_rate.sleep();
+    }
+    ROS_INFO_STREAM(
+            "[dummy_selection_provider] 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("[dummy_selection_provider] Received a valid scene with " << scene.objects().size()
+                                                                                          << " objects.");
+                ROS_DEBUG_STREAM("[dummy_selection_provider] Content:" << std::endl << s);
+            } else {
+                ROS_WARN_STREAM("[dummy_selection_provider] Received an invalid scene!" << std::endl
+                                                                                        << "[dummy_selection_provider] 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(
+                        "[dummy_selection_provider] 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("[dummy_selection_provider] No objects to select in a scene without boxes!");
+            } else if (bins.empty()) {
+                ROS_INFO("[dummy_selection_provider] 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("[dummy_selection_provider] 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("[dummy_selection_provider] Selecting random bin: " << bin);
+                sendSelection(bin);
+            }
+
+        } else if (rv == NNG_EAGAIN) {
+            // no message received in current spin
+        } else {
+            ROS_ERROR_STREAM("[dummy_selection_provider] nng_recv returned: " << nng_strerror(rv));
+        }
+
+        ros::spinOnce();
+
+        loop_rate.sleep();
+    }
+
+    return 0;
+}
\ No newline at end of file

src/dummy_sorting_controller.cpp

+/*! \file dummy_sorting_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_sorting_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_ccf"); // namespace where the connection_address is
+
+    auto connectionAddress = 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>(connectionAddress);
+    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(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + "/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;
+}
\ No newline at end of file

src/moveit_sorting_controller.cpp

+/*! \file moveit_sorting_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_sorting_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_ccf"); // namespace where the connection_address is
+
+    auto connectionAddress = 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>(connectionAddress);
+    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;
+}
\ No newline at end of file