utility.cpp

Go to the documentation of this file.

#include "pose_prediction_ism/utility.h"
#include <ros/ros.h>
namespace pose_prediction_ism
{
    ISM::Point attributedPoint2ismPoint(asr_msgs::AsrAttributedPoint attributedPoint)
    {
        ISM::Point p;
        p.eigen.x() = attributedPoint.pose.position.x;
        p.eigen.y() = attributedPoint.pose.position.y;
        p.eigen.z() = attributedPoint.pose.position.z;
        return p;
    }
    asr_msgs::AsrAttributedPoint ismPosePtr2attributedPoint(ISM::PosePtr ismPosePtr,
                                                               std::string type,
                                                               std::string identifier   )
    {
        asr_msgs::AsrAttributedPoint attributedPoint;
        attributedPoint.type = type;
        attributedPoint.identifier = identifier;
        attributedPoint.pose.position.x = ismPosePtr->point->eigen.x();
        attributedPoint.pose.position.y = ismPosePtr->point->eigen.y();
        attributedPoint.pose.position.z = ismPosePtr->point->eigen.z();
        attributedPoint.pose.orientation.w = ismPosePtr->quat->eigen.w();
        attributedPoint.pose.orientation.x = ismPosePtr->quat->eigen.x();
        attributedPoint.pose.orientation.y = ismPosePtr->quat->eigen.y();
        attributedPoint.pose.orientation.z = ismPosePtr->quat->eigen.z();
        return attributedPoint;
    }
}