test_client.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <tf/tf.h>
00003 //#include <actionlib/actionlib.h>
00004 #include <actionlib/client/simple_action_client.h>
00005 
00006 #include <find_base_pose/FindBasePoseAction.h>
00007 
00008 
00009 void test_fridge(actionlib::SimpleActionClient<find_base_pose::FindBasePoseAction> &ac)
00010 {
00011 
00012     float pts[][7] =
00013     {
00014         {0.478704, -1.0355, 1.18101, 0.767433, 0.639987, 0.022135, 0.0311955},
00015         {0.489086, -0.984206, 1.17956, 0.797904, 0.601535, 0.01726, 0.0347398},
00016         {0.494529, -0.937741, 1.1803, 0.830545, 0.555891, 0.0110758, 0.0325103},
00017         {0.504333, -0.909376, 1.18066, 0.849808, 0.526105, 0.00709967, 0.03147},
00018         {0.507886, -0.884252, 1.17954, 0.8814, 0.471274, 0.00699274, 0.0313926},
00019         {0.516993, -0.854729, 1.18006, 0.903026, 0.428457, 0.00859376, 0.0299171},
00020         {0.527833, -0.832331, 1.1803, 0.920176, 0.390256, 0.0125722, 0.0286066},
00021         {0.541463, -0.80644, 1.18, 0.931353, 0.362808, 0.0186723, 0.0245782},
00022         {0.571712, -0.760535, 1.17887, 0.936451, 0.349496, 0.024334, 0.017896},
00023         {0.608236, -0.715618, 1.17839, 0.944274, 0.327791, 0.0273483, 0.0123364},
00024         {0.647457, -0.676296, 1.17812, 0.954053, 0.298037, 0.0302956, 0.00623379},
00025         {0.690692, -0.638766, 1.17999, 0.964469, 0.262043, 0.0336022, 0.00195834},
00026         {0.734141, -0.609302, 1.18042, 0.974717, 0.220844, 0.0339708, -0.00102721},
00027         {0.781735, -0.583995, 1.17916, 0.983083, 0.180164, 0.0327274, -0.00426907},
00028         {0.828575, -0.564397, 1.17937, 0.990023, 0.137179, 0.0315954, -0.00617472},
00029         {0.870116, -0.550422, 1.17831, 0.995336, 0.0920069, 0.0283872, -0.00586025},
00030         {0.921693, -0.544899, 1.17853, 0.998734, 0.0415909, 0.0273629, -0.00714236},
00031         {0.971471, -0.549669, 1.17854, 0.998732, 0.0416648, 0.0273237, -0.00716123}
00032     };
00033 
00034     find_base_pose::FindBasePoseGoal goal;
00035     std_msgs::Int32 int32;
00036     int32.data = 0;
00037 
00038     geometry_msgs::PoseStamped ps;
00039     //ps.header.frame_id = "base_link";
00040     ps.header.frame_id = "map";
00041     ps.header.stamp = ros::Time::now();
00042 
00043     for (int k = 0; k < 17; ++k)
00044     {
00045         int adder = k * 8;
00046         int32.data = 0;
00047         ps.pose.position.x  = pts[k][0];
00048         ps.pose.position.y  = pts[k][1];
00049         ps.pose.position.z  = pts[k][2];
00050         ps.pose.orientation.x = pts[k][3];
00051         ps.pose.orientation.y = pts[k][4];
00052         ps.pose.orientation.z = pts[k][5];
00053         ps.pose.orientation.w = pts[k][6];
00054         goal.target_poses.push_back(ps);
00055         goal.arm.push_back(int32);
00056         ROS_INFO("pt");
00057     }
00058 
00059     ac.waitForServer(); //will wait for infinite time
00060 
00061     ac.sendGoal(goal);
00062 
00063     //wait for the action to return
00064     bool finished_before_timeout = ac.waitForResult(ros::Duration(15.0));
00065 
00066     if (finished_before_timeout)
00067     {
00068         actionlib::SimpleClientGoalState state = ac.getState();
00069         ROS_INFO("Action finished: %s",state.toString().c_str());
00070         find_base_pose::FindBasePoseResultConstPtr res = ac.getResult();
00071         for (int k = 0; k < res->base_poses.size(); ++k)
00072         {
00073             tf::Stamped<tf::Pose> respose;
00074             tf::poseStampedMsgToTF(res->base_poses[k], respose);
00075             ROS_INFO("Result: %f %f  %f", res->base_poses[k].pose.position.x, res->base_poses[k].pose.position.y,respose.getRotation().getAngle());
00076         }
00077 
00078     }
00079     else
00080         ROS_INFO("Action did not finish before the time out.");
00081 }
00082 
00083 
00084 
00085 
00086 int main(int argc, char **argv)
00087 {
00088     ros::init(argc, argv, "test_find_base_pose");
00089 
00090     // create the action client
00091     actionlib::SimpleActionClient<find_base_pose::FindBasePoseAction> ac("find_base_pose_action",true);
00092 
00093     find_base_pose::FindBasePoseGoal goal;
00094 
00095     geometry_msgs::PoseStamped ps;
00096     //ps.header.frame_id = "base_link";
00097     ps.header.frame_id = "base_link";
00098     ps.header.stamp = ros::Time::now();
00099 
00100     float pos[] = {0.418, -0.219, 0.802};
00101     float rot[] = {0.695, 0.719, 0.004, 0.033};
00102     //pos[0] += 1.0;
00103     ps.pose.position.x = pos[0];
00104     ps.pose.position.y = pos[1];
00105     ps.pose.position.z = pos[2];
00106     ps.pose.orientation.x = rot[0];
00107     ps.pose.orientation.y = rot[1];
00108     ps.pose.orientation.z = rot[2];
00109     ps.pose.orientation.w = rot[3];
00110 //   ps.pose.position.x = 0.221; ps.pose.position.y = -.411; ps.pose.position.z = 0.965;
00111 //   ps.pose.orientation.x = -0.426; ps.pose.orientation.y = 0.091; ps.pose.orientation.x = 0.645; ps.pose.orientation.w = .628;
00112 
00113     std_msgs::Int32 int32;
00114     int32.data = 0;
00115 
00116     if (argc > 1)
00117     {
00118         for (int k = 0; k < atoi(argv[1]); ++k)
00119         {
00120             int adder = k * 8;
00121             int32.data = atoi(argv[2 + adder]);
00122             ps.pose.position.x  = atof(argv[3 + adder]);
00123             ps.pose.position.y  = atof(argv[4 + adder]);
00124             ps.pose.position.z  = atof(argv[5 + adder]);
00125             ps.pose.orientation.x = atof(argv[6 + adder]);
00126             ps.pose.orientation.y = atof(argv[7 + adder]);
00127             ps.pose.orientation.z = atof(argv[8 + adder]);
00128             ps.pose.orientation.w = atof(argv[9 + adder]);
00129             goal.target_poses.push_back(ps);
00130             goal.arm.push_back(int32);
00131             ROS_INFO("pt");
00132         }
00133     }
00134 
00135 
00136     ROS_INFO("Waiting for action server to start.");
00137     // wait for the action server to start
00138     ac.waitForServer(); //will wait for infinite time
00139 
00140     test_fridge(ac);
00141 
00142 
00143     ac.sendGoal(goal);
00144 
00145     //wait for the action to return
00146     bool finished_before_timeout = ac.waitForResult(ros::Duration(15.0));
00147 
00148     if (finished_before_timeout)
00149     {
00150         actionlib::SimpleClientGoalState state = ac.getState();
00151         ROS_INFO("Action finished: %s",state.toString().c_str());
00152         find_base_pose::FindBasePoseResultConstPtr res = ac.getResult();
00153         for (int k = 0; k < res->base_poses.size(); ++k)
00154         {
00155             tf::Stamped<tf::Pose> respose;
00156             tf::poseStampedMsgToTF(res->base_poses[k], respose);
00157             ROS_INFO("Result: %f %f  %f", res->base_poses[k].pose.position.x, res->base_poses[k].pose.position.y,respose.getRotation().getAngle());
00158         }
00159 
00160     }
00161     else
00162         ROS_INFO("Action did not finish before the time out.");
00163 
00164     // shutdown the node
00165     ros::shutdown();
00166 
00167 }
00168 


find_base_pose
Author(s): Thomas Ruehr
autogenerated on Mon Oct 6 2014 08:57:31