00001 #include <ros/ros.h>
00002 #include <tf/tf.h>
00003
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
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();
00060
00061 ac.sendGoal(goal);
00062
00063
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
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
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
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
00111
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
00138 ac.waitForServer();
00139
00140 test_fridge(ac);
00141
00142
00143 ac.sendGoal(goal);
00144
00145
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
00165 ros::shutdown();
00166
00167 }
00168