Gazebo Gazebo: B21RConst.hh Source File

00001 #ifndef _B21R_CONST_H
00002 #define _B21R_CONST_H
00003 
00004 
00005 /* sensor coordinates in the robot frame */
00006 typedef struct RFLEX_CONFIG {
00007         double x;
00008         double y;
00009         double theta;
00010 } RFLEX_CONFIG;
00011 
00012 typedef struct RFLEX_SONARS_CONFIG {
00013         RFLEX_CONFIG tab[48];
00014 } RFLEX_SONARS_CONFIG;
00015 
00016 
00017 /*     US configuration of B21     */
00018 
00019 /*             Front Up             */
00020 /*          10 11 | 12 13             */
00021 /*        09             14           */
00022 /*      08                 15         */
00023 /*      -                   -         */ 
00024 /*      07                 16         */
00025 /*      06                 17         */
00026 /*      05                 18         */
00027 /*      04                 19         */
00028 /*      -                   -         */ 
00029 /*      03                 20         */
00030 /*        02             21           */
00031 /*          01 00 | 23 22             */
00032 
00033 
00034 /*                   Down           */
00035 /*          46 47 | 24 25             */
00036 /*        45             26           */
00037 /*        -               -         */ 
00038 /*       44               27         */
00039 /*       43               28         */
00040 /*       42               29         */
00041 /*       -                 -         */ 
00042 /*       41               30         */
00043 /*       40               31         */
00044 /*       39               32         */
00045 /*        -               -         */ 
00046 /*        38            33           */
00047 /*          37 36 | 35 34             */
00048 
00049 
00050 
00051 
00052 /* robot upper radius*/
00053 #define RA_UP_TH    0.2525
00054 #define RA_UP    (0.66/2)
00055 /* robot down radius*/
00056 #define RA_DOWN_TH 0.2625
00057 #define RA_DOWN (0.66/2)
00058 /* angle between two upper US */
00059 #define THETA_US_UP (0.065/RA_UP_TH)
00060 #define THETA_UP_2 (THETA_US_UP/2)
00061 /* angle between two down US */
00062 /* #define THETA_US_DOWN (7/RA_DOWN_TH) */
00063 /* #define THETA_DOWN_2 (THETA_US_DOWN/2) */
00064 #define THETA_US_DOWN THETA_US_UP 
00065 #define THETA_DOWN_2 (THETA_US_DOWN/2)
00066 
00067 /* position X and Y of US */
00068 #define X_SONAR_UP(ang) RA_UP*cos(ang)
00069 #define Y_SONAR_UP(ang) RA_UP*sin(ang)
00070 #define X_SONAR_DOWN(ang) RA_DOWN*cos(ang)
00071 #define Y_SONAR_DOWN(ang) RA_DOWN*sin(ang)
00072 
00073 
00074 #define RFLEX_SONARS_UP_CONFIG_CONST \
00075 {X_SONAR_UP(+23*THETA_UP_2), Y_SONAR_UP(+23*THETA_UP_2),  +23*THETA_UP_2},/* Sonar 00 */ \
00076 {X_SONAR_UP(+21*THETA_UP_2), Y_SONAR_UP(+21*THETA_UP_2),  +21*THETA_UP_2},/* Sonar 01 */ \
00077 {X_SONAR_UP(+19*THETA_UP_2), Y_SONAR_UP(+19*THETA_UP_2),  +19*THETA_UP_2},/* Sonar 02 */ \
00078 {X_SONAR_UP(+17*THETA_UP_2), Y_SONAR_UP(+17*THETA_UP_2),  +17*THETA_UP_2},/* Sonar 03 */\
00079 {X_SONAR_UP(+15*THETA_UP_2), Y_SONAR_UP(+15*THETA_UP_2),  +15*THETA_UP_2},/* Sonar 04 */ \
00080 {X_SONAR_UP(+13*THETA_UP_2), Y_SONAR_UP(+13*THETA_UP_2),  +13*THETA_UP_2},/* Sonar 05 */ \
00081 {X_SONAR_UP(+11*THETA_UP_2), Y_SONAR_UP(+11*THETA_UP_2),  +11*THETA_UP_2},/* Sonar 06 */ \
00082 {X_SONAR_UP(+9*THETA_UP_2), Y_SONAR_UP(+9*THETA_UP_2),  +9*THETA_UP_2},/* Sonar 07 */ \
00083 {X_SONAR_UP(+7*THETA_UP_2), Y_SONAR_UP(+7*THETA_UP_2),  +7*THETA_UP_2},/* Sonar 08 */ \
00084 {X_SONAR_UP(+5*THETA_UP_2), Y_SONAR_UP(+5*THETA_UP_2),  +5*THETA_UP_2},/* Sonar 09 */ \
00085 {X_SONAR_UP(+3*THETA_UP_2), Y_SONAR_UP(+3*THETA_UP_2),  +3*THETA_UP_2},/* Sonar 10 */ \
00086 {X_SONAR_UP(+THETA_UP_2)  , Y_SONAR_UP(+THETA_UP_2),    +THETA_UP_2},  /* Sonar 11 */ \
00087 {X_SONAR_UP(-THETA_UP_2)  , Y_SONAR_UP(-THETA_UP_2),    -THETA_UP_2},  /* Sonar 12*/ \
00088 {X_SONAR_UP(-3*THETA_UP_2), Y_SONAR_UP(-3*THETA_UP_2),  -3*THETA_UP_2},/* Sonar 13 */ \
00089 {X_SONAR_UP(-5*THETA_UP_2), Y_SONAR_UP(-5*THETA_UP_2),  -5*THETA_UP_2},/* Sonar 14 */ \
00090 {X_SONAR_UP(-7*THETA_UP_2), Y_SONAR_UP(-7*THETA_UP_2),  -7*THETA_UP_2},/* Sonar 15 */ \
00091 {X_SONAR_UP(-9*THETA_UP_2), Y_SONAR_UP(-9*THETA_UP_2),  -9*THETA_UP_2},/* Sonar 16 */ \
00092 {X_SONAR_UP(-11*THETA_UP_2), Y_SONAR_UP(-11*THETA_UP_2),  -11*THETA_UP_2},/* Sonar 17 */ \
00093 {X_SONAR_UP(-13*THETA_UP_2), Y_SONAR_UP(-13*THETA_UP_2),  -13*THETA_UP_2},/* Sonar 18 */ \
00094 {X_SONAR_UP(-15*THETA_UP_2), Y_SONAR_UP(-15*THETA_UP_2),  -15*THETA_UP_2},/* Sonar 19 */ \
00095 {X_SONAR_UP(-17*THETA_UP_2), Y_SONAR_UP(-17*THETA_UP_2),  -17*THETA_UP_2},/* Sonar 20 */ \
00096 {X_SONAR_UP(-19*THETA_UP_2), Y_SONAR_UP(-19*THETA_UP_2),  -19*THETA_UP_2},/* Sonar 21 */ \
00097 {X_SONAR_UP(-21*THETA_UP_2), Y_SONAR_UP(-21*THETA_UP_2),  -21*THETA_UP_2},/* Sonar 22 */ \
00098 {X_SONAR_UP(-23*THETA_UP_2), Y_SONAR_UP(-23*THETA_UP_2),  -23*THETA_UP_2}/* Sonar 23 */ 
00099 
00100 #define RFLEX_SONARS_DOWN_CONFIG_CONST \
00101 {X_SONAR_DOWN(-THETA_DOWN_2)  , Y_SONAR_DOWN(-THETA_DOWN_2),    -THETA_DOWN_2},  /* Sonar 24*/ \
00102 {X_SONAR_DOWN(-3*THETA_DOWN_2), Y_SONAR_DOWN(-3*THETA_DOWN_2),  -3*THETA_DOWN_2},/* Sonar 25*/ \
00103 {X_SONAR_DOWN(-5*THETA_DOWN_2), Y_SONAR_DOWN(-5*THETA_DOWN_2),  -5*THETA_DOWN_2},/* Sonar 26*/ \
00104 {X_SONAR_DOWN(-7*THETA_DOWN_2), Y_SONAR_DOWN(-7*THETA_DOWN_2),  -7*THETA_DOWN_2},/* Sonar 27*/ \
00105 {X_SONAR_DOWN(-9*THETA_DOWN_2), Y_SONAR_DOWN(-9*THETA_DOWN_2),  -9*THETA_DOWN_2},/* Sonar 28*/ \
00106 {X_SONAR_DOWN(-11*THETA_DOWN_2), Y_SONAR_DOWN(-11*THETA_DOWN_2),  -11*THETA_DOWN_2},/* Sonar 29*/ \
00107 {X_SONAR_DOWN(-13*THETA_DOWN_2), Y_SONAR_DOWN(-13*THETA_DOWN_2),  -13*THETA_DOWN_2},/* Sonar 30*/ \
00108 {X_SONAR_DOWN(-15*THETA_DOWN_2), Y_SONAR_DOWN(-15*THETA_DOWN_2),  -15*THETA_DOWN_2},/* Sonar 31*/ \
00109 {X_SONAR_DOWN(-17*THETA_DOWN_2), Y_SONAR_DOWN(-17*THETA_DOWN_2),  -17*THETA_DOWN_2},/* Sonar 32*/ \
00110 {X_SONAR_DOWN(-19*THETA_DOWN_2), Y_SONAR_DOWN(-19*THETA_DOWN_2),  -19*THETA_DOWN_2},/* Sonar 33*/ \
00111 {X_SONAR_DOWN(-21*THETA_DOWN_2), Y_SONAR_DOWN(-21*THETA_DOWN_2),  -21*THETA_DOWN_2},/* Sonar 34 */ \
00112 {X_SONAR_DOWN(-23*THETA_DOWN_2), Y_SONAR_DOWN(-23*THETA_DOWN_2),  -23*THETA_DOWN_2},/* Sonar 35 */ \
00113 {X_SONAR_DOWN(+23*THETA_DOWN_2), Y_SONAR_DOWN(+23*THETA_DOWN_2),  +23*THETA_DOWN_2},/* Sonar 36 */ \
00114 {X_SONAR_DOWN(+21*THETA_DOWN_2), Y_SONAR_DOWN(+21*THETA_DOWN_2),  +21*THETA_DOWN_2},/* Sonar 37 */ \
00115 {X_SONAR_DOWN(+19*THETA_DOWN_2), Y_SONAR_DOWN(+19*THETA_DOWN_2),  +19*THETA_DOWN_2},/* Sonar 38 */ \
00116 {X_SONAR_DOWN(+17*THETA_DOWN_2), Y_SONAR_DOWN(+17*THETA_DOWN_2),  +17*THETA_DOWN_2},/* Sonar 39 */ \
00117 {X_SONAR_DOWN(+15*THETA_DOWN_2), Y_SONAR_DOWN(+15*THETA_DOWN_2),  +15*THETA_DOWN_2},/* Sonar 40 */ \
00118 {X_SONAR_DOWN(+13*THETA_DOWN_2), Y_SONAR_DOWN(+13*THETA_DOWN_2),  +13*THETA_DOWN_2},/* Sonar 41 */ \
00119 {X_SONAR_DOWN(+11*THETA_DOWN_2), Y_SONAR_DOWN(+11*THETA_DOWN_2),  +11*THETA_DOWN_2},/* Sonar 42 */ \
00120 {X_SONAR_DOWN(+9*THETA_DOWN_2), Y_SONAR_DOWN(+9*THETA_DOWN_2),  +9*THETA_DOWN_2},/* Sonar 43 */ \
00121 {X_SONAR_DOWN(+7*THETA_DOWN_2), Y_SONAR_DOWN(+7*THETA_DOWN_2),  +7*THETA_DOWN_2},/* Sonar 44 */ \
00122 {X_SONAR_DOWN(+5*THETA_DOWN_2), Y_SONAR_DOWN(+5*THETA_DOWN_2),  +5*THETA_DOWN_2},/* Sonar 45 */ \
00123 {X_SONAR_DOWN(+3*THETA_DOWN_2), Y_SONAR_DOWN(+3*THETA_DOWN_2),  +3*THETA_DOWN_2},/* Sonar 46 */ \
00124 {X_SONAR_DOWN(+THETA_DOWN_2)  , Y_SONAR_DOWN(+THETA_DOWN_2),    +THETA_DOWN_2}  /* Sonar 47 */
00125 
00126 #define RFLEX_SONARS_CONFIG_CONST { \
00127  RFLEX_SONARS_UP_CONFIG_CONST, \
00128  RFLEX_SONARS_DOWN_CONFIG_CONST \
00129 }
00130 
00131 
00132 #endif