urg_utils.c
Go to the documentation of this file.
00001 
00009 #include "urg_c/urg_utils.h"
00010 #include "urg_c/urg_errno.h"
00011 #define _USE_MATH_DEFINES
00012 #include <math.h>
00013 
00014 #undef max
00015 #undef min
00016 
00017 
00018 static int max(int a, int b)
00019 {
00020     return (a > b) ? a : b;
00021 }
00022 
00023 
00024 static int min(int a, int b)
00025 {
00026     return (a < b) ? a : b;
00027 }
00028 
00029 
00030 const char *urg_error(const urg_t *urg)
00031 {
00032     typedef struct
00033     {
00034         int no;
00035         const char* message;
00036     } error_messages_t;
00037 
00038 
00039     error_messages_t errors[] = {
00040         { URG_NO_ERROR, "no error." },
00041         { URG_UNKNOWN_ERROR, "unknown error." },
00042         { URG_NOT_CONNECTED, "not connected." },
00043         { URG_NOT_IMPLEMENTED, "not implemented." },
00044         { URG_INVALID_RESPONSE, "invalid response." },
00045         { URG_NO_RESPONSE, "no response." },
00046 
00047         { URG_SEND_ERROR, "send error." },
00048         { URG_RECEIVE_ERROR, "receive error." },
00049         { URG_CHECKSUM_ERROR, "checksum error." },
00050         { URG_INVALID_PARAMETER, "invalid parameter." },
00051         { URG_MEASUREMENT_TYPE_MISMATCH, "measurement type mismatch." },
00052 
00053         { URG_SERIAL_OPEN_ERROR, "could not open serial device." },
00054         { URG_NOT_DETECT_BAUDRATE_ERROR, "could not detect serial baudrate." },
00055         { URG_ETHERNET_OPEN_ERROR, "could not open ethernet port." },
00056         { URG_SCANNING_PARAMETER_ERROR, "scanning parameter error." },
00057         { URG_DATA_SIZE_PARAMETER_ERROR, "data size parameter error." },
00058     };
00059 
00060     int n = sizeof(errors) / sizeof(errors[0]);
00061     int i;
00062 
00063     for (i = 0; i < n; ++i) {
00064         if (errors[i].no == urg->last_errno) {
00065             return errors[i].message;
00066         }
00067     }
00068 
00069     return "Unknown error.";
00070 }
00071 
00072 
00073 void urg_distance_min_max(const urg_t *urg,
00074                           long *min_distance, long *max_distance)
00075 {
00076     if (!urg->is_active) {
00077         *min_distance = 1;
00078         *max_distance = 0;
00079         return;
00080     }
00081 
00082     *min_distance = urg->min_distance;
00083 
00084     // urg_set_communication_data_size() を反映した距離を返す
00085     *max_distance =
00086         (urg->range_data_byte == URG_COMMUNICATION_2_BYTE) ?
00087         max(urg->max_distance, 4095) : urg->max_distance;
00088 }
00089 
00090 
00091 void urg_step_min_max(const urg_t *urg, int *min_index, int *max_index)
00092 {
00093     if (!urg->is_active) {
00094         *min_index = 1;
00095         *max_index = 0;
00096         return;
00097     }
00098 
00099     *min_index = urg->first_data_index - urg->front_data_index;
00100     *max_index = urg->last_data_index - urg->front_data_index;
00101 }
00102 
00103 
00104 long urg_scan_usec(const urg_t *urg)
00105 {
00106     if (!urg->is_active) {
00107         return URG_NOT_CONNECTED;
00108     }
00109 
00110     return urg->scan_usec;
00111 }
00112 
00113 
00114 int urg_max_data_size(const urg_t *urg)
00115 {
00116     if (!urg->is_active) {
00117         return URG_NOT_CONNECTED;
00118     }
00119     return urg->last_data_index + 1;
00120 }
00121 
00122 
00123 double urg_index2rad(const urg_t *urg, int index)
00124 {
00125     int actual_index;
00126     int step;
00127 
00128     if (!urg->is_active) {
00129         return URG_NOT_CONNECTED;
00130     }
00131 
00132     actual_index = min(max(0, index), urg->last_data_index);
00133     step = actual_index - urg->front_data_index + urg->received_first_index;
00134     return urg_step2rad(urg, step);
00135 }
00136 
00137 
00138 double urg_index2deg(const urg_t *urg, int index)
00139 {
00140     return urg_index2rad(urg, index) * 180.0 / M_PI;
00141 }
00142 
00143 
00144 int urg_rad2index(const urg_t *urg, double radian)
00145 {
00146     int index;
00147 
00148     if (!urg->is_active) {
00149         return URG_NOT_CONNECTED;
00150     }
00151 
00152     index =
00153         (int)(floor((urg->area_resolution * radian / (2.0 * M_PI) + 0.5)))
00154         + urg->front_data_index;
00155 
00156     return min(max(0, index), urg->last_data_index);
00157 }
00158 
00159 
00160 int urg_deg2index(const urg_t *urg, double degree)
00161 {
00162     return urg_rad2index(urg, degree * M_PI / 180.0);
00163 }
00164 
00165 
00166 int urg_rad2step(const urg_t *urg, double radian)
00167 {
00168     if (!urg->is_active) {
00169         return URG_NOT_CONNECTED;
00170     }
00171 
00172     return urg_rad2index(urg, radian) - urg->front_data_index;
00173 }
00174 
00175 
00176 int urg_deg2step(const urg_t *urg, double degree)
00177 {
00178     return urg_rad2step(urg, degree * M_PI / 180.0);
00179 }
00180 
00181 
00182 double urg_step2rad(const urg_t *urg, int step)
00183 {
00184     if (!urg->is_active) {
00185         return URG_NOT_CONNECTED;
00186     }
00187 
00188     return (2.0 * M_PI) * step / urg->area_resolution;
00189 }
00190 
00191 
00192 double urg_step2deg(const urg_t *urg, int step)
00193 {
00194     return urg_step2rad(urg, step) * 180.0 / M_PI;
00195 }
00196 
00197 
00198 int urg_step2index(const urg_t *urg, int step)
00199 {
00200     int measure_step;
00201 
00202     if (!urg->is_active) {
00203         return URG_NOT_CONNECTED;
00204     }
00205 
00206     measure_step = step - urg->received_first_index;
00207     return min(max(0, measure_step + urg->front_data_index),
00208                urg->last_data_index);
00209 }


urg_c
Author(s): Satofumi Kamimura , Katsumi Kimoto, Adrian Boeing
autogenerated on Thu Jun 6 2019 19:06:57