void crossing2() { GlobalPoint startingPos(-8.12604,-50.829,9.82116); GlobalVector startingDir(-0.517536,-5.09581,1.02212); GlobalPoint pos(-2.96723,-51.4573,14.8322); Surface::RotationType rot(0.995041,0.0994701,0.000124443, 0.000108324,-0.00233467,0.999997, 0.0994701,-0.995038,-0.00233387); std::cout << rot << std::endl; Plane plane(pos,rot); double rho = 0.00223254; HelixBarrelPlaneCrossingByCircle precise(startingPos, startingDir, rho,alongMomentum); bool cross; double s; std::tie(cross,s) = precise.pathLength(plane); HelixBarrelPlaneCrossing2OrderLocal crossing(startingPos, startingDir, rho, plane); std::cout << plane.toLocal(GlobalPoint(precise.position(s))) << " " << plane.toLocal(GlobalVector(precise.direction(s))) << std::endl; std::cout << HelixBarrelPlaneCrossing2OrderLocal::positionOnly(startingPos, startingDir, rho, plane) << ' '; std::cout << crossing.position() << ' ' << crossing.direction() << std::endl; LocalPoint thePos; LocalVector theDir; std::tie(thePos,theDir) = secondOrderAccurate(startingPos, startingDir, rho, plane); std::cout << thePos << ' ' << theDir << std::endl; }
void print_value(t_printer *p, t_value *v, char *str) { int not_zero; not_zero = (*str == '0' ? 0 : 1); if (SPEC == 'c' && *str == 0) v->char_null = 1; if (!ft_strchr("sc", SPEC) && *str == '0' && v->precision == 0 && ACT_P) *str = 0; else str = precise(str, v); if ((SPEC == 'p') || (SPEC == 'x' && PREFIX && not_zero && *str != 0)) str = prefix(str, "0x"); if (SPEC == 'b' && PREFIX && *str != 0 && not_zero) str = prefix(str, "0b"); if (SPEC == 'B' && PREFIX && *str != 0 && not_zero) str = prefix(str, "0B"); if (SPEC == 'X' && PREFIX && *str != 0 && not_zero) str = prefix(str, "0X"); if ((SPEC == 'o' || SPEC == 'O') && PREFIX && *str != 0 && *str != '0') str = prefix(str, "0"); if (SIGNED && ft_strchr("dDifF", SPEC) && *str != '-') str = prefix(str, "+"); if (BSIGNED && ft_strchr("dDifF", SPEC) && *str != '-') str = prefix(str, " "); print_with_width(str, v, p); ft_strdel(&str); }
void crossing1() { GlobalPoint startingPos(-7.79082,-47.6418,9.18163); GlobalVector startingDir(-0.553982,-5.09198,1.02212); GlobalPoint pos(-2.95456,-48.2127,3.1033); double rho = 0.00223254; Surface::RotationType rot(0.995292,0.0969201,0.000255868, 8.57131e-06,0.00255196,-0.999997, -0.0969205,0.995289,0.00253912); std::cout << rot << std::endl; Plane plane(pos,rot); HelixBarrelPlaneCrossingByCircle precise(startingPos, startingDir, rho,alongMomentum); bool cross; double s; std::tie(cross,s) = precise.pathLength(plane); HelixBarrelPlaneCrossing2OrderLocal crossing(startingPos, startingDir, rho, plane); std::cout << plane.toLocal(GlobalPoint(precise.position(s))) << " " << plane.toLocal(GlobalVector(precise.direction(s))) << std::endl; std::cout << HelixBarrelPlaneCrossing2OrderLocal::positionOnly(startingPos, startingDir, rho, plane) << ' '; std::cout << crossing.position() << ' ' << crossing.direction() << std::endl; LocalPoint thePos; LocalVector theDir; std::tie(thePos,theDir) = secondOrderAccurate(startingPos, startingDir, rho, plane); std::cout << thePos << ' ' << theDir << std::endl; }
void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg) { if (state == PROGRESS || state == FINAL){ size_t num_ranges = scan_msg->ranges.size(); float x[num_ranges]; float y[num_ranges]; bool m[num_ranges]; float angle; misdetections++; for (int i = 0; i <= num_ranges; i++){ angle = scan_msg->angle_min+i*scan_msg->angle_increment; x[i] = scan_msg->ranges[i]*cos(angle); y[i] = scan_msg->ranges[i]*sin(angle); m[i] = true; } int eval = 0; int max_iterations = 1000; int numHypotheses = 10; float a,b; float maxA[numHypotheses]; float maxB[numHypotheses]; int maxEval[numHypotheses]; for (int h = 0; h <= numHypotheses; h++){ maxEval[h] = 0; for (int i = 0; i <= max_iterations; i++) { int aIndex = rand()%num_ranges; int bIndex = rand()%num_ranges; while (bIndex == aIndex) bIndex = rand()%num_ranges; a = (y[bIndex]-y[aIndex])/(x[bIndex]-x[aIndex]); b = y[bIndex]-a*x[bIndex]; eval = 0; for (int j = 0; j <= num_ranges; j++){ if (fabs(a*x[j]-y[j]+b)<tolerance && m[j]) eval++; } if (maxEval[h] < eval){ maxEval[h]=eval; maxA[h]=a; maxB[h]=b; } } for (int j = 0; j <= num_ranges; j++){ if (fabs(maxA[h]*x[j]-y[j]+maxB[h])<tolerance && m[j]) m[j] = false; } } eval = 0; base_cmd.linear.x = base_cmd.angular.z = 0; for (int h1 = 0; h1 <= numHypotheses; h1++){ // fprintf(stdout,"Ramp hypothesis: %i %f %f %i ",h1,maxA[h1],maxB[h1],maxEval[h1]); precise(&maxA[h1],&maxB[h1],x,y,num_ranges); // fprintf(stdout,"correction: %i %f %f %i\n",h1,maxA[h1],maxB[h1],maxEval[h1]); } int b1 = -1; int b2 = -1; eval = 0; float realDist,realAngle,displacement; for (int h1 = 0; h1 <= numHypotheses; h1++){ for (int h2 = h1+1; h2 <= numHypotheses; h2++){ if (fabs(maxA[h1]-maxA[h2]) < angleTolerance && maxEval[h1] > minPoints && maxEval[h2] > minPoints ){ realAngle = (maxA[h1]+maxA[h2])/2.0; realDist = fabs(maxB[h1]-maxB[h2])*cos(atan(realAngle)); fprintf(stdout,"Ramp hypothesis: %i %i %f %f %i %i\n",h1,h2,realDist,fabs(maxA[h1]-maxA[h2]),maxEval[h1],maxEval[h2]); if (fabs(realDist-distance)<distanceTolerance){ if (maxEval[h1]+maxEval[h2] > eval){ eval = maxEval[h1] + maxEval[h2]; b1 = h1; b2 = h2; } } } } } if (b1 >= 0 && b2 >=0){ displacement = (maxB[b1]+maxB[b2])/2.0; realAngle = (maxA[b1]+maxA[b2])/2.0; fprintf(stdout,"Ramp found: %i %i %f %f %i %i\n",b1,b2,realDist,fabs(maxA[b1]-maxA[b2]),maxEval[b1],maxEval[b2]); if (maxEval[b1]+maxEval[b2]> 360){ state = FINAL; minPoints = 50; fwSpeed = fmax(fwSpeed,fmin(maxEval[b1],maxEval[b2])*0.0015); } eval = maxEval[b1]+maxEval[b2]; base_cmd.angular.z = realAngle+3*displacement; base_cmd.linear.x = fmax(fwSpeed,fmin(maxEval[b1],maxEval[b2])*0.0015); //if (fabs(displacement)>0.1 ) base_cmd.linear.x -= fabs(base_cmd.angular.z); if (base_cmd.linear.x < 0) base_cmd.linear.x = 0; float spdLimit = 0.8; if (base_cmd.angular.z > spdLimit) base_cmd.angular.z = spdLimit ; if (base_cmd.angular.z < -spdLimit) base_cmd.angular.z = -spdLimit ; cmd_vel.publish(base_cmd); misdetections=misdetections/2; }else if (state == FINAL){ base_cmd.angular.z = 0; base_cmd.linear.x = fwSpeed; cmd_vel.publish(base_cmd); }else{ base_cmd.angular.z = 0; base_cmd.linear.x = 0; cmd_vel.publish(base_cmd); } } }