test_kinematics_as_plugin.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <pluginlib/class_loader.h>
#include <kinematics_base/kinematics_base.h>
#include <gtest/gtest.h>


class MyTest
{
 public:
  bool initialize()
  {
    ros::NodeHandle rh;
    pluginlib::ClassLoader<kinematics::KinematicsBase> kinematics_loader("kinematics_base", "kinematics::KinematicsBase");
    kinematics_solver = NULL;
    try{
      kinematics_solver = kinematics_loader.createClassInstance("pr2_arm_kinematics/PR2ArmKinematicsPlugin");
    }
    catch(pluginlib::PluginlibException& ex){
      //handle the class failing to load
      ROS_ERROR("The plugin failed to load. Error: %s", ex.what());
      return false;
    }
    if(kinematics_solver->initialize("right_arm"))
      return true;
    else
      return false;
  };

  void pose_callback(const geometry_msgs::Pose &ik_pose, 
                     const std::vector<double> &joint_state, 
                     int &error_code)
  {
    std::vector<std::string> link_names;
    link_names.push_back("r_elbow_flex_link");
    std::vector<geometry_msgs::Pose> solutions;
    kinematics_solver->getPositionFK(link_names,joint_state,solutions);
    if(solutions[0].position.z > 0.0)
      error_code = 1;
    else
      error_code = -1;
  };

  void joint_state_callback(const geometry_msgs::Pose &ik_pose, 
                            const std::vector<double> &joint_state, 
                            int &error_code)
  {
    error_code = 1;
  };
  kinematics::KinematicsBase* kinematics_solver;
};

TEST(PR2ArmIKPlugin, testplugin)
{
  MyTest my_test;
  ASSERT_TRUE(my_test.initialize());
  // Test getting chain information
  std::string root_name = my_test.kinematics_solver->getBaseFrame();
  EXPECT_TRUE(root_name == std::string("torso_lift_link"));
  std::string tool_name = my_test.kinematics_solver->getToolFrame();
  EXPECT_TRUE(tool_name == std::string("r_wrist_roll_link"));
  std::vector<std::string> joint_names = my_test.kinematics_solver->getJointNames();
  EXPECT_EQ((int)joint_names.size(),7);
  
  EXPECT_EQ(joint_names[0],"r_shoulder_pan_joint");
  EXPECT_EQ(joint_names[1],"r_shoulder_lift_joint");
  EXPECT_EQ(joint_names[2],"r_upper_arm_roll_joint");
  EXPECT_EQ(joint_names[3],"r_elbow_flex_joint");
  EXPECT_EQ(joint_names[4],"r_forearm_roll_joint");
  EXPECT_EQ(joint_names[5],"r_wrist_flex_joint");
  EXPECT_EQ(joint_names[6],"r_wrist_roll_joint");
  
  //Test inverse kinematics
  geometry_msgs::Pose pose;
  pose.position.x = 0.75;
  pose.position.y = -0.188;
  pose.position.z = 0.0;
  pose.orientation.x = 0.0;
  pose.orientation.y = 0.0;
  pose.orientation.z = 0.0;
  pose.orientation.w = 1.0;
  std::vector<double> seed,solution;
  seed.resize(7,0.0);
  double timeout = 5.0;
  int error_code;
  bool result = my_test.kinematics_solver->searchPositionIK(pose,seed,timeout,solution,error_code);
  EXPECT_TRUE(result);
  result = my_test.kinematics_solver->getPositionIK(pose,solution,solution,error_code);
  EXPECT_TRUE(result);  

  result = my_test.kinematics_solver->searchPositionIK(pose,seed,timeout,solution,
                                                       boost::bind(&MyTest::pose_callback, &my_test, _1, _2, _3),
                                                       boost::bind(&MyTest::joint_state_callback, &my_test, _1, _2, _3),
                                                       error_code);
  EXPECT_TRUE(result);    
}

int main(int argc, char **argv){
  testing::InitGoogleTest(&argc, argv);
  ros::init (argc, argv, "pr2_right_arm_ik_test");
  return RUN_ALL_TESTS();
}