From 36a8ac41017b88ba1892ff7fe06833e6c9461e17 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Genesio=20=28=E4=BA=8C=E3=82=B3=E3=82=B2?=
 =?UTF-8?q?=E3=83=8D=29?= <nicolo.genesio@iit.it>
Date: Mon, 7 Dec 2020 10:50:50 +0100
Subject: [PATCH] iCub3: add realsense imu (#187)

* icub3: add imu in the realsense in the chest

* tests: fix cmake deprecation warning
---
 simmechanics/data/icub3/ICUB_3_all_options.yaml    | 12 +++++++++++-
 .../data/icub3/ICUB_3_all_options_gazebo.yaml.in   | 13 +++++++++++++
 .../data/icub3/conf/gazebo_icub_chest_inertial.ini | 14 ++++++++++++++
 tests/CMakeLists.txt                               |  3 +--
 4 files changed, 39 insertions(+), 3 deletions(-)
 create mode 100644 simmechanics/data/icub3/conf/gazebo_icub_chest_inertial.ini

diff --git a/simmechanics/data/icub3/ICUB_3_all_options.yaml b/simmechanics/data/icub3/ICUB_3_all_options.yaml
index 0877f3e6..924f93c9 100644
--- a/simmechanics/data/icub3/ICUB_3_all_options.yaml
+++ b/simmechanics/data/icub3/ICUB_3_all_options.yaml
@@ -396,7 +396,17 @@ sensors:
         <plugin name="iCub_yarp_gazebo_plugin_camera" filename="libgazebo_yarp_camera.so">
             <yarpConfigurationFile>model://iCub/conf_icub3/gazebo_icub_rgb_camera.ini</yarpConfigurationFile>
         </plugin>
-
+  # According to https://github.com/IntelRealSense/realsense-ros/issues/927 in the realsense
+  # the imu measurements are referred to the depth frame.
+  - frameName: SCSYS_CHEST_DEPTH
+    linkName: chest
+    sensorName: chest_imu_acc_1x1
+    sensorType: "accelerometer"
+    sensorBlobs:
+    - |
+        <plugin name="iCub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+            <yarpConfigurationFile>model://iCub/conf_icub3/gazebo_icub_chest_inertial.ini</yarpConfigurationFile>
+        </plugin>
 assignedMasses:
   # It fixes the right/left asymmetry for the legs
   r_hip_1: 2.48546
diff --git a/simmechanics/data/icub3/ICUB_3_all_options_gazebo.yaml.in b/simmechanics/data/icub3/ICUB_3_all_options_gazebo.yaml.in
index fb841ac1..c689a7d8 100644
--- a/simmechanics/data/icub3/ICUB_3_all_options_gazebo.yaml.in
+++ b/simmechanics/data/icub3/ICUB_3_all_options_gazebo.yaml.in
@@ -391,6 +391,19 @@ sensors:
         <plugin name="iCub_yarp_gazebo_plugin_camera" filename="libgazebo_yarp_camera.so">
             <yarpConfigurationFile>model://iCub/conf_icub3/gazebo_icub_rgb_camera.ini</yarpConfigurationFile>
         </plugin>
+  # According to https://github.com/IntelRealSense/realsense-ros/issues/927 in the realsense
+  # the imu measurements are referred to the depth frame.
+  - frameName: SCSYS_CHEST_DEPTH
+    linkName: chest
+    sensorName: chest_imu_acc_1x1
+    sensorType: "accelerometer"
+    sensorBlobs:
+    - |
+        <plugin name="iCub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+            <yarpConfigurationFile>model://iCub/conf_icub3/gazebo_icub_chest_inertial.ini</yarpConfigurationFile>
+        </plugin>
+
+
 
 @ASSIGNED_COLLISION_GEOMETRIES@
 
diff --git a/simmechanics/data/icub3/conf/gazebo_icub_chest_inertial.ini b/simmechanics/data/icub3/conf/gazebo_icub_chest_inertial.ini
new file mode 100644
index 00000000..b689b8a5
--- /dev/null
+++ b/simmechanics/data/icub3/conf/gazebo_icub_chest_inertial.ini
@@ -0,0 +1,14 @@
+[include "gazebo_icub_robotname.ini"]
+
+[WRAPPER]
+device multipleanalogsensorsserver
+name /${gazeboYarpPluginsRobotName}/chest/inertials
+period 10
+
+[ADDITIONAL_WRAPPER]
+device inertial
+name /${gazeboYarpPluginsRobotName}/chest/inertial
+period 0.01
+
+[IMU_DRIVER]
+device gazebo_imu
\ No newline at end of file
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 8e11af1a..67c1db85 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -4,8 +4,7 @@ find_package(iDynTree REQUIRED)
 find_package(YARP     REQUIRED)
 
 add_executable(icub-model-test icub-model-test.cpp)
-include_directories(${YARP_INCLUDE_DIRS})
-target_link_libraries(icub-model-test ${iDynTree_LIBRARIES} ${YARP_LIBRARIES})
+target_link_libraries(icub-model-test ${iDynTree_LIBRARIES} YARP::YARP_os)
 
 macro(add_icub_model_test yarpRobotName)
     add_test(NAME ${yarpRobotName}ConsistencyCheck