diff --git a/.gitignore b/.gitignore index 1c85d0e33ee62df4e08b1e05901d206de3b1e866..ddedb264f45361a21416cacc301bd5a7402e3a37 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ .idea cmake-build-* +doc diff --git a/CMakeLists.txt b/CMakeLists.txt index 9327b375e6b3d4899e5868a6ec30a7ab76b33af9..6cd8b5e22797bb46c68543302c4510e31cb494ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -335,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) diff --git a/README.md b/README.md index bbd4d3cc2c19814ddd48745d1a8f705d44701372..3b9b68b31f1f0edd7e3416f7d14f0bb586f9b8a3 100644 --- a/README.md +++ b/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 ``` -- You can view the generated files in you browser by opening the generated index.html via CLion in your browser. +2. Build the tests + ```bash + catkin build --make-args tests + ``` +3. Run the tests + ```bash + rostest -t ccf test_NodeUtil.test + ``` ## CGV Dummy Controller Demonstrator @@ -73,7 +86,7 @@ This demonstrator contains two mock-up nodes that can serve as stand-ins for fut - configure the scene (*not* required when using the dummy controller): ``` - rostopic pub -1 /scene_config/collision_objects std_msgs/String -f `rospack find ccf`/config_full.yaml + rostopic pub -1 /scene_config std_msgs/String -f `rospack find ccf`/config_full.yaml ``` #### Execution diff --git a/include/ccf/util/NodeUtil.h b/include/ccf/util/NodeUtil.h index c6e5cc180eaa034ceba2e4767e88e8ac9abe46fc..200d33ef873f8272393bdfd2271ff34a4ed5e699 100644 --- a/include/ccf/util/NodeUtil.h +++ b/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 diff --git a/src/ccf/controller/RobotArmController.cpp b/src/ccf/controller/RobotArmController.cpp index e3bad545e4d7825110bf428d001c0ff7c537a760..9ea39c59b97a8d89ed1d63c4403025dc7cc91d1b 100644 --- a/src/ccf/controller/RobotArmController.cpp +++ b/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"); diff --git a/src/dummy_cgv_controller.cpp b/src/dummy_cgv_controller.cpp index fcd8d58ce8d92b6a85c5f73908e2e9d5b56556ce..a05133b0cfa3dcc651b910f6aaf954dce29a7db0 100644 --- a/src/dummy_cgv_controller.cpp +++ b/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)); diff --git a/src/dummy_rag_controller_a.cpp b/src/dummy_rag_controller_a.cpp index e597bbadb1532da82f87fecebd5b5082c7e34156..f1ee30c697f28ef5c183553221b0865ae6aaa5ad 100644 --- a/src/dummy_rag_controller_a.cpp +++ b/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"); diff --git a/src/dummy_rag_controller_b.cpp b/src/dummy_rag_controller_b.cpp index 6a2a6ef8b32496e15fd118f9bc387bd5633424b5..0004ef290fa7d42533c9f87b2190daa729f8bea0 100644 --- a/src/dummy_rag_controller_b.cpp +++ b/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()); diff --git a/src/moveit_cgv_controller.cpp b/src/moveit_cgv_controller.cpp index 019a9634e42c400240706f9f50e876f026b0ddcf..69b85ceb660508041d3fdb6cbc8d5d35f61c13c4 100644 --- a/src/moveit_cgv_controller.cpp +++ b/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)); diff --git a/test/test_NodeUtil.cpp b/test/test_NodeUtil.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f34f46da9eb29196e3c4c798de2d9076ee2dc97b --- /dev/null +++ b/test/test_NodeUtil.cpp @@ -0,0 +1,201 @@ +#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 diff --git a/test/test_NodeUtil.test b/test/test_NodeUtil.test new file mode 100644 index 0000000000000000000000000000000000000000..ed5f89772cdfb75c118d0802d83867a2356523db --- /dev/null +++ b/test/test_NodeUtil.test @@ -0,0 +1,3 @@ +<launch> + <test pkg="ccf" type="ccf-NodeUtil-test" test-name="NodeUtil_test" /> +</launch>