mbicp_driver.cc
Go to the documentation of this file.
00001 
00177 #include <libplayercore/playercore.h>
00178 
00179 #include "calcul.h"
00180 #include "MbICP.h"
00181 
00182 #define LASER_MAX_SAMPLES 1024
00183 
00184 class mbicp : public Driver
00185 {
00186 
00187 public:
00188     
00189     mbicp( ConfigFile* cf, int section);
00190     virtual ~mbicp();
00191 
00192     virtual int Setup();
00193     virtual int Shutdown();
00194 
00195     virtual int ProcessMessage(QueuePointer &resp_queue,
00196                                player_msghdr * hdr,
00197                                void * data);
00198 private:
00199 
00200         float   max_laser_range;
00201         float   Bw;
00202         float   Br;
00203         float   L;
00204         int     laserStep;
00205         float   MaxDistInter;
00206         float   filter;
00207         int     ProjectionFilter;
00208         float   AsocError;
00209         int     MaxIter;
00210         float   errorRatio;
00211         float   errx_out;
00212         float   erry_out;
00213         float   errt_out;
00214         int     IterSmoothConv;
00215         Tsc     laserPoseTsc;
00216  
00217         player_pose2d_t         lastPoseOdom,
00218                                 currentPose,
00219                                 previousPose,
00220                                 scanmatchingPose;
00221 
00222         player_laser_data_t     currentScan,
00223                                 previousScan;
00224                                 
00225         bool            havePrevious;
00226 
00227         //Compute scanMatching
00228         void compute();
00229 
00230         //Transform structures between player and Tdata
00231         Tsc             playerPose2Tsc(player_pose2d_t posicion);
00232         player_pose2d_t         Tsc2playerPose(Tsc posicion);
00233         void            playerLaser2Tpfp(player_laser_data_t laserData,Tpfp *laserDataTpfp);
00234 
00235         // Main function for device thread.
00236         virtual void Main();
00237 
00238         int SetupDevice();
00239         int ShutdownDevice();
00240 
00241         // Odometry.
00242         void ProcessOdom(player_msghdr_t* hdr, player_position2d_data_t &data);
00243 
00244         // SubtypeLaser
00245         void ProcessSubtypeLaser(player_msghdr_t* hdr, player_laser_data_scanpose_t &data);
00246 
00247         // Check for new commands from server
00248         void ProcessCommand(player_msghdr_t* hdr, player_position2d_cmd_pos_t &);
00249 
00250         // Setup ScanMatching
00251         void setupScanMatching();
00252 
00253         // Position
00254         player_devaddr_t posicion_addr;
00255 
00256         // Odometry and laser
00257         Device *odom;
00258         player_devaddr_t odom_addr;
00259 
00260         Device *laser;
00261         player_devaddr_t laser_addr;
00262 
00263 };
00264 
00266 Driver* mbicp_Init(ConfigFile* cf, int section){
00267 
00268    return((Driver*)(new mbicp(cf, section)));
00269 
00270 }
00271 
00272 
00274 void mbicp_Register(DriverTable* table){
00275 
00276    table->AddDriver("mbicp", mbicp_Init);
00277 
00278 }
00279 
00280 
00282 int mbicp::Setup(){
00283 
00284    havePrevious = false;
00285 
00286    // Initialise the underlying position device.
00287    if(SetupDevice() != 0)
00288       return -1;
00289 
00290    setupScanMatching();
00291 
00292    puts("Setup Scanmatching");
00293    // Start the driver thread.
00294    StartThread();
00295    return 0;
00296 
00297 }
00298 
00299 
00301 void mbicp::setupScanMatching(){
00302 
00303 Init_MbICP_ScanMatching( 
00304                         this->max_laser_range,
00305                         this->Bw,        
00306                         this->Br,        
00307                         this->L,         
00308                         this->laserStep,
00309                         this->MaxDistInter,
00310                         this->filter,    
00311                         this->ProjectionFilter,
00312                         this->AsocError,
00313                         this->MaxIter,  
00314                         this->errorRatio,
00315                         this->errx_out, 
00316                         this->erry_out,  
00317                         this->errt_out,  
00318                         this->IterSmoothConv);
00319 }
00320 
00321 
00323 int mbicp::Shutdown(){
00324    // Stop the driver thread.
00325    StopThread();
00326 
00327    // Stop the odom device.
00328    ShutdownDevice();
00329    return 0;
00330 }
00331 
00332 
00334 mbicp::mbicp( ConfigFile* cf, int section)
00335   : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
00336            PLAYER_POSITION2D_CODE){
00337 
00338 
00339         this->max_laser_range           =  cf->ReadFloat(section, "max_laser_range", 7.9);
00340         this->Bw                        =  cf->ReadFloat(section, "angular_window", 1.57/3.0);
00341         this->Br                        =  cf->ReadFloat(section, "radial_window", 0.3);
00342         this->L                         =  cf->ReadFloat(section, "L", 3.00);
00343         this->laserStep                 =  cf->ReadInt(section, "laserStep", 1);
00344         this->MaxDistInter              =  cf->ReadFloat(section, "MaxDistInter", 0.5);
00345         this->filter                    =  cf->ReadFloat(section, "filter", 0.85);
00346         this->ProjectionFilter          =  cf->ReadInt(section, "ProjectionFilter", 1);
00347         this->AsocError                 =  cf->ReadFloat(section, "AsocError", 0.1);
00348         this->MaxIter                   =  cf->ReadInt(section, "MaxIter", 50);
00349         this->errorRatio                =  cf->ReadFloat(section, "errorRatio", 0.0001);
00350         this->errx_out                  =  cf->ReadFloat(section, "errx_out", 0.0001);
00351         this->erry_out                  =  cf->ReadFloat(section, "erry_out", 0.0001);
00352         this->errt_out                  =  cf->ReadFloat(section, "errt_out", 0.0001);
00353         this->IterSmoothConv            =  cf->ReadInt(section, "IterSmoothConv", 2);
00354         this->laserPoseTsc.x            =  cf->ReadFloat(section, "laserPose_x", 0.16);
00355         this->laserPoseTsc.y            =  cf->ReadFloat(section, "laserPose_y", 0);
00356         this->laserPoseTsc.tita         =  cf->ReadFloat(section, "laserPose_th", 0);
00357 
00358 
00359         if(cf->ReadDeviceAddr(&(posicion_addr), section, "provides",
00360                 PLAYER_POSITION2D_CODE, -1, NULL) != 0){
00361                 this->SetError(-1);
00362                 return;
00363         }
00364 
00365         odom = NULL;
00366         if(cf->ReadDeviceAddr(&odom_addr, section, "requires",
00367                 PLAYER_POSITION2D_CODE, -1, NULL) != 0){
00368                 SetError(-1);
00369                 return;
00370         }
00371 
00372         laser = NULL;
00373         if(cf->ReadDeviceAddr(&laser_addr, section, "requires",
00374                 PLAYER_LASER_CODE, -1, NULL) != 0){
00375                 SetError(-1);
00376         }
00377 
00378         return;
00379 }
00380 
00381 
00383 mbicp::~mbicp(){
00384    return;
00385 }
00386 
00387 
00389 int mbicp::SetupDevice(){
00390 
00391    if(!(odom = deviceTable->GetDevice(odom_addr))){
00392       PLAYER_ERROR("Unable to locate suitable position device");
00393       return -1;
00394    }
00395    if(odom->Subscribe(InQueue) != 0){
00396       PLAYER_ERROR("Unable to subscribe to position device");
00397       return -1;
00398    }
00399 
00400    if(!(laser = deviceTable->GetDevice(laser_addr))){
00401       PLAYER_ERROR("Unable to locate suitable laser device");
00402       return -1;
00403    }
00404    if(laser->Subscribe(InQueue) != 0){
00405       PLAYER_ERROR("Unable to subscribe to laser device");
00406       return -1;
00407    }
00408 
00409    return 0;
00410 }
00411 
00412 
00414 int mbicp::ShutdownDevice(){
00415    player_position2d_cmd_vel_t cmd;
00416 
00417    memset(&cmd, 0, sizeof(cmd));
00418    // Stop the robot (locks the motors) if the motor state is set to
00419    // disabled.  The P2OS driver does not respect the motor state.
00420    cmd.vel.px = 0;
00421    cmd.vel.py = 0;
00422    cmd.vel.pa = 0;
00423  
00424    odom->PutMsg(InQueue, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL,
00425                 (void*)&cmd,sizeof(cmd),NULL);
00426 
00427    odom->Unsubscribe(InQueue);
00428    laser->Unsubscribe(InQueue);
00429    puts("Shutdown mbicp");
00430    return 0;
00431 }
00432 
00433 
00435 void mbicp::Main(){
00436         while (true){
00437                 // Wait till we get new data
00438                 Wait();
00439                 
00440                 // Test if we are supposed to cancel this thread.
00441                 pthread_testcancel();
00442 
00443                 // Process any pending requests.
00444                 ProcessMessages();
00445         }
00446 }
00447 
00448 
00450 int mbicp::ProcessMessage(QueuePointer &resp_queue,player_msghdr * hdr, void * data){
00451 
00452         // PLAYER_LASER_DATA_SCANPOSE
00453         if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,PLAYER_LASER_DATA_SCANPOSE, laser_addr))
00454         {
00455                 assert(hdr->size == sizeof(player_laser_data_scanpose_t));
00456                 ProcessSubtypeLaser(hdr, *reinterpret_cast<player_laser_data_scanpose_t *> (data));
00457         }else
00458 
00459         // PLAYER_POSITION2D_DATA_STATE
00460         if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,PLAYER_POSITION2D_DATA_STATE, odom_addr))
00461         {
00462                 assert(hdr->size == sizeof(player_position2d_data_t));
00463                 ProcessOdom(hdr, *reinterpret_cast<player_position2d_data_t *> (data));
00464         }else
00465 
00466         if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,PLAYER_POSITION2D_DATA_GEOM, odom_addr))
00467         {
00468                 assert(hdr->size == sizeof(player_position2d_data_t));
00469                 player_msghdr_t newhdr = *hdr;
00470                 newhdr.addr = device_addr;
00471                 Publish(&newhdr, (void*)&data);
00472         }else 
00473 
00474         if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,/*PLAYER_POSITION2D_CMD_VEL*/ -1, device_addr))
00475         {
00476                 assert(hdr->size == sizeof(player_position2d_cmd_vel_t));
00477                 // make a copy of the header and change the address
00478                 player_msghdr_t newhdr = *hdr;
00479                 newhdr.addr = odom_addr;
00480                 odom->PutMsg(InQueue, &newhdr, (void*)data);
00481         }else
00482         { 
00483                 if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, -1, device_addr)){
00484                         // Pass the request on to the underlying position device and wait for
00485                         // the reply.
00486                         Message* msg;
00487 
00488                         if(!(msg = odom->Request(InQueue, hdr->type, hdr->subtype, (void*)data,hdr->size, &hdr->timestamp)))
00489                         {
00490                                 PLAYER_WARN1("failed to forward config request with subtype: %d\n",hdr->subtype);
00491                                 return(-1);
00492                         }
00493                         player_msghdr_t* rephdr = msg->GetHeader();
00494                         void* repdata = msg->GetPayload();
00495                         // Copy in our address and forward the response
00496                         rephdr->addr = device_addr;
00497                         Publish(resp_queue, rephdr, repdata);
00498                         delete msg;
00499                 }
00500         }
00501         return 0;
00502 }
00503 
00505 void mbicp::ProcessOdom(player_msghdr_t* hdr,player_position2d_data_t &data)
00506 {
00507 
00508         
00509         Tsc     outComposicion1,
00510                 outComposicion2,
00511                 outInversion1;
00512                         
00513         Tsc     lastPoseOdomTsc,
00514                 previousPoseTsc,
00515                 scanmatchingPoseTsc;            
00516 
00517 
00518         lastPoseOdom.px = data.pos.px;
00519         lastPoseOdom.py = data.pos.py;
00520         lastPoseOdom.pa = data.pos.pa;
00521 
00522         if (havePrevious)
00523         {
00524                 lastPoseOdomTsc         = playerPose2Tsc(lastPoseOdom);
00525                 previousPoseTsc         = playerPose2Tsc(previousPose);
00526                 scanmatchingPoseTsc     = playerPose2Tsc(scanmatchingPose);
00527 
00528                 inversion_sis(&previousPoseTsc, &outInversion1);
00529                 composicion_sis(&outInversion1, &lastPoseOdomTsc, &outComposicion1);
00530                 composicion_sis(&scanmatchingPoseTsc, &outComposicion1, &outComposicion2);
00531         
00532 
00533                 data.pos.px     = outComposicion2.x;
00534                 data.pos.py     = outComposicion2.y;
00535                 data.pos.pa     = outComposicion2.tita;
00536 
00537 
00538         }       
00539         
00540         player_msghdr_t newhdr = *hdr;
00541         newhdr.addr = device_addr;
00542         Publish(&newhdr, (void*)&data);
00543 }
00544 
00545 
00547 void mbicp::ProcessSubtypeLaser(player_msghdr_t* hdr,player_laser_data_scanpose_t &data){
00548 
00549         lastPoseOdom.px = data.pose.px;
00550         lastPoseOdom.py = data.pose.py;
00551         lastPoseOdom.pa = data.pose.pa;
00552 
00553         currentPose     = lastPoseOdom;
00554 
00555         currentScan.min_angle           = data.scan.min_angle;          
00556         currentScan.max_angle           = data.scan.max_angle;  
00557         currentScan.resolution          = data.scan.resolution;
00558         currentScan.max_range           = data.scan.max_range;  
00559         currentScan.ranges_count        = data.scan.ranges_count;
00560         currentScan.intensity_count     = data.scan.intensity_count;    
00561         currentScan.id                                  = data.scan.id;
00562         
00563         for (unsigned int i=0; i < currentScan.ranges_count; i++){
00564                 currentScan.ranges[i] = data.scan.ranges[i];
00565                 currentScan.intensity[i] = data.scan.intensity[i];
00566         }
00567 
00568         if (havePrevious && (   currentPose.px != previousPose.px || 
00569                                 currentPose.py != previousPose.py ||
00570                                 currentPose.pa != previousPose.pa))
00571         {
00572                 compute();
00573         }
00574 
00575         else if (!havePrevious)
00576         {
00577                 previousScan            = currentScan;
00578                 previousPose            = currentPose;
00579                 scanmatchingPose        = currentPose;
00580                 havePrevious            = true;
00581         }                                       
00582 }
00583 
00584 
00586 void mbicp::compute()
00587 {
00588         Tsc     previousPoseTsc,
00589                 currentPoseTsc,
00590                 scanmatchingPoseTsc,
00591                 solutionTsc;
00592         
00593         Tsc     outComposicion1,
00594                 outComposicion2,
00595                 outComposicion3,
00596                 outComposicion9,
00597                 outComposicion10,
00598                 outComposicion11,
00599                 outInversion1,
00600                 outInversion4;
00601                         
00602         Tpfp    previousScanTpfp[LASER_MAX_SAMPLES],
00603                 currentScanTpfp[LASER_MAX_SAMPLES];
00604                         
00605         int     salidaMbicp;            
00606                         
00607 
00608         currentPoseTsc          = playerPose2Tsc(currentPose);
00609         previousPoseTsc         = playerPose2Tsc(previousPose);
00610         scanmatchingPoseTsc     = playerPose2Tsc(scanmatchingPose);
00611 
00612 
00613         composicion_sis(&previousPoseTsc, &laserPoseTsc, &outComposicion1);
00614         composicion_sis(&currentPoseTsc, &laserPoseTsc, &outComposicion2);
00615         inversion_sis(&outComposicion1, &outInversion1);
00616         composicion_sis(&outInversion1, &outComposicion2, &outComposicion3);
00617 
00618         playerLaser2Tpfp(previousScan,previousScanTpfp);
00619         playerLaser2Tpfp(currentScan,currentScanTpfp);
00620 
00621         salidaMbicp = MbICPmatcher(previousScanTpfp,currentScanTpfp,&outComposicion3, &solutionTsc);    
00622 
00623         if (salidaMbicp == 1){
00624 
00625                 composicion_sis(&laserPoseTsc,&solutionTsc,&outComposicion9);
00626                 inversion_sis(&laserPoseTsc, &outInversion4);
00627                 composicion_sis(&outComposicion9,&outInversion4,&outComposicion10);
00628                 composicion_sis(&scanmatchingPoseTsc, &outComposicion10, &outComposicion11);
00629 
00630                 scanmatchingPoseTsc.x = outComposicion11.x;
00631                 scanmatchingPoseTsc.y = outComposicion11.y;
00632                 scanmatchingPoseTsc.tita = outComposicion11.tita;
00633 
00634         }
00635         else{
00636 
00637                 if (salidaMbicp == 2)
00638                         fprintf(stderr,"2 : Everything OK but reached the Maximum number of iterations\n");             
00639                 else{
00640                         if (salidaMbicp == -1)
00641                                 fprintf(stderr,"Failure in the association step\n");
00642                         if (salidaMbicp == -2)
00643                                 fprintf(stderr,"Failure in the minimization step\n");
00644                 }
00645                 composicion_sis(&laserPoseTsc,&outComposicion3,&outComposicion9);
00646                 inversion_sis(&laserPoseTsc, &outInversion4);
00647                 composicion_sis(&outComposicion9,&outInversion4,&outComposicion10);
00648                 composicion_sis(&scanmatchingPoseTsc, &outComposicion10, &outComposicion11);
00649 
00650                 scanmatchingPoseTsc.x = outComposicion11.x;
00651                 scanmatchingPoseTsc.y = outComposicion11.y;
00652                 scanmatchingPoseTsc.tita = outComposicion11.tita;
00653 
00654         }
00655 
00656         scanmatchingPose        = Tsc2playerPose(scanmatchingPoseTsc);
00657         previousScan            = currentScan;
00658         previousPose            = currentPose;
00659 }
00660 
00661 
00663 Tsc mbicp::playerPose2Tsc(player_pose2d_t posicion)
00664 {
00665         Tsc     posicionTsc;
00666         
00667         posicionTsc.x = posicion.px;
00668         posicionTsc.y = posicion.py;
00669         posicionTsc.tita = posicion.pa;
00670         return(posicionTsc);
00671 }
00672 
00673 
00675 player_pose2d_t mbicp::Tsc2playerPose(Tsc posicion)
00676 {
00677         player_pose2d_t posicionPlayer;
00678         
00679         posicionPlayer.px = posicion.x;
00680         posicionPlayer.py = posicion.y;
00681         posicionPlayer.pa = posicion.tita;
00682         return(posicionPlayer);
00683 }
00684 
00685 
00687 void mbicp::playerLaser2Tpfp(player_laser_data_t laserData,Tpfp *laserDataTpfp)
00688 {
00689         for(unsigned int i=0; i< laserData.ranges_count; i++){
00690                 laserDataTpfp[i].r      = laserData.ranges[i];
00691                 laserDataTpfp[i].t      = laserData.min_angle + (i*laserData.resolution);
00692         }
00693 }


csm
Author(s): Andrea Censi
autogenerated on Mon Jan 16 2017 03:48:29