int main (int argc, const char** argv){
if (argc<2){
printf( "Usage: urg_test <device> ");
return 0;
}
char buf[URG_BUFSIZE];
HokuyoURG urg;
int o=hokuyo_open(&urg,argv[1]);
if (o<=0)
return -1;
o=hokuyo_init(&urg);
if (o<=0)
return -1;
o=hokuyo_startContinuous(&urg, 0, 768, 0);
if (o<=0){
return -1;
}
//double astep=2*M_PI/1024;
int k=0;
while (k<20){
hokuyo_readPacket(&urg, buf, URG_BUFSIZE,10);
HokuyoRangeReading reading;
hokuyo_parseReading(&reading, buf);
//if we get too much maxranges, restart the laser
int validBeamsCount=0;
int i;
for (i=0; i<reading.n_ranges; i++){
if (reading.ranges[i]>20 && reading.ranges[i]< 5601)
validBeamsCount++;
}
fprintf(stderr, "ts=%d, ss=%d, es=%d, cc=%d, nr=%d, st=%d vbc=%d\n",
reading.timestamp,
reading.startStep,
reading.endStep,
reading.clusterCount,
reading.n_ranges,
reading.status,
validBeamsCount);
// cout << "set size ratio -1" << endl;
// cout << "plot [-5600:5600][-5600:5600]'-' w l" << endl;
// for (int i=0; i<reading.n_ranges; i++){
// double alpha=(-135./180.)*M_PI+astep*i;
// cout << reading.ranges[i] * cos(alpha) << " " << reading.ranges[i] * sin (alpha) << endl;
// }
// cout << "e" << endl << endl;
k++;
}
hokuyo_close(&urg);
return 0;
}
unsigned int hokuyo_readStatus(HokuyoURG* urg, char* cmd){
char buf[URG_BUFSIZE];
write (urg->fd, cmd, strlen(cmd));
while (1){
int c=hokuyo_readPacket(urg, buf, URG_BUFSIZE,10);
if (c>0 && !strncmp(buf,cmd+1,strlen(cmd)-1)){
char*s=buf;
s=skipLine(s);
char v[3]={s[0], s[1], 0};
return atoi(v);
}
}
return 0;
}
unsigned int hokuyo_readStatus(HokuyoLaser* hokuyoLaser, char* cmd, int echo __attribute__((unused))){
char buf[HOKUYO_BUFSIZE];
write_command(hokuyoLaser,cmd);
while (1){
int c=hokuyo_readPacket(hokuyoLaser, buf, HOKUYO_BUFSIZE,10);
if (c>0 && !strncmp(buf,cmd+1,strlen(cmd)-1)){
//if (echo)
//printf("%s",buf);
char*s=buf;
s=skipLine(s);
char v[3]={s[0], s[1], 0};
return atoi(v);
}
}
return 0;
}
