From 064b11e32475d1b8c5d51d4a3010c09e0fbe0707 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Tue, 31 Aug 2021 10:50:22 +0200
Subject: [PATCH] add dependent controllers that get the forwarded selection of
 a main controller

---
 config/config.yaml                            |  2 --
 ...ble__dummy_controller_and_dummy_cgv.launch | 11 ++++++++++
 .../cleaning/cleaning_dummy_controller.launch | 10 ++++++++-
 ...ning_dummy_controller_and_dummy_cgv.launch |  2 +-
 .../cleaning/cleaning_robot_controller.launch |  7 +++++-
 ...ning_robot_controller_and_dummy_cgv.launch | 14 ++----------
 launch/cleaning/cleaning_simulation.launch    |  8 ++++++-
 .../cleaning_simulation_and_dummy_cgv.launch  | 10 +++------
 src/ccf/controller/RobotArmController.cpp     |  2 --
 src/dummy_cgv_controller.cpp                  | 22 ++++++++++++++++---
 src/dummy_rag_controller_a.cpp                |  3 ++-
 src/dummy_rag_controller_b.cpp                |  3 ++-
 src/moveit_cgv_controller.cpp                 | 22 ++++++++++++++++---
 13 files changed, 81 insertions(+), 35 deletions(-)
 create mode 100644 launch/cleaning/cleaning_double__dummy_controller_and_dummy_cgv.launch

diff --git a/config/config.yaml b/config/config.yaml
index 97af7cc..22d7c81 100644
--- a/config/config.yaml
+++ b/config/config.yaml
@@ -1,5 +1,3 @@
-connector_node_ros_cgv:
-  cgv_address: "tcp://*:6576"
 max_grasp_approach_velocity: 0.2
 max_grasp_approach_acceleration: 0.2
 max_grasp_transition_velocity: 0.2
diff --git a/launch/cleaning/cleaning_double__dummy_controller_and_dummy_cgv.launch b/launch/cleaning/cleaning_double__dummy_controller_and_dummy_cgv.launch
new file mode 100644
index 0000000..757a39d
--- /dev/null
+++ b/launch/cleaning/cleaning_double__dummy_controller_and_dummy_cgv.launch
@@ -0,0 +1,11 @@
+<launch>
+    <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />
+    <node pkg="ccf" type="dummy_cgv_controller" name="my_first_dummy_cgv_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" type="dummy_cgv_controller" name="my_second_dummy_cgv_controller" output="screen">
+        <param name="connection_address" type="string" value="tcp://*:6586" />
+        <param name="client_controllers" type="yaml" value="[]" />
+    </node>
+</launch>
diff --git a/launch/cleaning/cleaning_dummy_controller.launch b/launch/cleaning/cleaning_dummy_controller.launch
index 3b28246..7ce2c9c 100644
--- a/launch/cleaning/cleaning_dummy_controller.launch
+++ b/launch/cleaning/cleaning_dummy_controller.launch
@@ -1,3 +1,11 @@
 <launch>
-    <node pkg="ccf" type="dummy_cgv_controller" name="my_dummy_cgv_controller" output="screen" />
+
+    <arg name="connection_address" default="tcp://*:6576" />
+    <arg name="client_controllers" default="[]" />
+
+    <node pkg="ccf" type="dummy_cgv_controller" name="my_dummy_cgv_controller" output="screen">
+        <param name="connection_address" type="string" value="$(arg connection_address)" />
+        <param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
+    </node>
+
 </launch>
diff --git a/launch/cleaning/cleaning_dummy_controller_and_dummy_cgv.launch b/launch/cleaning/cleaning_dummy_controller_and_dummy_cgv.launch
index ebb269c..0caf484 100644
--- a/launch/cleaning/cleaning_dummy_controller_and_dummy_cgv.launch
+++ b/launch/cleaning/cleaning_dummy_controller_and_dummy_cgv.launch
@@ -1,4 +1,4 @@
 <launch>
     <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />
-    <node pkg="ccf" type="dummy_cgv_controller" name="my_dummy_cgv_controller" output="screen" />
+    <include file="$(find ccf)/launch/cleaning/cleaning_dummy_controller.launch" />
 </launch>
