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"); }