00001 #include <algorithm>
00002 #include <cmath>
00003 #include <cstdlib>
00004 #include <ctime>
00005 #include <fstream>
00006 #include <sstream>
00007 #include <vector>
00008
00009 #include <geometry_msgs/Point32.h>
00010 #include <geometry_msgs/Polygon.h>
00011 #include <geometry_msgs/Pose.h>
00012 #include <gtest/gtest.h>
00013 #include <place_matcher_msgs/PolygonDissimilarity.h>
00014 #include <ros/ros.h>
00015 #include <tf/tf.h>
00016
00017 #include <place_matcher_hist/dissimilarity_getter.h>
00018
00019 #define FILE_OUTPUT 1
00020
00021 ros::NodeHandlePtr g_nh_ptr;
00022
00023 #if FILE_OUTPUT
00024 void saveToFile(const std::string& filename, const geometry_msgs::Polygon& poly)
00025 {
00026 std::ofstream fout(filename.c_str());
00027 if (!fout.is_open())
00028 {
00029 std::cerr << "\"" << filename << "\" cannot be opened for writing";
00030 return;
00031 }
00032 std::vector<geometry_msgs::Point32>::const_iterator it = poly.points.begin();
00033 for (; it < poly.points.end(); ++it)
00034 {
00035 fout << it->x << "," << it->y << "\n";
00036 }
00037 }
00038 #endif
00039
00040 void transformPoint(const geometry_msgs::Pose& transform, geometry_msgs::Point32& point)
00041 {
00042 const geometry_msgs::Point32 old_point = point;
00043 const double yaw = tf::getYaw(transform.orientation);
00044 const double cos_yaw = std::cos(yaw);
00045 const double sin_yaw = std::sin(yaw);
00046 point.x = transform.position.x + old_point.x * cos_yaw - old_point.y * sin_yaw;
00047 point.y = transform.position.y + old_point.x * sin_yaw + old_point.y * cos_yaw;
00048 }
00049
00050 void transformPolygon(const geometry_msgs::Pose& transform, geometry_msgs::Polygon& poly)
00051 {
00052 for (size_t i = 0; i < poly.points.size(); ++i)
00053 {
00054 transformPoint(transform, poly.points[i]);
00055 }
00056 }
00057
00058 geometry_msgs::Polygon getRandPolygon()
00059 {
00060 const double angle_resolution = M_PI / 180;
00061 const double range_min = 2.0;
00062 const double range_max = 5.0;
00063
00064 geometry_msgs::Polygon poly;
00065 const size_t point_count = (size_t) (2 * M_PI / angle_resolution);
00066 poly.points.reserve(point_count);
00067 for (double theta = - 2 * M_PI + 1e-10; theta < 2 * M_PI; theta += angle_resolution)
00068 {
00069 geometry_msgs::Point32 point;
00070 double r = range_min + (range_max - range_min) * ((double) std::rand()) / RAND_MAX;
00071 point.x = r * std::cos(theta);
00072 point.y = r * std::sin(theta);
00073 poly.points.push_back(point);
00074 }
00075 return poly;
00076 }
00077
00078 std::vector<geometry_msgs::Point32> interpolate(const geometry_msgs::Point32& a, const geometry_msgs::Point32 b, double max_distance)
00079 {
00080 std::vector<geometry_msgs::Point32> points;
00081
00082 const double dx = b.x - a.x;
00083 const double dy = b.y - a.y;
00084
00085 geometry_msgs::Point32 point;
00086 const double norm = std::sqrt(dx * dx + dy * dy);
00087 if (norm == 0)
00088 {
00089 points.push_back(a);
00090 return points;
00091 }
00092
00093
00094 const double ux = dx / norm;
00095 const double uy = dy / norm;
00096 for (double s = 0; s <= norm; s += max_distance)
00097 {
00098 point.x = a.x + s * ux;
00099 point.y = a.y + s * uy;
00100 points.push_back(point);
00101 }
00102 return points;
00103 }
00104
00105 geometry_msgs::Point32 point(double x, double y)
00106 {
00107 geometry_msgs::Point32 point;
00108 point.x = x;
00109 point.y = y;
00110 return point;
00111 }
00112
00113 geometry_msgs::Polygon loadFromString(std::stringstream& sstream)
00114 {
00115 geometry_msgs::Polygon poly;
00116
00117 do
00118 {
00119 double x;
00120 double y;
00121 sstream >> x >> y;
00122 geometry_msgs::Point32 pt;
00123 pt.x = x;
00124 pt.y = y;
00125 poly.points.push_back(pt);
00126 } while (!sstream.eof());
00127
00128 return poly;
00129 }
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140 geometry_msgs::Polygon getTestPolygon()
00141 {
00142 geometry_msgs::Polygon poly;
00143
00144 geometry_msgs::Point32 p0 = point(-0.6, 0);
00145 geometry_msgs::Point32 p1 = point(-1, -1);
00146 geometry_msgs::Point32 p2 = point(1, p1.y);
00147 geometry_msgs::Point32 p3 = point(0.8, 0.8);
00148 geometry_msgs::Point32 p4 = point(-0.9, p3.y);
00149
00150 std::vector<geometry_msgs::Point32> points;
00151 points = interpolate(p0, p1, 0.02);
00152 std::copy(points.begin(), points.end(), std::back_inserter(poly.points));
00153 points = interpolate(p1, p2, 0.02);
00154 std::copy(points.begin(), points.end(), std::back_inserter(poly.points));
00155 points = interpolate(p2, p3, 0.02);
00156 std::copy(points.begin(), points.end(), std::back_inserter(poly.points));
00157 points = interpolate(p3, p4, 0.02);
00158 std::copy(points.begin(), points.end(), std::back_inserter(poly.points));
00159 points = interpolate(p4, p0, 0.02);
00160 std::copy(points.begin(), points.end(), std::back_inserter(poly.points));
00161
00162 return poly;
00163 }
00164
00170 geometry_msgs::Polygon getRealPolygon()
00171 {
00172 std::stringstream poly_sstream;
00173 poly_sstream <<
00174 "0 1.116" <<
00175 "0.019634 1.12483\n" <<
00176 "0.039925 1.1433\n" <<
00177 "0.0603957 1.15242\n" <<
00178 "0.0811965 1.16116\n" <<
00179 "0.102321 1.16953\n" <<
00180 "0.124807 1.18746\n" <<
00181 "0.147706 1.20297\n" <<
00182 "0.171044 1.21704\n" <<
00183 "0.193509 1.22177\n" <<
00184 "0.218102 1.23692\n" <<
00185 "0.243663 1.25354\n" <<
00186 "0.269246 1.2667\n" <<
00187 "0.293561 1.27155\n" <<
00188 "0.322482 1.2934\n" <<
00189 "0.347335 1.29627\n" <<
00190 "0.379277 1.3227\n" <<
00191 "0.407566 1.33309\n" <<
00192 "0.437568 1.3467\n" <<
00193 "0.453842 1.31805\n" <<
00194 "0.470278 1.29208\n" <<
00195 "0.481288 1.2538\n" <<
00196 "0.498227 1.23315\n" <<
00197 "0.513811 1.21046\n" <<
00198 "0.522657 1.17391\n" <<
00199 "0.539684 1.15736\n" <<
00200 "0.551909 1.13158\n" <<
00201 "0.578384 1.13514\n" <<
00202 "0.610783 1.14871\n" <<
00203 "0.651584 1.17549\n" <<
00204 "0.686 1.18819\n" <<
00205 "0.728264 1.21203\n" <<
00206 "0.770503 1.23306\n" <<
00207 "0.813691 1.25297\n" <<
00208 "0.85892 1.2734\n" <<
00209 "0.914854 1.30655\n" <<
00210 "0.937517 1.29038\n" <<
00211 "0.956284 1.26903\n" <<
00212 "1.0023 1.28288\n" <<
00213 "3.17366 3.91915\n" <<
00214 "3.21715 3.83405\n" <<
00215 "3.25996 3.75015\n" <<
00216 "3.30417 3.66965\n" <<
00217 "3.34043 3.58217\n" <<
00218 "3.38229 3.50247\n" <<
00219 "3.4224 3.4224\n" <<
00220 "3.49455 3.37465\n" <<
00221 "3.62166 3.37726\n" <<
00222 "3.66445 3.29948\n" <<
00223 "3.69959 3.216\n" <<
00224 "3.59888 3.01982\n" <<
00225 "3.63704 2.94522\n" <<
00226 "3.73438 2.91762\n" <<
00227 "3.70088 2.78881\n" <<
00228 "3.76031 2.73203\n" <<
00229 "3.8451 2.69237\n" <<
00230 "3.81274 2.57173\n" <<
00231 "3.87634 2.51732\n" <<
00232 "3.9553 2.47154\n" <<
00233 "3.92411 2.35784\n" <<
00234 "3.99498 2.3065\n" <<
00235 "4.06961 2.25582\n" <<
00236 "4.02712 2.14126\n" <<
00237 "4.24476 2.16281\n" <<
00238 "4.28365 2.08928\n" <<
00239 "4.31674 2.01293\n" <<
00240 "4.35487 1.93891\n" <<
00241 "1.32092 0.560699\n" <<
00242 "1.30826 0.52857\n" <<
00243 "1.32475 0.508524\n" <<
00244 "1.21032 0.440522\n" <<
00245 "1.07695 0.370822\n" <<
00246 "0.966273 0.313961\n" <<
00247 "0.969693 0.296465\n" <<
00248 "0.975681 0.279772\n" <<
00249 "0.989108 0.265031\n" <<
00250 "0.994553 0.24797\n" <<
00251 "1.00847 0.232824\n" <<
00252 "1.0114 0.214981\n" <<
00253 "1.02482 0.199205\n" <<
00254 "1.02814 0.181289\n" <<
00255 "1.04004 0.164726\n" <<
00256 "1.05166 0.147802\n" <<
00257 "1.05408 0.129425\n" <<
00258 "1.05618 0.111009\n" <<
00259 "1.06892 0.0935181\n" <<
00260 "1.07537 0.0751975\n" <<
00261 "1.04656 0.0548481\n" <<
00262 "1.01538 0.0354579\n" <<
00263 "0.977851 0.0170685\n" <<
00264 "0.938 1.14136e-08\n" <<
00265 "0.899863 -0.0157072\n" <<
00266 "0.85248 -0.0297693\n" <<
00267 "0.848835 -0.0444856\n" <<
00268 "0.854912 -0.0597813\n" <<
00269 "0.864697 -0.0756512\n" <<
00270 "0.87319 -0.091776\n" <<
00271 "0.900239 -0.110535\n" <<
00272 "0.931842 -0.130962\n" <<
00273 "0.925464 -0.146579\n" <<
00274 "0.905038 -0.159583\n" <<
00275 "0.911932 -0.177262\n" <<
00276 "0.918481 -0.195229\n" <<
00277 "0.923703 -0.213254\n" <<
00278 "0.929543 -0.231761\n" <<
00279 "0.93405 -0.250278\n" <<
00280 "0.948765 -0.272054\n" <<
00281 "0.953436 -0.291495\n" <<
00282 "0.956763 -0.310871\n" <<
00283 "0.958756 -0.330126\n" <<
00284 "0.971642 -0.353649\n" <<
00285 "0.974658 -0.374136\n" <<
00286 "0.986524 -0.398581\n" <<
00287 "0.992304 -0.421208\n" <<
00288 "0.994851 -0.442936\n" <<
00289 "1.00328 -0.467838\n" <<
00290 "1.01384 -0.494483\n" <<
00291 "1.02198 -0.520727\n" <<
00292 "1.02863 -0.546934\n" <<
00293 "1.03642 -0.574499\n" <<
00294 "1.04876 -0.6055\n" <<
00295 "1.05432 -0.633497\n" <<
00296 "1.0626 -0.663989\n" <<
00297 "1.07266 -0.696593\n" <<
00298 "1.08438 -0.731424\n" <<
00299 "1.09603 -0.767445\n" <<
00300 "1.10512 -0.802915\n" <<
00301 "1.12288 -0.846152\n" <<
00302 "1.1308 -0.883474\n" <<
00303 "1.15328 -0.933911\n" <<
00304 "1.93579 -1.62432\n" <<
00305 "1.96602 -1.70903\n" <<
00306 "1.91954 -1.72836\n" <<
00307 "1.86422 -1.73841\n" <<
00308 "1.39624 -1.34833\n" <<
00309 "1.35906 -1.35906\n" <<
00310 "2.13538 -2.21125\n" <<
00311 "2.11692 -2.27012\n" <<
00312 "2.08903 -2.3201\n" <<
00313 "2.06724 -2.37809\n" <<
00314 "2.03764 -2.42836\n" <<
00315 "2.01886 -2.49308\n" <<
00316 "1.98736 -2.5437\n" <<
00317 "1.96493 -2.60754\n" <<
00318 "1.93675 -2.66571\n" <<
00319 "1.90542 -2.72122\n" <<
00320 "1.88113 -2.78888\n" <<
00321 "1.84469 -2.84058\n" <<
00322 "1.27658 -2.04295\n" <<
00323 "1.24021 -2.06406\n" <<
00324 "1.204 -2.08539\n" <<
00325 "1.16403 -2.09996\n" <<
00326 "1.12626 -2.11819\n" <<
00327 "1.09003 -2.13931\n" <<
00328 "1.05297 -2.1589\n" <<
00329 "1.01513 -2.17695\n" <<
00330 "0.980642 -2.20256\n" <<
00331 "0.942053 -2.21934\n" <<
00332 "0.902802 -2.23451\n" <<
00333 "0.864025 -2.25086\n" <<
00334 "0.154935 -0.425681\n" <<
00335 "0.150738 -0.437775\n" <<
00336 "0.133495 -0.410856\n" <<
00337 "0.128644 -0.420774\n" <<
00338 "0.119351 -0.416226\n" <<
00339 "0.11181 -0.41728\n" <<
00340 "0.10451 -0.419168\n" <<
00341 "0.0962791 -0.41703\n" <<
00342 "0.0939761 -0.442123\n" <<
00343 "0.0862457 -0.443696\n" <<
00344 "0.078489 -0.445133\n" <<
00345 "0.0703955 -0.44446\n" <<
00346 "0.064298 -0.457504\n" <<
00347 "0.0563036 -0.458556\n" <<
00348 "0.0482922 -0.459469\n" <<
00349 "0.0400045 -0.457253\n" <<
00350 "0.032088 -0.458879\n" <<
00351 "0.0245979 -0.469356\n" <<
00352 "0.016333 -0.467715\n" <<
00353 "0.00823755 -0.471928\n" <<
00354 "7.92135e-08 -3.255\n" <<
00355 "-0.0101224 -0.579912\n" <<
00356 "-0.0205558 -0.588641\n" <<
00357 "-0.0308782 -0.589191\n" <<
00358 "-0.0417841 -0.597541\n" <<
00359 "-0.0529907 -0.605686\n" <<
00360 "-0.0644941 -0.61362\n" <<
00361 "-0.0751934 -0.612401\n" <<
00362 "-0.0883749 -0.62882\n" <<
00363 "-0.0998052 -0.630145\n" <<
00364 "-0.112177 -0.636186\n" <<
00365 "-0.124789 -0.641984\n" <<
00366 "-0.136806 -0.643621\n" <<
00367 "-0.149817 -0.64893\n" <<
00368 "-0.163055 -0.653979\n" <<
00369 "-0.176773 -0.659727\n" <<
00370 "-0.191292 -0.667116\n" <<
00371 "-0.206122 -0.674195\n" <<
00372 "-0.22311 -0.686663\n" <<
00373 "-0.241246 -0.700629\n" <<
00374 "-0.257199 -0.706649\n" <<
00375 "-0.27666 -0.720724\n" <<
00376 "-0.293692 -0.726912\n" <<
00377 "-0.314539 -0.741006\n" <<
00378 "-0.33149 -0.74454\n" <<
00379 "-0.352464 -0.755861\n" <<
00380 "-0.374369 -0.76757\n" <<
00381 "-0.396334 -0.777849\n" <<
00382 "-0.423463 -0.796419\n" <<
00383 "-0.447964 -0.808149\n" <<
00384 "-0.4755 -0.82359\n" <<
00385 "-0.499587 -0.831452\n" <<
00386 "-0.529389 -0.8472\n" <<
00387 "-0.566425 -0.872217\n" <<
00388 "-0.592185 -0.877951\n" <<
00389 "-0.630934 -0.901067\n" <<
00390 "-0.66361 -0.91338\n" <<
00391 "-0.705327 -0.936001\n" <<
00392 "-0.746182 -0.955069\n" <<
00393 "-0.78665 -0.971432\n" <<
00394 "-0.836909 -0.99739\n" <<
00395 "-0.895521 -1.03018\n" <<
00396 "-0.951504 -1.05675\n" <<
00397 "-1.01072 -1.08387\n" <<
00398 "-1.08089 -1.11929\n" <<
00399 "-1.16673 -1.16673\n" <<
00400 "-1.2164 -1.17467\n" <<
00401 "-1.22867 -1.14576\n" <<
00402 "-1.24105 -1.11745\n" <<
00403 "-1.24225 -1.07987\n" <<
00404 "-1.24865 -1.04774\n" <<
00405 "-1.2582 -1.01887\n" <<
00406 "-1.2687 -0.991215\n" <<
00407 "-1.2866 -0.969524\n" <<
00408 "-1.29443 -0.940456\n" <<
00409 "-1.31064 -0.917722\n" <<
00410 "-1.319 -0.889676\n" <<
00411 "-1.33432 -0.866521\n" <<
00412 "-1.34246 -0.838862\n" <<
00413 "-1.35775 -0.81582\n" <<
00414 "-1.36312 -0.787\n" <<
00415 "-1.37928 -0.764545\n" <<
00416 "-1.38976 -0.738948\n" <<
00417 "-1.40155 -0.714127\n" <<
00418 "-1.4156 -0.690435\n" <<
00419 "-1.42653 -0.665201\n" <<
00420 "-1.44157 -0.64183\n" <<
00421 "-1.45164 -0.616183\n" <<
00422 "-1.46959 -0.593752\n" <<
00423 "-1.47972 -0.568013\n" <<
00424 "-1.49693 -0.544838\n" <<
00425 "-1.50621 -0.51863\n" <<
00426 "-1.51408 -0.491955\n" <<
00427 "-1.53104 -0.468087\n" <<
00428 "-1.53898 -0.441295\n" <<
00429 "-1.55611 -0.416958\n" <<
00430 "-1.57285 -0.392155\n" <<
00431 "-1.57945 -0.364646\n" <<
00432 "-1.59438 -0.338896\n" <<
00433 "-1.60987 -0.312927\n" <<
00434 "-1.6466 -0.29034\n" <<
00435 "-1.67018 -0.264531\n" <<
00436 "-1.67454 -0.235342\n" <<
00437 "-1.68733 -0.207178\n" <<
00438 "-1.70063 -0.178744\n" <<
00439 "-1.72342 -0.15078\n" <<
00440 "-1.73576 -0.121376\n" <<
00441 "-1.7476 -0.091588\n" <<
00442 "-1.76692 -0.0617024\n" <<
00443 "-1.77773 -0.0310304\n" <<
00444 "-1.798 -6.5634e-08\n" <<
00445 "-1.81772 0.0317284\n" <<
00446 "-1.83788 0.0641801\n" <<
00447 "-1.85046 0.0969785\n" <<
00448 "-1.79961 0.125841\n" <<
00449 "-1.73936 0.152174\n" <<
00450 "-1.72052 0.180834\n" <<
00451 "-1.72802 0.212174\n" <<
00452 "-2.03203 0.285583\n" <<
00453 "-2.95911 0.468678\n" <<
00454 "-2.98791 0.526848\n" <<
00455 "-3.00672 0.584448\n" <<
00456 "-3.044 0.647021\n" <<
00457 "-3.13845 0.724567\n" <<
00458 "-3.16802 0.789875\n" <<
00459 "-3.19625 0.856432\n" <<
00460 "-4.17572 1.19737\n" <<
00461 "-4.0729 1.24521\n" <<
00462 "-3.98873 1.29602\n" <<
00463 "-3.00202 1.03368\n" <<
00464 "-3.08971 1.12456\n" <<
00465 "-3.21245 1.23314\n" <<
00466 "-3.34343 1.35083\n" <<
00467 "-3.51357 1.49142\n" <<
00468 "-3.48061 1.54967\n" <<
00469 "-3.4059 1.5882\n" <<
00470 "-3.33003 1.62417\n" <<
00471 "-3.26019 1.66115\n" <<
00472 "-0.979189 0.520644\n" <<
00473 "-0.958583 0.531351\n" <<
00474 "-0.943968 0.545\n" <<
00475 "-0.932598 0.560361\n" <<
00476 "-0.904867 0.565424\n" <<
00477 "-0.886475 0.575683\n" <<
00478 "-0.868002 0.585475\n" <<
00479 "-0.85028 0.595372\n" <<
00480 "-0.831669 0.604243\n" <<
00481 "-0.812212 0.612046\n" <<
00482 "-0.794315 0.620587\n" <<
00483 "-0.774037 0.626803\n" <<
00484 "-0.76298 0.640216\n" <<
00485 "-0.744144 0.646874\n" <<
00486 "-0.726796 0.65441\n" <<
00487 "-0.707219 0.659492\n" <<
00488 "-0.696321 0.672429\n" <<
00489 "-0.676701 0.676701\n" <<
00490 "-0.663399 0.68697\n" <<
00491 "-0.645852 0.692592\n" <<
00492 "-0.632998 0.703015\n" <<
00493 "-0.616696 0.709427\n" <<
00494 "-0.596507 0.710889\n" <<
00495 "-0.58338 0.720414\n" <<
00496 "-0.570103 0.729698\n" <<
00497 "-0.557883 0.740335\n" <<
00498 "-0.538411 0.74106\n" <<
00499 "-0.52597 0.751162\n" <<
00500 "-0.51278 0.760227\n" <<
00501 "-0.498345 0.767384\n" <<
00502 "-0.485406 0.776812\n" <<
00503 "-0.46611 0.775736\n" <<
00504 "-0.4535 0.785485\n" <<
00505 "-0.439722 0.79328\n" <<
00506 "-0.42628 0.801716\n" <<
00507 "-0.412223 0.809034\n" <<
00508 "-0.398479 0.817004\n" <<
00509 "-0.383737 0.822928\n" <<
00510 "-0.369724 0.830413\n" <<
00511 "-0.355175 0.836739\n" <<
00512 "-0.340517 0.84281\n" <<
00513 "-0.325757 0.848625\n" <<
00514 "-0.310896 0.854181\n" <<
00515 "-0.295616 0.858531\n" <<
00516 "-0.283369 0.872119\n" <<
00517 "-0.268105 0.876932\n" <<
00518 "-0.25276 0.881477\n" <<
00519 "-0.240184 0.896379\n" <<
00520 "-0.224504 0.900434\n" <<
00521 "-0.208755 0.904215\n" <<
00522 "-0.194605 0.915546\n" <<
00523 "-0.178979 0.920766\n" <<
00524 "-0.164619 0.933598\n" <<
00525 "-0.148143 0.935341\n" <<
00526 "-0.13305 0.946696\n" <<
00527 "-0.116507 0.948874\n" <<
00528 "-0.10087 0.959714\n" <<
00529 "-0.0848897 0.970294\n" <<
00530 "-0.0679429 0.971627\n" <<
00531 "-0.0516033 0.984649\n" <<
00532 "-0.0347599 0.995393\n" <<
00533 "-0.0175223 1.00385";
00534
00535 geometry_msgs::Polygon poly = loadFromString(poly_sstream);
00536 return poly;
00537 }
00538
00539 TEST(TestSuite, testGetDissimilarity)
00540 {
00541 const geometry_msgs::Polygon poly_ref = getRealPolygon();
00542 const double dtheta = 1.3;
00543 geometry_msgs::Pose transform;
00544 transform.position.x = 0.;
00545 transform.position.y = 0.6;
00546 transform.orientation = tf::createQuaternionMsgFromYaw(dtheta);
00547 geometry_msgs::Polygon poly_sens = poly_ref;
00548 transformPolygon(transform, poly_sens);
00549
00550 #if FILE_OUTPUT
00551 saveToFile("/tmp/poly_ref.csv", poly_ref);
00552 saveToFile("/tmp/poly_sens.csv", poly_sens);
00553 #endif
00554
00555 place_matcher_msgs::PolygonDissimilarityRequest req;
00556 place_matcher_msgs::PolygonDissimilarityResponse res;
00557 req.polygon1 = poly_ref;
00558 req.polygon2 = poly_sens;
00559
00560 ros::ServiceClient client = g_nh_ptr->serviceClient<place_matcher_msgs::PolygonDissimilarity>("/place_matcher_hist/compute_dissimilarity");
00561 while (ros::ok() && !client.waitForExistence(ros::Duration(5)))
00562 {
00563 ROS_INFO_STREAM("Waiting for service " << client.getService());
00564 }
00565 ASSERT_TRUE(client.call(req, res));
00566
00567 EXPECT_NEAR(res.pose.position.x, transform.position.x, 1.1 * 0.1);
00568 EXPECT_NEAR(res.pose.position.y, transform.position.y, 1.1 * 0.1);
00569 EXPECT_NEAR(tf::getYaw(res.pose.orientation), dtheta, 1.1 * 0.01745);
00570 }
00571
00572 int main(int argc, char *argv[])
00573 {
00574 std::srand(std::time(0));
00575
00576
00577 ros::init(argc, argv, "test_cpp_place_matcher_hist");
00578 ros::NodeHandle nh;
00579 g_nh_ptr.reset(&nh);
00580
00581 testing::InitGoogleTest(&argc, argv);
00582 return RUN_ALL_TESTS();
00583 }
00584