00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00032 #include "lidar_egomotion.h"
00033
00034 ray_definition c_rays;
00035 ray_history h_rays;
00036 ray_definition*l_rays=NULL;
00037 ray_definition*l2_rays=NULL;
00038
00039 scan_matching_method scan_method = MBICP;
00040
00041 extern TSMparams params;
00042 struct sm_params params_sm;
00043 struct sm_result results_sm;
00044
00045 Tsc sensorMotion,solution,solution2;
00046
00047 int step=2;
00048
00049 LDP ldp_1=NULL;
00050 LDP ldp_2=NULL;
00051
00052 PMScan pm1;
00053 PMScan pm2;
00054
00055 Tpfp*fp1=NULL;
00056 Tpfp*fp2=NULL;
00057 Tpfp*fp3=NULL;
00058
00059 double bwa=3.40;
00060 double gl=2.6;
00061
00062 double z[2];
00063
00064 t_flag flags;
00065
00066 std::vector<t_posePtr> path_odo;
00067 std::vector<t_posePtr> path_laser;
00068
00069 std::vector<double>runtime_PlICP;
00070 std::vector<double>runtime_MbICP;
00071 std::vector<double>runtime_PSM;
00072
00073 std::vector<pair<double,double> >odometry_readings;
00074 std::vector<pair<double,double> >measurments;
00075
00076 tf::TransformListener *transform_listener;
00077 pcl::PointCloud<pcl::PointXYZ> point_cloud_2;
00078 pcl::PointCloud<pcl::PointXYZ> point_cloud_3;
00079 bool new_laser_2=false;
00080 bool new_laser_3=false;
00081 bool new_plc_status=false;
00082 bool new_velocity_status=false;
00083
00084 void CleanZone(ray_definition*rays,double steering_angle)
00085 {
00086 if(rays==NULL)
00087 return;
00088
00089 double left=0;
00090 double right=0;
00091
00092 double min_angle=0;
00093 double max_angle=0;
00094
00095
00096 if(steering_angle>0.2 || steering_angle<-0.2)
00097 {
00098 if(steering_angle>0.1)
00099 left=fabs(steering_angle)*100.;
00100 else
00101 right=fabs(steering_angle)*100.;
00102
00103 min_angle=(180.0-left)*M_PI/180.;
00104 max_angle=(180.0+right)*M_PI/180.;
00105
00106 for(int i=0; i<rays->cfg.num_rays; i++)
00107 if(rays->dt[i].theta>min_angle && rays->dt[i].theta<max_angle)
00108 cvClearSeq(rays->dt[i].range);
00109 }
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223 }
00224
00225 void RaysToPMScan(ray_definition*src,PMScan*dst)
00226 {
00227 if(!src)
00228 {
00229 printf("Watch out for memory allocation, null pointer in %s\n",__FUNCTION__);
00230 for(int i=0; i<PM_L_POINTS; i++)
00231 {
00232 dst->bad[i]=1;
00233 dst->r[i]=-1;
00234 dst->x[i]=-1;
00235 dst->y[i]=-1;
00236 }
00237 return;
00238 }
00239
00240 RaysToPMScan(&src->cfg,src->dt,dst);
00241 }
00242
00243 void ConvertEstimatedToMeasurment(double vl,double dir,float*dx,float*dy,float*dtheta,double dt,double l,double bwa)
00244 {
00245 double R,Rfw,s,db,b1;
00246 Vector cc(2);
00247
00248 if(isnan(vl) || isnan(dir))
00249 {
00250 printf("Warning, invalid estimated values in %s\n",__FUNCTION__);
00251 *dx=0;
00252 *dy=0;
00253 *dtheta=0;
00254 return;
00255 }
00256
00257 R=l/tan(dir);
00258
00259 cc(2)=R;
00260 cc(1)=-bwa;
00261
00262 Rfw=sqrt(cc(1)*cc(1)+cc(2)*cc(2));
00263 Rfw=R>0?Rfw:-Rfw;
00264
00265 s=vl*dt;
00266
00267 db=fabs(s/Rfw);
00268 db=vl>0?db:-db;
00269
00270 b1=atan2(bwa,fabs(cc(2)));
00271
00272 *dy=Rfw*cos(b1)-Rfw*cos(b1+db);
00273
00274 Rfw=R>0?Rfw:-Rfw;
00275 *dx=Rfw*sin(b1+db)-Rfw*sin(b1);
00276
00277 *dtheta=db;
00278
00279 if(isnan(*dx)||isnan(*dy))
00280 {
00281 *dx=0;
00282 *dy=0;
00283 }
00284
00285 *dx*=-1;
00286 *dy*=-1;
00287 *dtheta*=-1;
00288
00289 if(dir<0)
00290 *dtheta*=-1;
00291
00292 return;
00293 }
00294
00295 void RaysToPMScan(ray_config_t*cfg,ray_measurment_t*src,PMScan*dst)
00296 {
00297 if(dst==NULL)
00298 {
00299 printf("PMScan not alloc\n");
00300 return;
00301 }
00302
00303 double*range;
00304 dst->rx = 0;
00305 dst->ry = 0;
00306 dst->th = 0;
00307 dst->t = -1.0;
00308
00309 for(int i=0; i<cfg->num_rays; i++)
00310 {
00311 if(src[i].range->total>0)
00312 {
00313 range=(double*)cvGetSeqElem(src[i].range,0);
00314 dst->bad[i]=0;
00315 dst->r[i]=(*range)*100;
00316 dst->x[i]=(dst->r[i])*cos(src[i].theta);
00317 dst->y[i]=(dst->r[i])*sin(src[i].theta);
00318 }else
00319 {
00320 dst->bad[i]=1;
00321 dst->r[i]=-1;
00322 dst->x[i]=-1;
00323 dst->y[i]=-1;
00324 }
00325 }
00326
00327 return;
00328 }
00329
00330 void RaysToLDP(ray_definition*src,LDP dst,bool test_angle,double angle)
00331 {
00332 if(!src || !dst)
00333 {
00334 if(!dst)
00335 {
00336 printf("Both pointers are null, how can this be? serious bug!! :), in %s!\n",__FUNCTION__);
00337 return;
00338 }
00339
00340 printf("Wrong pointer, be carefull with memory allocation, in %s!\n",__FUNCTION__);
00341 for(int i=0;i<dst->nrays;i++)
00342 {
00343 dst->valid[i]=0;
00344 dst->readings[i]=0;
00345 dst->theta[i]=-1;
00346 }
00347 return;
00348 }
00349
00350 RaysToLDP(&src->cfg,src->dt,dst,test_angle,angle);
00351 }
00352
00353 void RaysToLDP(ray_config_t*cfg,ray_measurment_t*src,LDP dst,bool test_angle,double angle)
00354 {
00355 if(dst==NULL)
00356 {
00357 printf("LDP not alloc\n");
00358 for(int i=0;i<cfg->num_rays;i++)
00359 {
00360 dst->valid[i]=0;
00361 dst->readings[i]=0;
00362 dst->theta[i]=-1;
00363 }
00364 return;
00365 }
00366
00367 dst->min_theta=src[0].theta;
00368 dst->max_theta=src[cfg->num_rays-1].theta;
00369
00370 double*range;
00371
00372
00373 for(int i=0;i<cfg->num_rays;i++)
00374 {
00375 if(src[i].range->total>0)
00376 {
00377
00378 range=(double*)cvGetSeqElem(src[i].range,src[i].range->total-1);
00379
00380 dst->valid[i]=1;
00381 dst->readings[i]=*range;
00382 dst->theta[i]=src[i].theta;
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399 }else
00400 {
00401 dst->valid[i]=0;
00402 dst->readings[i]=0;
00403 dst->theta[i]=src[i].theta;
00404 }
00405
00406
00407
00408
00409
00410
00411
00412 }
00413
00414 dst->estimate[0]=0;
00415 dst->estimate[1]=0;
00416 dst->estimate[2]=0;
00417
00418 dst->odometry[0]=0;
00419 dst->odometry[1]=0;
00420 dst->odometry[2]=0;
00421
00422 dst->tv.tv_sec=0;
00423 dst->tv.tv_usec=0;
00424 }
00425
00426 int RaysToMbICP(ray_definition*src,Tpfp*dst)
00427 {
00428 if(!src)
00429 {
00430 printf("Watch out for memory allocation, null pointer in %s\n",__FUNCTION__);
00431 for(int i=0; i<MAXLASERPOINTS;i++)
00432 {
00433 dst[i].r=200.;
00434 dst[i].t=-1;
00435 }
00436 return -1;
00437 }
00438
00439 return RaysToMbICP(&src->cfg,src->dt,dst);
00440 }
00441
00442 int RaysToMbICP(ray_config_t*cfg,ray_measurment_t*src,Tpfp*dst,bool test_angle,double angle)
00443 {
00444 if(dst==NULL)
00445 {
00446 printf("MbICP not alloc\n");
00447 return -1;
00448 }
00449
00450 double*range;
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460 for(int i=0; i<cfg->num_rays; i++)
00461 {
00462 if(src[i].range->total>0)
00463 {
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489 range=(double*)cvGetSeqElem(src[i].range,src[i].range->total-1);
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511 dst[i].r=*range;
00512 dst[i].t=src[i].theta;
00513
00514
00515
00516
00517 }else
00518 {
00519 dst[i].r=200.;
00520 dst[i].t=src[i].theta;
00521 }
00522 }
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532 return 0;
00533 }
00534
00535 void CreateMeasurementFromDisplacement(double dx,double dy,double dtheta,double z[2],double dt,double l,double bwa)
00536 {
00537 double t1,t2,dbeta,k,R=0,s,rx,ry,Rfw=0;
00538
00539 rx=-sin(dtheta)*dy-dx*cos(dtheta);
00540 ry=-cos(dtheta)*dy+dx*sin(dtheta);
00541
00542 if(isnan(rx) || isnan(ry))
00543 {
00544 printf("Warning, invalid measurments in %s\n",__FUNCTION__);
00545 z[0]=0;
00546 z[1]=0;
00547 return;
00548 }
00549
00550 Vector v0(2);
00551 Vector v1(2);
00552 Vector p0(2);
00553 Vector p1(2);
00554 Vector p2(2);
00555 Vector cc(2);
00556 Vector mp(2);
00557
00558
00559
00560
00561
00562 p0(1)=0;
00563 p0(2)=0;
00564
00565 p1(1)=-bwa;
00566 p1(2)=0;
00567
00568 p2(1)=rx;
00569 p2(2)=ry;
00570
00571 v0(1)=p2(1)-p0(1);
00572 v0(2)=p2(2)-p0(2);
00573
00574 v1(1)=-v0(2);
00575 v1(2)=v0(1);
00576
00577 mp(1)=(p2(1)+p0(1))/2.;
00578 mp(2)=(p2(2)+p0(2))/2.;
00579
00580 k=(-bwa-mp(1))/v1(1);
00581
00582 if(isinf(k))
00583 {
00584 z[1]=0;
00585 s=rx;
00586 }else
00587 {
00588 cc(1)=mp(1)+k*v1(1);
00589 cc(2)=mp(2)+k*v1(2);
00590
00591 R=cc(2);
00592
00593 Rfw=sqrt(cc(1)*cc(1)+cc(2)*cc(2));
00594 Rfw=R>0?Rfw:-Rfw;
00595
00596 z[1]=atan(l/R);
00597
00598 t1=atan2(p0(1)-cc(1),p0(2)-cc(2));
00599 t2=atan2(p2(1)-cc(1),p2(2)-cc(2));
00600
00601 dbeta=t1-t2;
00602 s=dbeta*Rfw;
00603 }
00604
00605 z[0]=s/dt;
00606
00607
00608
00609 }
00610
00611 void SetDefaultConfiguration(void)
00612 {
00613 cout<<"Setting default configuration"<<endl;
00614
00615 srand(time(NULL));
00616
00617 InitRayDefinition(&c_rays);
00618 InitRayHistory(&h_rays);
00619
00620 ldp_1=ld_alloc_new(c_rays.cfg.num_rays);
00621 ldp_2=ld_alloc_new(c_rays.cfg.num_rays);
00622
00623 ConfigurePLICP(¶ms_sm,ldp_1,ldp_2);
00624
00625
00626 float max_laser_range=100.;
00627
00628 float Bw=deg2rad(5);
00629
00630
00631 float Br=1.5;
00632
00633
00634 float L=2.5;
00635
00636 int laserStep=1;
00637
00638 float MaxDistInter=1.5;
00639 float filter=0.80;
00640
00641
00642 int ProjectionFilter=0;
00643 float AsocError=0;
00644 int MaxIter=500;
00645 float errorRatio = 0.0001;
00646 float errx_out = 0.001;
00647 float erry_out = 0.001;
00648 float errt_out = 0.001;
00649 int IterSmoothConv=4;
00650
00651 Init_MbICP_ScanMatching(max_laser_range,Bw,Br,L,laserStep,MaxDistInter,filter,ProjectionFilter,AsocError,MaxIter,errorRatio,errx_out,erry_out,errt_out,IterSmoothConv);
00652 fp1=(Tpfp*)malloc(1080*sizeof(Tpfp));
00653 fp2=(Tpfp*)malloc(1080*sizeof(Tpfp));
00654 fp3=(Tpfp*)malloc(1080*sizeof(Tpfp));
00655
00656 memset(&solution,0,sizeof(Tsc));
00657
00658
00659
00660
00661
00662 pm_init();
00663
00664 return;
00665 }
00666
00667 void ConfigurePLICP(struct sm_params*params,LDP ref,LDP sens)
00668 {
00669 params->laser_ref=ref;
00670 params->laser_sens=sens;
00671 params->use_point_to_line_distance=1;
00672 params->use_corr_tricks=1;
00673 params->max_iterations=500;
00674
00676 params->epsilon_xy=0.001;
00678 params->epsilon_theta=0.001;
00679
00681 params->max_angular_correction_deg=5.;
00683 params->max_linear_correction=0.8;
00684
00685 params->max_correspondence_dist=1.0;
00687 params->sigma = 0.01;
00688 params->restart = 1;
00689
00690 params->min_reading = 0.5;
00691 params->max_reading = 100.;
00692
00693 params->use_sigma_weights = 0;
00694
00695 params->first_guess[0]=results_sm.x[0];
00696 params->first_guess[1]=results_sm.x[1];
00697 params->first_guess[2]=results_sm.x[2];
00698
00699 params->use_ml_weights=0;
00700
00701 params->restart_threshold_mean_error = 8.0 / 300.0;
00702 params->restart_dt= 0.01;
00703 params->restart_dtheta= 1* M_PI /180;
00704
00705 params->clustering_threshold = 0.8;
00706 params->orientation_neighbourhood = 0;
00707
00708 params->do_alpha_test = 0;
00709
00712 params->outliers_maxPerc = 0.8;
00713
00714 params->outliers_adaptive_order=1;
00715 params->outliers_adaptive_mult=4;
00716 params->do_visibility_test = 1;
00717 params->do_compute_covariance = 0;
00718
00719 return;
00720 }
00721
00722 pcl::PointCloud<pcl::PointXYZ> wrapper_Laserscan2PointCloud(const sensor_msgs::LaserScan& scan)
00723 {
00724
00725 laser_geometry::LaserProjection projector;
00726
00727 sensor_msgs::PointCloud2 point_cloud_input;
00728
00729 pcl::PointCloud<pcl::PointXYZ> point_cloud_pcl_input;
00730
00731 pcl::PointCloud<pcl::PointXYZ> point_cloud_pcl_output;
00732
00733 tf::StampedTransform transform;
00734
00735
00736 projector.transformLaserScanToPointCloud(scan.header.frame_id,scan,point_cloud_input,*transform_listener);
00737
00738
00739 pcl::fromROSMsg(point_cloud_input,point_cloud_pcl_input);
00740
00741
00742 try
00743 {
00744 transform_listener->lookupTransform(scan.header.frame_id,ros::names::remap("/tracking_frame"), ros::Time(0), transform);
00745 }catch (tf::TransformException ex)
00746 {
00747 ROS_ERROR("%s",ex.what());
00748 }
00749
00750
00751 pcl_ros::transformPointCloud(point_cloud_pcl_input,point_cloud_pcl_output,transform.inverse());
00752
00753 point_cloud_pcl_output.header.frame_id = ros::names::remap("/tracking_frame");
00754 return point_cloud_pcl_output;
00755 }
00756
00757 void laser_2_Handler(const sensor_msgs::LaserScan& scan)
00758 {
00759 point_cloud_2 = wrapper_Laserscan2PointCloud(scan);
00760 new_laser_2=true;
00761 }
00762
00763 void laser_3_Handler(const sensor_msgs::LaserScan& scan)
00764 {
00765 point_cloud_3 = wrapper_Laserscan2PointCloud(scan);
00766 new_laser_3=true;
00767 }
00768
00769 void plcStatusHandler(const atlascar_base::AtlascarStatus& scan)
00770 {
00771 cout<<"Received plc status"<<endl;
00772 new_plc_status=true;
00773 }
00774
00775 void velocityStatusHandler(const atlascar_base::AtlascarVelocityStatus& scan)
00776 {
00777 cout<<"Received velocity status"<<endl;
00778 new_velocity_status=true;
00779 }
00780
00781 int main(int argc,char**argv)
00782 {
00783
00784 ros::init(argc,argv,"lidar_egomotion");
00785
00786 ros::NodeHandle nh("~");
00787
00788 transform_listener= new tf::TransformListener(nh,ros::Duration(10));
00789
00790
00791
00792 ros::Subscriber sub_laser2 = nh.subscribe("/snr/las/2/scan",1,laser_2_Handler);
00793
00794 ros::Subscriber sub_laser3 = nh.subscribe("/snr/las/3/scan",1,laser_3_Handler);
00795
00796 ros::Subscriber sub_plc_status = nh.subscribe ("/vhc/plc/status",1,plcStatusHandler);
00797
00798 ros::Subscriber sub_velocity_status = nh.subscribe ("/vhc/velocity/status",1,velocityStatusHandler);
00799
00800 cout<<"Start to spin"<<endl;
00801
00802 ros::Rate r(500);
00803
00804 constant_velocity_nh motion_model(gl,1./50.,scan_method);
00805
00806 Vector u(0);
00807
00808 double time_interval=(double)step/50.;
00809 double dtsm;
00810
00811 double rtPlICP=0;
00812 double rtMbICP=0;
00813 double rtPSM=0;
00814 int ret=0;
00815
00816 t_fps mean_fps;
00817
00818 bool invalid=false;
00819
00820 Vector zekf(2);
00821 long counter=0;
00822 double t_elapsed=0;
00823
00824 SetDefaultConfiguration();
00825
00826 plSpace PlotSpace(2,2,true);
00827
00828
00829 ros::Time t=ros::Time::now();
00830
00831 while(ros::ok())
00832 {
00833 ros::spinOnce();
00834
00835 if(!new_laser_2 || !new_laser_3)
00836 continue;
00837
00838 double freq=1./((point_cloud_2.header.stamp-t).toSec());
00839 cout<<"Input freq:"<<freq;
00840 if(freq<40)
00841 cout<<"\033[1;31m"<<" Error!! messages too far apart"<<"\033[0m"<<endl;
00842 else
00843 cout<<endl;
00844
00845 t=point_cloud_2.header.stamp;
00846
00847 new_laser_2=false;
00848 new_laser_3=false;
00849
00850 continue;
00851
00852
00853 ClearRays(&c_rays);
00854
00855 PointCloud2Ray(point_cloud_2,&c_rays);
00856 PointCloud2Ray(point_cloud_3,&c_rays);
00857 RemoveOverlappingPoints(&c_rays);
00858
00859 Vector first_guess_estimated=motion_model.predict(u);
00860
00861
00862
00863
00864
00865
00866
00867
00868
00869
00870
00871
00872
00873
00874
00875
00876
00877
00878
00879
00880
00881
00882
00883 if(h_rays.data->total>step)
00884 {
00885 l_rays=(ray_definition*)cvGetSeqElem(h_rays.data,step-1);
00886 time_interval=c_rays.cfg.timestamp-l_rays->cfg.timestamp;
00887 }
00888
00889 cout<<"Time interval "<<time_interval<<endl;
00890
00891 if(time_interval>0.05)
00892 {
00893 cout<<endl<<endl<<"Jumping messages"<<endl<<endl<<endl;
00894 }
00895
00896 dtsm=ros::Time::now().toSec();
00897
00898
00899 CleanZone(&c_rays,first_guess_estimated(5));
00900 CleanZone(l_rays,first_guess_estimated(5));
00901
00902 switch(scan_method)
00903 {
00904 case MBICP:
00905 RaysToMbICP(&c_rays,fp1);
00906 RaysToMbICP(l_rays,fp2);
00907 break;
00908
00909 case PSM:
00910 RaysToPMScan(&c_rays,&pm1);
00911 RaysToPMScan(l_rays,&pm2);
00912 break;
00913
00914 case PLICP:
00915 RaysToLDP(&c_rays,ldp_1,1);
00916 RaysToLDP(l_rays,ldp_2,1);
00917 break;
00918 }
00919
00920
00921
00922
00923 dtsm=ros::Time::now().toSec();
00924
00925 ConvertEstimatedToMeasurment(first_guess_estimated(3),first_guess_estimated(5),&sensorMotion.x,&sensorMotion.y,&sensorMotion.tita,time_interval,gl,bwa);
00926
00927
00928
00929 invalid=false;
00930
00931 double flip;
00932 int np1=0,np2=0;
00933
00934 switch(scan_method)
00935 {
00936 case MBICP:
00937 break;
00938 if(fabs(first_guess_estimated(5))>0.35)
00939 params.filter=0.95;
00940 else if(fabs(first_guess_estimated(5))>0.25)
00941 params.filter=0.90;
00942 else if(fabs(first_guess_estimated(5))>0.20)
00943 params.filter=0.85;
00944 else
00945 params.filter=0.85;
00946
00947
00948
00949
00950
00951
00952
00953
00954
00955
00956
00957 dtsm=ros::Time::now().toSec();
00958 if(!flags.fi)
00959 ret = MbICPmatcher(fp1,fp2,&sensorMotion,&solution);
00960
00961 rtMbICP=(ros::Time::now().toSec()-dtsm)*1000;
00962
00963 switch(ret)
00964 {
00965 case 1:
00966
00967 break;
00968 case 2:
00969 printf("%c[1mMbICP%c[0m Max iterrations reached\n",27,27);
00970 invalid=true;
00971 memset(&solution,0,sizeof(Tsc));
00972 break;
00973 case -1:
00974 printf("%c[1mMbICP%c[0m Failed in association\n",27,27);
00975 invalid=true;
00976 break;
00977 case -2:
00978 printf("%c[1mMbICP%c[0m Failed in minimization\n",27,27);
00979 invalid=true;
00980 break;
00981 default:
00982 printf("%c[1mMbICP%c[0m Error code %d\n",27,27,ret);
00983 invalid=true;
00984 memset(&solution,0,sizeof(Tsc));
00985 break;
00986 }
00987
00988 runtime_MbICP.push_back(rtMbICP);
00989
00990 solution.x=fabs(solution.x)<0.0005?0:solution.x;
00991 solution.y=fabs(solution.y)<0.0005?0:solution.y;
00992 solution.tita=fabs(solution.tita)<0.0005?0:solution.tita;
00993
00994 break;
00995 case PSM:
00996 pm_preprocessScan(&pm1);
00997 pm_preprocessScan(&pm2);
00998
00999
01000
01001
01002
01003
01004 pm1.rx=0;
01005 pm1.ry=0;
01006 pm1.th=0;
01007
01008
01009
01010 pm2.rx=-sensorMotion.y*100.;
01011 pm2.ry=sensorMotion.x*100.;
01012 pm2.th=-sensorMotion.tita;
01013
01014
01015
01016
01017
01018 dtsm=ros::Time::now().toSec();
01019
01020 for(int i=0;i<PM_L_POINTS;i++)
01021 {
01022 if(!pm1.bad[i])np1++;
01023 if(!pm2.bad[i])np2++;
01024 }
01025
01026 if(!flags.fi && np1!=0 && np2!=0)
01027 {
01028 try{
01029 pm_psm(&pm2,&pm1);
01030 }catch(int e){
01031 printf("%c[1mPM_PSM throw an error!%c[0m %d\n",27,27,e);
01032 }
01033 }
01034
01035 rtPSM=(ros::Time::now().toSec()-dtsm)*1000;
01036
01037 runtime_PSM.push_back(rtPSM);
01038
01039 flip=pm1.rx;
01040
01041 pm1.rx=pm1.ry/100.;
01042 pm1.ry=-flip/100.-0.003;
01043 pm1.th=-pm1.th;
01044
01045 solution.x=fabs(pm1.rx)<0.001?0:pm1.rx;
01046 solution.y=fabs(pm1.ry)<0.001?0:pm1.ry;
01047 solution.tita=fabs(pm1.th)<0.001?0:pm1.th;
01048 break;
01049
01050 case PLICP:
01051 ConfigurePLICP(¶ms_sm,ldp_1,ldp_2);
01052 params_sm.first_guess[0]=sensorMotion.x;
01053 params_sm.first_guess[1]=sensorMotion.y;
01054 params_sm.first_guess[2]=sensorMotion.tita;
01055
01056 dtsm=ros::Time::now().toSec();
01057 sm_icp(¶ms_sm, &results_sm);
01058 rtPlICP=(ros::Time::now().toSec()-dtsm)*1000;
01059
01060 runtime_PlICP.push_back(rtPlICP);
01061
01062 solution.x=fabs(results_sm.x[0])<0.0005?0:results_sm.x[0];
01063 solution.y=fabs(results_sm.x[1])<0.0005?0:results_sm.x[1];
01064 solution.tita=fabs(results_sm.x[2])<0.0005?0:results_sm.x[2];
01065 break;
01066 }
01067
01068
01069
01070
01071 CreateMeasurementFromDisplacement(solution.x,solution.y,solution.tita,z,time_interval,motion_model.l,bwa);
01072
01073
01074
01075
01076
01077 if(scan_method==PLICP)
01078 z[1]=z[1]-0.009;
01079
01080
01081
01082
01083
01084
01085 if(z[0]<-10.)
01086 z[0]=-10.;
01087
01088 if(z[0]>33.)
01089 z[0]=33.;
01090
01091 if(z[1]<-0.43)
01092 z[1]=-0.43;
01093
01094 if(z[1]>0.43)
01095 z[1]=0.43;
01096
01097 if(scan_method==MBICP)
01098 {
01099 if(fabs(z[1])>0.41)
01100 z[1]=z[1]>0?0.41:-0.41;
01101 }
01102
01103 if(fabs(z[0])<0.02)
01104 z[1]=0;
01105
01106
01107 double z_ori[2]={z[0],z[1]};
01108
01109
01110
01111
01112
01113
01114
01115
01116 double faultDir=0.1;
01117 double faultSpeed=1.2;
01118
01119 bool faultA=fabs(z[1]-first_guess_estimated(5))>faultDir;
01120 bool faultB=fabs(z[0]-first_guess_estimated(3))>faultSpeed;
01121 bool fault= faultA || faultB;
01122
01123
01124
01125 if( fault && counter<2 && !invalid)
01126 {
01127 if(faultA && faultB)
01128 printf("%c[1mFault AB%c[0m\n",27,27);
01129 else if(faultA)
01130 printf("%c[1mFault A%c[0m\n",27,27);
01131 else
01132 printf("%c[1mFault B%c[0m\n",27,27);
01133
01134 invalid=true;
01135 counter++;
01136 }else if( fault && counter<3)
01137 {
01138 counter++;
01139 printf("%c[1mCorrecting%c[0m\n",27,27);
01140
01141 if(faultA)
01142 {
01143 if(z[1]>first_guess_estimated(5))
01144 {
01145 printf("Steering weel Truncating SW rate of change, positive high\n");
01146 z[1]=first_guess_estimated(5)+faultDir;
01147 }else
01148 {
01149 printf("Steering weel Truncating SW rate of change, negative high\n");
01150 z[1]=first_guess_estimated(5)-faultDir;
01151 }
01152 }
01153
01154 if(faultB)
01155 {
01156 if(z[0]>first_guess_estimated(3))
01157 {
01158 printf("Linear Speed Truncating LS rate of change, positive high\n");
01159 z[0]=first_guess_estimated(3)+faultSpeed;
01160 }else
01161 {
01162 printf("Linear Speed Truncating LS rate of change, negative high\n");
01163 z[0]=first_guess_estimated(3)-faultSpeed;
01164 }
01165 }
01166 }else
01167 counter=0;
01168
01169
01170
01171 if(!invalid)
01172 {
01173 printf("%c[1mFg %c[0m %6.4f %6.4f %6.4f\n",27,27,sensorMotion.x,sensorMotion.y,sensorMotion.tita);
01174 printf("%c[1mSM %c[0m %6.4f %6.4f %6.4f\n",27,27,solution.x,solution.y,solution.tita);
01175 printf("%c[1mDiff %c[0m %6.4f %6.4f %6.4f\n",27,27,fabs(solution.x-sensorMotion.x),fabs(solution.y-sensorMotion.y),fabs(solution.tita-sensorMotion.tita));
01176 printf("\n");
01177
01178 }else
01179 {
01180 printf("%c[1mMbICP%c[0m Invalid solution, resetting to prediction\n",27,27);
01181
01182 z[0]=first_guess_estimated(3);
01183 z[1]=first_guess_estimated(5);
01184
01185
01186 if(z[0]<-10.)
01187 z[0]=-10.;
01188
01189 if(z[0]>33.)
01190 z[0]=33.;
01191
01192 if(z[1]<-0.5)
01193 z[1]=-0.5;
01194
01195 if(z[1]>0.5)
01196 z[1]=0.5;
01197 }
01198
01199 if(path_laser.size()<20)
01200 {
01201 z[0]=z_ori[0];
01202 z[1]=z_ori[1];
01203 }
01204
01205
01206
01207
01208
01209
01210 zekf(1)=z[0];
01211 zekf(2)=z[1];
01212
01213 motion_model.step(u,zekf);
01214
01215 Vector x_corrected=motion_model.getX();
01216
01217 t_posePtr path_node(new t_pose);
01218
01219 path_node->phi=x_corrected(5);
01220
01221 path_node->x=x_corrected(1);
01222 path_node->y=x_corrected(2);
01223 path_node->vl=x_corrected(3);
01224 path_node->orientation=x_corrected(4);
01225
01226 path_node->m_vl=z[0];
01227 path_node->m_phi=z[1];
01228
01229 path_node->timestamp=point_cloud_2.header.stamp.toSec();
01230
01231 path_laser.push_back(path_node);
01232
01233
01234
01235
01236 static bool innnn=true;
01237 if(innnn)
01238 {
01239 ret=system("rm /home/atlas/Desktop/XY_laser.txt");
01240 ret=system("rm /home/atlas/Desktop/Laser_Pose.txt");
01241 ret=system("rm /home/atlas/Desktop/Filter.txt");
01242 ret=system("rm /home/atlas/Desktop/RunTime.txt");
01243 innnn=false;
01244 }
01245
01246 ofstream Path("/home/atlas/Desktop/XY_laser.txt",ios::app);
01247 Path<<path_node->x<<" "<<path_node->y<<endl;
01248
01249 ofstream Pose("/home/atlas/Desktop/Laser_Pose.txt",ios::app);
01250 Pose<<*path_node<<endl;
01251
01252 ofstream Filter("/home/atlas/Desktop/Filter.txt",ios::app);
01253 Filter<<x_corrected(1)<<" "<<x_corrected(2)<<" "<<x_corrected(3)<<" "<<x_corrected(4)<<" "<<x_corrected(5)<<" "<<x_corrected(6)<<endl;
01254
01255 ofstream RunTime("/home/atlas/Desktop/RunTime.txt",ios::app);
01256
01257 switch(scan_method)
01258 {
01259 case MBICP:
01260 RunTime<<rtMbICP<<endl;
01261 break;
01262
01263 case PSM:
01264 RunTime<<rtPSM<<endl;
01265 break;
01266
01267 case PLICP:
01268 RunTime<<rtPlICP<<endl;
01269 break;
01270 }
01271
01272
01273
01274 dtsm=ros::Time::now().toSec();
01275
01276 PlotSpace.SetPath(0,path_odo,"Map");
01277 PlotSpace.SetSecondaryPath(0,path_laser,PL_RED);
01278
01279 PlotSpace.SetMember_UseTimestamp(1,path_odo,get_vl,PL_BLUE,make_pair(-1.0,15.0),"Time (s)","Linear Speed (m/s)","Linear Speed",20.0);
01280 PlotSpace.SetSecondaryPlot(1,path_laser,get_m_vl,PL_CYAN,20.0);
01281 PlotSpace.SetThirdPlot(1,path_laser,get_vl,PL_RED,20.0);
01282
01283
01284
01285
01286
01287 PlotSpace.SetMember_UseTimestamp(2,path_odo,get_orientation,PL_BLUE,make_pair(-5,5),"Time (s)","yaw (rad)","YAW",20.0);
01288 PlotSpace.SetSecondaryPlot(2,path_laser,get_orientation,PL_YELLOW,20.0);
01289
01290
01291
01292 switch(scan_method)
01293 {
01294 case MBICP:
01295 PlotSpace.SetRunTime(3,runtime_MbICP,PL_YELLOW,"RunTime MbICP",40.,500.);
01296 break;
01297 case PLICP:
01298 PlotSpace.SetRunTime(3,runtime_PlICP,PL_YELLOW,"RunTime PlICP",40.,500.);
01299 break;
01300 case PSM:
01301 PlotSpace.SetRunTime(3,runtime_PSM,PL_YELLOW,"RunTime PSM",40.,500.);
01302 break;
01303 }
01304
01305 PlotSpace.Plot();
01306
01307 AddToHistory(&c_rays,&h_rays);
01308
01309 double fps=get_fps(ros::Time::now().toSec()-t_elapsed,&mean_fps);
01310 t_elapsed=ros::Time::now().toSec();
01311 printf("Fps %.1lf \r",fps); fflush(stdout);
01312
01313 flags.fi=false;
01314 }
01315
01316 return 0;
01317 }
