helper.h
Go to the documentation of this file.
00001 /*
00002  * A footstep planner for humanoid robots
00003  *
00004  * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
00005  * http://www.ros.org/wiki/footstep_planner
00006  *
00007  *
00008  * This program is free software: you can redistribute it and/or modify
00009  * it under the terms of the GNU General Public License as published by
00010  * the Free Software Foundation, version 3.
00011  *
00012  * This program is distributed in the hope that it will be useful,
00013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  * GNU General Public License for more details.
00016  *
00017  * You should have received a copy of the GNU General Public License
00018  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00021 #ifndef FOOTSTEP_PLANNER_HELPER_H_
00022 #define FOOTSTEP_PLANNER_HELPER_H_
00023 
00024 #define DEBUG_HASH 0
00025 #define DEBUG_TIME 0
00026 
00027 
00028 #include <gridmap_2d/GridMap2D.h>
00029 #include <angles/angles.h>
00030 #include <tf/tf.h>
00031 
00032 #include <math.h>
00033 
00034 
00035 namespace footstep_planner
00036 {
00037 static const double TWO_PI = 2 * M_PI;
00038 
00039 static const double FLOAT_CMP_THR = 0.0001;
00040 
00041 enum Leg { RIGHT=0, LEFT=1, NOLEG=2 };
00042 
00043 
00048 inline double euclidean_distance_sq(int x1, int y1, int x2, int y2)
00049 {
00050   // note: do *not* use pow() to square!
00051   return (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2);
00052 }
00053 
00054 
00056 inline double euclidean_distance(int x1, int y1, int x2, int y2)
00057 {
00058   return sqrt(double(euclidean_distance_sq(x1, y1, x2, y2)));
00059 }
00060 
00061 
00063 inline double euclidean_distance(double x1, double y1, double x2, double y2)
00064 {
00065   return sqrt(euclidean_distance_sq(x1, y1, x2, y2));
00066 }
00067 
00068 
00070 inline double euclidean_distance_sq(double x1, double y1, double x2,
00071                                     double y2)
00072 {
00073   // note: do *not* use pow() to square!
00074   return (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2);
00075 }
00076 
00077 
00079 inline double grid_cost(int x1, int y1, int x2, int y2, float cell_size)
00080 {
00081   int x = abs(x1 - x2);
00082   int y = abs(y1 - y2);
00083 
00084   if (x + y > 1)
00085     return M_SQRT2 * cell_size;
00086   else
00087     return cell_size;
00088 }
00089 
00090 
00092 inline int angle_state_2_cell(double angle, int angle_bin_num)
00093 {
00094   double bin_size_half = M_PI / angle_bin_num;
00095   return int(angles::normalize_angle_positive(angle + bin_size_half) /
00096              TWO_PI * angle_bin_num);
00097 }
00098 
00099 
00101 inline double angle_cell_2_state(int angle, int angle_bin_num)
00102 {
00103   double bin_size = TWO_PI / angle_bin_num;
00104   return angle * bin_size;
00105 }
00106 
00107 
00112 inline int state_2_cell(float value, float cell_size)
00113 {
00114   return value >= 0 ? int(value / cell_size) : int(value / cell_size) - 1;
00115 }
00116 
00117 
00122 inline double cell_2_state(int value, double cell_size)
00123 {
00124   return (double(value) + 0.5) * cell_size;
00125 }
00126 
00127 
00129 // TODO: check consistency for negative values
00130 inline int disc_val(double length, double cell_size)
00131 {
00132   return int(floor((length / cell_size) + 0.5));
00133 }
00134 
00135 
00140 // TODO: check consistency for negative values
00141 inline double cont_val(int length, double cell_size)
00142 {
00143   return double(length * cell_size);
00144 }
00145 
00146 
00148 inline unsigned int int_hash(int key)
00149 {
00150   key += (key << 12);
00151   key ^= (key >> 22);
00152   key += (key << 4);
00153   key ^= (key >> 9);
00154   key += (key << 10);
00155   key ^= (key >> 2);
00156   key += (key << 7);
00157   key ^= (key >> 12);
00158   return key;
00159 }
00160 
00161 
00166 inline unsigned int calc_hash_tag(int x, int y, int theta, int leg,
00167                                   int max_hash_size)
00168 {
00169   return int_hash((int_hash(x) << 3) + (int_hash(y) << 2) +
00170                   (int_hash(theta) << 1) + (int_hash(leg)))
00171       % max_hash_size;
00172 }
00173 
00174 
00176 inline int round(double r)
00177 {
00178   return (r > 0.0) ? floor(r + 0.5) : ceil(r - 0.5);
00179 }
00180 
00181 
00200 bool collision_check(double x, double y, double theta,
00201                      double height, double width, int accuracy,
00202                      const gridmap_2d::GridMap2D& distance_map);
00203 
00204 
00212 bool pointWithinPolygon(int x, int y,
00213                         const std::vector<std::pair<int, int> >& edges);
00214 }
00215 #endif  /* FOOTSTEP_PLANNER_HELPER_H_ */


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Wed Aug 26 2015 11:54:32