diff --git a/.gitmodules b/.gitmodules
index 4e7a02da204090c9f7537b81efa451157606ad44..684da74d9a99ed673b6e1065dd0c3f867ca10fa9 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,7 +1,3 @@
-[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
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6cd8b5e22797bb46c68543302c4510e31cb494ab..700a16ac506e1d53b388c81b020297d24b33bc2e 100644
--- a/CMakeLists.txt
+++ b/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}
diff --git a/lib/nng b/lib/nng
deleted file mode 160000
index 8d3b53879bdaf1b1d617a40633a66eea9eb345ef..0000000000000000000000000000000000000000
--- a/lib/nng
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 8d3b53879bdaf1b1d617a40633a66eea9eb345ef
diff --git a/package.xml b/package.xml
index 803504abc388934b58d601befb16833e32ff73f7..6a59e59d7bfeed3cb9a20006e8d69441cec10e2a 100644
--- a/package.xml
+++ b/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>
diff --git a/src/ccf/connection/MqttConnection.cpp b/src/connection/MqttConnection.cpp
similarity index 100%
rename from src/ccf/connection/MqttConnection.cpp
rename to src/connection/MqttConnection.cpp
diff --git a/src/ccf/connection/NngConnection.cpp b/src/connection/NngConnection.cpp
similarity index 100%
rename from src/ccf/connection/NngConnection.cpp
rename to src/connection/NngConnection.cpp
diff --git a/src/ccf/controller/Controller.cpp b/src/controller/Controller.cpp
similarity index 100%
rename from src/ccf/controller/Controller.cpp
rename to src/controller/Controller.cpp
diff --git a/src/ccf/controller/DummyRobotArmController.cpp b/src/controller/DummyRobotArmController.cpp
similarity index 100%
rename from src/ccf/controller/DummyRobotArmController.cpp
rename to src/controller/DummyRobotArmController.cpp
diff --git a/src/ccf/controller/MoveItRobotArmController.cpp b/src/controller/MoveItRobotArmController.cpp
similarity index 100%
rename from src/ccf/controller/MoveItRobotArmController.cpp
rename to src/controller/MoveItRobotArmController.cpp
diff --git a/src/ccf/controller/RobotArmController.cpp b/src/controller/RobotArmController.cpp
similarity index 100%
rename from src/ccf/controller/RobotArmController.cpp
rename to src/controller/RobotArmController.cpp
diff --git a/src/dummies/dummy_cgv.cpp b/src/dummies/dummy_cgv.cpp
deleted file mode 100644
index aebccaa5716d23b6b0742d2530d54e2be5d0bdba..0000000000000000000000000000000000000000
--- a/src/dummies/dummy_cgv.cpp
+++ /dev/null
@@ -1,148 +0,0 @@
-/*! \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
diff --git a/src/dummies/dummy_pick_place.cpp b/src/dummies/dummy_pick_place.cpp
deleted file mode 100644
index 02b942451e2c9cd1c940652b047b572dce267187..0000000000000000000000000000000000000000
--- a/src/dummies/dummy_pick_place.cpp
+++ /dev/null
@@ -1,206 +0,0 @@
-/*! \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;
-}
diff --git a/src/dummy_cgv_controller.cpp b/src/dummy_cgv_controller.cpp
deleted file mode 100644
index 8e03d60c03825c6ee3e5b7794ece422641b0ac53..0000000000000000000000000000000000000000
--- a/src/dummy_cgv_controller.cpp
+++ /dev/null
@@ -1,134 +0,0 @@
-/*! \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 = [¤tlyPickedBox, &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 = [¤tlyPickedBox, &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;
-}
diff --git a/src/dummy_rag_controller_a.cpp b/src/dummy_rag_controller_a.cpp
deleted file mode 100644
index 2d44325f68f9b9e3c43e835c208bdd2ded372c37..0000000000000000000000000000000000000000
--- a/src/dummy_rag_controller_a.cpp
+++ /dev/null
@@ -1,128 +0,0 @@
-/*! \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 = [¤tlyPickedBox, &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
diff --git a/src/dummy_rag_controller_b.cpp b/src/dummy_rag_controller_b.cpp
deleted file mode 100644
index 464a69c28919feadc976156c2e40eb1c6e1c7d24..0000000000000000000000000000000000000000
--- a/src/dummy_rag_controller_b.cpp
+++ /dev/null
@@ -1,143 +0,0 @@
-/*! \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;
-}
diff --git a/src/moveit_cgv_controller.cpp b/src/moveit_cgv_controller.cpp
deleted file mode 100644
index c8b548cd4aaaec5042e6aa101f6140bbed225ff8..0000000000000000000000000000000000000000
--- a/src/moveit_cgv_controller.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-/*! \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 = [¤tlyPickedBox, &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 = [¤tlyPickedBox, &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;
-}
diff --git a/src/ccf/util/scene_collision_object.cpp b/src/util/scene_collision_object.cpp
similarity index 100%
rename from src/ccf/util/scene_collision_object.cpp
rename to src/util/scene_collision_object.cpp
diff --git a/src/ccf/util/scene_constructor_util.cpp b/src/util/scene_constructor_util.cpp
similarity index 100%
rename from src/ccf/util/scene_constructor_util.cpp
rename to src/util/scene_constructor_util.cpp
diff --git a/src/ccf/util/scene_controller_node.cpp b/src/util/scene_controller_node.cpp
similarity index 100%
rename from src/ccf/util/scene_controller_node.cpp
rename to src/util/scene_controller_node.cpp