added test infrastructure

robotology · Jun 24, 2023 · 489abb9 · 489abb9
1 parent b45e8d7
commit 489abb9
Show file tree

Hide file tree

Showing 9 changed files with 241 additions and 12 deletions.
diff --git a/README.md b/README.md
@@ -27,6 +27,10 @@ Here's below a quick how-to guide; please, refer to the section [🚶🏻‍♂
    ```console
    icub-grasp.sh
    ```
+1. You can run a systematic test with always the same object in the same pose by specifying the option `test`:
+   ```console
+   icub-grasp.sh test
+   ```
 1. If needed, you can clean up hanging resources by specifying the option `clean`:
    ```console
    icub-grasp.sh clean

diff --git a/dockerfiles/Dockerfile b/dockerfiles/Dockerfile
@@ -38,6 +38,9 @@ RUN apt install -y python3 python3-dev python3-pip python3-setuptools && \
 # Install magic-wormwhole to get things from one computer to another safely
 RUN apt install -y magic-wormhole
 
+# Install glog
+RUN apt install -y libgoogle-glog-dev
+
 # Install noVNC
 RUN git clone https://github.com/novnc/noVNC.git /opt/novnc && \
     git clone https://github.com/novnc/websockify /opt/novnc/utils/websockify && \

diff --git a/gazebo/worlds/icub-gazebo-grasping-sandbox-test.sdf b/gazebo/worlds/icub-gazebo-grasping-sandbox-test.sdf
@@ -0,0 +1,64 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+  <world name="default">
+
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <light name='user_point_light_0' type='point'>
+        <pose frame=''>0.197302 -0.215077 1 0 -0 0</pose>
+        <diffuse>0.5 0.5 0.5 1</diffuse>
+        <specular>0.1 0.1 0.1 1</specular>
+        <attenuation>
+          <range>20</range>
+          <constant>0.5</constant>
+          <linear>0.01</linear>
+          <quadratic>0.001</quadratic>
+        </attenuation>
+        <cast_shadows>0</cast_shadows>
+        <direction>0 0 -1</direction>
+    </light>
+
+    <state world_name='default'>
+
+      <light name='sun'>
+        <pose frame=''>0 0 10 0 -0 3.14</pose>
+      </light>
+
+      <light name='user_point_light_0'>
+        <pose frame=''>0.197302 -0.215077 1 0 -0 0</pose>
+      </light>
+    </state>
+
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <include>
+      <uri>model://table</uri>
+      <pose>-0.530772 0 0 0 0 -1.57</pose>
+    </include>
+
+    <include>
+      <uri>model://pudding_box</uri>
+      <pose>-0.35 -0.1 0.5 0 0 -1.57</pose>
+    </include>
+
+    <model name="iCub">
+      <include>
+        <uri>model://iCubGazeboV2_5_visuomanip</uri>
+        <pose>0.0 0.0 0.63 0.0 0.0 0.0</pose>
+      </include>
+    </model>
+
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>-1.73754 2.13332 1.77051 0 0.463643 -1.0638</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+
+  </world>
+</sdf>
diff --git a/scripts/CMakeLists.txt b/scripts/CMakeLists.txt
@@ -7,6 +7,7 @@
 
 # Install the application
 yarp_install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/icub-grasp.xml DESTINATION ${ICUBCONTRIB_APPLICATIONS_INSTALL_DIR})
+yarp_install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/icub-grasp-test.xml DESTINATION ${ICUBCONTRIB_APPLICATIONS_INSTALL_DIR})
 
 # Install startup script
 install(PROGRAMS ${CMAKE_CURRENT_SOURCE_DIR}/icub-grasp.sh DESTINATION bin)
diff --git a/scripts/icub-grasp-test.xml b/scripts/icub-grasp-test.xml
@@ -0,0 +1,130 @@
+<application>
+    <name>iCub Grasp</name>
+
+    <dependencies>
+    </dependencies>
+
+    <module>
+        <name>gzserver</name>
+        <parameters>-e dart icub-gazebo-grasping-sandbox-test.sdf</parameters>
+        <node>localhost</node>
+    </module>
+
+    <module>
+        <name>gzclient</name>
+        <node>localhost</node>
+    </module>
+
+    <module>
+        <name>yarprobotinterface</name>
+        <parameters>--context gazeboCartesianControl --config no_legs.xml</parameters>
+        <dependencies>
+            <port timeout="20">/icubSim/torso/state:o</port>
+            <port timeout="20">/icubSim/left_arm/state:o</port>
+            <port timeout="20">/icubSim/right_arm/state:o</port>
+        </dependencies>
+        <ensure>
+            <wait when="stop">5</wait>
+        </ensure>
+        <node>localhost</node>
+    </module>
+
+    <module>
+        <name>iKinCartesianSolver</name>
+        <parameters>--context gazeboCartesianControl --part right_arm</parameters>
+        <dependencies>
+            <port timeout="20">/icubSim/torso/state:o</port>
+            <port timeout="20">/icubSim/right_arm/state:o</port>
+        </dependencies>
+        <node>localhost</node>
+    </module>
+
+    <module>
+        <name>iKinCartesianSolver</name>
+        <parameters>--context gazeboCartesianControl --part left_arm</parameters>
+        <dependencies>
+            <port timeout="20">/icubSim/torso/state:o</port>
+            <port timeout="20">/icubSim/left_arm/state:o</port>
+        </dependencies>
+        <node>localhost</node>
+    </module>
+
+    <module>
+        <name>iKinGazeCtrl</name>
+        <parameters>--context gazeboCartesianControl --from iKinGazeCtrl.ini</parameters>
+        <dependencies>
+            <port timeout="20">/icubSim/torso/state:o</port>
+            <port timeout="20">/icubSim/head/state:o</port>
+        </dependencies>
+        <ensure>
+            <wait when="stop">5</wait>
+        </ensure>
+        <node>localhost</node>
+    </module>
+
+    <module>
+        <name>yarpview</name>
+        <parameters>--name /view/left --compact --p 10 --x 20 --y 20</parameters>
+        <dependencies>
+            <port timeout="20">/icub-grasp/rpc</port>
+        </dependencies>
+        <node>localhost</node>
+    </module>
+
+    <module>
+        <name>find-superquadric</name>
+        <parameters>--remove-outliers "(0.01 10)" --random-sample 0.2 --disable-viewer</parameters>
+        <dependencies>
+            <port timeout="20">/icub-grasp/rpc</port>
+        </dependencies>
+        <node>localhost</node>
+    </module>
+
+    <module>
+        <name>icub-gazebo-grasping-sandbox</name>
+        <parameters></parameters>
+        <dependencies>
+            <port timeout="20">/iKinGazeCtrl/rpc</port>
+            <port timeout="20">/icubSim/cartesianController/right_arm/state:o</port>
+            <port timeout="20">/icubSim/cartesianController/left_arm/state:o</port>
+        </dependencies>
+        <node>localhost</node>
+    </module>
+
+    <connection>
+        <from>/icub-grasp/mustard_bottle/mover:o</from>
+        <to>/mustard_bottle/model-mover/delta-pose:i</to>
+        <protocol>tcp</protocol>
+    </connection>
+
+    <connection>
+        <from>/icub-grasp/pudding_box/mover:o</from>
+        <to>/pudding_box/model-mover/delta-pose:i</to>
+        <protocol>tcp</protocol>
+    </connection>
+
+    <connection>
+        <from>/icubSim/cam/left/rgbImage:o</from>
+        <to>/view/left</to>
+        <protocol>fast_tcp</protocol>
+    </connection>
+
+    <connection>
+        <from>/icubSim/cam/left/rgbImage:o</from>
+        <to>/icub-grasp/rgb:i</to>
+        <protocol>fast_tcp</protocol>
+    </connection>
+
+    <connection>
+        <from>/icubSim/cam/left/depthImage:o</from>
+        <to>/icub-grasp/depth:i</to>
+        <protocol>fast_tcp</protocol>
+    </connection>
+
+    <connection>
+        <from>/icub-grasp/sq:rpc</from>
+        <to>/find-superquadric/points:rpc</to>
+        <protocol>tcp</protocol>
+    </connection>
+
+</application>
diff --git a/scripts/icub-grasp.sh b/scripts/icub-grasp.sh
@@ -9,11 +9,23 @@
 
 # launch the demo
 run() {
+    if [[ $# -eq 0 ]]; then
+      app="icub-grasp.xml"
+      random_pose="on"
+    elif [ "$1" == "test" ]; then
+      app="icub-grasp-test.xml"
+      random_pose="off"
+    else
+      echo "unknown option!"
+      exit 1
+    fi
+
     yarpserver --write --silent &
-    yarpmanager-console --application ${ROBOTOLOGY_SUPERBUILD_INSTALL_PREFIX}/share/ICUBcontrib/applications/icub-grasp.xml --run --connect --exit --silent
+    yarp wait /root > /dev/null
+    yarpmanager-console --application ${ROBOTOLOGY_SUPERBUILD_INSTALL_PREFIX}/share/ICUBcontrib/applications/${app} --run --connect --exit --silent
 
     yarp wait /icub-grasp/rpc
-    echo "go" | yarp rpc /icub-grasp/rpc
+    echo "go ${random_pose}" | yarp rpc /icub-grasp/rpc
 
     sleep 5
     declare -a modules=("icub-gazebo-grasping-sandbox" "find-superquadric" "yarpview")
@@ -49,10 +61,15 @@ if [[ $# -eq 0 ]]; then
     echo "demo is starting up..."
     run
     echo "...demo done"
+elif [ "$1" == "test" ]; then
+    echo "test is starting up..."
+    run test
+    echo "...test done"
 elif [ "$1" == "clean" ]; then
     echo "cleaning up resources..."
     clean
     echo "...cleanup done"
 else
     echo "unknown option!"
+    exit 1
 fi
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
@@ -7,6 +7,7 @@
 
 find_package(YARP REQUIRED COMPONENTS os dev sig math)
 find_package(ICUB REQUIRED COMPONENTS iKin)
+find_package(glog REQUIRED)
 
 # Get around conflicts between VTK and Gazebo in Ubuntu 2022.04
 set(CMAKE_MODULE_PATH_BAK ${CMAKE_MODULE_PATH})
@@ -23,7 +24,7 @@ yarp_add_idl(IDL_GEN_FILES ${CMAKE_CURRENT_SOURCE_DIR}/rpc.thrift)
 add_executable(${PROJECT_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/main.cpp ${IDL_GEN_FILES})
 target_include_directories(${PROJECT_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
 target_compile_definitions(${PROJECT_NAME} PRIVATE _USE_MATH_DEFINES)
-target_link_libraries(${PROJECT_NAME} PRIVATE ${YARP_LIBRARIES} ICUB::iKin ${VTK_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} PRIVATE ${YARP_LIBRARIES} ICUB::iKin ${VTK_LIBRARIES} glog::glog)
 
 if (VTK_VERSION VERSION_GREATER_EQUAL "8.90.0")
   vtk_module_autoinit(TARGETS ${PROJECT_NAME} MODULES ${VTK_LIBRARIES})

diff --git a/src/main.cpp b/src/main.cpp
@@ -15,6 +15,8 @@
 #include <random>
 #include <fstream>
 
+#include <glog/logging.h>
+
 #include <yarp/os/Network.h>
 #include <yarp/os/LogStream.h>
 #include <yarp/os/ResourceFinder.h>
@@ -42,6 +44,7 @@
 #include "cardinal_points_grasp.h"
 
 using namespace std;
+using namespace google;
 using namespace yarp::os;
 using namespace yarp::dev;
 using namespace yarp::sig;
@@ -238,14 +241,17 @@ class GrasperModule : public RFModule, public rpc_IDL {
     }
 
     /**************************************************************************/
-    bool go() override {
-        if (randomize()) {
-            if (home()) {
-                if (segment()) {
-                    if (fit()) {
-                        if (grasp()) {
-                            return true;
-                        }
+    bool go(const string& random_pose) override {
+        if (random_pose == "on") {
+            if (!randomize()) {
+                return false;
+            }
+        }
+        if (home()) {
+            if (segment()) {
+                if (fit()) {
+                    if (grasp()) {
+                        return true;
                     }
                 }
             }
@@ -592,6 +598,8 @@ class GrasperModule : public RFModule, public rpc_IDL {
 
 /******************************************************************************/
 int main(int argc, char *argv[]) {
+    InitGoogleLogging(argv[0]);
+
     Network yarp;
     if (!yarp.checkNetwork()) {
         yError() << "Unable to find YARP server!";

diff --git a/src/rpc.thrift b/src/rpc.thrift
@@ -42,7 +42,8 @@ service rpc_IDL
 
    /**
    * Go the whole hog.
+   * @param random_pose iff "on" it performs object's pose randomization
    * @return true/false on success/failure.
    */
-   bool go();
+   bool go(1:string random_pose);
 }