Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <collision_proximity_planner/GetFreePath.h>
00003
00004 int main(int argc, char **argv){
00005 ros::init (argc, argv, "cp_test");
00006 ros::NodeHandle rh;
00007
00008 ros::service::waitForService("collision_proximity_planner/plan");
00009 ros::ServiceClient ik_client = rh.serviceClient<collision_proximity_planner::GetFreePath>("collision_proximity_planner/plan");
00010
00011
00012 collision_proximity_planner::GetFreePath::Request request;
00013 collision_proximity_planner::GetFreePath::Response response;
00014
00015 request.robot_state.joint_state.name.push_back("r_shoulder_pan_joint");
00016 request.robot_state.joint_state.name.push_back("r_shoulder_lift_joint");
00017 request.robot_state.joint_state.name.push_back("r_upper_arm_roll_joint");
00018
00019 request.robot_state.joint_state.name.push_back("r_elbow_flex_joint");
00020 request.robot_state.joint_state.name.push_back("r_forearm_roll_joint");
00021 request.robot_state.joint_state.name.push_back("r_wrist_flex_joint");
00022
00023 request.robot_state.joint_state.name.push_back("r_wrist_roll_joint");
00024
00025 request.robot_state.joint_state.position.push_back(-0.330230);
00026 request.robot_state.joint_state.position.push_back(0.008300);
00027 request.robot_state.joint_state.position.push_back(-1.550000);
00028
00029 request.robot_state.joint_state.position.push_back(-0.859908);
00030 request.robot_state.joint_state.position.push_back(3.139403);
00031 request.robot_state.joint_state.position.push_back(-0.529580);
00032
00033 request.robot_state.joint_state.position.push_back(-1.591270);
00034
00035 if(ik_client.call(request, response))
00036 {
00037 ROS_INFO("Service call was a success");
00038 }
00039 else
00040 ROS_ERROR("Service call failed");
00041 ros::shutdown();
00042 }