From 8d6e1e5dd0fa630e418c2c0321a14695b2ff346c Mon Sep 17 00:00:00 2001
From: Nicogene <nicolo.genesio@iit.it>
Date: Thu, 2 Feb 2023 12:17:10 +0100
Subject: [PATCH] ergoCubSN000: add xml files for running ROS2 nws

---
 ergoCubSN000/ergocub_all_ros2.xml             | 108 ++++++++++++++++++
 .../wrappers/FT/left_arm-FT_nws_ros2.xml      |  19 +++
 .../wrappers/FT/left_foot-FT_nws_ros2.xml     |  19 +++
 .../wrappers/FT/right_arm-FT_nws_ros2.xml     |  19 +++
 .../wrappers/FT/right_foot-FT_nws_ros2.xml    |  19 +++
 .../motorControl/alljoints-mc_nws_ros2.xml    |  11 ++
 .../motorControl/alljoints-mc_remapper.xml    |  38 +++---
 7 files changed, 211 insertions(+), 22 deletions(-)
 create mode 100644 ergoCubSN000/ergocub_all_ros2.xml
 create mode 100644 ergoCubSN000/wrappers/FT/left_arm-FT_nws_ros2.xml
 create mode 100644 ergoCubSN000/wrappers/FT/left_foot-FT_nws_ros2.xml
 create mode 100644 ergoCubSN000/wrappers/FT/right_arm-FT_nws_ros2.xml
 create mode 100644 ergoCubSN000/wrappers/FT/right_foot-FT_nws_ros2.xml
 create mode 100644 ergoCubSN000/wrappers/motorControl/alljoints-mc_nws_ros2.xml

