Merge branch 'feature/distributed-sync' into feature/package_split

config/config.yaml

 connector_node_ros_cgv:
-  cgv_address: "tcp://*:6576"
+  topics:
+    selection: "selection"
 max_grasp_approach_velocity: 0.2
 max_grasp_approach_acceleration: 0.2
 max_grasp_transition_velocity: 0.2

include/ccf/connection/Connection.h

@@ -2,8 +2,8 @@
 // Created by Johannes Mey on 28/04/2021.
 //
 
-#ifndef CGV_CONNECTOR_WORKSPACE_CONNECTION_H
-#define CGV_CONNECTOR_WORKSPACE_CONNECTION_H
+#ifndef CCF_CONNECTION_H
+#define CCF_CONNECTION_H
 
 #include <string>
 #include <functional>
@@ -34,4 +34,4 @@ public:
     virtual bool send(const std::string &channel, const std::string &message) = 0;
 };
 
-#endif //CGV_CONNECTOR_WORKSPACE_CONNECTION_H
+#endif //CCF_CONNECTION_H

include/ccf/connection/MqttConnection.h

@@ -2,8 +2,8 @@
 // Created by Sebastian Ebert on 04.05.21.
 //
 
-#ifndef CGV_SIM_MQTTCONNECTION_H
-#define CGV_SIM_MQTTCONNECTION_H
+#ifndef CCF_MQTTCONNECTION_H
+#define CCF_MQTTCONNECTION_H
 
 #include "Connection.h"
 
@@ -40,4 +40,4 @@ private:
     bool ensureConnection();
 };
 
-#endif //CGV_SIM_MQTTCONNECTION_H
\ No newline at end of file
+#endif //CCF_MQTTCONNECTION_H
\ No newline at end of file

include/ccf/connection/NngConnection.h

@@ -2,8 +2,8 @@
 // Created by jm on 28/04/2021.
 //
 
-#ifndef CGV_CONNECTOR_WORKSPACE_NNGCONNECTION_H
-#define CGV_CONNECTOR_WORKSPACE_NNGCONNECTION_H
+#ifndef CCF_NNGCONNECTION_H
+#define CCF_NNGCONNECTION_H
 
 #include "Connection.h"
 
@@ -15,7 +15,8 @@
 /// see https://nng.nanomsg.org/man/v0.2.0/nng_pair.html
 class NngConnection : public Connection {
 
-    const std::string &cgv_address;
+    const std::string &connection_address;
+    const bool server;
     nng_socket sock{};
     std::unique_ptr<std::thread> nng_receiver_thread;
     std::string sendTopic;
@@ -23,8 +24,10 @@ class NngConnection : public Connection {
 public:
 
     /// Constructor requiring an address as defined in https://nng.nanomsg.org/man/v1.3.2/nng.7.html
-    /// \param cgvAddress
-    explicit NngConnection(const std::string &cgvAddress);
+    /// \param connection_address in server mode the port on which the connections listens for connections, in client
+    ///        mode the address of the server
+    /// \param server true, if the connection is a server (listens for connections), otherwise client mode is enabled
+    explicit NngConnection(const std::string &connection_address, bool server = true);
 
     bool send(const std::string &channel, const std::string &message) override;
 
@@ -50,4 +53,4 @@ public:
 };
 
 
-#endif //CGV_CONNECTOR_WORKSPACE_NNGCONNECTION_H
+#endif //CCF_NNGCONNECTION_H

include/ccf/controller/Controller.h

@@ -2,8 +2,8 @@
 // Created by Johannes Mey on 17/01/2021.
 //
 
-#ifndef CGV_CONNECTOR_WORKSPACE_CONTROLLER_H
-#define CGV_CONNECTOR_WORKSPACE_CONTROLLER_H
+#ifndef CCF_CONTROLLER_H
+#define CCF_CONTROLLER_H
 
 #include <functional>
 #include <optional>
@@ -35,4 +35,4 @@ public:
 
 };
 
