iCubGenova11 add files for fineCalibrationChecker

iCubGenova11/CMakeLists.txt

@@ -10,4 +10,5 @@ yarp_install(DIRECTORY cartesian DESTINATION ${ICUBCONTRIB_ROBOTS_INSTALL_DIR}/$
 yarp_install(DIRECTORY wrappers DESTINATION ${ICUBCONTRIB_ROBOTS_INSTALL_DIR}/${appname})
 yarp_install(DIRECTORY hardware DESTINATION ${ICUBCONTRIB_ROBOTS_INSTALL_DIR}/${appname})
 yarp_install(DIRECTORY camera DESTINATION ${ICUBCONTRIB_ROBOTS_INSTALL_DIR}/${appname})
-yarp_install(DIRECTORY estimators DESTINATION ${ICUBCONTRIB_ROBOTS_INSTALL_DIR}/${appname}) 
+yarp_install(DIRECTORY estimators DESTINATION ${ICUBCONTRIB_ROBOTS_INSTALL_DIR}/${appname})
+yarp_install(DIRECTORY checker DESTINATION ${ICUBCONTRIB_ROBOTS_INSTALL_DIR}/${appname}) 

iCubGenova11/calibrators/right_arm-calib.xml

@@ -47,4 +47,3 @@
 		<action phase="interrupt3" level="1" type="abort" />
 
 	</device>
-

iCubGenova11/checker/fineCalibrationCheckerConfig.xml

@@ -0,0 +1,27 @@
+<?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="fineCalibrationChecker" type="fineCalibrationChecker">
+        <param name="devicename">fineCalibrationChecker</param>
+        <param name="robotname">iCubGenova11</param>
+        <param name="remoteRawValuesPort">/left_arm/raw_data</param> <!--right_arm/raw_data if needed other control board-->
+        <!-- <param name="axesNamesList">(r_shoulder_pitch, r_shoulder_roll, r_shoulder_yaw, r_elbow)</param> -->
+        <param name="axesNamesList">(l_shoulder_pitch, l_shoulder_roll, l_shoulder_yaw, l_elbow)</param>
+        <!-- <param name="goldPositions">(35535, 33135, 24960, 25983)</param> --> <!--right_arm-->
+        <param name="goldPositions">(58559, 22639, 48830, 51983)</param> <!--left_arm-->
+        <!--<param name="calibrationDeltas">(1.0, -9.2, -14.2, 0 )</param> --> <!--right_arm-->
+        <param name="calibrationDeltas">(0, -9.2, -17.1, -2.7 )</param> <!--left_arm-->
+        <param name="encoderResolutions">(262144, 262144, 262144, 262144)</param> <!--2^18 raw encoder resolution-->
+
+
+        <action phase="startup" level="15" type="attach">
+            <paramlist name="networks">
+                <elem name="setup_joint_0">left_arm-eb1-j0_3-mc</elem>
+                <!-- <elem name="setup_joint_0">right_arm-eb3-j0_3-mc</elem> --> <!--right_arm -->
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="2" type="detach" />
+
+    </device>

iCubGenova11/icub_all_no_legs.xml

@@ -95,5 +95,9 @@
     <xi:include href="calibrators/head-calib.xml" />
     <xi:include href="calibrators/face-calib.xml" />
 
+    <!-- FINE CALIBRATOR CHECKER DEMO and TEST -->
+    <xi:include href="wrappers/motorControl/left_arm-mc-rawval-nws_wrapper.xml" />
+    <!-- <xi:include href="wrappers/motorControl/right_arm-mc-rawval-nws_wrapper.xml" /> -->
+    <xi:include href="checker/fineCalibrationCheckerConfig.xml" />
     </devices>
 </robot>

iCubGenova11/wrappers/motorControl/left_arm-mc-rawval-nws_wrapper.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="left_arm-rawval_nws_yarp" type="rawValuesPublisherServer">
+    <param name="name"> /left_arm/raw_data </param>
+    <param name="period"> 2 </param>
+    <action phase="startup" level="10" type="attach">
+        <param name="device"> left_arm-eb1-j0_3-mc </param>
+    </action>
+    <action phase="shutdown" level="15" type="detach" />
+</device>

iCubGenova11/wrappers/motorControl/right_arm-mc-rawval-nws_wrapper.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="right_arm-rawval_nws_yarp" type="rawValuesPublisherServer">
+    <param name="name"> /right_arm/raw_data </param>
+    <param name="period"> 2 </param>
+    <action phase="startup" level="10" type="attach">
+        <param name="device"> right_arm-eb3-j0_3-mc </param>
+    </action>
+    <action phase="shutdown" level="15" type="detach" />
+</device>

tests/dry-run/check-xml/schemas/wrapper.xsd

@@ -75,6 +75,7 @@
     <xs:enumeration value="controlBoard_nws_yarp" />
     <xs:enumeration value="controlBoard_nws_ros" />
     <xs:enumeration value="controlBoard_nws_ros2" />
+    <xs:enumeration value="rawValuesPublisherServer"/>
   </xs:restriction>
 </xs:simpleType>
 

tests/dry-run/check-xml/src/check-xml.cpp

@@ -110,7 +110,51 @@ bool loadXmlFile(const std::string& robot_dir, const std::string& filename, pugi
     }
 }
 
