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