static char near_enough(t_ray *r1, t_ray *r2)
{
t_mtx p1;
t_mtx p2;
p1 = mtx_add(mtx_mult(r1->dir, r1->t), r1->pos);
p2 = mtx_add(mtx_mult(r2->dir, r2->t), r2->pos);
return (NEAR(p1.mtx[0], p2.mtx[0])
&& NEAR(p1.mtx[1], p2.mtx[1])
&& NEAR(p1.mtx[2], p2.mtx[2]));
}
void
Driver::init()
{
// read driver-specific properties
std::string prefix = "HokuyoAist.";
int baudrate = context_.properties().getPropertyAsIntWithDefault( prefix+"Baudrate", 19200 );
std::string dev = context_.properties().getPropertyWithDefault( prefix+"Device", "/dev/ttyACM0" );
bool highSensitivity = context_.properties().getPropertyAsIntWithDefault( prefix+"HighSensitivity", 0 );
context_.tracer().debug( "initializing driver",2 );
try
{
stringstream ss;
ss << "type=serial,device=" << dev << ",timeout=1,baud="<< baudrate;
laser_->Open ( ss.str());
laser_->SetPower(true);
if(highSensitivity){
laser_->SetHighSensitivity(true);
}else{
laser_->SetHighSensitivity(false);
}
try{
laser_->SetBaud(baudrate);
}
catch (hokuyo_aist::HokuyoError &e){
ss.str("");
ss << "Failed to change baud rate: [" << e.Code () << "] " << e.what ()
<< " This can happen if the device is USB" << endl;
context_.tracer().info( ss.str() );
}
hokuyo_aist::HokuyoSensorInfo info;
laser_->GetSensorInfo (&info);
ss.str("");
ss << "Laser info:\n" << info.AsString ();
context_.tracer().info( ss.str() );
//check that hokuyo config matches our config
if( NEAR(config_.maxRange, info.maxRange/1000.0, 1e-4) // ~1/10 mm
&& NEAR(config_.minRange, info.minRange/1000.0, 1e-4) // ~1/10 mm
&& NEAR(config_.startAngle, info.minAngle, 1e-4) // ~1/200 deg
&& NEAR(config_.fieldOfView, info.maxAngle-info.minAngle, 1e-4) // ~1/200 deg
&& config_.numberOfSamples == (int)info.scanableSteps
) {
//all good
context_.tracer().info("cfg matches!");
}
else{
ss.str("");
ss << "cfg mismatch!\n";
ss << config_.maxRange << " ?? " << info.maxRange/1000.0 << endl
<< config_.minRange << " ?? " << info.minRange/1000.0 << endl
<< config_.startAngle << " ?? " << info.minAngle << endl
<< config_.fieldOfView << " ?? " << info.maxAngle-info.minAngle << endl
<< config_.numberOfSamples << " ?? " << info.scanableSteps << endl;
throw gbxutilacfr::Exception( ERROR_INFO, ss.str() );
}
}
catch (hokuyo_aist::HokuyoError &e)
{
std::stringstream ss;
ss << "failed during initialisation of hokuyo_aist. Errorcode: "
<< e.Code()
<< ", Errordescription: "
<< e.what();
throw gbxutilacfr::HardwareException( ERROR_INFO, ss.str() );
}
context_.tracer().debug( "driver initialized",2 );
}
static Int
getNearSidesArea(Area a, Area b, Int distance)
{ int d=valInt(distance);
int a_top, a_center, a_bottom, a_left, a_middle, a_right;
int b_top, b_center, b_bottom, b_left, b_middle, b_right;
register unsigned long mask;
InitAreaA;
InitAreaB;
NormaliseArea(ax, ay, aw, ah);
NormaliseArea(bx, by, bw, bh);
a_top = ay;
a_bottom = ay+ah-1;
a_center = (a_top+a_bottom+1)/2;
a_left = ax;
a_right = ax+aw-1;
a_middle = (a_left+a_right+1)/2;
b_top = by;
b_bottom = by+bh-1;
b_center = (b_top+b_bottom+1)/2;
b_left = bx;
b_right = bx+bw-1;
b_middle = (b_left+b_right+1)/2;
mask = 0;
NEAR(a_top, b_top, d, mask, 01);
NEAR(a_top, b_center, d, mask, 02);
NEAR(a_top, b_bottom, d, mask, 04);
NEAR(a_center, b_top, d, mask, 010);
NEAR(a_center, b_center, d, mask, 020);
NEAR(a_center, b_bottom, d, mask, 040);
NEAR(a_bottom, b_top, d, mask, 0100);
NEAR(a_bottom, b_center, d, mask, 0200);
NEAR(a_bottom, b_bottom, d, mask, 0400);
NEAR(a_left, b_left, d, mask, 01000);
NEAR(a_left, b_middle, d, mask, 02000);
NEAR(a_left, b_right, d, mask, 04000);
NEAR(a_middle, b_left, d, mask, 010000);
NEAR(a_middle, b_middle, d, mask, 020000);
NEAR(a_middle, b_right, d, mask, 040000);
NEAR(a_right, b_left, d, mask, 0100000);
NEAR(a_right, b_middle, d, mask, 0200000);
NEAR(a_right, b_right, d, mask, 0400000);
answer(toInt(mask));
}
