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..9578719d8317abf6a037903360df2ab4085d0dc4 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
```
-- You can view the generated files in you browser by opening the generated index.html via CLion in your browser.
+## 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
+ ```
## CGV Dummy Controller Demonstrator
@@ -72,8 +85,8 @@ This demonstrator contains two mock-up nodes that can serve as stand-ins for fut
#### Configuration
- 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
+ ```bash
+ rostopic pub -1 /scene_config std_msgs/String "\"$(roscat ccf config_scene.yaml)\""
```
#### Execution
diff --git a/config/config.yaml b/config/config.yaml
index e706283aee5eebd754dbad620b1dca4ec338bd26..97af7cc5fff92078f36a216b2c48870529fbb16c 100644
--- a/config/config.yaml
+++ b/config/config.yaml
@@ -1,8 +1,7 @@
connector_node_ros_cgv:
cgv_address: "tcp://*:6576"
-scene_controller:
- max_grasp_approach_velocity: 0.2
- max_grasp_approach_acceleration: 0.2
- max_grasp_transition_velocity: 0.2
- max_grasp_transition_acceleration: 0.2
- tud_grasp_force: 12.5
+max_grasp_approach_velocity: 0.2
+max_grasp_approach_acceleration: 0.2
+max_grasp_transition_velocity: 0.2
+max_grasp_transition_acceleration: 0.2
+tud_grasp_force: 12.5
diff --git a/config/config_pick_place_scene.json b/config/config_pick_place_scene.json
deleted file mode 100644
index ddefea7c5a933d27f7028a8cba1a4ce8b336de09..0000000000000000000000000000000000000000
--- a/config/config_pick_place_scene.json
+++ /dev/null
@@ -1,20 +0,0 @@
-{ "objects": [
- { "id": "tablePillar1","pos": { "x": 0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
- { "id": "tablePillar2","pos": { "x": -0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
- { "id": "tablePillar3","pos": { "x": -0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
- { "id": "tablePillar4","pos": { "x": 0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
- { "id": "table","pos": { "z": 0.7 },"size": { "length": 1.6,"width": 1.6,"height": 0.1 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
- { "id": "binBlue","type": "DROP_OFF_LOCATION","pos": { "x": -0.34,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "b": 1 } },
- { "id": "binRed","type": "DROP_OFF_LOCATION","pos": { "x": 0.06,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } },
- { "id": "binGreen","type": "DROP_OFF_LOCATION","pos": { "x": 0.46,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "g": 1 } },
- { "id": "objectRed1","type": "BOX","pos": { "x": 0.5,"y": -0.1,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } },
- { "id": "objectRed2","type": "BOX","pos": { "x": 0.25,"y": -0.2,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "w": 1 },"color": { "r": 1 } },
- { "id": "objectRed3","type": "BOX","pos": { "x": -0.4,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } },
- { "id": "objectGreen1","type": "BOX","pos": { "x": 0.45,"y": -0.3,"z": 0.8105 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } },
- { "id": "objectGreen2","type": "BOX","pos": { "x": -0.45,"y": -0.2,"z": 0.8105 },"size": {"length":0.031,"width":0.031,"height":0.138},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } },
- { "id": "objectGreen3","type": "BOX","pos": { "x": 0.1,"y": -0.3,"z": 0.819 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } },
- { "id": "objectBlue1","type": "BOX","pos": { "x": 0.25,"y": -0.4,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
- { "id": "objectBlue2","type": "BOX","pos": { "x": -0.3,"y": -0.3,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
- { "id": "objectBlue3","type": "BOX","pos": { "x": 0.4,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
- { "id": "arm","type": "ARM","pos": { "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } }
-] }
diff --git a/config/config_scene.yaml b/config/config_scene.yaml
index 9f09d081b7d6caa749305865649366b180d27286..2620d0c0e5242d87d6dfbdc8d2c6e4b39d6e6c18 100644
--- a/config/config_scene.yaml
+++ b/config/config_scene.yaml
@@ -1,20 +1,20 @@
-{ "objects": [
- { "id": "tablePillar1","pos": { "x": 0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
- { "id": "tablePillar2","pos": { "x": -0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
- { "id": "tablePillar3","pos": { "x": -0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
- { "id": "tablePillar4","pos": { "x": 0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
- { "id": "table","pos": { "z": 0.7 },"size": { "length": 1.6,"width": 1.6,"height": 0.1 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
- { "id": "binBlue","type": "BIN","pos": { "x": -0.34,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "b": 1 } },
- { "id": "binRed","type": "BIN","pos": { "x": 0.06,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } },
- { "id": "binGreen","type": "BIN","pos": { "x": 0.46,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "g": 1 } },
- { "id": "objectRed1","type": "BOX","pos": { "x": 0.5,"y": -0.1,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } },
- { "id": "objectRed2","type": "BOX","pos": { "x": 0.25,"y": -0.2,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "w": 1 },"color": { "r": 1 } },
- { "id": "objectRed3","type": "BOX","pos": { "x": -0.4,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } },
- { "id": "objectGreen1","type": "BOX","pos": { "x": 0.45,"y": -0.3,"z": 0.8105 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } },
- { "id": "objectGreen2","type": "BOX","pos": { "x": -0.45,"y": -0.2,"z": 0.8105 },"size": {"length":0.031,"width":0.031,"height":0.138},"orientation": { "z": 1,"w": 0.92388 },"color": { "g": 1 } },
- { "id": "objectGreen3","type": "BOX","pos": { "x": 0.1,"y": -0.3,"z": 0.819 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } },
- { "id": "objectBlue1","type": "BOX","pos": { "x": 0.25,"y": -0.4,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
- { "id": "objectBlue2","type": "BOX","pos": { "x": -0.3,"y": -0.3,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
- { "id": "objectBlue3","type": "BOX","pos": { "x": 0.4,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
- { "id": "arm","type": "ARM","pos": { "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } }
+{ 'objects': [
+ { 'id': 'tablePillar1','pos': { 'x': 0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'tablePillar2','pos': { 'x': -0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'tablePillar3','pos': { 'x': -0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'tablePillar4','pos': { 'x': 0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'table','pos': { 'z': 0.7 },'size': { 'length': 1.6,'width': 1.6,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
+ { 'id': 'binBlue','type': 'BIN','pos': { 'x': -0.34,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
+ { 'id': 'binRed','type': 'BIN','pos': { 'x': 0.06,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+ { 'id': 'binGreen','type': 'BIN','pos': { 'x': 0.46,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+ { 'id': 'objectRed1','type': 'BOX','pos': { 'x': 0.5,'y': -0.1,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+ { 'id': 'objectRed2','type': 'BOX','pos': { 'x': 0.25,'y': -0.2,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+ { 'id': 'objectRed3','type': 'BOX','pos': { 'x': -0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
+ { 'id': 'objectGreen1','type': 'BOX','pos': { 'x': 0.45,'y': -0.3,'z': 0.8105 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
+ { 'id': 'objectGreen2','type': 'BOX','pos': { 'x': -0.45,'y': -0.2,'z': 0.8105 },'size': {'length':0.031,'width':0.031,'height':0.138},'orientation': { 'z': 1,'w': 0.92388 },'color': { 'g': 1 } },
+ { 'id': 'objectGreen3','type': 'BOX','pos': { 'x': 0.1,'y': -0.3,'z': 0.819 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
+ { 'id': 'objectBlue1','type': 'BOX','pos': { 'x': 0.25,'y': -0.4,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+ { 'id': 'objectBlue2','type': 'BOX','pos': { 'x': -0.3,'y': -0.3,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+ { 'id': 'objectBlue3','type': 'BOX','pos': { 'x': 0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
+ { 'id': 'arm','type': 'ARM','pos': { 'z': 0.75 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
] }
diff --git a/config_full.yaml b/config_full.yaml
deleted file mode 100644
index 6cf18beb1b027d1e3f8c69522e01178c2858d2b0..0000000000000000000000000000000000000000
--- a/config_full.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
- "{'objects':[
-{'id':'objectRed1','type':'BOX','pos':{'x':0.5,'y':-0.1,'z':0.8105},'size':{'length':0.031,'width':0.062,'height':0.121},'orientation':{'x': 0, 'y': 0, 'z': 0.382683, 'w': 0.92388},'color':{'r':1.00,'g':0.00,'b':0.00}},
-{'id':'objectRed2','type':'BOX','pos':{'x':0.25,'y':-0.2,'z':0.8105},'size':{'length':0.031,'width':0.062,'height':0.121},'orientation':{'x': 0, 'y': 0, 'z': 0, 'w': 1.0},'color':{'r':1.00,'g':0.00,'b':0.00}},
-{'id':'objectRed3','type':'BOX','pos':{'x':-0.4,'y':0.0,'z':0.819},'size':{'length':0.031,'width':0.031,'height':0.138},'orientation':{'x': 0, 'y': 0, 'z': 0.382683, 'w': 0.92388},'color':{'r':1.00,'g':0.00,'b':0.00}},
-{'id':'objectGreen1','type':'BOX','pos':{'x':0.45,'y':-0.3,'z':0.8105},'size':{'length':0.031,'width':0.062,'height':0.121},'orientation':{'x': 0, 'y': 0, 'z': 0.382683, 'w': 0.92388},'color':{'r':0.00,'g':1.00,'b':0.00}},
-{'id':'objectGreen2','type':'BOX','pos':{'x':-0.45,'y':-0.2,'z':0.819},'size':{'length':0.031,'width':0.031,'height':0.138},'orientation':{'x': 0, 'y': 0, 'z': 1.0, 'w': 0.92388},'color':{'r':0.00,'g':1.00,'b':0.00}},
-{'id':'objectGreen3','type':'BOX','pos':{'x':0.1,'y':-0.3,'z':0.8105},'size':{'length':0.031,'width':0.062,'height':0.121},'orientation':{'x': 0, 'y': 0, 'z': 0.382683, 'w': 0.92388},'color':{'r':0.00,'g':1.00,'b':0.00}},
-{'id':'objectBlue1','type':'BOX','pos':{'x':0.25,'y':-0.4,'z':0.8105},'size':{'length':0.031,'width':0.062,'height':0.121},'orientation':{'x': 0, 'y': 0, 'z': 0.382683, 'w': 0.92388},'color':{'r':0.00,'g':0.00,'b':1.00}},
-{'id':'objectBlue2','type':'BOX','pos':{'x':-0.3,'y':-0.3,'z':0.8105},'size':{'length':0.031,'width':0.062,'height':0.121},'orientation':{'x': 0, 'y': 0, 'z': 0.382683, 'w': 0.92388},'color':{'r':0.00,'g':0.00,'b':1.00}},
-{'id':'objectBlue3','type':'BOX','pos':{'x':0.4,'y':0.0,'z':0.819},'size':{'length':0.031,'width':0.031,'height':0.138},'orientation':{'x': 0, 'y': 0, 'z': 0.382683, 'w': 0.92388},'color':{'r':0.00,'g':0.00,'b':1.00}},
-{'id':'tablePillar1','type':'UNKNOWN','pos':{'x':0.77,'y':0.77,'z':0.325},'size':{'length':0.06,'width':0.06,'height':0.65},'orientation':{'x':0.00,'y':0.00,'z':0.00,'w':1.00},'color':{'r':0.839,'g':0.839,'b':0.839}},
-{'id':'tablePillar2','type':'UNKNOWN','pos':{'x':-0.77,'y':-0.77,'z':0.325},'size':{'length':0.06,'width':0.06,'height':0.65},'orientation':{'x':0.00,'y':0.00,'z':0.00,'w':1.00},'color':{'r':0.839,'g':0.839,'b':0.839}},
-{'id':'tablePillar3','type':'UNKNOWN','pos':{'x':-0.77,'y':0.77,'z':0.325},'size':{'length':0.06,'width':0.06,'height':0.65},'orientation':{'x':0.00,'y':0.00,'z':0.00,'w':1.00},'color':{'r':0.839,'g':0.839,'b':0.839}},
-{'id':'tablePillar4','type':'UNKNOWN','pos':{'x':0.77,'y':-0.77,'z':0.325},'size':{'length':0.06,'width':0.06,'height':0.65},'orientation':{'x':0.00,'y':0.00,'z':0.00,'w':1.00},'color':{'r':0.839,'g':0.839,'b':0.839}},
-{'id':'table','type':'UNKNOWN','pos':{'x':0.00,'y':0.00,'z':0.7},'size':{'length':1.60,'width':1.60,'height':0.10},'orientation':{'x':0.00,'y':0.00,'z':0.00,'w':1.00},'color':{'r':0.839,'g':0.839,'b':0.839}},
-{'id':'binBlue','type':'BIN','pos':{'x':-0.34,'y':0.49,'z':0.8325},'size':{'length':0.21,'width':0.30,'height':0.165},'orientation':{'w':1},'color':{'r':0.00,'g':0.00,'b':1.00}},
-{'id':'binRed','type':'BIN','pos':{'x': 0.06,'y':0.49,'z':0.8325},'size':{'length':0.21,'width':0.30,'height':0.165},'orientation':{'w':1},'color':{'r': 1.00,'g':0.00,'b':0.00}},
-{'id':'binGreen','type':'BIN','pos':{'x': 0.46,'y':0.49,'z':0.8325},'size':{'length':0.21,'width':0.30,'height':0.165},'orientation':{'w':1},'color':{'r':0.00,'g':1.00,'b':0.00}},
-{'id':'arm','type':'ARM','pos':{'x': 0,'y': 0,'z':0.75},'size':{'length':0.0,'width':0.0,'height':0.0},'orientation':{'w':1},'color':{'r':0.00,'g':0.00,'b':0.00}}
-]}"
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/MoveItRobotArmController.cpp b/src/ccf/controller/MoveItRobotArmController.cpp
index 64530a67792af5519156daf09fccf517aa29437a..1ba6e3af95701a0051d862b20da095bca8786db2 100644
--- a/src/ccf/controller/MoveItRobotArmController.cpp
+++ b/src/ccf/controller/MoveItRobotArmController.cpp
@@ -32,7 +32,13 @@ void MoveItRobotArmController::updateScene(ccf::SceneUpdateRequest &req) {
} else {
ccf::BinCheck srv;
srv.request.id = req.ids[i];
- bin_check_client.call(srv);
+
+ if (!bin_check_client.call(srv)) {
+ ROS_ERROR_STREAM("[MoveItRobotArmController] "
+ "Unable to call " << bin_check_client.getService() <<
+ ". It " << (bin_check_client.exists() ? "exists" : "it does not exist")
+ << ". Assuming each object is a BOX.");
+ }
if (srv.response.isBin) {
object->set_type(Object_Type_BIN);
@@ -98,7 +104,13 @@ bool MoveItRobotArmController::pickAndDrop(Object &robot, Object &object, Object
ccf::PickDropService srv;
srv.request.object = object.id();
srv.request.bin = bin.id();
- pick_drop_client.call(srv);
+ if (!pick_drop_client.call(srv)) {
+ ROS_ERROR_STREAM("[MoveItRobotArmController] "
+ "Unable to call " << pick_drop_client.getService() <<
+ ". It " << (pick_drop_client.exists() ? "exists" : "it does not exist")
+ << ".");
+ return false;
+ }
// we explicitly do not remove the object right now, so we do not call the super method
return true;
@@ -120,7 +132,13 @@ bool MoveItRobotArmController::pickAndPlace(Object &robot, Object &object, Objec
srv.request.pose.position.x = location.pos().x();
srv.request.pose.position.y = location.pos().y();
srv.request.pose.position.z = location.pos().z();
- pick_place_client.call(srv);
+ if (!pick_place_client.call(srv)) {
+ ROS_ERROR_STREAM("[MoveItRobotArmController] "
+ "Unable to call " << pick_place_client.getService() <<
+ ". It " << (pick_place_client.exists() ? "exists" : "it does not exist")
+ << ".");
+ return false;
+ }
// we explicitly do not move the object right now, so we do not call the super method
return true;
@@ -129,10 +147,10 @@ bool MoveItRobotArmController::pickAndPlace(Object &robot, Object &object, Objec
MoveItRobotArmController::MoveItRobotArmController(ros::NodeHandle &nodeHandle, const std::string &cellName)
: RobotArmController(nodeHandle, cellName),
- pick_drop_client(nodeHandle.serviceClient<ccf::PickDropService>("/pick_drop_service")),
- pick_place_client(nodeHandle.serviceClient<ccf::PickPlaceService>("/pick_place_service")) {
+ pick_drop_client(nodeHandle.serviceClient<ccf::PickDropService>("/scene_controller/pick_drop_service")),
+ pick_place_client(nodeHandle.serviceClient<ccf::PickPlaceService>("/scene_controller/pick_place_service")) {
- bin_check_client = nodeHandle.serviceClient<ccf::BinCheck>("/check_bin_service");
+ bin_check_client = nodeHandle.serviceClient<ccf::BinCheck>("/scene_controller/check_bin_service");
get_scene_service = nodeHandle.advertiseService<ccf::SceneUpdateRequest, ccf::SceneUpdateResponse>(
"updateCgvScene",
[this](auto &req, auto &res) {
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/ccf/util/scene_controller_node.cpp b/src/ccf/util/scene_controller_node.cpp
index fe89962caeb17a51485cb084b2345ccf17847da4..c2654747a09f06d19a38fba703ce69c0ffd102be 100644
--- a/src/ccf/util/scene_controller_node.cpp
+++ b/src/ccf/util/scene_controller_node.cpp
@@ -40,7 +40,7 @@ namespace world_state {
std::string current_picked_object_id;
std::string current_bin_id;
geometry_msgs::Point current_transform;
- double open_amount = 0.08;
+ double open_amount = 0.078;
bool is_initialized = false;
bool is_table_configured = false;
bool is_simulated_robot = false;
@@ -712,7 +712,7 @@ bool pickAndDropObjectCallback(ccf::PickDropService::Request &req,
int main(int argc, char **argv) {
ros::init(argc, argv, "scene_controller");
- ros::NodeHandle n;
+ ros::NodeHandle n("scene_controller");
ros::AsyncSpinner spinner(7);
spinner.start();
@@ -730,7 +730,7 @@ int main(int argc, char **argv) {
if (!world_state::is_simulated_robot) {
- ROS_INFO_STREAM("[SCENECONTROLLER] Using setup for real robot.");
+ ROS_ERROR_STREAM("[SCENECONTROLLER] Using setup for real robot.");
if (!ros::param::has("max_grasp_approach_velocity") ||
@@ -746,26 +746,6 @@ int main(int argc, char **argv) {
ros::param::set("max_grasp_transition_velocity", 0.05);
ros::param::set("max_grasp_transition_acceleration", 0.05);
- } else {
- double max_grasp_approach_velocity = 0;
- double max_grasp_approach_acceleration = 0;
- double max_grasp_transition_velocity = 0;
- double max_grasp_transition_acceleration = 0;
-
- ros::param::get("max_grasp_approach_velocity", max_grasp_approach_velocity);
- ros::param::get("max_grasp_approach_acceleration", max_grasp_approach_acceleration);
-
- ros::param::get("max_grasp_transition_velocity", max_grasp_transition_velocity);
- ros::param::get("max_grasp_transition_acceleration", max_grasp_transition_acceleration);
-
- ROS_INFO_STREAM(
- "[SCENECONTROLLER] Applied max_grasp_approach_velocity " << max_grasp_approach_velocity);
- ROS_INFO_STREAM("[SCENECONTROLLER] Applied max_grasp_approach_acceleration "
- << max_grasp_approach_acceleration);
- ROS_INFO_STREAM(
- "[SCENECONTROLLER] Applied max_grasp_transition_velocity " << max_grasp_transition_velocity);
- ROS_INFO_STREAM("[SCENECONTROLLER] Applied max_grasp_transition_acceleration "
- << max_grasp_transition_acceleration);
}
if (!ros::param::has("tud_grasp_force")) {
@@ -804,7 +784,7 @@ int main(int argc, char **argv) {
boost::bind(&manualScenePush, _1,
boost::ref(n)));
- ros::ServiceServer service = n.advertiseService("/check_bin_service", checkBin);
+ ros::ServiceServer service = n.advertiseService("check_bin_service", checkBin);
ros::ServiceServer pick_drop_service = n.advertiseService<ccf::PickDropService::Request, ccf::PickDropService::Response>(
"pick_drop_service",
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..296b943d8b94300fd3279bf33ac0db95db3973a8 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));
@@ -117,4 +117,4 @@ int main(int argc, char **argv) {
ros::spin();
return 0;
-}
\ No newline at end of file
+}
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>