RosMsgTreeFk.cpp

Go to the documentation of this file.

#include "robodyn_controllers/RosMsgTreeFk.h"

RosMsgTreeFk::RosMsgTreeFk()
    : KdlTreeFk()
{
}

RosMsgTreeFk::~RosMsgTreeFk()
{
}

void RosMsgTreeFk::initialize()
{
    KdlTreeFk::initialize();

    KdlTreeFk::getJointNames(jointNames);
    joints.resize(jointNames.size());
    poseMap.clear();

    for (KDL::SegmentMap::const_iterator it = tree.getSegments().begin(); it != tree.getSegments().end(); ++it)
    {
        poseMap.insert(std::make_pair(it->first, KDL::FrameVel()));
    }
}

bool RosMsgTreeFk::getPoseState(const sensor_msgs::JointState& jointState, r2_msgs::PoseState& poseState)
{
    try
    {
        RosMsgConverter::JointStateToJntArrayVel(jointState, jointNames, joints);

        KdlTreeFk::getVelocities(joints, poseMap);

        poseState.header.stamp    = jointState.header.stamp;
        poseState.header.frame_id = getBaseName();
        poseState.name.resize(poseMap.size());
        poseState.positions.resize(poseMap.size());
        poseState.velocities.resize(poseMap.size());
        poseState.accelerations.resize(poseMap.size());

        bool clearAcc = true;
        unsigned int index = 0;

        // iterate through poses
        for (std::map<std::string, KDL::FrameVel>::const_iterator it = poseMap.begin(); it != poseMap.end(); ++it)
        {
            poseState.name[index] = it->first;
            // get reference to pose and update values
            geometry_msgs::Pose& pose = poseState.positions[index];
            pose.position.x = it->second.p.p.x();
            pose.position.y = it->second.p.p.y();
            pose.position.z = it->second.p.p.z();
            it->second.M.R.GetQuaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);

            // get reference to vel and update values
            geometry_msgs::Twist& vel = poseState.velocities[index];
            vel.linear.x  = it->second.p.v.x();
            vel.linear.y  = it->second.p.v.y();
            vel.linear.z  = it->second.p.v.z();
            vel.angular.x = it->second.M.w.x();
            vel.angular.y = it->second.M.w.y();
            vel.angular.z = it->second.M.w.z();

            // check for existance of previous velocity
            prevVel_type::iterator prevIt = prevVel.find(it->first);
            clearAcc = true;
            // get reference to acceleration and update values
            geometry_msgs::Twist& acc = poseState.accelerations[index];

            if (prevIt != prevVel.end())
            {
                // there is a previous velocity, get elapsed time
                double elapsedTime = (poseState.header.stamp - prevIt->second.first).toSec();

                if (elapsedTime > 0.)
                {
                    // good acceleration values
                    acc.linear.x  = (vel.linear.x - prevIt->second.second.linear.x) / elapsedTime;
                    acc.linear.y  = (vel.linear.y - prevIt->second.second.linear.y) / elapsedTime;
                    acc.linear.z  = (vel.linear.z - prevIt->second.second.linear.z) / elapsedTime;
                    acc.angular.x = (vel.angular.x - prevIt->second.second.angular.x) / elapsedTime;
                    acc.angular.y = (vel.angular.y - prevIt->second.second.angular.y) / elapsedTime;
                    acc.angular.z = (vel.angular.z - prevIt->second.second.angular.z) / elapsedTime;
                    clearAcc      = false;
                }

                // reset previous velocity
                prevIt->second.first  = poseState.header.stamp;
                prevIt->second.second = vel;
            }
            else
            {
                // no previous velocity, create
                std::pair<ros::Time, geometry_msgs::Twist>& val = prevVel[it->first];
                val.first                                       = poseState.header.stamp;
                val.second                                      = vel;
            }

            if (clearAcc)
            {
                acc.linear.x  = 0.;
                acc.linear.y  = 0.;
                acc.linear.z  = 0.;
                acc.angular.x = 0.;
                acc.angular.y = 0.;
                acc.angular.z = 0.;
            }

            ++index;
        }

        return true;
    }
    catch (std::exception& e)
    {
        return false;
    }
}