pose_helper.hpp

Go to the documentation of this file.

#pragma once

#include <ros/ros.h>
#include <math.h>
#include <boost/shared_ptr.hpp>
#include <eigen3/Eigen/Geometry>

#include "geometry_msgs/Pose.h"

namespace world_model {

class PoseHelper {

private:
    static boost::shared_ptr<PoseHelper> instance_ptr_;

    Eigen::Quaterniond convertPoseQuatToQuat(const geometry_msgs::Pose &pose) {
        return Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
    }

    PoseHelper();

public:

    static boost::shared_ptr<PoseHelper> getInstance();
    static void resetInstance();

    double calcDistancePositionEucl(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2);
    double calcAngularDistanceInRad(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2);

    bool checkPosesAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double position_threshold, const double orientation_rad_threshold);
    bool checkPositionsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double position_threshold);
    bool checkOrientationsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double orientation_rad_threshold);

};

typedef boost::shared_ptr<PoseHelper> PoseHelperPtr;

}