Merge branch 'feature/generic_connector' into merge/update

.gitignore

 .idea
 cmake-build-*
+doc

CMakeLists.txt

@@ -179,6 +179,12 @@ add_library(${PROJECT_NAME}_scene_collision_object src/ccf/util/scene_collision_
 ## 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}_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
@@ -206,6 +212,13 @@ set_target_properties(${PROJECT_NAME}_scene_controller PROPERTIES OUTPUT_NAME sc
 ## 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
 
@@ -322,10 +335,10 @@ target_link_libraries(${PROJECT_NAME}_scene_controller
 #############
 
 ## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_ccf.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+catkin_add_gtest(${PROJECT_NAME}-NodeUtil-test test/test_NodeUtil.cpp)
+if(TARGET ${PROJECT_NAME}-NodeUtil-test)
+  target_link_libraries(${PROJECT_NAME}-NodeUtil-test ${catkin_LIBRARIES} )
+endif()
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)

README.md

@@ -20,15 +20,28 @@ for internal and external testing.
   ```
   $ source devel/setup.bash
   ```
-- Get the path to this package
+- Generate the documentation
   ```
-  $ rospack find ccf
+  $ rosdoc_lite `rospack find ccf`
   ```
-- Generate the documentation
+- You can view the generated files in you browser by opening the generated `index.html`
   ```
-  $ rosdoc_lite <PATH_TO_PACKAGE>
+  xdg-open `rospack find ccf`/doc/html/index.html
+  ```
+## Running the Tests
+
+1. Build the package (this is not done automatically by the following commands)
+  ```bash
+  catkin build
+  ```
+2. Build the tests
+   ```bash
+   catkin build --make-args tests
+   ```
+3. Run the tests
+   ```bash
+   rostest -t ccf test_NodeUtil.test
    ```
-- You can view the generated files in you browser by opening the generated index.html via CLion in your browser.  
 
 ## CGV Dummy Controller Demonstrator
 
@@ -71,25 +84,25 @@ This demonstrator contains two mock-up nodes that can serve as stand-ins for fut
 
 #### Configuration
 
-- configure the collision objects:
-  ```
-  rostopic pub -1 /scene_config/collision_objects std_msgs/String -f `rospack find ccf`/config_objects.yaml
+- configure the scene (*not* required when using the dummy controller):
   ```
-- configure the bin:
+  rostopic pub -1 /scene_config std_msgs/String -f `rospack find ccf`/config_full.yaml
   ```
-  rostopic pub -1 /scene_config/collision_bin std_msgs/String -f `rospack find ccf`/config_bin.yaml
-  ```
-- configure the table: 
-  ```
-  rostopic pub -1 /scene_config/collision_table std_msgs/String -f `rospack find ccf`/config_table.yaml
-  ```
-  A parameter <safety_mode> has to be set in the configuration file to "base" or "safe", where the safe modes a safety-space around the table
 
 #### Execution
 
 - pick an object by id: `rostopic pub -1 /robot/pick std_msgs/String "objectRed1"`
 - place a picked object into a bin by id: `rostopic pub -1 /robot/place std_msgs/String "binRed"`
 
+### Connect to the CGV framework
+
+- **CASE 1** The robot controller is *not* accepting connections, but the AR server is.
+  - ensure a SSH server is running on the AR server
+  -  On the robot controller, connect to it
+     ```
+     ssh stadmin@141.76.65.71 -R 6576:localhost:6576
+     ```
+
 ### Using the Example with Docker
 
 The following instructions can be used to run either the cgv dummy or the ros-connector dummy from within a docker container.

include/ccf/util/NodeUtil.h

@@ -2,20 +2,40 @@
 // Created by jm on 07/05/2021.
 //
 
-#ifndef CGV_CONNECTOR_WORKSPACE_NODEUTIL_H
-#define CGV_CONNECTOR_WORKSPACE_NODEUTIL_H
+#ifndef CCF_NODEUTIL_H
+#define CCF_NODEUTIL_H
 
 #include <ros/ros.h>
 
+#include <sstream>
+
 namespace CetiRosToolbox {
 
     /// Gets a 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 n the NodeHandle, in the namespace of which the parameter is queried
     /// \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 an empty string
-    std::string getParameter(const ros::NodeHandle &n, const std::string &key, std::string fallback = "") {
+    /// \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 getParameter(const ros::NodeHandle &n, const std::string &key, T fallback = T()) {
         if (!n.getParam(key, fallback)) {
             ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
                                 << n.getNamespace() << "/" << key
@@ -23,6 +43,110 @@ namespace CetiRosToolbox {
         }
         return 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) {
+        if (!n.getParam(key, fallback)) {
+            ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
+                                << n.getNamespace() << "/" << key
+                                << " from param server, using default '" << fallback.toXml() << "'.");
+        }
+        return fallback;
+    }
+    template<>
+    std::vector<std::string> getParameter(const ros::NodeHandle &n, const std::string &key, std::vector<std::string> fallback) {
+        if (!n.getParam(key, fallback)) {
+            std::ostringstream vector;
+            std::copy(fallback.begin(), fallback.end(), std::ostream_iterator<std::string>(vector, " "));
+            ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
+                                << n.getNamespace() << "/" << key
+                                                           << " from param server, using default [ " << vector.str() << "].");
+        }
+        return fallback;
+    }
+    template<>
+    std::vector<double> getParameter(const ros::NodeHandle &n, const std::string &key, std::vector<double> fallback) {
+        if (!n.getParam(key, fallback)) {
+            ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
+                                << n.getNamespace() << "/" << key
+                                << " from param server, using default empty vector.");
+        }
+        return fallback;
+    }
+    template<>
+    std::vector<float> getParameter(const ros::NodeHandle &n, const std::string &key, std::vector<float> fallback) {
+        if (!n.getParam(key, fallback)) {
+            ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
+                                << n.getNamespace() << "/" << key
+                                << " from param server, using default empty vector.");
+        }
+        return fallback;
+    }
+    template<>
+    std::vector<int> getParameter(const ros::NodeHandle &n, const std::string &key, std::vector<int> fallback) {
+        if (!n.getParam(key, fallback)) {
+            ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
+                                << n.getNamespace() << "/" << key
+                                << " from param server, using default empty vector.");
+        }
+        return fallback;
+    }
+    template<>
+    std::vector<bool> getParameter(const ros::NodeHandle &n, const std::string &key, std::vector<bool> fallback) {
+        if (!n.getParam(key, fallback)) {
+            ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
+                                << n.getNamespace() << "/" << key
+                                << " from param server, using default empty vector.");
+        }
+        return fallback;
+    }
+    template<>
+    std::map<std::string, std::string> getParameter(const ros::NodeHandle &n, const std::string &key, std::map<std::string, std::string> fallback) {
+        if (!n.getParam(key, fallback)) {
+            ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
+                                << n.getNamespace() << "/" << key
+                                << " from param server, using default empty map.");
+        }
+        return fallback;
+    }
+    template<>
+    std::map<std::string, double> getParameter(const ros::NodeHandle &n, const std::string &key, std::map<std::string, double> fallback) {
+        if (!n.getParam(key, fallback)) {
+            ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
+                                << n.getNamespace() << "/" << key
+                                << " from param server, using default empty map.");
+        }
+        return fallback;
+    }
+    template<>
+    std::map<std::string, float> getParameter(const ros::NodeHandle &n, const std::string &key, std::map<std::string, float> fallback) {
+        if (!n.getParam(key, fallback)) {
+            ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
+                                << n.getNamespace() << "/" << key
+                                << " from param server, using default empty map.");
+        }
+        return fallback;
+    }
+    template<>
+    std::map<std::string, int> getParameter(const ros::NodeHandle &n, const std::string &key, std::map<std::string, int> fallback) {
+        if (!n.getParam(key, fallback)) {
+            ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
+                                << n.getNamespace() << "/" << key
+                                << " from param server, using default empty map.");
+        }
+        return fallback;
+    }
+    template<>
+    std::map<std::string, bool> getParameter(const ros::NodeHandle &n, const std::string &key, std::map<std::string, bool> fallback) {
+        if (!n.getParam(key, fallback)) {
+            ROS_WARN_STREAM("[" << ros::this_node::getName() << "] Could not get string value for "
+                                << n.getNamespace() << "/" << key
+                                << " from param server, using default empty map.");
+        }
+        return fallback;
+    }
+    /** @endcond */
 }
 