diff --git a/ergoCubSN000/ergocub_all_ros2.xml b/ergoCubSN000/ergocub_all_ros2.xml
new file mode 100644
index 0000000000..6409718aa2
--- /dev/null
+++ b/ergoCubSN000/ergocub_all_ros2.xml
@@ -0,0 +1,108 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="ergoCubSN000" xmlns:xi="http://www.w3.org/2001/XInclude">
+
+    <params>
+    <xi:include href="hardware/electronics/pc104.xml" />
+    </params>
+
+    <devices>
+
+    <!-- POS4 -->
+    <xi:include href="wrappers/POS/right_hand-pos_wrapper4.xml" />
+    <xi:include href="hardware/POS/right_hand-pos4.xml" />
+    <xi:include href="wrappers/POS/left_hand-pos_wrapper4.xml" />
+    <xi:include href="hardware/POS/left_hand-pos4.xml" />
+
+    <!-- POS2 -->
+    <xi:include href="wrappers/POS/right_hand-pos_wrapper2.xml" />
+    <xi:include href="hardware/POS/right_hand-pos2.xml" />
+    <xi:include href="wrappers/POS/left_hand-pos_wrapper2.xml" />
+    <xi:include href="hardware/POS/left_hand-pos2.xml" />
+
+    <!-- motor controllers wrappers -->
+    <xi:include href="wrappers/motorControl/left_leg-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/right_leg-mc_wrapper.xml" />
+
+    <xi:include href="wrappers/motorControl/left_leg-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/right_leg-mc_remapper.xml" />
+
+    <xi:include href="wrappers/motorControl/right_arm-mc_wrapper.xml" />
+
+    <xi:include href="wrappers/motorControl/right_arm-mc_remapper.xml" />
+
+    <xi:include href="wrappers/motorControl/left_arm-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/left_arm-mc_remapper.xml" />
+
+    <xi:include href="wrappers/motorControl/torso-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/torso-mc_remapper.xml" />
+
+    <xi:include href="wrappers/motorControl/head-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/head-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/alljoints-mc_remapper.xml" />
+
+    <!-- HEAD -->
+    <xi:include href="hardware/motorControl/head-eb20-j0_1-mc.xml" />
+    <xi:include href="hardware/motorControl/head-eb21-j2_3-mc.xml" />
+
+    <!-- TORSO -->
+    <xi:include href="hardware/motorControl/torso-eb5-j0_2-mc.xml" />
+
+    <!-- LEFT ARM -->
+    <xi:include href="hardware/motorControl/left_arm-eb2-j0_1-mc.xml" />
+    <xi:include href="hardware/motorControl/left_arm-eb4-j2_3-mc.xml" />
+    <xi:include href="hardware/motorControl/left_arm-eb31-j4_6-mc.xml" />
+    <xi:include href="hardware/motorControl/left_arm-eb23-j7_10-mc.xml" />
+    <xi:include href="hardware/motorControl/left_arm-eb25-j11_12-mc.xml" />
+
+    <!-- RIGHT ARM -->
+    <xi:include href="hardware/motorControl/right_arm-eb1-j0_1-mc.xml" />
+    <xi:include href="hardware/motorControl/right_arm-eb3-j2_3-mc.xml" />
+    <xi:include href="hardware/motorControl/right_arm-eb30-j4_6-mc.xml" />
+    <xi:include href="hardware/motorControl/right_arm-eb22-j7_10-mc.xml" />
+    <xi:include href="hardware/motorControl/right_arm-eb24-j11_12-mc.xml" />
+
+    <!-- LEFT LEG -->
+    <xi:include href="hardware/motorControl/left_leg-eb8-j0_3-mc.xml" />
+    <xi:include href="hardware/motorControl/left_leg-eb9-j4_5-mc.xml" />
+
+    <!-- RIGHT LEG -->
+    <xi:include href="hardware/motorControl/right_leg-eb6-j0_3-mc.xml" />
+    <xi:include href="hardware/motorControl/right_leg-eb7-j4_5-mc.xml" />
+
+   <!-- ANALOG SENSORS FT -->
+   <xi:include href="wrappers/FT/left_arm-FT_wrapper.xml" />
+   <xi:include href="wrappers/FT/right_arm-FT_wrapper.xml" />
+
+   <xi:include href="hardware/FT/left_arm-eb2-j0_1-strain.xml" />
+   <xi:include href="hardware/FT/right_arm-eb1-j0_1-strain.xml" />
+
+
+   <xi:include href="wrappers/FT/left_foot-FT_wrapper.xml" />
+   <xi:include href="wrappers/FT/right_foot-FT_wrapper.xml" />
+
+   <xi:include href="hardware/FT/left_leg-eb9-j4_5-strain.xml" />
+   <xi:include href="hardware/FT/right_leg-eb7-j4_5-strain.xml" />
+
+    <!-- INERTIAL SENSOR -->
+    <xi:include href="wrappers/inertials/head-imuFilter_wrapper.xml" />
+    <xi:include href="wrappers/inertials/head-imuFilter.xml" />
+    <xi:include href="wrappers/inertials/head-inertials_wrapper.xml" />
+    <xi:include href="hardware/inertials/head-inertial.xml" />
+
+    <!--  CALIBRATORS -->
+    <xi:include href="calibrators/torso-calib.xml" />
+    <xi:include href="calibrators/left_leg-calib.xml" />
+    <xi:include href="calibrators/right_leg-calib.xml" />
+    <xi:include href="calibrators/left_arm-calib.xml" />
+    <xi:include href="calibrators/right_arm-calib.xml" />
+    <xi:include href="calibrators/head-calib.xml" />
+    <!-- ROS2 -->
+    <xi:include href="wrappers/motorControl/alljoints-mc_nws_ros2.xml" />
+    <xi:include href="wrappers/FT/left_arm-FT_nws_ros2.xml" />
+    <xi:include href="wrappers/FT/right_arm-FT_nws_ros2.xml" />
+    <xi:include href="wrappers/FT/left_foot-FT_nws_ros2.xml" />
+    <xi:include href="wrappers/FT/right_foot-FT_nws_ros2.xml" />
+  </devices>
+</robot>
diff --git a/ergoCubSN000/wrappers/FT/left_arm-FT_nws_ros2.xml b/ergoCubSN000/wrappers/FT/left_arm-FT_nws_ros2.xml
new file mode 100644
index 0000000000..3d64ce4c1b
--- /dev/null
+++ b/ergoCubSN000/wrappers/FT/left_arm-FT_nws_ros2.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_arm-FT_wrapper_ros2" type="wrenchStamped_nws_ros2">
+        <param name="topic_name">      /left_arm_ft                      </param>
+        <param name="node_name">      ergocub_left_arm_ft          </param>
+        <param name="period">      0.01                          </param>
+
+        <action phase="startup" level="10" type="attach">
+            <paramlist name="networks">
+        <!-- The param value must match the device name in the corresponding body_part-ebX-jA_B-strain.xml file -->
+                <elem name="FirstStrain">  left_arm-eb2-j0_1-strain </elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="15" type="detach" />
+    </device>
+
diff --git a/ergoCubSN000/wrappers/FT/left_foot-FT_nws_ros2.xml b/ergoCubSN000/wrappers/FT/left_foot-FT_nws_ros2.xml
new file mode 100644
index 0000000000..2509e3783c
--- /dev/null
+++ b/ergoCubSN000/wrappers/FT/left_foot-FT_nws_ros2.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_foot-FT_wrapper_ros2" type="wrenchStamped_nws_ros2">
+        <param name="topic_name">      /left_foot_heel_tiptoe_ft </param>
+        <param name="node_name">      ergocub_left_foot_heel_tiptoe_ft          </param>
+        <param name="period">      0.01                          </param>
+
+        <action phase="startup" level="10" type="attach">
+            <paramlist name="networks">
+        <!-- The param value must match the device name in the corresponding body_part-ebX-jA_B-strain.xml file -->
+                <elem name="FirstStrain">  left_leg-eb9-j4_5-strain  </elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="15" type="detach" />
+    </device>
+
diff --git a/ergoCubSN000/wrappers/FT/right_arm-FT_nws_ros2.xml b/ergoCubSN000/wrappers/FT/right_arm-FT_nws_ros2.xml
new file mode 100644
index 0000000000..ddb3049c83
--- /dev/null
+++ b/ergoCubSN000/wrappers/FT/right_arm-FT_nws_ros2.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_arm-FT_wrapper_ros2" type="wrenchStamped_nws_ros2">
+        <param name="topic_name">      /right_arm_ft                      </param>
+        <param name="node_name">      ergocub_right_arm_ft          </param>
+        <param name="period">      0.01                          </param>
+
+        <action phase="startup" level="10" type="attach">
+            <paramlist name="networks">
+        <!-- The param value must match the device name in the corresponding body_part-ebX-jA_B-strain.xml file -->
+                <elem name="FirstStrain">  right_arm-eb1-j0_1-strain  </elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="15" type="detach" />
+    </device>
+
diff --git a/ergoCubSN000/wrappers/FT/right_foot-FT_nws_ros2.xml b/ergoCubSN000/wrappers/FT/right_foot-FT_nws_ros2.xml
new file mode 100644
index 0000000000..0e8a365892
--- /dev/null
+++ b/ergoCubSN000/wrappers/FT/right_foot-FT_nws_ros2.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_foot-FT_wrapper_ros2" type="wrenchStamped_nws_ros2">
+        <param name="topic_name">      /right_foot_heel_tiptoe_ft                      </param>
+        <param name="node_name">      ergocub_right_foot_heel_tiptoe_ft          </param>
+        <param name="period">      0.01                          </param>
+
+        <action phase="startup" level="10" type="attach">
+            <paramlist name="networks">
+        <!-- The param value must match the device name in the corresponding body_part-ebX-jA_B-strain.xml file -->
+                <elem name="FirstStrain">  right_leg-eb7-j4_5-strain  </elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="15" type="detach" />
+    </device>
+
diff --git a/ergoCubSN000/wrappers/motorControl/alljoints-mc_nws_ros2.xml b/ergoCubSN000/wrappers/motorControl/alljoints-mc_nws_ros2.xml
new file mode 100644
index 0000000000..52ac8b756e
--- /dev/null
+++ b/ergoCubSN000/wrappers/motorControl/alljoints-mc_nws_ros2.xml
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="alljoints-mc_nws_ros2" type="controlBoard_nws_ros2">
+    <param name="node_name"> ergocub_cb_node </param>
+    <param name="topic_name"> /joint_states </param>
+    <action phase="startup" level="15" type="attach">
+        <param name="device"> alljoints-mc_remapper </param>
+    </action>
+    <action phase="shutdown" level="20" type="detach" />
+</device>
\ No newline at end of file
diff --git a/ergoCubSN000/wrappers/motorControl/alljoints-mc_remapper.xml b/ergoCubSN000/wrappers/motorControl/alljoints-mc_remapper.xml
index 5873b4fabc..c415f10d8e 100644
--- a/ergoCubSN000/wrappers/motorControl/alljoints-mc_remapper.xml
+++ b/ergoCubSN000/wrappers/motorControl/alljoints-mc_remapper.xml
@@ -3,29 +3,23 @@
 
 <device xmlns:xi="http://www.w3.org/2001/XInclude" name="alljoints-mc_remapper" type="controlboardremapper">
     <paramlist name="networks">
