From 2c0de6f3c7114e28be46ac4ea9b3de2e3481d44c Mon Sep 17 00:00:00 2001
From: Konstantinos Karapas <DatSpace@users.noreply.github.com>
Date: Thu, 11 Nov 2021 08:13:18 +0100
Subject: [PATCH] Fixes for nws odometry and cer joints (#306)

---
 R1SN003nws/hardware/odometry/odometry.xml                   | 4 ++--
 R1SN003nws/wrappers/motorControl/cer_alljoints_remapper.xml | 6 ++++--
 R1SN003nws/wrappers/odometry/odometry_nws_ros.xml           | 4 ++--
 3 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/R1SN003nws/hardware/odometry/odometry.xml b/R1SN003nws/hardware/odometry/odometry.xml
index 536c19c66b..85c1927e5d 100644
--- a/R1SN003nws/hardware/odometry/odometry.xml
+++ b/R1SN003nws/hardware/odometry/odometry.xml
@@ -2,8 +2,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="cer_odometry" type="cerOdometry">
-    <param name="geom_r">   10 </param>
-    <param name="geom_l">   10 </param>
+    <param name="geom_r">   0.16416 </param> <!--//m wheels radius          ((320.0 / 2 / 1000.0)*1.026) -->
+    <param name="geom_l">   0.338   </param> <!--//m wheels distance                                     -->
     
     <action phase="startup" level="20" type="attach">
             <param name="device">  cer_mobile_base_mc_remapper </param>
diff --git a/R1SN003nws/wrappers/motorControl/cer_alljoints_remapper.xml b/R1SN003nws/wrappers/motorControl/cer_alljoints_remapper.xml
index 7d6f919128..c0f2798574 100644
--- a/R1SN003nws/wrappers/motorControl/cer_alljoints_remapper.xml
+++ b/R1SN003nws/wrappers/motorControl/cer_alljoints_remapper.xml
@@ -4,7 +4,8 @@
     <param name="joints"> 28 </param>
     <paramlist name="networks">
         <elem name="head">(            0  1   0  1 )</elem>
-        <elem name="torso">(           2  5   0  3 )</elem>
+        <elem name="torso">(           2  4   0  2 )</elem>
+        <elem name="torso_yaw">(       5  5   3  3 )</elem>
         <elem name="right_arm">(       6  13  0  7 )</elem>
         <elem name="left_arm">(        14 21  0  7 )</elem>
         <elem name="right_hand">(      22 23  0  1 )</elem>
@@ -14,7 +15,8 @@
     <action phase="startup" level="6" type="attach">
         <paramlist name="networks">
             <elem name="head">            cer_head_mc               </elem>
-            <elem name="torso">           cer_torso_mc              </elem>
+            <elem name="torso">           cer_torso_equivalent_mc   </elem>
+            <elem name="torso_yaw">       cer_torso_mc              </elem>
             <elem name="base">            cer_mobile_base_mc        </elem>
             <elem name="right_arm">       cer_right_arm_no_hand_mc_remapper  </elem>
             <elem name="right_hand">      cer_right_hand_mc         </elem>
diff --git a/R1SN003nws/wrappers/odometry/odometry_nws_ros.xml b/R1SN003nws/wrappers/odometry/odometry_nws_ros.xml
index caf443fd9e..9d393d1f4f 100644
--- a/R1SN003nws/wrappers/odometry/odometry_nws_ros.xml
+++ b/R1SN003nws/wrappers/odometry/odometry_nws_ros.xml
@@ -4,8 +4,8 @@
 <device xmlns:xi="http://www.w3.org/2001/XInclude" name="cer_odometry_nws_ros" type="odometry2D_nws_ros">
     <param name="topic_name">   /odometry </param>
     <param name="node_name">    /cer_odometry_nws_ros </param>
-    <param name="base_frame">   mobile_base_link </param>
-    <param name="odom_frame">   odometry </param>
+    <param name="base_frame">   mobile_base_body_link </param>
+    <param name="odom_frame">   odom </param>
     
     <action phase="startup" level="10" type="attach">
             <param name="device">  cer_odometry </param>