pose_helper.cpp

Go to the documentation of this file.

#include "world_model/helper/pose_helper.hpp"

namespace world_model {

PoseHelperPtr PoseHelper::instance_ptr_;

PoseHelperPtr PoseHelper::getInstance() {
    if (!instance_ptr_) {
        instance_ptr_ = PoseHelperPtr(new PoseHelper());
        resetInstance();
    }
    return instance_ptr_;
}

void PoseHelper::resetInstance() {
    if (instance_ptr_) {
        // Params can be reseted here
    }
}

PoseHelper::PoseHelper() {
}


double PoseHelper::calcDistancePositionEucl(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2) {
    return sqrt(pow(pose1.position.x - pose2.position.x, 2)
                + pow(pose1.position.y - pose2.position.y, 2)
                + pow(pose1.position.z - pose2.position.z, 2));
}

double PoseHelper::calcAngularDistanceInRad(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2) {
    Eigen::Quaterniond rotation1 = convertPoseQuatToQuat(pose1);
    rotation1.normalize();
    Eigen::Quaterniond rotation2 = convertPoseQuatToQuat(pose2);
    rotation2.normalize();
    return rotation1.angularDistance(rotation2);
}

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

bool PoseHelper::checkPositionsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double position_threshold) {
    const double distance_position = calcDistancePositionEucl(pose1, pose2);
    return position_threshold > distance_position;
}

bool PoseHelper::checkOrientationsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double orientation_rad_threshold) {
    const double distance_orientation_rad = calcAngularDistanceInRad(pose1, pose2);
    return orientation_rad_threshold > distance_orientation_rad;
}

}