Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00040 #include <ros/ros.h>
00041
00042 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00043 #include <kinematics_msgs/GetPositionIK.h>
00044
00045
00046 ros::ServiceClient client_;
00047
00048 bool getConstraintAwareIk(kinematics_msgs::GetConstraintAwarePositionIK::Request &req,
00049 kinematics_msgs::GetConstraintAwarePositionIK::Response &resp) {
00050
00051 kinematics_msgs::GetPositionIK srv;
00052 srv.request.ik_request = req.ik_request;
00053 srv.request.timeout = req.timeout;
00054
00055 client_.call(srv);
00056
00057 ROS_DEBUG("non_constraint_aware_ik_adapter: get'ConstraintAware'Ik is called");
00058 resp.error_code = srv.response.error_code;
00059 resp.solution = srv.response.solution;
00060
00061 return true;
00062 }
00063
00064
00065 int main(int argc, char** argv)
00066 {
00067 ros::init(argc, argv, "non_constraint_aware_ik_adapter");
00068 ros::NodeHandle n;
00069
00070 ros::NodeHandle pn("~");
00071 std::string ik_service;
00072 pn.param<std::string>("ik_service", ik_service, "get_ik");
00073
00074 client_ = n.serviceClient<kinematics_msgs::GetPositionIK>(ik_service);
00075
00076 ros::ServiceServer service = pn.advertiseService("get_constraint_aware_ik", getConstraintAwareIk);
00077
00078 ros::spin();
00079
00080 return 0;
00081 }