-#endif //CGV_CONNECTOR_WORKSPACE_CONTROLLER_H
+#endif //CCF_CONTROLLER_H

include/ccf/controller/DummyRobotArmController.h

@@ -2,8 +2,8 @@
 // Created by Johannes Mey on 17/01/2021.
 //
 
-#ifndef CGV_CONNECTOR_WORKSPACE_DUMMYROBOTARMCONTROLLER_H
-#define CGV_CONNECTOR_WORKSPACE_DUMMYROBOTARMCONTROLLER_H
+#ifndef CCF_DUMMYROBOTARMCONTROLLER_H
+#define CCF_DUMMYROBOTARMCONTROLLER_H
 
 #include "RobotArmController.h"
 
@@ -33,4 +33,4 @@ public:
     virtual bool reachableObject(const Object &robot, const Object &object) override;
 };
 
-#endif //CGV_CONNECTOR_WORKSPACE_DUMMYROBOTARMCONTROLLER_H
+#endif //CCF_DUMMYROBOTARMCONTROLLER_H

include/ccf/controller/MoveItRobotArmController.h

@@ -2,8 +2,8 @@
 // Created by Johannes Mey on 17/01/2021.
 //
 
-#ifndef CGV_CONNECTOR_WORKSPACE_MOVEITROBOTARMCONTROLLER_H
-#define CGV_CONNECTOR_WORKSPACE_MOVEITROBOTARMCONTROLLER_H
+#ifndef CCF_MOVEITROBOTARMCONTROLLER_H
+#define CCF_MOVEITROBOTARMCONTROLLER_H
 
 #include <ccf/SceneUpdate.h>
 
@@ -35,4 +35,4 @@ public:
 };
 
 
-#endif //CGV_CONNECTOR_WORKSPACE_MOVEITROBOTARMCONTROLLER_H
+#endif //CCF_MOVEITROBOTARMCONTROLLER_H

include/ccf/controller/RobotArmController.h

@@ -2,8 +2,8 @@
 // Created by Johannes Mey on 17/01/2021.
 //
 
-#ifndef CGV_CONNECTOR_WORKSPACE_ROBOTARMCONTROLLER_H
-#define CGV_CONNECTOR_WORKSPACE_ROBOTARMCONTROLLER_H
+#ifndef CCF_ROBOTARMCONTROLLER_H
+#define CCF_ROBOTARMCONTROLLER_H
 
 #include <functional>
 #include <optional>
@@ -95,4 +95,4 @@ public:
     Object *resolveObject(const std::string &id);
 };
 
-#endif //CGV_CONNECTOR_WORKSPACE_ROBOTARMCONTROLLER_H
+#endif //CCF_ROBOTARMCONTROLLER_H

include/ccf/util/NodeUtil.h

@@ -44,6 +44,34 @@ namespace CetiRosToolbox {
         return fallback;
     }
 
