add new main controller node for demo

CMakeLists.txt

@@ -136,6 +136,7 @@ include_directories(
 ## 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}_dummy_main_controller src/dummy_main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} )
 add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 
 
@@ -145,18 +146,23 @@ add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_cont
 ## 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}_dummy_main_controller PROPERTIES OUTPUT_NAME dummy_main_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}_dummy_main_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}_dummy_main_controller
+        ${catkin_LIBRARIES}
+        )
 target_link_libraries(${PROJECT_NAME}_moveit_sorting_controller
         ${catkin_LIBRARIES}
         )

launch/virtual-table_dummy_main.launch

+<launch>
+
+    <arg name="connection_address" default="tcp://*:6576" />
+    <arg name="client_controllers" default="[]" />
+
+
+    <node pkg="ccf_immersive_sorting" type="dummy_main_controller" name="dummy_main_controller_instance" 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_virtual-table.json" />
+    </node>
+
+</launch>

src/dummy_main_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
+*/
+
+#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;
+
+const float SELECT = 2.0;
+const float UNSELECT = 0.5;
+const float DELETING = 0.08;
+
+void highlight(Object *selectedBin, float factor) {
+    if (selectedBin) {
+        selectedBin->mutable_color()->set_r(selectedBin->color().r()*factor);
+        selectedBin->mutable_color()->set_g(selectedBin->color().g()*factor);
+        selectedBin->mutable_color()->set_b(selectedBin->color().b()*factor);
+    }
+}
+
+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("Connecting to " << clientControllers.size() << " client controllers.");
+    for (const auto &client: clientControllers) {
+        ROS_INFO_STREAM("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("Unable to accept selections while picking is in progress.");
+            return;
+        }
+
+        Object *object = connector.resolveObject(selection.id());
+        if (!object) {
+            ROS_ERROR_STREAM("Selected unknown object '" << selection.id() << "'");
+            return;
+        }
+        auto type = Object::Type_Name(object->type());
+        if (object->type() == Object::BOX) {
+            highlight(selectedBox, UNSELECT);
+            if (selectedBox == object) {
+                selectedBox = nullptr;
+            } else {
+                selectedBox = object;
+                highlight(selectedBox, SELECT);
+            }
+            connector.sendScene();
+            ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
+        } else if (object->type() == Object::BIN) {
+            highlight(selectedBin, UNSELECT);
+            if (selectedBin == object) {
+                selectedBin = nullptr;
+            } else {
+                selectedBin = object;
+                highlight(selectedBin, SELECT);
+            }
+            connector.sendScene();
+            ROS_INFO_STREAM("Selected " << type << " '" << selection.id() << "'");
+        } else {
+            ROS_WARN_STREAM("Selected unsupported " << type << " '" << selection.id() << "'");
+        }
+
+        if (selectedBin && selectedBox) {
+            auto boxId = selectedBox->id();
+            ROS_INFO_STREAM("Got task: Put " << boxId << " into " << selectedBin->id());
+            currentlyPickedBox = boxId;
+            highlight(selectedBin, UNSELECT);
+            highlight(selectedBox, DELETING);
+            connector.sendScene();
+            if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
+                ROS_WARN_STREAM("Unable to remove box '" << boxId << "'!");
+            } else {
+                ROS_INFO_STREAM("Job is done! '" << boxId << "' is no more.");
+            }
+            selectedBox = nullptr;
+            selectedBin = nullptr;
+            connector.sendScene();
+        } else {
+
+        }
+    };
+    connector.reactToSelectionMessage(selectionMessageCallback);
+
+    auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
+        if (currentlyPickedBox.has_value()) {
+            auto resolved = connector.resolveObject(currentlyPickedBox.value());
+            if (!resolved) {
+                ROS_INFO_STREAM("box " << currentlyPickedBox.value() << " has been removed from the scene!");
+                currentlyPickedBox.reset();
+            }
+        }
+    };
+    connector.reactToSceneUpdateMessage(sceneUpdateCallback);
+
+    ros::spin();
+
+    return 0;
+}
\ No newline at end of file