void EKFLocalization::predict() {
float movx, movy, movt;
float diffx, diffy, difft;
bool apply;
vector<float> pos = pmotion->getRobotPosition(USE_SENSOR);
diffx = pos[0] *1000.0 - lastpos[0]*1000.0;
diffy = pos[1] *1000.0 - lastpos[1]*1000.0;
difft = normalizePi(pos[2] - lastpos[2]);
movx = diffx*cos(-lastpos[2]) - diffy*sin(-lastpos[2]);
movy = diffx*sin(-lastpos[2]) + diffy*cos(-lastpos[2]);
movt = difft;
lastpos = pos;
if((movx>0.0001) || (movy>0.0001) || (movt>0.001))
{
mgrid->predict(movx, movy, movt);
ekf.filter->predict(movx, movy, movt);
}
}
void
Airport::checkLandings()
{
if(flights.empty()) return;
//pthread_mutex_lock (&mutex);
std::list<Flight*>::iterator it;
it = flights.begin();
while(it != flights.end())
{
if((final_pos.distance((*it)->getPosition()) < LANDING_DIST) &&
(toDegrees(normalizePi(fabs((*it)->getBearing() - toRadians(LANDING_BEAR))))<LANDING_BEAR_MAX_ERROR) &&
((*it)->getSpeed()<LANDING_SPEED))
{
std::cerr<<"Flight "<<(*it)->getId()<<" landed"<<std::endl;
points += (int)(*it)->getPoints();
it = removeFlight((*it)->getId());
std::cerr<<"*";
return;
}else
it++;
}
//pthread_mutex_unlock (&mutex);
}
void EKFLocalization::UpdateInfo()
{
//motion
float diffx, diffy, difft;
bool apply;
vector<float> pos = pmotion->getRobotPosition(USE_SENSOR);
diffx = pos[0] *1000.0 - lastpos[0]*1000.0;
diffy = pos[1] *1000.0 - lastpos[1]*1000.0;
difft = normalizePi(pos[2] - lastpos[2]);
_motion.movx = diffx*cos(-lastpos[2]) - diffy*sin(-lastpos[2]);
_motion.movy = diffx*sin(-lastpos[2]) + diffy*cos(-lastpos[2]);
_motion.movt = difft;
lastpos = pos;
//perception info
ObjectState* goalp0r = &(_GoalDetector->goals->p0RightEstimate);
ObjectState* goalp0l = &(_GoalDetector->goals->p0LeftEstimate);
ObjectState* goalp1r = &(_GoalDetector->goals->p1RightEstimate);
ObjectState* goalp1l = &(_GoalDetector->goals->p1LeftEstimate);
_perception.p0ElapsedTimeSinceLastObs = _GoalDetector->getObj()->p0ElapsedTimeSinceLastObs;
_perception.p1ElapsedTimeSinceLastObs = _GoalDetector->getObj()->p1ElapsedTimeSinceLastObs;
_perception.p0r.reliability = goalp0r->getReliability();
_perception.p0r.x = goalp0r->getPositionInRelativeCoordinates().x;
_perception.p0r.y = goalp0r->getPositionInRelativeCoordinates().y;
_perception.p0l.reliability = goalp0l->getReliability();
_perception.p0l.x = goalp0l->getPositionInRelativeCoordinates().x;
_perception.p0l.y = goalp0l->getPositionInRelativeCoordinates().y;
_perception.p1r.reliability = goalp1r->getReliability();
_perception.p1r.x = goalp1r->getPositionInRelativeCoordinates().x;
_perception.p1r.y = goalp1r->getPositionInRelativeCoordinates().y;
_perception.p1l.reliability = goalp1l->getReliability();
_perception.p1l.x = goalp1l->getPositionInRelativeCoordinates().x;
_perception.p1l.y = goalp1l->getPositionInRelativeCoordinates().y;
}
void
EKFLocalization::getPosition(vector<tRobotHipothesis> &hipotheses)
{
hipotheses.clear();
tRobotHipothesis aux;
aux.x = ekf.filter->x();
aux.y = ekf.filter->y();
aux.t = ekf.filter->t();
aux.prob = getQ(ekf.filter);
aux.id = 0;
hipotheses.push_back(aux);
aux.x = -ekf.filter->x();
aux.y = -ekf.filter->y();
aux.t = normalizePi(ekf.filter->t()+M_PI);
aux.id = 1;
hipotheses.push_back(aux);
}
Pose2D EKFLocalization::getIdealPose(Pose2D &robotPose, LineSample &detline, wmLine *wmline)
{
Pose2D ret;
Vector2<double> pd, p;
float angle_p, dist_p;
pd.x = (detline.ps3D.X+detline.pe3D.X)/2.0;
pd.y = (detline.ps3D.Y+detline.pe3D.Y)/2.0;
p = Pose2D::relative2FieldCoord(robotPose, pd.x, pd.y);
//cerr<<"("<<pd.x<<","<<pd.y<<") -> ("<<p.x<<","<<p.y<<")"<<endl;
angle_p = atan2(p.y - robotPose.translation.y, p.x - robotPose.translation.x);
dist_p = sqrt(pd.x*pd.x+pd.y*pd.y);
//cerr<<"Dist = "<<dist_p<<"\tangle = "<<angle_p<<endl;
Vector2<double> vd;
vd.x = cos(angle_p);
vd.y = sin(angle_p);
double A,B,C;
getEqLine(robotPose.translation, vd, A, B, C);
Vector2<double> i;
getInterseccLines (A, B, C, wmline->A, wmline->B, wmline->C, i);
//cerr<<"intersecction in ("<<i.x<<","<<i.y<<")"<<endl;
ret.translation.x = i.x + (-vd.x * dist_p);
ret.translation.y = i.y + (-vd.y * dist_p);
//Rotación
//el ángulo de la línea es wmline->angle
//yo lo veo con un ángulo angle_z
// mi orientación será wm_line - angle_z (o dado la vuelta);
Vector2<double> ps3Da, pe3Da;
ps3Da = Pose2D::relative2FieldCoord(robotPose, detline.ps3D.X, detline.ps3D.Y);
pe3Da = Pose2D::relative2FieldCoord(robotPose, detline.pe3D.X, detline.pe3D.Y);
double angle_z = atan2(detline.ps3D.Y-detline.pe3D.Y, detline.ps3D.X-detline.pe3D.X);
double angle_za = atan2(ps3Da.y-pe3Da.y, ps3Da.x-pe3Da.x);
double diff;
/*diff = normalize(wmline->angle - angle_z);
if(fabs(diff) > (pi_2))
diff = normalize(wmline->angle + angle_z);
ret.rotation = robotPose.rotation - diff;
*/
//if(fabs(angle_z) > (pi_2))
// angle_z = normalize(angle_z+M_PI);
ret.rotation = normalizePi(wmline->angle - angle_z);
if(fabs(ret.rotation-robotPose.rotation)>(pi_2))
ret.rotation = normalizePi(ret.rotation+M_PI);
return ret;
/*
//Obtenemos la recta que une la posición del robot con la línea del mundo a la que dice que está
float Apwm, Bpwm, Cpwm;
Apwm = normalize(wmline->angle + (pi_2));
Bpwm = -1.0;
Cpwm = (-Apwm*robotPose.x + robotPose.y);
//Obtención del punto de intersección entre ambas rectas
float D, E, F;
float ix, iy;
D = wmline->A;
E = wmline->B;
F = wmline->C;
if(A!=0.0)
{
iy = (((D*C)/A)- F )/(((-DB)/A) + E);
ix = ((-B*iy)-C)/A;
}else
{
iy = (((A*F)/D)- C )/(((-EA)/D) + B);
ix = ((-E*iy)-F)/D;
}
float rx, ry;
rx = robotPose.translation.x;
ry = robotPose.translation.y;
//Y ahora la recta detectada
Vector2<double> ps3D, pe3D;
ps3D = Pose2D::relative2FieldCoord(robotPose, detline.ps3D.X/detline.ps3D.H, detline.ps3D.Y/detline.ps3D.H);
pe3D = Pose2D::relative2FieldCoord(robotPose, detline.pe3D.X/detline.pe3D.H, detline.pe3D.Y/detline.pe3D.H);
float Adet, Bdet, Cdet;
Adet = normalize(atan2(ps3D.y-pe3D.y, ps3D.x-pe3D.x));
Bdet = -1.0;
Cdet = (-Adet*robotPose.x + robotPose.y);
//Obtenemos la distancia del robot a la línea observada
float distDet = fabs(Adet*rx+Bdet*yr+Cdet)/sqrt(Adet*Adet+Bdet*Bdet);
/* Vector2<double> ps3D, pe3D;
HPoint3D p1, p2;
ps3D = Pose2D::relative2FieldCoord(robotPose, detline.ps3D.X/detline.ps3D.H, detline.ps3D.Y/detline.ps3D.H);
pe3D = Pose2D::relative2FieldCoord(robotPose, detline.pe3D.X/detline.pe3D.H, detline.pe3D.Y/detline.pe3D.H);
float diffAngle;
float detangle;
detangle = atan2(ps3D.y-pe3D.y, ps3D.x-pe3D.x);
diffangle = normalize(detangle - wmline->angle);
if(diffangle>(pi_2))
diffangle = normalize(diffangle - (pi_2));
if(diffangle<(-pi_2))
diffangle = normalize(diffangle + (pi_2));
*/
}
bica::ShapeList
EKFLocalization::getGrDebugAbs()
{
shapeListAbs.clear();
pthread_mutex_lock(&mutex);
{
Point2D p2d, p22;
p2d.x = (float)ekf.filter->x();
p2d.y = (float)ekf.filter->y();
p22.x = (float)ekf.filter->x() + 600.0 * cos(ekf.filter->t());
p22.y = (float)ekf.filter->y() + 600.0 * sin(ekf.filter->t());
bica::Point3DPtr src(new bica::Point3D);
bica::Point3DPtr dst(new bica::Point3D);
bica::ArrowPtr a(new bica::Arrow);
//cerr<<"---> ("<<(*it)->x()<<", "<<(*it)->y()<<", "<<(*it)->t()<<")"<<endl;
src->x = (float)ekf.filter->x();
src->y = (float)ekf.filter->y();
src->z = 600.0;
dst->x = (float)ekf.filter->x() + 600.0 * cos(ekf.filter->t());
dst->y = (float)ekf.filter->y() + 600.0 * sin(ekf.filter->t());
dst->z = 600.0;
a->src = src;
a->dst = dst;
a->width = 10.0;
a->color = bica::BLUE;
a->filled = true;
a->opacity = 255;
a->accKey = "c";
a->label = "KFE";
shapeListAbs.push_back(a);
bica::EllipsePtr e(new bica::Ellipse);
bica::Point3DPtr c(new bica::Point3D);
c->x = (float)ekf.filter->x();
c->y = (float)ekf.filter->y();
c->z = 550;
e->center = c;
GaussianDistribution2D distrib;
distrib.mean[0] = ekf.filter->x();
distrib.mean[1] = ekf.filter->y();
distrib.covariance[0][0] = ekf.filter->get_P()->e(0, 0);
distrib.covariance[0][1] = ekf.filter->get_P()->e(0, 1);
distrib.covariance[1][0] = ekf.filter->get_P()->e(1, 0);
distrib.covariance[1][1] = ekf.filter->get_P()->e(1, 1);
double va1, va2;
Vector2D_BH<double> ve1, ve2;
double x,y;
distrib.getEigenVectorsAndEigenValues(ve1, ve2, va1, va2);
double angle;
angle = atan2(ve1[1], ve1[0]);
if(ekf.filter->get_P()->e(0, 0)<ekf.filter->get_P()->e(1, 1))
angle = normalizePi(angle+pi_2);
e->width = (float)sqrt(va1);
e->length = (float)sqrt(va2);
e->angle = (float)angle;
e->color = bica::WHITE;
e->filled = true;
e->opacity = 125;
e->accKey = "c";
e->label = "KFEE";
shapeListAbs.push_back(e);
}
//cerr<<endl;
for(int i=0; i<mgrid->getRows(); i++)
for(int j=0; j<mgrid->getColumns(); j++)
{
bica::RectanglePtr a(new bica::Rectangle);
bica::Point3DPtr p2d(new bica::Point3D);
bica::Point3DPtr p22(new bica::Point3D);
p2d->x=(i*CELL_SIZE)-3000.0;
p2d->y=(j*CELL_SIZE)-2000.0;
p2d->z=500.0;
p22->x = ((i+1)*CELL_SIZE)-3000.0;
p22->y = ((j+1)*CELL_SIZE)-2000.0;
p22->z=500.0;
float media;
media = 1.0/((float)mgrid->getRows()*(float)mgrid->getColumns());
float value = (((mgrid->getProbIJ(i,j)/media)*255.0))/2.0;
float p = mgrid->getProbIJ(i,j);
if (p>0.01)
value = (p/0.02)*255.0;
else
value = 0.0;
//value = mgrid->getProbIJ(i,j);
if (value>255.0f)
value = 255.0f;
//cerr<<"("<<p2d->x<<", "<<p2d->y<<"," <<value<<") ";
a->p1 = p2d;
a->p2 = p22;
a->color = bica::RED;
a->filled = true;
a->opacity = value;
a->accKey = "c";
a->label = "KF";
shapeListAbs.push_back(a);
bica::Point3DPtr src(new bica::Point3D);
bica::Point3DPtr dst(new bica::Point3D);
bica::ArrowPtr r(new bica::Arrow);
float cx, cy;
cx = (((((float)i)*CELL_SIZE)-3000.0) + ((((float)(i+1))*CELL_SIZE)-3000.0))/2.0;
cy = (((((float)j)*CELL_SIZE)-2000.0) + ((((float)(j+1))*CELL_SIZE)-2000.0))/2.0;
src->x = cx;
src->y = cy;
src->z = 600.0;
dst->x = cx + (CELL_SIZE/2.0) * cos(mgrid->getThetaIJ(i, j));
dst->y = cy + (CELL_SIZE/2.0) * sin(mgrid->getThetaIJ(i, j));
dst->z = 600.0;
r->src = src;
r->dst = dst;
r->width = 10.0;
r->color = bica::BLUE;
r->filled = true;
r->opacity = 255;
r->accKey = "c";
r->label = "KFE";
shapeListAbs.push_back(r);
}
//cerr<<endl;
pthread_mutex_unlock(&mutex);
return shapeListAbs;
}
wmLine *EKFLocalization::getMatchingWorldLine(LineSample detline, Pose2D &robotPose)
{
Vector2<double> ps3D, pe3D;
HPoint3D p1, p2;
ps3D = Pose2D::relative2FieldCoord(robotPose, detline.ps3D.X/detline.ps3D.H, detline.ps3D.Y/detline.ps3D.H);
pe3D = Pose2D::relative2FieldCoord(robotPose, detline.pe3D.X/detline.pe3D.H, detline.pe3D.Y/detline.pe3D.H);
float detangle;
detangle = atan2(ps3D.y-pe3D.y, ps3D.x-pe3D.x);
list<LineCandidate> candidates;
candidates.clear();
list<wmLine*> *wmlines = _WorldModel->getLines();
//cerr<<"{("<<ps3D.x<<","<<ps3D.y<<") - ("<<pe3D.x<<","<<pe3D.y<<")}"<<endl;
list<wmLine*>::iterator itlwm;
for(list<wmLine*>::iterator itlwm = wmlines->begin(); itlwm != wmlines->end(); ++itlwm)
{
float f1, f2, f3;
float dif1, dif2;
f1 = 0.4;
f2 = 0.4;
f3 = 0.2;
LineCandidate tcand;
tcand.line = (*itlwm);
tcand.points = 0.0;
dif1 = fabs(normalizePi(detangle - (*itlwm)->angle));
dif2 = fabs(normalizePi(-detangle - (*itlwm)->angle));
if(dif1<dif2)
{
if(dif1>(M_PI/4.0))
continue;
tcand.points = tcand.points + (fabs(M_PI-dif1)/(M_PI)) * f1;
}
else
{
if(dif2>(M_PI/4.0))
continue;
tcand.points = tcand.points + (fabs(M_PI-dif2)/(M_PI)) * f1;
}
float dist = getDistance((*itlwm), ps3D, pe3D);
tcand.points = tcand.points + ((3000.0-dist)/3000.0) * f2;
float diffLong = getDiffLong((*itlwm), ps3D, pe3D);
tcand.points = tcand.points + ((1000.0-diffLong)/1000.0) * f3;
p1 = _WorldModel->getPoint(tcand.line->p1)->p;
p2 = _WorldModel->getPoint(tcand.line->p2)->p;
candidates.push_back(tcand);
}
wmLine* winner=NULL;
float maxpoint = 0.0;
for(list<LineCandidate>::iterator itc = candidates.begin(); itc != candidates.end(); ++itc)
{
p1 = _WorldModel->getPoint((*itc).line->p1)->p;
p2 = _WorldModel->getPoint((*itc).line->p2)->p;
if(maxpoint<(*itc).points)
{
maxpoint = (*itc).points;
winner = (*itc).line;
}
}
if(winner != NULL)
{
p1 = _WorldModel->getPoint(winner->p1)->p;
p2 = _WorldModel->getPoint(winner->p2)->p;
}
return winner;
// //mirar a ver su ángulo
// float dif1, dif2;
// float A, B, C;
// float x1, y1, x2, y2;
// float dist, dist1, dist2;
// float xmedz_, ymedz_;
//
// x1 = _WorldModel->getPoint((*itlwm)->p1)->p.X;
// y1 = _WorldModel->getPoint((*itlwm)->p1)->p.Y;
// x2 = _WorldModel->getPoint((*itlwm)->p2)->p.X;
// y2 = _WorldModel->getPoint((*itlwm)->p2)->p.Y;
//
// A = (y2-y1)/(x2-x1);
// B = -1.0;
// C = y1 - x1 * K;
//
// xmedz_ = (p1z_->x + p2z_->x) / 2.0;
// ymedz_ = (p1z_->y + p2z_->y) / 2.0;
//
// dist = fabs(A*xmedz_+B*ymedz_+C)/sqrt(A*A+B*B);
// dist1 = sqrt((x1 - xmedz_)*(x1 - xmedz_)+(y1 - ymedz_)*(y1 - ymedz_));
// dist2 = sqrt((x2 - xmedz_)*(x2 - xmedz_)+(y2 - ymedz_)*(y2 - ymedz_));
//
// bool cond1, cond2, cond3;
//
// cond1 = (dif1<toRadians(30.0)) || (dif2<toRadians(30.0));
// cond2 = dist < 500.0;
// cond3 = (dist1 < (*itlwm)->lenght) && (dist2 < (*itlwm)->lenght);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, std::string("behavior"));
kobuki_msgs::Sound sound;
tf::TransformListener tfL;
ros::NodeHandle n;
Pid pid;
ros::Subscriber robotposesub = n.subscribe("/MCLRobotica_pos", 1000, &poseCB);
ros::Publisher cmdpub = n.advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1000);
ros::Publisher sonido = n.advertise<kobuki_msgs::Sound>("mobile_base/commands/sound",1);
ros::Subscriber lost_sub = n.subscribe("/MCLRobotica_lost", 1000, &lostCB);
ros::Subscriber notlost_sub = n.subscribe("/MCLRobotica_notlost", 1000, ¬lostCB);
// ros::Publisher chatter_pub = n.advertise<geometry_msgs::Twist>("mobile_base/commands/velocity", 1000);
ros::Rate loop_rate(10);
int count = 0;
int state = SEARCH;
std::string target;
std::string blacklist[NUMBOLAS];
int bolas = 0;
lastPose.pose.position.z = -1.0;
geometry_msgs::Twist cmd;
cmd.linear.y = 0.0;
cmd.linear.z = 0.0;
cmd.angular.x = 0.0;
cmd.angular.y = 0.0;
cmd.angular.z = 0.0;
cmd.linear.x = 0.0;
float angle2goal;
while (ros::ok())
{
if(bolas == 3)
break;
switch(state){
case SEARCH:
{
target = "A";
std::vector<std::string> frameList;
tfL.getFrameStrings(frameList);
std::vector<std::string>::iterator it;
float dmin = 99.9;
for (it = frameList.begin(); it != frameList.end(); ++it) {
std::string frame = *it;
tf::StampedTransform BL2B;
try {
tfL.lookupTransform("base_link", frame,
ros::Time::now(), BL2B);
if (isPrefix("ball_", frame) && !isBlacklisted(frame, blacklist, bolas)) {
int d = getDistanceTo(frame);
if(dmin > d){
dmin = d;
target = frame;
}
state = GOTOBALL;
}
} catch (tf::TransformException & ex) {
;//ROS_WARN("%s", ex.what());
}
}
if(target.compare("A")==0)
std::cout<<"NADA"<<std::endl;
else
std::cout<<"TARGET = "<<target<<std::endl;
cmd.linear.x = 0.2;
if(cmd.angular.z <= 0.4)
cmd.angular.z = cmd.angular.z + 0.01;
break;
}
case GOTOBALL:
{
tf::StampedTransform BL2B;
try{
tfL.lookupTransform("base_link", target,
ros::Time::now() - ros::Duration(1.0), BL2B);
}catch(tf::TransformException & ex) {
ROS_WARN("LA HE PERDIDO: %s", ex.what());
state = SEARCH;
cmd.angular.z = 0.0;
break;
}
float ro;
ro = sqrt((BL2B.getOrigin().x())*(BL2B.getOrigin().x()) + (BL2B.getOrigin().y())*(BL2B.getOrigin().y()));
double roll, pitch, yaw;
float v,w, usoPid;
Pid velDeGiro;
float theta;
std::cout<<"pelota "<<target<<" ro = "<<ro<<std::endl;
if(ro < 0.7){
w = v = 0.0;
sound.value=kobuki_msgs::Sound::CLEANINGSTART;
sonido.publish(sound); //AQUI SE REPRODUCE UN SONIDO
state = GOTOBIN;
cmd.linear.x = v;
cmd.angular.z = w;
}else{
theta = normalizePi(atan2(BL2B.getOrigin().y(), BL2B.getOrigin().x()));
std::cerr<<"theta: "<<fabs(theta)<<std::endl;
usoPid=velDeGiro.OperarMiPid(theta);
cmd.angular.z = usoPid;
if(fabs(theta) > 0.1){
v = 0.1;
cmd.linear.x = v;
}else{
v = 0.2;//AQUI HABRIA QUE HACER UN VFF SENCILLO QUE SOLO TENGA EN CUENTA LA DISTANCIA HASTA LA PELOTA
cmd.linear.x = v;
}
}
break;
}
case GOTOBIN:
{
if(lost){
std::cout<<"LAST POSE: "<<lastPose.pose.position.z<<std::endl;
if(lastPose.pose.position.z == -1.0){
if(count % 200 < 160){
std::cout<<"giro para encontrarme"<<std::endl;
cmd.angular.z = 0.2;
cmd.linear.x = 0.0;
}else{
std::cout<<"Avanzo para encontrarme"<<std::endl;
cmd.linear.x = 0.1;
cmd.angular.z = 0.0;
}
}else{
tf::StampedTransform W2BIn;
try{
tfL.lookupTransform("world", "bin",
ros::Time::now() - ros::Duration(1.0), W2BIn);
}catch(tf::TransformException & ex) {
ROS_WARN("%s", ex.what());
break;
}
double roll, pitch, yaw;
tf::Quaternion q(pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w);
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
angle2goal2 = normalizePi(atan2(W2BIn.getOrigin().y() - lastPose.pose.position.y,
W2BIn.getOrigin().x() -lastPose.pose.position.x) - yaw);
ETA = ros::Time::now() + ros::Duration(fabs(angle2goal2*23)/(2*M_PI));
state = GIRANDO;
diffpose2 = sqrt((lastPose.pose.position.x-W2BIn.getOrigin().x())*(lastPose.pose.position.x-W2BIn.getOrigin().x())
+ (lastPose.pose.position.y-W2BIn.getOrigin().y())*(lastPose.pose.position.y-W2BIn.getOrigin().y()));
}
break;
}else{
tf::StampedTransform W2BIn;
try{
tfL.lookupTransform("world", "bin",
ros::Time::now() - ros::Duration(1.0), W2BIn);
}catch(tf::TransformException & ex) {
ROS_WARN("%s", ex.what());
break;
}
float diffpose;
lastPose = pose;
diffpose = sqrt((pose.pose.position.x-W2BIn.getOrigin().x())*(pose.pose.position.x-W2BIn.getOrigin().x())
+ (pose.pose.position.y-W2BIn.getOrigin().y())*(pose.pose.position.y-W2BIn.getOrigin().y()));
double roll, pitch, yaw;
tf::Quaternion q(pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w);
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
float v,w, usoPid;
Pid velDeGiro;
if(diffpose < 0.1){
//std::cerr<<"diffpose: "<<diffpose<<std::endl;
w = v = 0.0;
sound.value=kobuki_msgs::Sound::CLEANINGSTART;
sonido.publish(sound);
//AQUI SE REPRODUCE UN SONIDO Y SE ELIMINA LA PELOTA
blacklist[bolas] = target;
bolas++;
state = SEARCH;
cmd.angular.z = 0.0;
cmd.linear.x = 0.0;
lastPose.pose.position.z = -1;
}else{
angle2goal = normalizePi(atan2(W2BIn.getOrigin().y() - pose.pose.position.y,
W2BIn.getOrigin().x() -pose.pose.position.x) - yaw);
//std::cerr<<"angle2goal: "<<fabs(angle2goal)<<std::endl;
usoPid=velDeGiro.OperarMiPid(angle2goal);
cmd.angular.z = usoPid;
if(fabs(angle2goal) > 0.2){
v = 0.0;
cmd.linear.x = v;
}else{
v = 0.3;//AQUI HABRIA QUE HACER UN VFF SENCILLO QUE SOLO TENGA EN CUENTA LA DISTANCIA HASTA BIn
cmd.linear.x = v;
}
}
break; //si no funciona según lo esperado, este break lo tenía puesto antes encima del case GIRANDO, pero creo que aqui
//hace mejor su función
}
}
case GIRANDO:
{
cmd.angular.z = 0.0;
cmd.angular.y = 0.0;
cmd.angular.x = 0.0;
cmd.linear.x = 0.0;
cmd.linear.y = 0.0;
cmd.linear.z = 0.0;
if(ros::Time::now() < ETA){
if(angle2goal2>=0){
std::cout<<"entro a girar izquierda"<<std::endl;
cmd.angular.z = 0.3;
}else{
std::cout<<"entro a girar derecha"<<std::endl;
cmd.angular.z = -0.3;
}
}else{
state = AVANZAR_ESPARTANOS;
ETA2 = ros::Time::now() + ros::Duration(diffpose2/VELOCIDAD_LINEAL);
}
cmdpub.publish(cmd);
break;
}
case AVANZAR_ESPARTANOS:
{
cmd.angular.z = 0.0;
cmd.angular.y = 0.0;
cmd.angular.x = 0.0;
cmd.linear.x = 0.0;
cmd.linear.y = 0.0;
cmd.linear.z = 0.0;
if(ros::Time::now() < ETA2){
std::cout<<"entro a avanzar"<<std::endl;
cmd.linear.x = 0.3;
}else{
sound.value=6;//kobuki_msgs::Sound::CLEANINGSTART;
sonido.publish(sound);
blacklist[bolas] = target;
bolas++;
state = VE_HACIA_ATRAS;
//voy a retroceder el mismo tiempo y a la misma velocidad de lo que habia avanzado
Tiempo_Hacia_Atras = ros::Time::now() + ros::Duration(diffpose2/VELOCIDAD_LINEAL);
}
//lastPose.pose.position.z = -1;
cmdpub.publish(cmd);
break;
}
case VE_HACIA_ATRAS:
{
cmd.angular.z = 0.0;
cmd.angular.y = 0.0;
cmd.angular.x = 0.0;
cmd.linear.x = 0.0;
cmd.linear.y = 0.0;
cmd.linear.z = 0.0;
if(ros::Time::now() < Tiempo_Hacia_Atras){
std::cout<<"voy a dar marcha atras"<<std::endl;
cmd.linear.x = -0.3;
}else{
state = SEARCH;
}
lastPose.pose.position.z = -1;
cmdpub.publish(cmd);
break;
}
default:
std::cout<<"WHAT THE F**K"<<std::endl;
}
cmdpub.publish(cmd);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}
void
Flight::update(float delta_t)
{
float trans;
Position CPpos;
if(routed())
{
float goal_bearing, diff_bearing, new_w;
CPpos = route.front().pos;
pos.angles(CPpos, goal_bearing, inclination);
goal_bearing = normalizePi(goal_bearing + M_PI);
diff_bearing = normalizePi(goal_bearing - bearing);
new_w = diff_bearing;
if(fabs(new_w)>MAX_FLIFGT_W) new_w = (fabs(new_w)/new_w) * MAX_FLIFGT_W;
//std::cout<<"["<<id<<"]angle = "<<bearing<<"\tnew = "<<goal_bearing<<"\t["<<diff_bearing<<"]\tideal w = "<<new_w<<" -> "<<new_w_b<<std::endl;
bearing = bearing + new_w*delta_t;
float goal_speed, diff_speed, acc;
goal_speed = route.front().speed;
acc = (goal_speed - speed);
if(fabs(acc)>MAX_ACELERATION) acc = (acc/fabs(acc))*MAX_ACELERATION;
speed = speed + acc*delta_t;
//std::cout<<"["<<id<<"]speed = "<<speed<<"\tnew = "<<goal_speed<<"\t["<<acc<<"]\t"<<std::endl;
}else
inclination = 0.0;
last_pos = pos;
trans = speed * delta_t;
pos.set_x(pos.get_x() + trans * cos(bearing) * cos(inclination));
pos.set_y(pos.get_y() + trans * sin(bearing) * cos(inclination));
pos.set_z(pos.get_z() + ( trans * sin(inclination)));
// if(pos.distance(last_pos) > pos.distance(CPpos))
// route.pop_front();
if(pos.distance(CPpos)<DIST_POINT)
route.pop_front();
if(inStorm)
{
//std::cout<<"["<<id<<"]In Storm"<<std::endl;
points = points - 2*delta_t;
}
else
points = points - delta_t;
}
void RecorderSession::Record_state_code(void)
{
//BUILDER COMMENT. DO NOT REMOVE. begin Record_state_code
if((getCurrentTime() - lastts)>5000000) //5 secs
{
//restart
cerr<<"Restarting log"<<endl;
fout = fopen("/tmp/log", "w");
lastpos = pmotion->getRobotPosition(USE_SENSOR);
initts = getCurrentTime();
}else
{
fout = fopen("/tmp/log", "a");
}
MotionInfo _motion;
PerceptionInfo _perception;
//motion
float diffx, diffy, difft;
bool apply;
vector<float> pos = pmotion->getRobotPosition(USE_SENSOR);
diffx = pos[0] *1000.0 - lastpos[0]*1000.0;
diffy = pos[1] *1000.0 - lastpos[1]*1000.0;
difft = normalizePi(pos[2] - lastpos[2]);
_motion.movx = diffx*cos(-lastpos[2]) - diffy*sin(-lastpos[2]);
_motion.movy = diffx*sin(-lastpos[2]) + diffy*cos(-lastpos[2]);
_motion.movt = difft;
lastpos = pos;
//perception info
ObjectState* goalp0r = &(_GoalDetector->goals->p0RightEstimate);
ObjectState* goalp0l = &(_GoalDetector->goals->p0LeftEstimate);
ObjectState* goalp1r = &(_GoalDetector->goals->p1RightEstimate);
ObjectState* goalp1l = &(_GoalDetector->goals->p1LeftEstimate);
_perception.p0ElapsedTimeSinceLastObs = _GoalDetector->getObj()->p0ElapsedTimeSinceLastObs;
_perception.p1ElapsedTimeSinceLastObs = _GoalDetector->getObj()->p1ElapsedTimeSinceLastObs;
_perception.p0r.reliability = goalp0r->getReliability();
_perception.p0r.x = goalp0r->getPositionInRelativeCoordinates().x;
_perception.p0r.y = goalp0r->getPositionInRelativeCoordinates().y;
_perception.p0l.reliability = goalp0l->getReliability();
_perception.p0l.x = goalp0l->getPositionInRelativeCoordinates().x;
_perception.p0l.y = goalp0l->getPositionInRelativeCoordinates().y;
_perception.p1r.reliability = goalp1r->getReliability();
_perception.p1r.x = goalp1r->getPositionInRelativeCoordinates().x;
_perception.p1r.y = goalp1r->getPositionInRelativeCoordinates().y;
_perception.p1l.reliability = goalp1l->getReliability();
_perception.p1l.x = goalp1l->getPositionInRelativeCoordinates().x;
_perception.p1l.y = goalp1l->getPositionInRelativeCoordinates().y;
double xpos, ypos, tpos;
_GTLocalization->getPosition(xpos, ypos, tpos);
fprintf(fout, "%ld\t%f\t%f\t%f\t%f\t%f\t%f\t%ld\t%ld\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", (getCurrentTime() - initts)/1000,
(float)xpos, (float)ypos, (float)tpos,
_motion.movx, _motion.movy, _motion.movt,
_perception.p0ElapsedTimeSinceLastObs, _perception.p1ElapsedTimeSinceLastObs,
_perception.p0r.reliability, _perception.p0r.x, _perception.p0r.y,
_perception.p0l.reliability, _perception.p0l.x, _perception.p0l.y,
_perception.p1r.reliability, _perception.p1r.x, _perception.p1r.y,
_perception.p1l.reliability, _perception.p1l.x, _perception.p1l.y);
//cerr<<"["<<getCurrentTime()<<", "<<initts<<"]"<<endl;
lastts = getCurrentTime();
fclose(fout);
//BUILDER COMMENT. DO NOT REMOVE. end Record_state_code
}