+    /// Gets a private parameter from the parameter server. If the parameter does not exist, the fallback is used.
+    /// If no fallback value is specified, an empty string is returned.
+    ///
+    /// If the parameter is \a not on the parameter server, a warning is issued at \c ROS_WARN.
+    ///
+    /// Usually, when using the method, the template parameter can be omitted if a fallback value is provided. This does
+    /// \a not work when providing \c const \c char* as fallback (i.e., string constants). In this case, the template
+    /// parameter must be provided:
+    /// \code{.cpp}
+    /// auto intValue1 = getParameter<int>(n, "foo"); // no fallback means no automatic matching
+    /// auto intValue2 = getParameter(n, "foo", 42); // the compiler matches T with int, this is okay
+    /// auto stringValue = getParameter<std::string>(n, "bar", "fourtytwo"); // const char* requires the std::string template parameter
+    /// \endcode
+    /// \tparam T a type supported by the parameter server.
+    /// The parameter server supports \c std::string double \c float \c int \c bool
+    /// and \c std::vector and \std::map of these types.
+    /// Additionally, also \c XmlRpc::XmlRpcValue is supported, though not as a vector or map.
+    /// \param key the name of the parameter
+    /// \param fallback the default value
+    /// \return the parameter (if it exists), otherwise the fallback value (if it exists), otherwise the default
+    ///         instance of the respective object
+    /// \warning Currently, this is a header-only library, but this might change in the future
+    template<class T>
+    T getPrivateParameter(const std::string &key, T fallback = T()) {
+        ros::NodeHandle n("~"); // a private node handle
+        return getParameter(n, key, fallback);
+    }
+
     /** @cond */ // the specializations are required to print the warning correctly
     template<>
     XmlRpc::XmlRpcValue getParameter(const ros::NodeHandle &n, const std::string &key, XmlRpc::XmlRpcValue fallback) {

include/ccf/util/scene_collision_object.h

@@ -2,8 +2,8 @@
 // Created by sebastian on 14.01.21.
 //
 
-#ifndef CGV_CONNECTOR_SCENECOLLISIONOBJECT_H
-#define CGV_CONNECTOR_SCENECOLLISIONOBJECT_H
+#ifndef CCF_SCENECOLLISIONOBJECT_H
+#define CCF_SCENECOLLISIONOBJECT_H
 
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
 #include <moveit/move_group_interface/move_group_interface.h>
@@ -37,4 +37,4 @@ private:
 };
 
 
-#endif //CGV_CONNECTOR_SCENECOLLISIONOBJECT_H
+#endif //CCF_SCENECOLLISIONOBJECT_H

include/ccf/util/scene_constructor_util.h

@@ -2,8 +2,8 @@
 // Created by Sebastian Ebert on 03.08.20.
 //
 
-#ifndef CGV_CONNECTOR_SCENE_CONSTRUCTOR_H
-#define CGV_CONNECTOR_SCENE_CONSTRUCTOR_H
+#ifndef CCF_SCENE_CONSTRUCTOR_H
+#define CCF_SCENE_CONSTRUCTOR_H
 
 
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
@@ -55,4 +55,4 @@ public:
                                  std::vector<std_msgs::ColorRGBA> &collision_objects_colors, const Scene &scene);
 };
 
-#endif //CGV_CONNECTOR_SCENE_CONSTRUCTOR_H
\ No newline at end of file
+#endif //CCF_SCENE_CONSTRUCTOR_H
\ No newline at end of file

launch/cleaning/cleaning_dummy_controller.launch

 <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>

launch/cleaning/cleaning_dummy_controller_and_dummy_cgv.launch

 <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>

launch/cleaning/cleaning_dummy_main_dummy_client_dummy_cgv.launch

+<launch>
+    <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />
+    <node pkg="ccf" type="dummy_cgv_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" type="dummy_cgv_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/cleaning/cleaning_dummy_main_simulated_client.launch

+<launch>
+    <include file="$(find ccf)/launch/cleaning/_simulation_setup.launch"/>
+
+    <!--<node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />-->
+    <node pkg="ccf" type="moveit_cgv_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" type="dummy_cgv_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/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

launch/cleaning/cleaning_robot_controller_and_dummy_cgv.launch

 <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" />
-
+    <include file="$(find ccf)/launch/cleaning/cleaning_robot_controller.launch" />
 </launch>

launch/cleaning/cleaning_simulated_main_dummy_client.launch

+<launch>
+    <include file="$(find ccf)/launch/cleaning/_simulation_setup.launch"/>
+
+    <!--<node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />-->
+    <node pkg="ccf" type="dummy_cgv_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" type="moveit_cgv_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/cleaning/cleaning_simulation.launch

 <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

launch/cleaning/cleaning_simulation_and_dummy_cgv.launch

 <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" />
-
+    <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />
+    <include file="$(find ccf)/launch/cleaning/cleaning_simulation.launch" />
 </launch>