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;
}
int main(int argc, char **argv)
{
int ret; //Don't know what this variable is for
ArRobot robot;// Robot object
ArSick sick; // Laser scanner
ArSerialConnection laserCon; // Scanner connection
ArSerialConnection con; // Robot connection
std::string str; // Standard output
// sonar, must be added to the robot
ArSonarDevice sonar;
// the actions we'll use to wander
// recover from stalls
ArActionStallRecover recover;
// react to bumpers
ArActionBumpers bumpers;
// limiter for close obstacles
ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250, 1.1);
// limiter for far away obstacles
ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 600, 1.1);
// limiter for the table sensors
ArActionLimiterTableSensor tableLimiter;
// actually move the robot
ArActionConstantVelocity constantVelocity("Constant Velocity", 400);
// turn the orbot if its slowed down
ArActionTurn turn;
// mandatory init
Aria::init();
// Parse all our args
ArSimpleConnector connector(&argc, argv);
connector.parseArgs();
if (argc > 1)
{
connector.logOptions();
exit(1);
}
// add the sonar to the robot
robot.addRangeDevice(&sonar);
// add the laser to the robot
robot.addRangeDevice(&sick);
// NOTE: HARDCODED USB PORT!
// Attempt to open hard-coded USB to robot
if ((ret = con.open("/dev/ttyUSB2")) != 0){
// If connection fails, exit
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// set the robot to use the given connection
robot.setDeviceConnection(&con);
// do a blocking connect, if it fails exit
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// turn on the motors, turn off amigobot sounds
//robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::SOUNDTOG, 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{
