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