void KfieldGui::draw_ball(belief Belief, BallObject Ball) { pthread_mutex_lock(&lock); CvPoint pt1, pt2; int radius = Ball.ball_diameter() / 2 * 1000.0 / scale; //double max = 0; pt1.x = Belief.x + Ball.dist() * 1000 * cos((Belief.theta + Ball.bearing())); pt1.y = Belief.y + Ball.dist() * 1000 * sin((Belief.theta + Ball.bearing())); pt2.x = (pt1.x + (2 * margintoline + field_width) / 2.0) / scale; pt2.y = (-pt1.y + (2 * margintoline + field_height) / 2.0) / scale; cout << "Ball Dist" << Ball.dist() << " Ball diameter " << Ball.ball_diameter() << " Bearing " << Ball.bearing() << " pt x:" << pt1.x << " y: " << pt1.y << endl; cout << " Points in the field pt2.x " << pt2.x << " pt2.y " << pt2.y << endl; cvCopy(cleanfield, field); cvCircle(field, pt2, radius, color["orange"], CV_FILLED, CV_AA, 0); tmp = Kutils::to_string(Ball.dist()); cvPutText(field, tmp.c_str(), cvPoint((2 * margintoline + field_width - 400) / scale, (2 * margintoline + field_height + 850) / scale), &font, color["orange"]); pthread_mutex_unlock(&lock); //cout << " Ball Drawn " << endl; }
QRectF KFieldScene::visionBallRect (BallObject bob, WorldInfo wim) { float xmiddle = (wim.myposition().x() + bob.dist() * cos ( (wim.myposition().phi() + bob.bearing() ) ) ) * 1000; float ymiddle = (wim.myposition().y() + bob.dist() * sin ( (wim.myposition().phi() + bob.bearing() ) ) ) * 1000; return rectFromFC ( xmiddle, ymiddle, 75, 75); }
void LocalWorldState::calculateBallEstimate(Localization::KMotionModel const & robotModel) { KSystem::Time::TimeDuration duration; KSystem::Time::TimeAbsolute observationTime; //float dt; float dx,dy,gx,gy; bool ballseen = false; //cout << " Ball position estimation " << endl; if (obsm.get() && obsm->has_ball()) { //cout << " Ball observation " << endl; //used for removing the ball if exceeds a threshold BallObject aball = obsm->ball(); dx = aball.dist() * cos(aball.bearing()); dy = aball.dist() * sin(aball.bearing()); gx = AgentPosition.x + dx * cos(AgentPosition.phi) + dy * sin(AgentPosition.phi); gy = AgentPosition.y - dx * sin(AgentPosition.phi) + dy * cos(AgentPosition.phi); if ( fabs(gx) < locConfig.fieldMaxX + 2 && fabs(gy) < locConfig.fieldMaxY + 2) { ballseen = true; observationTime = now; if (MyWorld.balls_size() < 1) { //cout << " Inserting new ball " << endl; MyWorld.add_balls(); myBall.reset(aball.dist(), 0, aball.bearing(), 0); ballTimeReset = 1; lastObservationTime = now; } else { if (ballTimeReset < MAX_TIME_TO_RESET) ballTimeReset+= 0.5; //Predict //cout << " Ball exists.. Predict + update " << endl; duration = observationTime - lastObservationTime; lastObservationTime = now; if (duration > KSystem::Time::TimeAbsolute::seconds(MAX_TIME_TO_RESET)) { myBall.reset(aball.dist(), 0, aball.bearing(), 0); } else{ duration = observationTime - lastFilterTime; // dt = duration.total_microseconds() / 1000000.0f; myBall.update(aball.dist(), 0.25 , aball.bearing(), 0.03); myBall.predict(duration.toFloat(),robotModel); } } lastFilterTime = now; //cout << " Sending ball position and velocity " << endl; //Create message MyWorld.mutable_balls(0)->set_relativex(myBall.state(0,0)); MyWorld.mutable_balls(0)->set_relativey(myBall.state(1,0)); MyWorld.mutable_balls(0)->set_relativexspeed(myBall.state(2,0)); MyWorld.mutable_balls(0)->set_relativeyspeed(myBall.state(3,0)); } } if (!ballseen) { //cout << " ball is not being seen " << endl; duration = now - lastObservationTime; //dt = duration.total_microseconds() / 1000000.0f; if (duration.toFloat() > ballTimeReset) { if (MyWorld.balls_size() > 0){ ballTimeReset = 0; //cout << " Reseting ball " << endl; myBall.reset(0, 0, 0, 0); MyWorld.clear_balls(); } } else { duration = now - lastFilterTime; //dt = duration.total_microseconds() / 1000000.0f; if (MyWorld.balls_size() > 0){ //cout << " ball is not being seen .. predict movement" << endl; myBall.predict(duration.toFloat(),robotModel); MyWorld.mutable_balls(0)->set_relativex(myBall.state(0,0)); MyWorld.mutable_balls(0)->set_relativey(myBall.state(1,0)); MyWorld.mutable_balls(0)->set_relativexspeed(myBall.state(2,0)); MyWorld.mutable_balls(0)->set_relativeyspeed(myBall.state(3,0)); } } lastFilterTime = now; } //myBall.state.prettyPrint(); }