-#endif //CGV_CONNECTOR_WORKSPACE_NODEUTIL_H
+#endif //CCF_NODEUTIL_H

src/ccf/controller/RobotArmController.cpp

@@ -93,7 +93,7 @@ RobotArmController::RobotArmController(const ros::NodeHandle &nodeHandle, const
           selectionAction([](const Selection &s) {}),
           cellName(cellName) {
 
-    selectionTopic = getParameter(nodeHandle, "topics/selection","selection");
+    selectionTopic = getParameter<std::string>(nodeHandle, "topics/selection","selection");
     initSceneTopic = getParameter(nodeHandle, "topics/initScene",cellName + "/scene/init");
     sendSceneTopic = getParameter(nodeHandle, "topics/sendScene",cellName + "/scene/update");
     commandTopic = getParameter(nodeHandle, "topics/command",cellName + "/command");

src/dummy_cgv_controller.cpp

@@ -41,7 +41,7 @@ int main(int argc, char **argv) {
 
     // add an NNG connection
     std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address);
-    connection->setReceiveTopic(getParameter(n, "topics/selection", "selection"));
+    connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
     connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
     connector.addConnection(std::move(connection));
 

src/dummy_rag_controller_a.cpp

@@ -37,7 +37,7 @@ int main(int argc, char **argv) {
 
     ros::NodeHandle n("ceti_connector"); // namespace
 
-    std::string cgv_address = getParameter(n, "cgv_address", "tcp://*:6576");
+    std::string cgv_address = getParameter<std::string>(n, "cgv_address", "tcp://*:6576");
 
     DummyRobotArmController connector{n, CELL_NAME};
 
@@ -46,12 +46,12 @@ int main(int argc, char **argv) {
 
     // add an NNG connection
     std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address);
-    connection->setReceiveTopic(getParameter(n, "topics/selection", "selection"));
+    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(n, "mqtt/mqtt_address", "tcp://localhost:1883");
+    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");

src/dummy_rag_controller_b.cpp

@@ -36,7 +36,7 @@ int main(int argc, char **argv) {
 
     ros::NodeHandle n("ceti_connector"); // namespace
 
-    std::string cgv_address = getParameter(n, "cgv_address", "tcp://*:6576");
+    std::string cgv_address = getParameter<std::string>(n, "cgv_address", "tcp://*:6576");
 
     std::string cell_prefix = getParameter(n, "place_b_prefix", CELL_NAME);
 
@@ -47,7 +47,7 @@ int main(int argc, char **argv) {
     // 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(n, "mqtt/mqtt_address", "tcp://localhost:1883");
+    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());

src/moveit_cgv_controller.cpp

@@ -40,7 +40,7 @@ int main(int argc, char **argv) {
 
     // add an NNG connection
     std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(cgv_address);
-    connection->setReceiveTopic(getParameter(n, "topics/selection", "selection"));
+    connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
     connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
     connector.addConnection(std::move(connection));
 

test/test_NodeUtil.cpp

+#include <ros/ros.h>
+
+#include <string>
+#include <map>
+#include<vector>
+
+#include "ccf/util/NodeUtil.h"
+
+#include <gtest/gtest.h>
+
+using CetiRosToolbox::getParameter;
+
+class NodeUtilTest : public ::testing::Test {
+protected:
+
+    ros::NodeHandle n;
+
+    void SetUp() override {
+        /**
+         * Register a "node_util_test" node
+         */
+        n = ros::NodeHandle("node_util_test");
+
+        n.setParam("stringKey", std::string("stringValue"));
+        n.setParam("intKey", 42);
+        n.setParam("doubleKey", 0.42);
+        n.setParam("boolKey", true);
+        n.setParam("xmlKey", XmlRpc::XmlRpcValue(42));
+        n.setParam("stringListKey", std::vector<std::string>{"a", "b", "c"});
+        n.setParam("boolListKey", std::vector<bool>{true, false, true});
+        n.setParam("intListKey", std::vector<int>{1, 2, 4});
+        n.setParam("floatListKey", std::vector<float>{0.0f, 1.0f, 0.42f});
+        n.setParam("doubleListKey", std::vector<double>{0.5, -2.0});
+        std::map<std::string, std::string> stringMap;
+        stringMap["key1"] = "value1";
+        n.setParam("stringMapKey", stringMap);
+        std::map<std::string, int> intMap;
+        intMap["key1"] = 42;
+        n.setParam("intMapKey", intMap);
+    }
+};
+
+TEST_F(NodeUtilTest, ReadNonExistingString) {
+    std::string param = "initialValue";
+    EXPECT_FALSE(n.getParam("key", param));
+    EXPECT_EQ(param, "initialValue");
+    auto result = getParameter<std::string>(n, "key");
+    ASSERT_EQ(result, "");
+}
+
+TEST_F(NodeUtilTest, ReadNonExistingStringWithFallback) {
+    std::string param = "initialValue";
+    EXPECT_FALSE(n.getParam("key", param));
+    EXPECT_EQ(param, "initialValue");
+    auto result = getParameter<std::string>(n, "key", "fallBackParam");
+    ASSERT_EQ(result, "fallBackParam");
+}
+
+TEST_F(NodeUtilTest, ReadExistingSimpleDataTypes) {
+    {
+        auto stringValue = getParameter<std::string>(n, "stringKey");
+        ASSERT_EQ(stringValue, std::string("stringValue"));
+    }
+    {
+        auto intValue = getParameter<int>(n, "intKey");
+        ASSERT_EQ(intValue, 42);
+    }
+    {
+        auto doubleValue = getParameter<double>(n, "doubleKey");
+        ASSERT_EQ(doubleValue, 0.42);
+    }
+    {
+        auto boolValue = getParameter<bool>(n, "boolKey");
+        ASSERT_EQ(boolValue, true);
+    }
+    {
+        auto xmlValue = getParameter<XmlRpc::XmlRpcValue>(n, "xmlKey");
+        // XmlRpc prints the synonymous types 'int' and 'i4' both as 'i4'
+        ASSERT_EQ(xmlValue.toXml(), "<value><i4>42</i4></value>");
+    }
+}
+
+TEST_F(NodeUtilTest, ReadExistingVectorDataTypes) {
+    {
+        auto stringListValue = getParameter<std::vector<std::string>>(n, "stringListKey");
+        std::vector<std::string> expectedStringListValue{"a", "b", "c"};
+        ASSERT_EQ(stringListValue, expectedStringListValue);
+    }
+    {
+        auto boolListValue = getParameter<std::vector<bool>>(n, "boolListKey");
+        std::vector<bool> expectedBoolListValue{true, false, true};
+        ASSERT_EQ(boolListValue, expectedBoolListValue);
+    }
+    {
+        auto intListValue = getParameter<std::vector<int>>(n, "intListKey");
+        std::vector<int> expectedIntListValue{1, 2, 4};
+        ASSERT_EQ(intListValue, expectedIntListValue);
+    }
+    {
+        auto floatListValue = getParameter<std::vector<float>>(n, "floatListKey");
+        std::vector<float> expectedFloatListValue{0.0f, 1.0f, 0.42f};
+        ASSERT_EQ(floatListValue, expectedFloatListValue);
+    }
+    {
+        auto doubleListValue = getParameter<std::vector<double>>(n, "doubleListKey");
+        std::vector<double> expectedDoubleListValue{0.5, -2.0};
+        ASSERT_EQ(doubleListValue, expectedDoubleListValue);
+    }
+}
+
+TEST_F(NodeUtilTest, ReadExistingMapDataTypes) {
+    {
+        auto stringMapValue = getParameter<std::map<std::string, std::string>>(n, "stringMapKey");
+        ASSERT_EQ(stringMapValue["key1"], "value1");
+    }
+    {
+        auto intMapValue = getParameter<std::map<std::string, int>>(n, "intMapKey");
+        ASSERT_EQ(intMapValue["key1"], 42);
+    }
+}
+
+TEST_F(NodeUtilTest, ReadExistingSimpleDataTypesWithFallback) {
+    {
+        auto stringValue = getParameter<std::string>(n, "stringKey", "fallback");
+        ASSERT_EQ(stringValue, std::string("stringValue"));
+    }
+    {
+        auto intValue = getParameter<int>(n, "intKey", -1);
+        ASSERT_EQ(intValue, 42);
+        intValue = getParameter(n, "intKey", -1);
+        ASSERT_EQ(intValue, 42);
+    }
+    {
+        auto doubleValue = getParameter<double>(n, "doubleKey", -1.0);
+        ASSERT_EQ(doubleValue, 0.42);
+        doubleValue = getParameter(n, "doubleKey", -1.0);
+        ASSERT_EQ(doubleValue, 0.42);
+    }
+    {
+        auto boolValue = getParameter<bool>(n, "boolKey", false);
+        ASSERT_EQ(boolValue, true);
+        boolValue = getParameter(n, "boolKey", false);
+        ASSERT_EQ(boolValue, true);
+    }
+    {
+        auto xmlValue = getParameter<XmlRpc::XmlRpcValue>(n, "xmlKey", XmlRpc::XmlRpcValue(23));
+        ASSERT_EQ(xmlValue.toXml(), "<value><i4>42</i4></value>");
+        xmlValue = getParameter(n, "xmlKey", XmlRpc::XmlRpcValue(23));
+        ASSERT_EQ(xmlValue.toXml(), "<value><i4>42</i4></value>");
+    }
+}
+
+TEST_F(NodeUtilTest, ReadExistingVectorDataTypesWithFallback) {
+    {
+        std::vector<std::string> expectedStringListValue{"a", "b", "c"};
+        auto stringListValue = getParameter<std::vector<std::string>>(n, "stringListKey", std::vector<std::string>{"x"});
+        ASSERT_EQ(stringListValue, expectedStringListValue);
+        stringListValue = getParameter(n, "stringListKey", std::vector<std::string>{"x"});
+        ASSERT_EQ(stringListValue, expectedStringListValue);
+    }
+    {
+        std::vector<bool> expectedBoolListValue{true, false, true};
+        auto boolListValue = getParameter<std::vector<bool>>(n, "boolListKey", std::vector<bool>{true});
+        ASSERT_EQ(boolListValue, expectedBoolListValue);
+        boolListValue = getParameter(n, "boolListKey", std::vector<bool>{true});
+        ASSERT_EQ(boolListValue, expectedBoolListValue);
+    }
+    {
+        auto intListValue = getParameter<std::vector<int>>(n, "intListKey", std::vector<int>{7});
+        std::vector<int> expectedIntListValue{1, 2, 4};
+        ASSERT_EQ(intListValue, expectedIntListValue);
+    }
+    {
+        auto floatListValue = getParameter<std::vector<float>>(n, "floatListKey", std::vector<float>{0.5f});
+        std::vector<float> expectedFloatListValue{0.0f, 1.0f, 0.42f};
+        ASSERT_EQ(floatListValue, expectedFloatListValue);
+    }
+    {
+        auto doubleListValue = getParameter<std::vector<double>>(n, "doubleListKey", std::vector<double>{0.5});
+        std::vector<double> expectedDoubleListValue{0.5, -2.0};
+        ASSERT_EQ(doubleListValue, expectedDoubleListValue);
+    }
+}
+
+TEST_F(NodeUtilTest, ReadNonExistingMapDataTypesWithFallback) {
+    {
+        auto stringMapValue = getParameter<std::map<std::string, std::string>>(n, "unknownKey", std::map<std::string,std::string>{});
+        ASSERT_EQ(stringMapValue.size(), 0);
+    }
+    {
+        auto intMapValue = getParameter<std::map<std::string, int>>(n, "unknownKey", std::map<std::string, int>{});
+        ASSERT_EQ(intMapValue["key1"], 0);
+    }
+}
+
+int main(int argc, char **argv) {
+    testing::InitGoogleTest(&argc, argv);
+    ros::init(argc, argv, "node_util_test");
+    return RUN_ALL_TESTS();
+
+}
\ No newline at end of file

test/test_NodeUtil.test

+<launch>
+    <test pkg="ccf" type="ccf-NodeUtil-test" test-name="NodeUtil_test" />
+</launch>