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


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Tue Oct 15 2013 10:06:51