geometry_test_helpers.h

Go to the documentation of this file.

/* vim: set sw=4 sts=4 et foldmethod=syntax : */

#ifndef SRC_GUARD_TEST_GEOMETRY_TEST_HELPERS_H
#define SRC_GUARD_TEST_GEOMETRY_TEST_HELPERS_H 1

namespace geometry_test_helpers
{
    namespace comparison
    {
        template<class T>
        void display_diff(const T & obj1, const T & obj2, const std::string & msg)
        {
            std::cerr << msg << std::endl << std::endl
                      << "Obj1" << std::endl << obj1 << std::endl
                      << "Obj2" << std::endl << obj2 << std::endl;
            return;
        }

        bool quaternions_are_equals(const geometry_msgs::Quaternion q1,
                                    const geometry_msgs::Quaternion q2,
                                    bool display_error = true)
        {
            if ((q1.x == q2.x) &&
                (q1.y == q2.y) &&
                (q1.z == q2.z) && 
                (q1.w == q2.w))
                return true;

            if (display_error)
                display_diff<geometry_msgs::Quaternion>(q1, q2, "Quaternions are not equal");

            return false;
        }

        bool points_are_equals(const geometry_msgs::Point & p1,
                               const geometry_msgs::Point & p2,
                               bool display_error = true)
        {
            if ((p1.x == p2.x) &&
                (p1.y == p2.y) &&
                (p1.z == p2.z))
                return true;

            if (display_error)
                display_diff<geometry_msgs::Point>(p1, p2, "Points are not equal");

            return false;
        }

        bool poses_are_equal(const geometry_msgs::Pose & p1,
                             const geometry_msgs::Pose & p2,
                             bool display_error = true)
        {
            if ((points_are_equals(p1.position, p2.position, false)) &&
                (quaternions_are_equals(p1.orientation, p2.orientation, false)))
                return true;

            if (display_error)
                display_diff<geometry_msgs::Pose>(p1, p2, "Poses are not equal");

            return false;
        }

        bool poses_stamped_are_equal(const geometry_msgs::PoseStamped p1,
                                     const geometry_msgs::PoseStamped p2,
                                     bool display_error = true)
        {
            if ((p1.header.frame_id == p2.header.frame_id) && 
                (poses_are_equal(p1.pose, p2.pose, false)))
                return true;

            if (display_error)
                display_diff<geometry_msgs::PoseStamped>(p1, p2, "Poses Stamped are not equal");

            return false;
        }
    }

    namespace generator
    {
        geometry_msgs::Pose generate_fixed_pose(int value)
        {
            geometry_msgs::Pose p;

            p.position.x = value;
            p.position.y = value;
            p.position.z = value;
            p.orientation.x = 0;
            p.orientation.y = 1;
            p.orientation.z = 0;
            p.orientation.w = 0;

            return p;
        }
    }
}



#endif