$search
00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/footstep_planner/include/footstep_planner/helper.h $ 00002 // SVN $Id: helper.h 3810 2013-01-20 18:57:24Z garimort@informatik.uni-freiburg.de $ 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 00208 #endif /* FOOTSTEP_PLANNER_HELPER_H_ */