grasp_generator.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>
#include <iri_wam_common_msgs/SimpleBhandPickUpAction.h>
#include "../../include/grasp_generator.h"
#include "geometry_test_helpers.h"

using namespace geometry_test_helpers;

class GraspGeneratorEnvTest : public ::testing::Test
{
    protected:
        iri_wam_common_msgs::SimpleBhandPickUpGoal empty_goal_;
        iri_wam_common_msgs::SimpleBhandPickUpGoal unitary_goal_;
        GraspGenerator generator_;


        virtual void SetUp()
        {
            unitary_goal_.grasp_pose.pose    = generator::generate_fixed_pose(1);
            unitary_goal_.pre_grasp_modifier = generator::generate_fixed_pose(1);
        }
};

TEST_F(GraspGeneratorEnvTest, number_of_phases)
{
    ASSERT_EQ(5, generator_.generate(empty_goal_).size());
}

TEST_F(GraspGeneratorEnvTest, pre_grasp_check)
{
    geometry_msgs::PoseStamped pose_generated;
    geometry_msgs::PoseStamped pre_grasp_expected;

    pre_grasp_expected.pose.position.x = 0;
    pre_grasp_expected.pose.position.y = 2;
    pre_grasp_expected.pose.position.z = 0;
    pre_grasp_expected.pose.orientation.x = 0;
    pre_grasp_expected.pose.orientation.y = 0;
    pre_grasp_expected.pose.orientation.z = 0;
    pre_grasp_expected.pose.orientation.w = 1;

    GraspPhaseSequence sequence = generator_.generate(unitary_goal_);
    pose_generated = static_cast<GraspPhaseMoveArm *>(sequence[0].get())->pose_;

    ASSERT_TRUE(comparison::poses_stamped_are_equal(pre_grasp_expected, pose_generated));
}

// Run all the tests that were declared with TEST()
int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}