42 #include <kinematics_msgs/IKServiceWithCost.h> 44 #include <kinematics_msgs/IKQuery.h> 45 #include <kinematics_msgs/IKService.h> 46 #include <kinematics_msgs/FKService.h> 48 #include <gtest/gtest.h> 51 #define IK_NEAR_TRANSLATE 1e-5 53 static const std::string
ARM_FK_NAME =
"/pr2_ik_right_arm/fk_service";
54 static const std::string
ARM_IK_NAME =
"/pr2_ik_right_arm/ik_with_cost_service";
61 int rand_num = rand()%100+1;
62 double result = min + (double)((max-min)*rand_num)/101.0;
66 bool NOT_NEAR(
const double &v1,
const double &v2,
const double &NEAR)
68 if(fabs(v1-v2) > NEAR)
84 kinematics_msgs::IKServiceWithCost::Request request;
85 kinematics_msgs::IKServiceWithCost::Response response;
87 request.data.pose_stamped.header.frame_id =
"torso_lift_link";
89 request.data.pose_stamped.pose.position.x = 0.75;
90 request.data.pose_stamped.pose.position.y = -0.188;
91 request.data.pose_stamped.pose.position.z = 0.0;
93 request.data.pose_stamped.pose.orientation.x = 0.0;
94 request.data.pose_stamped.pose.orientation.y = 0.0;
95 request.data.pose_stamped.pose.orientation.z = 0.0;
96 request.data.pose_stamped.pose.orientation.w = 1.0;
98 request.data.positions.resize(7);
99 request.data.joint_names.resize(7);
101 request.data.joint_names[0] =
"r_shoulder_pan_joint";
102 request.data.joint_names[1] =
"r_shoulder_lift_joint";
103 request.data.joint_names[2] =
"r_upper_arm_roll_joint";
104 request.data.joint_names[3] =
"r_elbow_flex_joint";
105 request.data.joint_names[4] =
"r_forearm_roll_joint";
106 request.data.joint_names[5] =
"r_wrist_flex_joint";
107 request.data.joint_names[6] =
"r_wrist_roll_joint";
109 request.link_names.resize(1);
110 request.link_names[0] =
"r_wrist_roll_link";
112 bool ik_service_call = ik_client.call(request,response);
115 for(
unsigned int i=0; i < response.solution.size(); i++)
116 ROS_INFO(
"joint: %s, value: %f",request.data.joint_names[i].c_str(),response.solution[i]);
125 int main(
int argc,
char **argv){
126 ros::init (argc, argv,
"pr2_ik_node_test");
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
static const int NUM_TESTS
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
double gen_rand(double min, double max)
static const std::string ARM_IK_NAME
bool NOT_NEAR(const double &v1, const double &v2, const double &NEAR)
static const std::string ARM_QUERY_NAME
int main(int argc, char **argv)
ROSCPP_DECL void shutdown()
ROSCPP_DECL void spinOnce()
static const std::string ARM_FK_NAME
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)