C++ ArSick::getRawReadings方法代码示例

int main(int argc, char **argv)
{

	int t, cnt;
	double laser_dist[900];
	double laser_angle[900];
	std::list<ArSensorReading *> *readings;
	std::list<ArSensorReading *>::iterator it;


	ArKeyHandler keyHandler;

	Aria::init();

	// Add the key handler to Aria so other things can find it
	Aria::setKeyHandler(&keyHandler);
	robot.attachKeyHandler(&keyHandler);

	// add the laser to the robot
	robot.addRangeDevice(&sick);

	// Parse all our args
	ArSimpleConnector connector(&argc, argv);

	if (!connector.parseArgs() || argc > 1)
	{
		connector.logOptions();
		exit(1);
	}

	robot.addRangeDevice(&sick);

	// try to connect, if we fail exit
	if (!connector.connectRobot(&robot))
	{
		printf("Could not connect to robot... exiting\n");
		Aria::shutdown();
		return 1;
	}


	// start the robot running, true so that if we lose connection the run stops
	robot.runAsync(true);

	// now set up the laser
	sick.configureShort(true,ArSick::BAUD38400,ArSick::DEGREES180,ArSick::INCREMENT_ONE);

	connector.setupLaser(&sick);

	sick.runAsync();

	if (!sick.blockingConnect())
	{
		printf("Could not connect to SICK laser... exiting\n");
		Aria::shutdown();
		return 1;
	}


    cnt = 1;
	while(cnt<10000){
		readings=(list<ArSensorReading *,allocator<ArSensorReading *> > *)sick.getRawReadings();//CurrentBuffer..
		while (readings == NULL){
			readings = (list<ArSensorReading *, allocator<ArSensorReading *> > *)sick.getRawReadings();
		}
			t=0;
			for(it=readings->begin(); it!=readings->end(); it++){
				//cout << "t: " << t << endl;
				laser_dist[t]=(*it)->getRange();
				laser_angle[t]=-90+t;
				//cout << "laser angle: " << laser_angle[t] << " laser dist.: " << laser_dist[t] <<" "<<"\n";
				t++;
			}
			cout << "count: " << cnt << endl; //for some reason this line needs to be here
			cnt++;
						
	}
	
    for (t=0; t<181; t++){
		cout << "laser angle: " << laser_angle[t] << " laser dist.: " << laser_dist[t] <<" "<<"\n";
    }
	
	robot.waitForRunExit();
	Aria::shutdown();
	return 0;
}

//.........这里部分代码省略.........
    
    // start the robot running, true so that if we lose connection the run stops
    robot.runAsync(true);
    
    
    // Attempt to connect to SICK using another hard-coded USB connection
    sick.setDeviceConnection(&laserCon);
    if((ret=laserCon.open("/dev/ttyUSB3")) !=0) {
        //If connection fails, shutdown
        Aria::shutdown();
        return 1;
    }
    
    //Configure the SICK
    sick.configureShort(false,/*not using sim*/ArSick::BAUD38400,ArSick::DEGREES180,ArSick::INCREMENT_HALF);
    
    //Run the sick
    sick.runAsync();
    
    // Presumably test to make sure that the connection is good
    if(!sick.blockingConnect()){
        printf("Could not get sick...exiting\n");
        Aria::shutdown();
        return 1;
    }
    printf("We are connected to the laser!");
    
    /*
     robot.lock();
     robot.comInt(ArCommands::ENABLE, 1);
     robot.unlock();
     */
    int range [361] = {0};
    int drange [360] = {0};
    int i = 0;
    int obj_range [2];
    int old_range [360]={0};
    clock_t now, prev;
    while(1){
        range [361] = {0};
        drange [360] = {0};
        i = 0;
        obj_range[2];
        
        std::list<ArSensorReading *> *readings;
        std::list<ArSensorReading *>::iterator it;
        sick.lockDevice();
        readings=(list<ArSensorReading *,allocator<ArSensorReading *> > *)sick.getRawReadings();
        
        if(NULL!=readings){
            if ((readings->end() != readings->begin())){
                for (it = readings->begin(); it!= readings->end(); it++){
                    //	      std::cout << (*it)->getRange()<<" ";
                    range[i] = ((*it)->getRange());
                    if(i){
                        drange[i-1] = range[i] - range[i-1];
                        printf("%f %i %i\r\n", (float)i/2.0, range[i], drange[i-1]);
                    }
                    i++;
                }
                int i = 0;
                //detect the object range
                while (i < 360) {
                    if (range[i]>Default_Distance + alpha) {
                        ;
                    } else {
                        if (obj_range[0]=0)
                            obj_range[0]=i;
                        else
                            obj_range[1]=i;
                    }
                }
                if (!now)
                    prev=now;
                now=clock();
                duration=now-prev;
                /******moving straight*******/
                float speed = avg_speed(obj_range,old_range,range,(float)duration)
                
                /*while(i < 360){
                 int r_edge = 0;
                 int l_edge = 0;
                 float obsticle_degree = 0;
                 if(drange[i] > D_DISTANCE){
                 r_edge = i;
                 while(drange[i] > -(D_DISTANCE)){
                 i++;
                 }
                 l_edge = i;
                 obsticle_degree = (r_edge + (l_edge - r_edge)/2.0)/2.0;
                 printf("\r\n object detected at %f\r\n", obsticle_degree);
                 }
                 std::cout<<std::endl;
                 }*/
            }
            else{
                std::cout << "(readings->end() == readings -> begin())" << std::endl;
            }
        }
        else{