void Vision::fetchAndProcess()
{
leds.Clear();
obs.Clear();
vdm.Clear();
//unsigned long startt = SysCall::_GetCurrentTimeInUSec();
KSystem::Time::TimeAbsolute oldstamp = stamp;
boost::shared_ptr<const KRawImage> img = _blk.readData<KRawImage> ("image", Messaging::MessageEntry::HOST_ID_LOCAL_HOST, &stamp);
if(stamp <= oldstamp)
return ;
if(img.get() == 0)
return;
// cout << "haveimage" << endl;
//Remove constness, tricky stuff :/
rawImage.copyFrom(img->image_rawdata().data(),
img->width(), img->height(), img->bytes_per_pix());
obs.set_image_timestamp(stamp.toString());
vdm.set_image_timestamp(stamp.toString());
//unsigned long endt = SysCall::_GetCurrentTimeInUSec()-startt;
//cout<<"Fetch image takes:"<<endt<<endl;
stamp += KSystem::Time::TimeAbsolute::milliseconds(config.sensordelay);
if (img->active_camera() == KRawImage::BOTTOM)//bottom cam
{
//Get Kinematics first!
std::vector<float> val = kinext.getKinematics("CameraBottom");
p.cameraPitch = (KMath::KMat::transformations::PI * 40.0) / 180.0;
//cout<<"CameraPitch:"<<cameraPitch<<endl;
seg = segbottom;
p.cameraX = val[0];
p.cameraY = val[1];
p.cameraZ = val[2];//3rd element
}
else
{
//Get Kinematics first!
std::vector<float> val = kinext.getKinematics("CameraTop");
p.cameraX = val[0];
p.cameraY = val[1];
p.cameraZ = val[2];//3rd element
p.cameraPitch = 0;
seg = segtop;
}
seg->setLumaScale(pow(img->luminance_scale(), config.cameraGamma) );
//std::cout<<img->luminance_scale()<<std::endl;
//cout<<"Attach to Image:"<<seg<<rawImage<<endl;
seg->attachToIplImage(rawImage);//Make segmentator aware of a new image
//saveFrame(rawImage);
//return;
asvmo = _blk.readData<AllSensorValuesMessage> ("sensors", Messaging::MessageEntry::HOST_ID_LOCAL_HOST, &timeo, &stamp, Blackboard::DATA_NEAREST_NOTNEWER);
asvmn = _blk.readData<AllSensorValuesMessage> ("sensors", Messaging::MessageEntry::HOST_ID_LOCAL_HOST, &timen, &stamp, Blackboard::DATA_NEAREST_NOTOLDER);
#ifdef DEBUGVISION
cout << "ImageTimestamp:" << KSystem::Time::to_iso_string(stamp) << endl;
cout << "Now:" << KSystem::Time::SystemTime::now() << endl;
cout << "SensorTimestamp:" << KSystem::Time::to_iso_string(timeo) << "," << KSystem::Time::to_iso_string(timen) << endl;
// KSystem::Time::TimeAbsolute t;
// _blk.readData<AllSensorValuesMessage> ("sensors", Messaging::MessageEntry::HOST_ID_LOCAL_HOST, &t);
// cout << "Lasttimestamp:"<< KSystem::Time::to_iso_string(t) << endl;
#endif
if (asvmn.get() == NULL || asvmo.get() == NULL) //No sensor data!
{
LogEntry(LogLevel::Error,GetName()) << "Warning!!! Vision has no head joint msg in all sensor (allsm) data!";
return;
}
float imcomp = ( (float ) ((stamp - timeo).raw()) ) / KSystem::Time::TimeAbsolute::TPS;
assert(imcomp >= 0);
//each value is (n-o)*(stamp-timeo) +o
p.yaw = asvmo->jointdata(KDeviceLists::HEAD + KDeviceLists::YAW).sensorvalue();
p.yaw += (asvmn->jointdata(KDeviceLists::HEAD + KDeviceLists::YAW).sensorvalue() - p.yaw) * imcomp;
p.pitch = asvmo->jointdata(KDeviceLists::HEAD + KDeviceLists::PITCH).sensorvalue();
p.pitch += (asvmn->jointdata(KDeviceLists::HEAD + KDeviceLists::PITCH).sensorvalue() - p.pitch) * imcomp;
//p.Vyaw = asvmo->jointdata(KDeviceLists::HEAD+KDeviceLists::YAW).sensorvaluediff();
//p.Vpitch = asvmo->jointdata(KDeviceLists::HEAD+KDeviceLists::PITCH).sensorvaluediff();
p.angX = asvmo->computeddata(KDeviceLists::ANGLE + KDeviceLists::AXIS_X).sensorvalue();
p.angX += (asvmn->jointdata(KDeviceLists::HEAD + KDeviceLists::AXIS_X).sensorvalue() - p.angX) * imcomp;
p.angX += config.rolloffset;
p.angY = asvmo->computeddata(KDeviceLists::ANGLE + KDeviceLists::AXIS_Y).sensorvalue();
p.angY += (asvmn->jointdata(KDeviceLists::HEAD + KDeviceLists::AXIS_Y).sensorvalue() - p.angY) * imcomp;
p.angY += config.pitchoffset;
//p.VangX = asvm->computeddata(KDeviceLists::ANGLE+KDeviceLists::AXIS_X).sensorvaluediff();
//p.VangY = asvm->computeddata(KDeviceLists::ANGLE+KDeviceLists::AXIS_Y).sensorvaluediff();
//p.timediff = asvm->timediff();//Get time from headmessage
#ifdef DEBUGVISION
cout << p.yaw << " " << p.pitch << " " << p.angX << " " << p.angY << imcomp << endl;
#endif
p.focallength = sqrt(sqrd(rawImage.width) + sqrd(rawImage.height) ) / (2 * tan(config.Dfov * TO_RAD / 2));
//Logger::Instance().WriteMsg("Vision", _toString("Focal Length ")+_toString(p.focallength), Logger::Error);
kinext.setPose(p);
KMath::KMat::transformations::makeRotation(simpleRot, -kinext.getRoll());
//Now change y axis :)
simpleRot(0, 1) = -simpleRot(0, 1);
simpleRot(1, 1) = -simpleRot(1, 1);
//Weird stuff. simpleRot-^-1=simpleRot'=simpleRot. therefore translaton from raw image to rotated and axis inverted
//image and back is done by simpleRot
//
// Vup.y = -cos(-kinext.getRoll());
// Vup.x = sin(-kinext.getRoll());
Vup = simpleRot.slow_mult(KVecFloat2(0, 1));
Vdn = simpleRot.slow_mult(KVecFloat2(0, -1));
Vlt = simpleRot.slow_mult(KVecFloat2(-1, 0));
Vrt = simpleRot.slow_mult(KVecFloat2(1, 0));
KVecFloat2 c2d;
KVecFloat3 c3d;
//Publish the projection of the camera limits on the ground
PointObject *p1;
c2d = imageToCamera(KVecFloat2(0, 0));
c3d = kinext.camera2dToGround(c2d);
if(c3d(2) < 0) //Not Looking up :)
{
p1 = obs.add_view_limit_points();
measurement *m = kinext.projectionDistance(c2d, 0);
p1->set_distance(m[0].mean);
p1->set_bearing(m[1].mean);
//cout<<"See bot:"<<newpost.distBot.mean<<endl;
delete[] m;
}
c2d = imageToCamera(KVecFloat2(0, rawImage.height));
c3d = kinext.camera2dToGround(c2d);
if(c3d(2) < 0) //Not Looking up :)
{
p1 = obs.add_view_limit_points();
measurement *m = kinext.projectionDistance(c2d, 0);
p1->set_distance(m[0].mean);
p1->set_bearing(m[1].mean);
//cout<<"See bot:"<<newpost.distBot.mean<<endl;
delete[] m;
}
c2d = imageToCamera(KVecFloat2(rawImage.width, rawImage.height));
c3d = kinext.camera2dToGround(c2d);
if(c3d(2) < 0) //Not Looking up :)
{
p1 = obs.add_view_limit_points();
measurement *m = kinext.projectionDistance(c2d, 0);
p1->set_distance(m[0].mean);
p1->set_bearing(m[1].mean);
//cout<<"See bot:"<<newpost.distBot.mean<<endl;
delete[] m;
}
c2d = imageToCamera(KVecFloat2(rawImage.width, 0));
c3d = kinext.camera2dToGround(c2d);
if(c3d(2) < 0) //Not Looking up :)
{
p1 = obs.add_view_limit_points();
measurement *m = kinext.projectionDistance(c2d, 0);
p1->set_distance(m[0].mean);
p1->set_bearing(m[1].mean);
//cout<<"See bot:"<<newpost.distBot.mean<<endl;
delete[] m;
}
/*Vdn.x = -Vup.x;
Vdn.y = -Vup.y;
Vlt.x = Vup.y;//-Vup.x*cos(-45*TO_RAD)-Vup.y*sin(-45*TO_RAD);
Vlt.y = -Vup.x;//-Vup.x*sin(-45*TO_RAD)+Vup.y*sin(-45*TO_RAD);
Vrt.x = -Vup.y;
Vrt.y = Vup.x;*/
//unsigned long startt,endt;
//startt=SysCall::_GetCurrentTimeInUSec();
ballpixels.clear();
ygoalpost.clear();
obstacles.clear();
tobeshown.clear();
gridScan(orange);
//endt = SysCall::_GetCurrentTimeInUSec()-startt;
//cout<<"GridScan takes:"<<endt<<endl;
//startt=SysCall::_GetCurrentTimeInUSec();
balldata_t b = locateBall(ballpixels);
//endt = SysCall::_GetCurrentTimeInUSec()-startt;
//cout<<"locateball takes:"<<endt<<endl;
//unsigned long endt = SysCall::_GetCurrentTimeInUSec()-startt;
//cout<<"locateball takes:"<<endt<<endl;
//cout<<b.r<<endl;
locateGoalPost(ygoalpost, yellow);
#ifdef DEBUGVISION
cout << "Ballpixelsize:" << ballpixels.size() << endl;
//cout << b.x << " " << b.y << " " << b.cr << endl;
//KVecFloat2 & w=camToRobot(o)
cout << "Bearing:" << b.bearing.mean << " " << b.bearing.var << endl;
cout << "Distance:" << b.distance.mean << " " << b.distance.var << endl;
#endif
if (b.cr > 0)
{
//Fill message and publish
KVecFloat2 im;
im(0) = b.x;
im(1) = b.y;
KVecFloat2 c;
KVecFloat3 c3d;
c = imageToCamera(im);
c3d = kinext.camera2dToTorso(c);
//c3d(0)+p.cameraX;c3d(1)+cameraY;
//if(sqrt(sqrd(c3d(0)) + sqrd(c3d(1)))<0.1)
// c3d.scalar_mult(2);
float pitch, yaw;
pitch = atan(abs(c3d(2)) / sqrt(sqrd(c3d(0)) + sqrd(c3d(1))));
yaw = atan2(c3d(1), c3d(0));
float w = b.distance.mean * 3;
w = w > 1 ? 1 : w;
BallObject ballpos;
ballpos.set_dist(b.distance.mean);
ballpos.set_bearing(b.bearing.mean);
ballpos.set_ball_diameter(b.ballradius * 2);
obs.mutable_ball()->CopyFrom(ballpos);
LedValues* l = leds.add_leds();
l->set_chain("l_eye");
l->set_color("red");
}
else
{
LedValues* l = leds.add_leds();
l->set_chain("l_eye");
l->set_color("white");
}
if (obs.regular_objects_size() > 0)
{
bool yellow = false;
::google::protobuf::RepeatedPtrField<const ::NamedObject>::const_iterator ptr = obs.regular_objects().begin();
while (ptr != obs.regular_objects().end())
{
if ((*ptr).object_name().c_str()[0] == 'Y')
yellow = true;
++ptr;
}
LedValues* l = leds.add_leds();
l->set_chain("r_eye");
if (obs.regular_objects_size() == 1 && obs.regular_objects(0).object_name().compare("Yellow")==0)
{
l->set_color("blue");
}else if(obs.regular_objects_size() == 1){
l->set_color("yellow");
}else if (obs.regular_objects_size() == 2)
{
l->set_color("purple");
}
}
else
{
LedValues* l = leds.add_leds();
l->set_chain("r_eye");
l->set_color("black");
}
_blk.publishSignal(vdm, "debug");
_blk.publishSignal(leds, "leds");
_blk.publishSignal(obs, "vision");
}
