iri_grasp_actions_test.cpp

Go to the documentation of this file.

#include <time.h>
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <gtest/gtest.h>
#include <sensor_msgs/JointState.h>
#include <iri_wam_common_msgs/SimpleBhandPickUpAction.h>
#include <object_manipulation_msgs/GripperTranslation.h>

#include "geometry_test_helpers.h"
#include "iri_bhand_fake_recorder.cpp"
#include "iri_wam_joint_move_fake_recorder.cpp"
#include "iri_ik_fake_recorder.cpp"
#include "../../include/grasp_actions_alg_node.h"

using namespace geometry_test_helpers;

const size_t DOF = 7;

std::vector<double> get_random_seven_joints_position()
{
    std::vector<double> m;

    srand(time(NULL));

    for (int i=0; i < 7; i++)
        m.push_back(rand() % 10 + 1);

    return m;
}

std::string ns = "/iri_grasp_actions_test/";

TEST(Bhand, constructor)
{
    ASSERT_NO_THROW( 
            BhandFakeRecorder * p = new BhandFakeRecorder("bhand_cmd");
            delete p;
    );
}

TEST(Bhand, simple_cmd)
{
    ros::NodeHandle   n;
    BhandFakeRecorder bhand_fake(ns + "bhand_cmd");

    iri_wam_common_msgs::bhand_cmd cmd;

    cmd.request.bhandcmd = "GTO";

    ros::ServiceClient client = n.serviceClient<iri_wam_common_msgs::bhand_cmd>
                                                                   (ns + "bhand_cmd");
    client.call(cmd);
    usleep(400); // some time is needed to transmit the msg through topic

    ASSERT_EQ(cmd.request.bhandcmd, bhand_fake.getLastMsg());
}


TEST(JointsMove, constructor)
{
    ASSERT_NO_THROW( 
            WamJointMoveFakeRecorder<DOF> * p = new WamJointMoveFakeRecorder<DOF>("joints_move");
            delete p;
    );
}

TEST(JointsMove, simple_cmd)
{
    ros::NodeHandle n;
    WamJointMoveFakeRecorder<DOF> joints_move_fake(ns + "joints_move");

    iri_wam_common_msgs::joints_move cmd;

    cmd.request.joints = get_random_seven_joints_position();

    ros::ServiceClient client = n.serviceClient<iri_wam_common_msgs::joints_move>
                                                                   (ns + "joints_move");
    client.call(cmd);
    usleep(400); // some time is needed to transmit the msg through topic

    ASSERT_EQ(cmd.request.joints, joints_move_fake.getLastMsg());
}

TEST(JointsMove, bad_joints)
{
    ros::NodeHandle n;
    WamJointMoveFakeRecorder<DOF> joints_move_fake(ns + "joints_move");

    iri_wam_common_msgs::joints_move cmd;

    cmd.request.joints = get_random_seven_joints_position();
    // Add a bad one more position
    cmd.request.joints.push_back(0.0);

    ros::ServiceClient client = n.serviceClient<iri_wam_common_msgs::joints_move>
                                                                   (ns + "joints_move");
    client.call(cmd);
    usleep(400); // some time is needed to transmit the msg through topic

    ASSERT_FALSE(cmd.response.success);
}

TEST(IK, constructor)
{
    ASSERT_NO_THROW(
            IKFakeRecorder * p = new IKFakeRecorder("get_wam_ik");
            delete p;
    );
}

TEST(IK, simple_ik)
{
    ros::NodeHandle n;
    IKFakeRecorder ik_fake(ns + "get_wam_ik");
    geometry_msgs::PoseStamped pose_st;
    pose_st.pose = geometry_test_helpers::generator::generate_fixed_pose(1);

    iri_wam_common_msgs::wamInverseKinematics cmd;

    cmd.request.pose = pose_st;
    ros::ServiceClient client = n.serviceClient<iri_wam_common_msgs::wamInverseKinematics>
                                                                   (ns + "get_wam_ik");
    client.call(cmd);
    usleep(400); // some time is needed to transmit the msg through topic

    ASSERT_TRUE(comparison::poses_stamped_are_equal(cmd.request.pose, ik_fake.getLastMsg()));
}

TEST(GraspActions, constructor)
{
    ASSERT_NO_THROW(
            GraspActionsAlgNode * p = new GraspActionsAlgNode();
            delete p;
    );
}

TEST(GraspActions, test_movement)
{
    ros::NodeHandle n;
    GraspActionsAlgNode           grasp_node;
    BhandFakeRecorder             bhand_fake(ns + "bhand_cmd");
    WamJointMoveFakeRecorder<DOF> joint_move_fake(ns + "joints_move");
    IKFakeRecorder                ik_fake(ns + "get_wam_ik");
    std::string                   default_grasp("SGO");
    std::string                   default_pre_grasp("SGC");
    geometry_msgs::Pose           grasp_pose, pre_grasp_modifier;
    object_manipulation_msgs::GripperTranslation lift;

    lift.direction.vector.x =  1;
    lift.direction.vector.y =  1;
    lift.direction.vector.z = -1;
    lift.desired_distance = 0.2;

    iri_wam_common_msgs::SimpleBhandPickUpGoal goal;

    actionlib::SimpleActionClient<iri_wam_common_msgs::SimpleBhandPickUpAction> ac(ns + "pickup", true);
    ac.waitForServer(); //will wait for infinite time

    // Fullfill the message
    grasp_pose                          = generator::generate_fixed_pose(1);
    pre_grasp_modifier                  = generator::generate_fixed_pose(2);
    goal.grasp_pose.pose                = grasp_pose;
    goal.pre_grasp_modifier             = pre_grasp_modifier;
    goal.lift                           = lift;
    goal.fingers_position_for_grasp.push_back(default_grasp);
    goal.fingers_position_for_pre_grasp.push_back(default_pre_grasp);

    ac.sendGoal(goal);

    //wait for the action to return
    bool finished_before_timeout = ac.waitForResult(ros::Duration(10.0));

    if (! finished_before_timeout)
        FAIL() << "Action did not finish before the time out.";

    actionlib::SimpleClientGoalState state = ac.getState();

    std::vector<std::vector<double> > movements   = joint_move_fake.getHistoryOfMsg();
    std::vector<std::string>  fingers_positions   = bhand_fake.getHistoryOfMsg();
    std::vector<geometry_msgs::PoseStamped> poses = ik_fake.getHistoryOfMsg();

    ASSERT_EQ(state.toString(), "SUCCEEDED");
    ASSERT_EQ(movements.size(), 3); // pre-grasp, grasp, lift
    ASSERT_EQ(poses.size(), 3);
    // Check if pre_grasp movement is not deeper (in Z) than grasp movement
    ASSERT_LE(poses[0].pose.position.z, poses[1].pose.position.z);
    // Check if lift movement is not deeper (in Z) than grasp movement
    ASSERT_LE(poses[2].pose.position.z, poses[1].pose.position.z);
    ASSERT_EQ(fingers_positions.size(), 2); // pre-grasp, grasp
    ASSERT_EQ(fingers_positions[0], default_pre_grasp);
    ASSERT_EQ(fingers_positions[1], default_grasp);
}

// Run all the tests that were declared with TEST()
int main(int argc, char **argv)
{
    ros::init(argc, argv, "iri_grasp_actions_test");

    // create asynchronous thread for handling service requests
    ros::AsyncSpinner spinner(1);
    spinner.start();

    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}