46 #define UPPER_MAGIC 0xeeff
47 #define LOWER_MAGIC 0xddff
51 #define RADIANS_PER_LSB 0.00017453293
52 #define METERS_PER_LSB 0.002
58 return (datalen / 3) +1;
62 const void *_data,
int datalen)
64 vd->
data = (uint8_t*) _data;
68 if (datalen != 1206) {
69 printf(
"velodyne: bad len %i\n", datalen);
75 if (vd->
data[1202]==
'v') {
97 uint8_t *data = vd->
data;
105 int magic = data[0] + (data[1]<<8);
112 printf(
"Unknown velodyne magic %4x\n", magic);
139 sample->
phi = params->
vcf;
142 double sin_theta, cos_theta;
143 sin_theta = sin(sample->
theta);
144 cos_theta = cos(sample->
theta);
148 sample->
xyz[0] = sample->
range * cos_theta * cos_phi;
149 sample->
xyz[1] = sample->
range * sin_theta * cos_phi;
150 sample->
xyz[2] = sample->
range * sin_phi;
163 double *ranges,
double *intensities,
double *thetas,
164 double *phis,
int *laserids,
int *nsamples,
int *badscans)
166 if (datalen != 1206) {
167 printf(
"velodyne: bad len %i\n", datalen);
172 int laser_offset = 0;
175 for (data = (uint8_t*) _data;
177 datalen-=100, data += 100) {
179 int magic = data[0] + (data[1]<<8);
186 printf(
"Unknown velodyne magic %4x\n", magic);
194 for (i = 0; i < 32; i++) {
197 ranges[out_idx] = (data[4 + i*3] + (data[5+i*3]<<8)) *
METERS_PER_LSB;
202 if (ranges[out_idx] < 0.5) {
203 badscans[laser_offset+i]++;
207 intensities[out_idx] = data[6 + i*3]/255.0;
208 thetas[out_idx] = theta0 + params->
rcf;
209 phis[out_idx] = params->
vcf;
210 laserids[out_idx] = laser_offset + i;
223 printf(
"struct velodyne_laser_calib velodyne_NAME_ME_HERE[] = {\n");
227 printf(
" { %11.7f, %11.7f, %8.4f, %8.4f, %10.6f }, // laser %2d\n",
237 int a = *((
int*) _a);
238 int b = *((
int*) _b);
int physical2logical[VELODYNE_NUM_LASERS]
Defines a struc for velodyne calibration.
header file with velodyne types
void velodyne_calib_dump(velodyne_calib_t *v)
int logical2physical[VELODYNE_NUM_LASERS]
struct velodyne_laser_calib velodyne_uncalibrated[]
int velodyne_decoder_estimate_samples(velodyne_calib_t *v, const void *_data, int datalen)
static velodyne_calib_t * __v
velodyne_calib_t * velodyne_calib_create()
static int velodyne_physical_to_logical(velodyne_calib_t *v, int phys)
int velodyne_decoder_next(velodyne_calib_t *v, velodyne_decoder_t *vd, velodyne_sample_t *sample)
int velodyne_decode(velodyne_calib_t *v, const void *_data, int datalen, double *_theta0, double *ranges, double *intensities, double *thetas, double *phis, int *laserids, int *nsamples, int *badscans)
static double mod2pi_ref(double ref, double vin)
struct velodyne_laser_calib lasers[VELODYNE_NUM_LASERS]
static int laser_phi_compare(const void *_a, const void *_b)
double sincos[VELODYNE_NUM_LASERS][2]
int velodyne_decoder_init(velodyne_calib_t *v, velodyne_decoder_t *vd, const void *_data, int datalen)
int velodyne_calib_precompute(velodyne_calib_t *v)
double range_scale_offset
header file for math utilities
#define VELODYNE_NUM_LASERS