CoeffMapLoader_Test.cpp

Go to the documentation of this file.

#include <iostream>
#include <gtest/gtest.h>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include "robot_instance/StringUtilities.h"
#include "robot_instance/RobotInstanceFactory.h"
#include "robot_instance/CoeffMapLoader.h"
#include <ros/package.h>

using namespace std;

class CoeffMapLoaderTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        packagePath      = ros::package::getPath("robot_instance");
        configPath       = packagePath + "/test/config";
        configSafetyPath = packagePath + "/test/configSafety";
        fileName         = "TestRobotInstance.xml";
        instance         = RobotInstanceFactory::createRobotInstanceFromFile(configPath, configSafetyPath, fileName);

        motorCoeffMap     = boost::make_shared<CoeffMap>();
        brainstemCoeffMap = boost::make_shared<CoeffMap>();
    }

    virtual void TearDown()
    {
    }

    string           packagePath, configPath, configSafetyPath, fileName;
    RobotInstancePtr instance;
    CoeffMapPtr      motorCoeffMap;
    CoeffMapPtr      brainstemCoeffMap;
};

TEST_F(CoeffMapLoaderTest, LoadElements)
{
    CoeffMapLoader::loadElements(instance, motorCoeffMap, brainstemCoeffMap);

    EXPECT_EQ(1222, motorCoeffMap->size());
    EXPECT_EQ(144, brainstemCoeffMap->size());

    // Normal assertions to check good data
    // std::vector<std::string> names;
    // for(CoeffMap::iterator it = motorCoeffMap->begin(); it != motorCoeffMap->end(); ++it)
    // {
    //     //cout << it->first << " " << it->second << endl;
    //     names.push_back(it->first);
    // }
    // EXPECT_EQ(1093, names.size());  //                (TestSafety==14 * 9) =  126
    //                 (TestClass==45 * 5) =  225
    //            (TestThumbClass==75 * 1) =   75
    //    (TestPrimaryFingerClass==75 * 1) =   75
    // (TestSecondaryFingersClass==53 * 1) =   53
    //       (TestRightWristClass==89 * 1) =   89
    //           (TestCalibration==50 * 9) =  450 +
    //                                       ====
    //                                       1093

    // Breakdown by mechanism
    //  /r2/left_leg/joint0
    //            TestClass ==  42
    //      TestCalibration ==  48
    //           TestSafety ==  14
    //                         === +
    //                         104
    //
    //  /r2/left_leg/joint1
    //            TestClass ==  42
    //      TestCalibration ==  48
    //           TestSafety ==  14
    //                         === +
    //                         104
    //
    //  /r2/right_leg/gripper/joint0
    //            TestClass ==  42
    //      TestCalibration ==  48
    //           TestSafety ==  14
    //                         === +
    //                         104
    //
    //  /r2/left_arm/joint0
    //            TestClass ==  42
    //      TestCalibration ==  48
    //           TestSafety ==  14
    //                         === +
    //                         104
    //
    //  /r2/left_arm/joint1
    //            TestClass ==  42
    //      TestCalibration ==  48
    //           TestSafety ==  14
    //                         === +
    //                         104
    //
    //  /r2/right_arm/hand/thumb
    //       TestThumbClass ==  75
    //      TestCalibration ==  48
    //           TestSafety ==  14
    //                         === +
    //                         137
    //
    //  /r2/right_arm/hand/index
    //      TestPrimaryFingerClass ==  75
    //             TestCalibration ==  48
    //                  TestSafety ==  14
    //                                === +
    //                                137
    //
    //  /r2/right_arm/hand/ringlittle
    //      TestSecondaryFingersClass ==  53
    //                TestCalibration ==  48
    //                     TestSafety ==  14
    //                                   === +
    //                                   115
    //
    //  /r2/right_arm/wrist
    //      TestRightWristClass ==  89
    //          TestCalibration ==  48
    //               TestSafety ==  14
    //                             === +
    //                             151

    // sort(names.begin(), names.end()); // The underlying map can be in any order; sort to make sure

    // EXPECT_STREQ("/r2/left_arm/joint0/APS1_BiasComp_Offset", names[0].c_str());
    // EXPECT_FLOAT_EQ(0, motorCoeffMap->operator[](names[0]));

    // EXPECT_STREQ("/r2/left_arm/joint0/APS1_C_COS_OFFSET", names[1].c_str());
    // EXPECT_FLOAT_EQ(2064.822523, motorCoeffMap->operator[](names[1]));

    // // skip the middle...

    // commented out because Dustin said so
    //    EXPECT_STREQ("/r2/right_leg/gripper/joint0/YawGain", names[1058].c_str());
    //    EXPECT_FLOAT_EQ(1, motorCoeffMap->operator[](names[1058]));

    //    EXPECT_STREQ("/r2/right_leg/gripper/joint0/YawOffset", names[1059].c_str());
    //    EXPECT_FLOAT_EQ(0, motorCoeffMap->operator[](names[1059]));

    // // Example for how to generate location of coeff on associated Robonet node
    // std::string coeffName = "/r2/left_leg/joint0/Velocity_Limit";
    // std::string mechanism = StringUtilities::getRoboDynEverythingButJoint(coeffName);  //! @todo should use method for getting everything but element...
    // std::string node      = instance->mechanismNodeMap[mechanism];
    // std::string element   = StringUtilities::getRoboDynJoint(coeffName);
    // EXPECT_STREQ("/left_leg/node0/Velocity_Limit", StringUtilities::makeFullyQualifiedRobonetElement(node, element).c_str());
}

int main(int argc, char** argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}