$search
00001 //Software License Agreement (BSD License) 00002 00003 //Copyright (c) 2008, Willow Garage, Inc. 00004 //All rights reserved. 00005 00006 //Redistribution and use in source and binary forms, with or without 00007 //modification, are permitted provided that the following conditions 00008 //are met: 00009 00010 // * Redistributions of source code must retain the above copyright 00011 // notice, this list of conditions and the following disclaimer. 00012 // * Redistributions in binary form must reproduce the above 00013 // copyright notice, this list of conditions and the following 00014 // disclaimer in the documentation and/or other materials provided 00015 // with the distribution. 00016 // * Neither the name of Willow Garage, Inc. nor the names of its 00017 // contributors may be used to endorse or promote products derived 00018 // from this software without specific prior written permission. 00019 00020 //THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00021 //"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00022 //LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00023 //FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00024 //COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00025 //INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00026 //BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00027 //LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00028 //CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00029 //LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00030 //ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 //POSSIBILITY OF SUCH DAMAGE. 00032 00033 #include <stdio.h> 00034 #include <stdlib.h> 00035 #include <time.h> 00036 #include <ros/ros.h> 00037 00038 #include <kinematics_msgs/IKServiceWithCost.h> 00039 00040 #include <kinematics_msgs/IKQuery.h> 00041 #include <kinematics_msgs/IKService.h> 00042 #include <kinematics_msgs/FKService.h> 00043 00044 #include <gtest/gtest.h> 00045 00046 #define IK_NEAR 1e-4 00047 #define IK_NEAR_TRANSLATE 1e-5 00048 00049 static const std::string ARM_FK_NAME = "/pr2_ik_right_arm/fk_service"; 00050 static const std::string ARM_IK_NAME = "/pr2_ik_right_arm/ik_with_cost_service"; 00051 static const std::string ARM_QUERY_NAME = "/pr2_ik_right_arm/ik_query"; 00052 00053 static const int NUM_TESTS = 1000; 00054 00055 double gen_rand(double min, double max) 00056 { 00057 int rand_num = rand()%100+1; 00058 double result = min + (double)((max-min)*rand_num)/101.0; 00059 return result; 00060 } 00061 00062 bool NOT_NEAR(const double &v1, const double &v2, const double &NEAR) 00063 { 00064 if(fabs(v1-v2) > NEAR) 00065 return true; 00066 return false; 00067 } 00068 00069 void test() 00070 { 00071 srand ( time(NULL) ); // initialize random seed 00072 ros::NodeHandle rh; 00073 ros::service::waitForService(ARM_QUERY_NAME); 00074 ros::service::waitForService(ARM_FK_NAME); 00075 ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::IKQuery>(ARM_QUERY_NAME, true); 00076 ros::ServiceClient fk_client = rh.serviceClient<kinematics_msgs::FKService>(ARM_FK_NAME, true); 00077 ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::IKServiceWithCost>(ARM_IK_NAME, true); 00078 00079 // define the service messages 00080 kinematics_msgs::IKServiceWithCost::Request request; 00081 kinematics_msgs::IKServiceWithCost::Response response; 00082 00083 request.data.pose_stamped.header.frame_id = "torso_lift_link"; 00084 request.data.pose_stamped.header.stamp = ros::Time::now(); 00085 request.data.pose_stamped.pose.position.x = 0.75; 00086 request.data.pose_stamped.pose.position.y = -0.188; 00087 request.data.pose_stamped.pose.position.z = 0.0; 00088 00089 request.data.pose_stamped.pose.orientation.x = 0.0; 00090 request.data.pose_stamped.pose.orientation.y = 0.0; 00091 request.data.pose_stamped.pose.orientation.z = 0.0; 00092 request.data.pose_stamped.pose.orientation.w = 1.0; 00093 00094 request.data.positions.resize(7); 00095 request.data.joint_names.resize(7); 00096 00097 request.data.joint_names[0] = "r_shoulder_pan_joint"; 00098 request.data.joint_names[1] = "r_shoulder_lift_joint"; 00099 request.data.joint_names[2] = "r_upper_arm_roll_joint"; 00100 request.data.joint_names[3] = "r_elbow_flex_joint"; 00101 request.data.joint_names[4] = "r_forearm_roll_joint"; 00102 request.data.joint_names[5] = "r_wrist_flex_joint"; 00103 request.data.joint_names[6] = "r_wrist_roll_joint"; 00104 00105 request.link_names.resize(1); 00106 request.link_names[0] = "r_wrist_roll_link"; 00107 00108 bool ik_service_call = ik_client.call(request,response); 00109 if(ik_service_call) 00110 { 00111 for(unsigned int i=0; i < response.solution.size(); i++) 00112 ROS_INFO("joint: %s, value: %f",request.data.joint_names[i].c_str(),response.solution[i]); 00113 } 00114 else 00115 { 00116 ROS_ERROR("Call was unsuccessful"); 00117 } 00118 ros::shutdown(); 00119 } 00120 00121 int main(int argc, char **argv){ 00122 ros::init (argc, argv, "pr2_ik_node_test"); 00123 ros::spinOnce(); 00124 test(); 00125 }