TreeFkBenchmark.cpp

Go to the documentation of this file.

#include <iostream>

#include <hayai/hayai.hpp>

#include "robodyn_controllers/RosMsgTreeFk.h"
#include "r2_msgs/JointControlDataArray.h"
#include <ros/package.h>

class TreeFkBenchmark : public hayai::Fixture
{
protected:
    virtual void SetUp()
    {
        ros::Time::init();

        std::string packagePath = ros::package::getPath("robodyn_controllers");
        treeFk.loadFromFile(packagePath + "/test/urdf/r2c_full_body_dynamics.urdf.xml");

        jointStatesMsg.name.push_back("/r2/left_leg/joint0");
        jointStatesMsg.name.push_back("/r2/left_leg/joint1");
        jointStatesMsg.name.push_back("/r2/left_leg/joint2");
        jointStatesMsg.name.push_back("/r2/left_leg/joint3");
        jointStatesMsg.name.push_back("/r2/left_leg/joint4");
        jointStatesMsg.name.push_back("/r2/left_leg/joint5");
        jointStatesMsg.name.push_back("/r2/left_leg/joint6");
        jointStatesMsg.name.push_back("/r2/right_leg/joint0");
        jointStatesMsg.name.push_back("/r2/right_leg/joint1");
        jointStatesMsg.name.push_back("/r2/right_leg/joint2");
        jointStatesMsg.name.push_back("/r2/right_leg/joint3");
        jointStatesMsg.name.push_back("/r2/right_leg/joint4");
        jointStatesMsg.name.push_back("/r2/right_leg/joint5");
        jointStatesMsg.name.push_back("/r2/right_leg/joint6");
        jointStatesMsg.name.push_back("/r2/waist/joint0");
        jointStatesMsg.name.push_back("/r2/left_arm/joint0");
        jointStatesMsg.name.push_back("/r2/left_arm/joint1");
        jointStatesMsg.name.push_back("/r2/left_arm/joint2");
        jointStatesMsg.name.push_back("/r2/left_arm/joint3");
        jointStatesMsg.name.push_back("/r2/left_arm/joint4");
        jointStatesMsg.name.push_back("/r2/left_arm/joint5");
        jointStatesMsg.name.push_back("/r2/left_arm/joint6");
        jointStatesMsg.name.push_back("/r2/right_arm/joint0");
        jointStatesMsg.name.push_back("/r2/right_arm/joint1");
        jointStatesMsg.name.push_back("/r2/right_arm/joint2");
        jointStatesMsg.name.push_back("/r2/right_arm/joint3");
        jointStatesMsg.name.push_back("/r2/right_arm/joint4");
        jointStatesMsg.name.push_back("/r2/right_arm/joint5");
        jointStatesMsg.name.push_back("/r2/right_arm/joint6");
        jointStatesMsg.name.push_back("/r2/neck/joint0");
        jointStatesMsg.name.push_back("/r2/neck/joint1");
        jointStatesMsg.name.push_back("/r2/neck/joint2");

        jointStatesMsg.position.resize(jointStatesMsg.name.size(), 0.);
        jointStatesMsg.velocity.resize(jointStatesMsg.name.size(), 0.);
        jointStatesMsg.effort.resize(jointStatesMsg.name.size(), 0.);

        jointStatusMsg.joint = jointStatesMsg.name;
        jointStatusMsg.data.resize(jointStatusMsg.joint.size());

        // add some random data
        for (unsigned int i = 0; i < jointStatesMsg.name.size(); ++i)
        {
            // position between -90 and 90 deg
            jointStatesMsg.position[i] = 3.1416 * (static_cast<double>(rand() % 10001) / 10000.) - 1.5708;
            // velocity between -1 and 1 rad/s
            jointStatesMsg.velocity[i] = 2.*(static_cast<double>(rand() % 10001) / 10000.) - 1.;
            // effort between -100 and 100 N/m
            jointStatesMsg.effort[i]   = 200.*(static_cast<double>(rand() % 10001) / 10000.) - 100.;

            jointStatusMsg.data[i].coeffState.state = rand() % 2;
        }
    }

    virtual void TearDown()
    {
    }

    RosMsgTreeFk                               treeFk;
    sensor_msgs::JointState                    jointStatesMsg;
    r2_msgs::JointControlDataArray jointStatusMsg;

    typedef std::vector<std::string>::iterator StringItr;
    StringItr                                  jointStatesItr;
    r2_msgs::PoseState             poseStatesOutMsg;
    unsigned int                               count;
};

BENCHMARK_F(TreeFkBenchmark, IntegratedTest, 3, 1000)
{
    for (unsigned int i = 0; i < jointStatusMsg.data.size(); i++)
    {
        if (jointStatusMsg.data[i].coeffState.state == r2_msgs::JointControlCoeffState::NOTLOADED)
        {
            jointStatesItr = std::find(jointStatesMsg.name.begin(), jointStatesMsg.name.end(), jointStatusMsg.joint[i]);

            if (jointStatesItr != jointStatesMsg.name.end())
            {
                int index                      = jointStatesItr - jointStatesMsg.name.begin();
                jointStatesMsg.position[index] = 0;
                jointStatesMsg.velocity[index] = 0;
                jointStatesMsg.effort[index]   = 0;
            }
        }
    }

    treeFk.getPoseState(jointStatesMsg, poseStatesOutMsg);
}

int main(int argc, char** argv)
{
    hayai::Benchmarker::RunAllTests();
    return 0;
}