/**
* \return true if the given point is on this shape.
*/
bool RShape::isOnShape(const RVector& point, bool limited, double tolerance) const {
double d = getDistanceTo(point, limited);
if (RMath::isNaN(d)) {
return false;
}
// much more tolerance here (e.g. for ellipses):
return d < tolerance;
}
vector<PlayerFragment> burst(const Virus &virus, int max_fragment_id, int yet_cnt)
{
vector<PlayerFragment> fragments;
double dist = getDistanceTo(virus);
double dy = y - virus.y, dx = x - virus.x;
double angle = atan2(y, x);
if (dist > 0)
{
angle = asin(dy / dist);
if (dx < 0)
angle = M_PI - angle;
}
mass += BURST_BONUS;
int new_frags_cnt = int(mass / MIN_BURST_MASS) - 1;
new_frags_cnt = min(new_frags_cnt, _restFragmentsCount(yet_cnt));
double new_mass = mass / (new_frags_cnt + 1);
for (int I = 0; I < new_frags_cnt; I++)
{
int new_fId = max_fragment_id + I + 1;
PlayerFragment new_fragment;
new_fragment.x = x;
new_fragment.y = y;
new_fragment.addMass(new_mass);
new_fragment.playerId = playerId;
new_fragment.fragmentId = new_fId;
double burst_angle = angle - BURST_ANGLE_SPECTRUM / 2 + I * BURST_ANGLE_SPECTRUM / new_frags_cnt;
new_fragment.speed = ::Point::byAngle(burst_angle) * BURST_START_SPEED;
new_fragment.isFast = true;
new_fragment.ttf = Config::TICKS_TIL_FUSION;
fragments.push_back(new_fragment);
}
speed = ::Point::byAngle(angle + BURST_ANGLE_SPECTRUM / 2) * BURST_START_SPEED;
isFast = true;
fragmentId = max_fragment_id + new_frags_cnt + 1;
addMass(new_mass - mass);
ttf = Config::TICKS_TIL_FUSION;
return fragments;
}
double RVector::getClosestDistance(const QList<RVector>& list, int counts) {
double ret=RMAXDOUBLE;
int i=list.count();
if (counts<i) {
i=counts;
}
if (i<1) {
return ret;
}
for (int j=0; j<i; j++) {
double d = getDistanceTo(list[j]);
if (d<ret) {
ret=d;
}
}
return ret;
}
int RVector::getClosestIndex(const QList<RVector>& list, bool ignoreZ) const {
double minDist = RMAXDOUBLE;
int index = -1;
for (int i = 0; i<list.size(); ++i) {
if (list[i].valid) {
double dist;
if (ignoreZ) {
dist = getDistanceTo2D(list[i]);
}
else {
dist = getDistanceTo(list[i]);
}
if (dist < minDist) {
minDist = dist;
index = i;
}
}
}
return index;
}
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;
}
QList<RLine> REllipse::getTangents(const RVector& point) const {
QList<RLine> ret;
if (getDistanceTo(point, false) < RS::PointTolerance) {
// point is on ellipse:
return ret;
}
// point is at center (prevents recursion when swapping ellipse minor / major):
if (point.getDistanceTo(getCenter())<RS::PointTolerance) {
return ret;
}
// swap ellipse minor / major if point is on minor axis
// 20120928: and not also on major axis (prevent recursion):
RLine minorAxis(getCenter(), getCenter() + getMinorPoint());
RLine majorAxis(getCenter(), getCenter() + getMajorPoint());
if (minorAxis.isOnShape(point, false) && !majorAxis.isOnShape(point, false)) {
REllipse e2 =*this;
e2.majorPoint = getMinorPoint();
e2.ratio = 1.0/ratio;
return e2.getTangents(point);
}
double a = getMajorRadius(); // the length of the major axis / 2
double b = getMinorRadius(); // the length of the minor axis / 2
// rotate and move point:
RVector point2 = point;
point2.move(-getCenter());
point2.rotate(-getAngle());
double xp = point2.x; // coordinates of the given point
double yp = point2.y;
double xt1; // Tangent point 1
double yt1;
double xt2; // Tangent point 2
double yt2;
double a2 = a * a;
double b2 = b * b;
double d = a2 / b2 * yp / xp;
double e = a2 / xp;
double af = b2 * d * d + a2;
double bf = -b2 * d * e * 2.0;
double cf = b2 * e * e - a2 * b2;
double t = sqrt(bf * bf - af * cf * 4.0);
if (RMath::isNaN(t)) {
return ret;
}
yt1 = (t - bf) / (af * 2.0);
xt1 = e - d * yt1;
yt2 = (-t - bf) / (af * 2.0);
xt2 = e - d * yt2;
RVector s1(xt1, yt1);
s1.rotate(getAngle());
s1.move(getCenter());
RVector s2(xt2, yt2);
s2.rotate(getAngle());
s2.move(getCenter());
if (s1.isValid()) {
ret.append(RLine(point, s1));
}
if (s2.isValid()) {
ret.append(RLine(point, s2));
}
return ret;
}
Vector3f Plane::getIntersectionPoint(const Line& line) const
{
return line * (-getDistanceTo(line.o) / n.getDotProduct(line.d));
}
float TileObject::getDistanceTo(sp<TileObject> target)
{
return getDistanceTo(target->getPosition());
}