diff --git a/launch/cleaning/cleaning_robot_controller.launch b/launch/cleaning/cleaning_robot_controller.launch
index a44470f..206780e 100644
--- a/launch/cleaning/cleaning_robot_controller.launch
+++ b/launch/cleaning/cleaning_robot_controller.launch
@@ -2,12 +2,17 @@
 
     <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/cleaning/_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_cgv_controller" name="moveit_cgv_controller_instance" output="screen" />
+    <node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_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
diff --git a/launch/cleaning/cleaning_robot_controller_and_dummy_cgv.launch b/launch/cleaning/cleaning_robot_controller_and_dummy_cgv.launch
index d5e84b2..190cb85 100644
--- a/launch/cleaning/cleaning_robot_controller_and_dummy_cgv.launch
+++ b/launch/cleaning/cleaning_robot_controller_and_dummy_cgv.launch
@@ -1,14 +1,4 @@
 <launch>
-
-    <arg name="robot_ip" default="172.31.1.13" />
-    <arg name="load_gripper" default="true" />
-
-    <include file="$(find ccf)/launch/cleaning/_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_cgv_controller" name="moveit_cgv_controller_instance" output="screen" />
     <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />
-
-</launch>
\ No newline at end of file
+    <include file="$(find ccf)/launch/cleaning/cleaning_robot_controller.launch" />
+</launch>
diff --git a/launch/cleaning/cleaning_simulation.launch b/launch/cleaning/cleaning_simulation.launch
index 60f3f7c..f9b5a6c 100644
--- a/launch/cleaning/cleaning_simulation.launch
+++ b/launch/cleaning/cleaning_simulation.launch
@@ -1,7 +1,13 @@
 <launch>
 
+    <arg name="connection_address" default="tcp://*:6576" />
+    <arg name="client_controllers" default="[]" />
+
     <include file="$(find ccf)/launch/cleaning/_simulation_setup.launch"/>
 
-    <node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_controller_instance" output="screen" />
+    <node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_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
diff --git a/launch/cleaning/cleaning_simulation_and_dummy_cgv.launch b/launch/cleaning/cleaning_simulation_and_dummy_cgv.launch
index 2188048..96d7379 100644
--- a/launch/cleaning/cleaning_simulation_and_dummy_cgv.launch
+++ b/launch/cleaning/cleaning_simulation_and_dummy_cgv.launch
@@ -1,8 +1,4 @@
 <launch>