-        <elem name="head_joints">       (  0   5   0  5 )</elem>
-        <elem name="face_joints">       (  0   1   0  1 )</elem>
-        <elem name="left_arm_joints">   (  6  12   0  6 )</elem>
-        <elem name="left_hand_joints">  ( 13  21   0  8 )</elem>
-        <elem name="right_arm_joints">  ( 22  28   0  6 )</elem>
-        <elem name="right_hand_joints"> ( 29  37   0  8 )</elem>
-        <elem name="torso_joints">      ( 38  40   0  2 )</elem>
-        <elem name="left_leg_joints">   ( 41  46   0  5 )</elem>
-        <elem name="right_leg_joints">  ( 47  52   0  5 )</elem>
+        <elem name="head_joints">       (  0   3   0  3 )</elem>
+        <elem name="left_arm_joints">   (  4  16   0  12 )</elem>
+        <elem name="right_arm_joints">  ( 18  30   0  12 )</elem>
+        <elem name="torso_joints">      ( 31  33   0  2 )</elem>
+        <elem name="left_leg_joints">   ( 34  39   0  5 )</elem>
+        <elem name="right_leg_joints">  ( 40  45   0  5 )</elem>
     </paramlist>
-    <param name="joints"> 53 </param>
-    <action phase="startup" level="5" type="attach">
+    <param name="joints"> 46 </param>
+    <action phase="startup" level="6" type="attach">
         <paramlist name="networks">
-            <elem name="head_joints">       head_mc </elem>
-            <elem name="face_joints">       face_mc </elem>
-            <elem name="left_arm_joints">   left_arm_mc </elem>
-            <elem name="left_hand_joints">  left_hand_mc </elem>
-            <elem name="right_arm_joints">  right_arm_mc </elem>
-            <elem name="right_hand_joints"> right_hand_mc </elem>
-            <elem name="torso_joints">      torso_mc </elem>
-            <elem name="left_leg_joints">   left_leg_mc </elem>
-            <elem name="right_leg_joints">  right_leg_mc </elem>
+            <elem name="head_joints">       head-mc_remapper </elem>
+            <elem name="left_arm_joints">   left_arm-mc_remapper</elem>
+            <elem name="right_arm_joints">  right_arm-mc_remapper </elem>
+            <elem name="torso_joints">      torso-mc_remapper </elem>
+            <elem name="left_leg_joints">   left_leg-mc_remapper </elem>
+            <elem name="right_leg_joints">  right_leg-mc_remapper </elem>
         </paramlist>
     </action>
-    <action phase="shutdown" level="20" type="detach" />
-</device>
+    <action phase="shutdown" level="19" type="detach" />
+</device>
\ No newline at end of file