-bool checkWrappersRemappers(const std::string& robot_dir, std::vector<std::string>& vectorAllFiles,
+/**
+check wrappers that are independent from calibrators and remappers
+1- find devices that are inside hardware/motorControl whose file ends with -mc.xml
+2- for each file found, look for the device name and save in a list
+3- compare this list of targets with the devices inside *-rawval-nws_wrapper.xml
+ */
+bool checkMotorControlWrappers(const std::string& robot_dir, std::vector<std::string>& vectorAllFiles, std::vector<std::string>& vectorUnremapped, bool& pass){
+    std::vector<std::string> list_of_targets;
+    pugi::xml_document doc_mc;
+    std::string device, target;
+
+    for (auto ele : vectorAllFiles)
+    {
+        if(ele.find("hardware/motorControl") != std::string::npos && ele.find("-mc.xml") != std::string::npos) {
+            if(!loadXmlFile(robot_dir, ele, doc_mc)) return false;
+            pugi::xpath_node device_name = doc_mc.select_node("device");
+            device = trim(device_name.node().attribute("name").value());
+            std::cout << "Adding device: " << device << " to the list of targets to be checked" << std::endl;
+            list_of_targets.push_back(device);
+            doc_mc.reset();
+        }
+    }
+
+    for (auto ele : vectorAllFiles)
+    {
+        if(ele.find("wrappers/motorControl") != std::string::npos && ele.find("rawval-nws_wrapper.xml") != std::string::npos) {
+            if(!loadXmlFile(robot_dir, ele, doc_mc)) return false;
+            pugi::xpath_node action_startup = doc_mc.select_node("//action[@phase='startup']/param[@name='device']/text()");
+            target = trim(action_startup.node().value());
+            if(std::find(list_of_targets.begin(), list_of_targets.end(), target) != list_of_targets.end()){
+                vectorUnremapped.push_back(ele);
+                std::cout << target << " - MOTOR CONTROL WRAPPER CHECK PASSED!" << std::endl;
+            }
+            else{
+                std::cerr << target << " - MOTOR CONTROL WRAPPER CHECK FAILED!" << std::endl;
+                pass = false;
+            }
+            doc_mc.reset();
+        }
+    }
+
+    return true;
+}
+
+bool checkWrappersRemappers(const std::string& robot_dir, std::vector<std::string>& vectorAllFiles, std::vector<std::string>& vectorUnremapped,
                             const std::string& part, const std::string& target, bool& pass){
     bool found = false;
     pugi::xml_document doc_wrapper, doc_remapper;
@@ -133,6 +177,14 @@ bool checkWrappersRemappers(const std::string& robot_dir, std::vector<std::strin
         if(ele.find("wrappers/motorControl") != std::string::npos){
             if(ele.find("wrapper.xml") != std::string::npos) {
                 if(!loadXmlFile(robot_dir, ele, doc_wrapper)) return false;
+                pugi::xpath_node device_name = doc_wrapper.select_node("device");
+                device = trim(device_name.node().attribute("name").value());
+                if (std::find(vectorUnremapped.begin(), vectorUnremapped.end(), ele) != vectorUnremapped.end())
+                {
+                    std::cout << part << " - SKIPPED DEVICE " << device << " SINCE UNREMAPPED!" << std::endl;
+                    found = true;
+                    continue;
+                }
                 pugi::xpath_node action_startup = doc_wrapper.select_node("//action[@phase='startup']/param[@name='device']/text()");
                 device = trim(action_startup.node().value());
                 if(device == target) std::cout << part <<" - WRAPPER CHECK PASSED!" << std::endl;
@@ -183,6 +235,13 @@ bool checkCartesian(const std::string& robot_dir, std::vector<std::string>& vect
 }
 
 bool checkCalibratorsWrappersRemappers(const std::string& robot_dir, std::vector<std::string>& vectorAllFiles, bool& pass){
+    // save first list of unremapped devices to skip the check for wrappers/remappers
+    // we cannot check the single element inside the for loop by passing ele to method
+    // since we first need to fill the list of targets by looking in hardware/motorControl directory
+    // while the unramapped devices are inside wrappers/motorControl directory 
+    std::vector<std::string> vectorUnremapped;
+    checkMotorControlWrappers(robot_dir, vectorAllFiles, vectorUnremapped, pass);
+
     for (auto ele : vectorAllFiles)
     {
         std::string part, target1, target2;
@@ -209,7 +268,7 @@ bool checkCalibratorsWrappersRemappers(const std::string& robot_dir, std::vector
                     pass = false;
                 }
 
-                checkWrappersRemappers(robot_dir, vectorAllFiles, part, target1, pass);
+                checkWrappersRemappers(robot_dir, vectorAllFiles, vectorUnremapped, part, target1, pass);
                 if(ele.find("arm") != std::string::npos){
                     checkCartesian(robot_dir, vectorAllFiles, part, target1, pass);
                 }
@@ -248,7 +307,7 @@ int main(int argc, char *argv[]) {
     std::cout << std::endl << "2 - test included xml files presence **************" << std::endl << std::endl;
 
     if(!checkIncludedFiles(robot_dir, vectorAllFiles, filename, doc)) return EXIT_FAILURE;
-   
+
     // checks calibrators/wrappers/remappers/cartesian xml files consistency
     std::cout << std::endl << "3 - test calibrators/wrappers/remappers/cartesian xml files consistency **************" << std::endl << std::endl;