-
-    <include file="$(find ccf)/launch/cleaning/_simulation_setup.launch"/>
-
-    <node pkg="ccf" type="dummy_cgv" name="dummy_cgv_instance" output="screen" />
-    <node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_controller_instance" output="screen" />
-
-</launch>
\ No newline at end of file
+    <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />
+    <include file="$(find ccf)/launch/cleaning/cleaning_simulation.launch" />
+</launch>
diff --git a/src/ccf/controller/RobotArmController.cpp b/src/ccf/controller/RobotArmController.cpp
index 9ea39c5..9b39591 100644
--- a/src/ccf/controller/RobotArmController.cpp
+++ b/src/ccf/controller/RobotArmController.cpp
@@ -44,8 +44,6 @@ Object *RobotArmController::resolveObject(const std::string &id) {
 void RobotArmController::sendScene() {
     if (scene) { // meaning if the (smart) pointer is not a nullptr
 
-        scene->SerializeAsString();
-
         ROS_INFO_STREAM("[ros2cgv] Sending scene with " << scene->objects().size() << " objects.");
         sendToAll(sendSceneTopic, scene->SerializeAsString());
         sceneUpdateAction();
diff --git a/src/dummy_cgv_controller.cpp b/src/dummy_cgv_controller.cpp
index 191078a..5714c2f 100644
--- a/src/dummy_cgv_controller.cpp
+++ b/src/dummy_cgv_controller.cpp
@@ -19,19 +19,21 @@
 #include "ccf/connection/NngConnection.h"
 #include "ccf/util/NodeUtil.h"
 
-const std::string NODE_NAME = "ros2cgv_dummy";
+std::string NODE_NAME;
 
 using CetiRosToolbox::getParameter;
+using CetiRosToolbox::getPrivateParameter;
 
 int main(int argc, char **argv) {
 
     GOOGLE_PROTOBUF_VERIFY_VERSION;
 
+    NODE_NAME = ros::this_node::getName();
     ros::init(argc, argv, NODE_NAME);
 
     ros::NodeHandle n("connector_node_ros_cgv"); // namespace where the connection_address is
 
-    std::string cgv_address = getParameter<std::string>(n, "connection_address", "tcp://*:6576");
+    auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
 
     DummyRobotArmController connector{n, NODE_NAME};
 
@@ -41,6 +43,16 @@ int main(int argc, char **argv) {
     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"));
+        connector.addConnection(std::move(client_connection));
+    }
+
     connector.loadScene(ros::package::getPath("ccf") + "/config/config_scene.yaml");
 
     Object *robot = nullptr;
@@ -53,9 +65,13 @@ int main(int argc, char **argv) {
     ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
             const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
 
-    auto selectionMessageCallback = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin](
+    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;
diff --git a/src/dummy_rag_controller_a.cpp b/src/dummy_rag_controller_a.cpp
index f1ee30c..2d44325 100644
--- a/src/dummy_rag_controller_a.cpp
+++ b/src/dummy_rag_controller_a.cpp
@@ -28,6 +28,7 @@ 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) {
 
@@ -37,7 +38,7 @@ int main(int argc, char **argv) {
 
     ros::NodeHandle n("ceti_connector"); // namespace
 
-    std::string cgv_address = getParameter<std::string>(n, "cgv_address", "tcp://*:6576");
+    auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
 
     DummyRobotArmController connector{n, CELL_NAME};
 
diff --git a/src/dummy_rag_controller_b.cpp b/src/dummy_rag_controller_b.cpp
index 0004ef2..464a69c 100644
--- a/src/dummy_rag_controller_b.cpp
+++ b/src/dummy_rag_controller_b.cpp
@@ -27,6 +27,7 @@ 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) {
 
@@ -36,7 +37,7 @@ int main(int argc, char **argv) {
 
     ros::NodeHandle n("ceti_connector"); // namespace
 
-    std::string cgv_address = getParameter<std::string>(n, "cgv_address", "tcp://*:6576");
+    auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
 
     std::string cell_prefix = getParameter(n, "place_b_prefix", CELL_NAME);
 
diff --git a/src/moveit_cgv_controller.cpp b/src/moveit_cgv_controller.cpp
index 58cb659..d38f637 100644
--- a/src/moveit_cgv_controller.cpp
+++ b/src/moveit_cgv_controller.cpp
@@ -18,19 +18,21 @@
 #include "ccf/connection/NngConnection.h"
 #include "ccf/util/NodeUtil.h"
 
-const std::string NODE_NAME = "ros2cgv_moveit";
+std::string NODE_NAME;
 
 using CetiRosToolbox::getParameter;
+using CetiRosToolbox::getPrivateParameter;
 
 int main(int argc, char **argv) {
 
     GOOGLE_PROTOBUF_VERIFY_VERSION;
 
+    NODE_NAME = ros::this_node::getName();
     ros::init(argc, argv, NODE_NAME);
 
     ros::NodeHandle n("connector_node_ros_cgv"); // namespace where the connection_address is
 
-    std::string cgv_address = getParameter<std::string>(n, "connection_address", "tcp://*:6576");
+    auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
 
     MoveItRobotArmController connector{n, NODE_NAME};
 
@@ -40,6 +42,16 @@ int main(int argc, char **argv) {
     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"));
+        connector.addConnection(std::move(client_connection));
+    }
+
     // scene loading is not required, the scene is updated by moveit
 
     Object *robot = nullptr;
@@ -52,9 +64,13 @@ int main(int argc, char **argv) {
     ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
             const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
 
-    auto selectionMessageCallback = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin](
+    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;
-- 
GitLab