Changeset 872


Ignore:
Timestamp:
01/25/2011 08:40:53 AM (10 years ago)
Author:
lmalek
Message:

minor organization changes in Player2Samgar

Location:
libs/SAMGAR/trunk/Modules/Samgar2Player
Files:
4 edited

Legend:

Unmodified
Added
Removed

  • libs/SAMGAR/trunk/Modules/Samgar2Player/PROTOCOL.txt

    r828 r872  
    3434        Ack     = 1 
    3535 
    36  
     36enum PlayerProxy_Position2d_CMD {Position2d_SET_SPEED=1, Position2d_SET_GOAL, Position2d_SET_ODOM, Position2d_SET_MOTOR}; 
    3737enum PlayerProxy_Localize_CMD {Localize_SET_POSE=1}; 
    3838enum PlayerProxy_Planner_CMD {Planner_SET_GOAL=1, Planner_SET_ENABLE}; 
    … …  
    4343 
    4444 
     45enum PlayerProxy_Position2d_DATA {Position2d_SPEED=2, Position2d_POSE, Position2d_SIZE, Position2d_STALL}; 
    4546enum PlayerProxy_Localize_DATA {Localize_MAP_INFO=2, Localize_HYPOTHS}; 
    4647enum PlayerProxy_Planner_DATA {Planner_GOAL=2, Planner_POSE, Planner_CURR_WAYPOINT, Planner_PATH, Planner_WAYPOINTS}; 
    … …  
    104105        Ack                     = 1 
    105106        Position2d_SPEED        = 2 
    106         Position2d_POSITION     = 3 
     107        Position2d_POSE         = 3 
    107108        Position2d_SIZE         = 4 
    108109        Position2d_STALL        = 5 
    … …  
    143144 
    144145+------++---------------+-----------------------+-----------------------+-----------------------+-----------------------+ 
    145 | desc || command_id    | Position2d_SPEED      | Position2d_POSITION   | Position2d_SIZE       | Position2d_STALL      |  
     146| desc || command_id    | Position2d_SPEED      | Position2d_POSE       | Position2d_SIZE       | Position2d_STALL      |  
    146147+------++---------------+-----------------------+-----------------------+-----------------------+-----------------------+ 
    147148| type || SET_REQ       | bool                  | bool                  | bool                  | bool                  | 
    … …  
    161162 
    162163+------++-----------------------+---------------+ 
    163 | desc || command_id            | goal          | 
    164 +------++-----------------------+---------------+ 
    165 | type || Position2d_POSITION   | coord_type    | 
     164| desc || command_id            | pose          | 
     165+------++-----------------------+---------------+ 
     166| type || Position2d_POSE       | coord_type    | 
    166167+------++-----------------------+---------------+ 
    167168| info ||                       |               | 

  • libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarModule.cpp

    r828 r872  
    22 */ 
    33#include "Player2SamgarModule.h" 
    4 //#include "Player2SamgarThread.h" 
    54#include <iostream> 
    65 
    … …  
    5049      //return -1; 
    5150    } 
    52   player->StartThread(); 
     51  player->SetDataMode(PLAYER_DATAMODE_PULL); 
     52  player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 
     53 
     54  //player->StartThread(); 
    5355 
    5456  std::list<playerc_device_info_t > devices; 
    … …  
    7375      currDriver.player_port=gPort; 
    7476 
    75       name = currDriver.interfName;  
     77      name = currDriver.driverName+"-"; 
     78      name += currDriver.interfName;  
    7679      name += "-"+itoa(currDriver.index); 
    7780       
    … …  
    146149          pthread_create(threads.back(), NULL, &Position2dThread, &(*it)); 
    147150        } 
    148       if (it->interfName.compare("localize")==0) 
     151      else if (it->interfName.compare("localize")==0) 
    149152        { 
    150153          threads.push_back(new pthread_t); 
    … …  
    152155        } 
    153156      // wafewront has a little bug in its interface so there is a need of such a trick 
    154       if (it->interfName.compare("unknown")==0 && it->driverName.compare("wavewront")) 
     157      else if (it->interfName.compare("unknown")==0 && it->driverName.compare("wavewront")) 
    155158        { 
    156159          threads.push_back(new pthread_t); 
    157160          pthread_create(threads.back(), NULL, &PlannerThread, &(*it)); 
    158161        } 
    159       if (it->interfName.compare("laser")==0) 
     162      else if (it->interfName.compare("laser")==0) 
    160163        { 
    161164          threads.push_back(new pthread_t); 
    162165          pthread_create(threads.back(), NULL, &LaserThread, &(*it)); 
    163166        } 
    164       if (it->interfName.compare("map")==0) 
     167      else if (it->interfName.compare("map")==0) 
    165168        { 
    166169          threads.push_back(new pthread_t); 
    167170          pthread_create(threads.back(), NULL, &MapThread, &(*it)); 
    168171        } 
    169       if (it->interfName.compare("sonar")==0) 
     172      else if (it->interfName.compare("sonar")==0) 
    170173        { 
    171174          threads.push_back(new pthread_t); 
    172175          pthread_create(threads.back(), NULL, &SonarThread, &(*it)); 
    173176        } 
    174       if (it->interfName.compare("bumper")==0) 
     177      else if (it->interfName.compare("bumper")==0) 
    175178        { 
    176179          threads.push_back(new pthread_t); 

  • libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarModule.h

    r828 r872  
    1313 
    1414 
    15 enum PlayerProxy_TYPE {Position2d, Localize, Planner, Laser, Map, Sonar, Bumper}; 
     15enum PlayerProxy_TYPE {Position2d=0, Localize, Planner, Laser, Map, Sonar, Bumper}; 
    1616 
    1717enum PlayerProxy_FIELDS {TYPE=0, CMD}; 
    … …  
    2525enum PlayerProxy_DATA {Error=0, Ack=1}; 
    2626 
    27 enum PlayerProxy_Position2d_DATA {Position2d_SPEED=2, Position2d_POSITION, Position2d_SIZE, Position2d_STALL}; 
     27enum PlayerProxy_Position2d_DATA {Position2d_SPEED=2, Position2d_POSE, Position2d_SIZE, Position2d_STALL}; 
    2828enum PlayerProxy_Localize_DATA {Localize_MAP_INFO=2, Localize_HYPOTHS}; 
    2929enum PlayerProxy_Planner_DATA {Planner_GOAL=2, Planner_POSE, Planner_CURR_WAYPOINT, Planner_PATH, Planner_WAYPOINTS}; 

  • libs/SAMGAR/trunk/Modules/Samgar2Player/Player2SamgarThread.cpp

    r829 r872  
    2121      //return -1; 
    2222    } 
     23 
     24  player->SetDataMode(PLAYER_DATAMODE_PULL); 
     25  player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 
    2326 
    2427  PlayerCc::Position2dProxy pp(player,data->index); 
    … …  
    6770                  data->samgarPort->write(); 
    6871                } 
    69               if (input->get(Position2d_POSITION).asInt()) 
     72              if (input->get(Position2d_POSE).asInt()) 
    7073                { 
    7174                  yarp::os::Bottle& B = data->samgarPort->prepare();      // prepare the bottle/port 
    7275                  B.clear(); 
    7376                  B.addInt(Position2d); 
    74                   B.addInt(Position2d_POSITION); 
     77                  B.addInt(Position2d_POSE); 
    7578                  B.addDouble(pp.GetXPos()); 
    7679                  B.addDouble(pp.GetYPos()); 
    … …  
    108111             
    109112        } 
     113#if DEBUG 
    110114      else{ 
    111115        yarp::os::Bottle& B = data->samgarPort->prepare();        // prepare the bottle/port 
    … …  
    116120        data->samgarPort->write(); 
    117121      } 
     122#endif 
    118123    } // while(1) 
    119124  return NULL; 
    … …  
    138143      //return -1; 
    139144    } 
     145 
     146  player->SetDataMode(PLAYER_DATAMODE_PULL); 
     147  player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 
    140148 
    141149  PlayerCc::LocalizeProxy pp(player,data->index); 
    … …  
    150158          // read state from Player 
    151159          player->Read(); 
    152            
    153160          // perform Samgar command 
    154161          if (input->get(CMD).asInt() == Localize_SET_POSE) 
    … …  
    165172                { 
    166173                  yarp::os::Bottle& B = data->samgarPort->prepare();      // prepare the bottle/port 
     174                  //std::cout<<"MAP_INFO"<<std::endl; 
    167175                  B.clear(); 
    168176                  B.addInt(Localize); 
    … …  
    174182                  B.addDouble(pp.GetMapScale()); 
    175183                  data->samgarPort->write(); 
     184                  usleep(1000); 
    176185                } 
    177186              if (input->get(Localize_HYPOTHS).asInt()) 
    178187                { 
    179188                  yarp::os::Bottle& B = data->samgarPort->prepare();      // prepare the bottle/port 
     189                  //std::cout<<"HYPOTHS"<<std::endl; 
    180190                  B.clear(); 
    181191                  B.addInt(Localize); 
    182192                  B.addInt(Localize_HYPOTHS); 
     193                  //std::cout<<pp.GetNumHypoths()<<" "<<pp.GetNumParticles()<<" "<<pp.GetParticles()<<" "<<pp.GetHypothCount()<<" "<< pp.GetPendingCount()<<std::endl; 
    183194                  int hypothsCount = pp.GetHypothCount(); 
    184195                  player_localize_hypoth_t hypoth; 
    … …  
    192203                  } 
    193204                  data->samgarPort->write(); 
     205                  usleep(1000); 
    194206                } 
    195207            } 
    … …  
    203215             
    204216        } 
     217#if DEBUG 
    205218      else{ 
    206219        yarp::os::Bottle& B = data->samgarPort->prepare();        // prepare the bottle/port 
    … …  
    211224        data->samgarPort->write(); 
    212225      } 
     226#endif 
    213227    } // while(1) 
    214228  return NULL; 
    … …  
    232246      //return -1; 
    233247    } 
     248 
     249  player->SetDataMode(PLAYER_DATAMODE_PULL); 
     250  player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 
    234251 
    235252  PlayerCc::PlannerProxy pp(player,data->index); 
    … …  
    244261          // read state from Player 
    245262          player->Read(); 
    246            
    247263          // perform Samgar command 
    248264          if (input->get(CMD).asInt() == Planner_SET_GOAL) 
    … …  
    254270          if (input->get(1).asInt() == Planner_SET_ENABLE) 
    255271            { 
    256               pp.SetEnable(input->get(2).asDouble()); 
     272              pp.SetEnable(input->get(2).asInt()); 
    257273            } 
    258274 
    … …  
    329345             
    330346        } 
     347#if DEBUG 
    331348      else{ 
    332349        yarp::os::Bottle& B = data->samgarPort->prepare();        // prepare the bottle/port 
    … …  
    337354        data->samgarPort->write(); 
    338355      } 
     356#endif 
    339357    } // while(1) 
    340358  return NULL; 
    … …  
    360378    } 
    361379 
     380  player->SetDataMode(PLAYER_DATAMODE_PULL); 
     381  player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 
     382 
    362383  PlayerCc::LaserProxy pp(player,data->index); 
    363384   
    … …  
    366387      // read data from Samgar 
    367388      yarp::os::Bottle *input = data->samgarPort->read(true); // blocking read 
     389 
    368390      // analize data from samgar 
    369391      if (input->get(TYPE).asInt() == Laser) 
    … …  
    470492          B.addInt(Laser); 
    471493          B.addInt(Ack); 
    472           data->samgarPort->write(); 
    473              
     494          data->samgarPort->write();    
    474495        } 
     496#if DEBUG 
    475497      else{ 
    476498        yarp::os::Bottle& B = data->samgarPort->prepare();        // prepare the bottle/port 
    … …  
    481503        data->samgarPort->write(); 
    482504      } 
     505#endif 
    483506    } // while(1) 
    484507  return NULL; 
    … …  
    503526      //return -1; 
    504527    } 
     528 
     529  player->SetDataMode(PLAYER_DATAMODE_PULL); 
     530  player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 
    505531 
    506532  PlayerCc::MapProxy pp(player,data->index); 
    … …  
    546572             
    547573        } 
     574#if DEBUG 
    548575      else{ 
    549576        yarp::os::Bottle& B = data->samgarPort->prepare();        // prepare the bottle/port 
    … …  
    554581        data->samgarPort->write(); 
    555582      } 
     583#endif 
    556584    } // while(1) 
    557585  return NULL; 
    … …  
    575603      //return -1; 
    576604    } 
     605 
     606  player->SetDataMode(PLAYER_DATAMODE_PULL); 
     607  player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 
    577608 
    578609  PlayerCc::SonarProxy pp(player,data->index); 
    … …  
    634665             
    635666        } 
     667#if DEBUG 
    636668      else{ 
    637669        yarp::os::Bottle& B = data->samgarPort->prepare();        // prepare the bottle/port 
    … …  
    642674        data->samgarPort->write(); 
    643675      } 
     676#endif 
    644677    } // while(1) 
    645678  return NULL; 
    … …  
    663696      //return -1; 
    664697    } 
     698 
     699  player->SetDataMode(PLAYER_DATAMODE_PULL); 
     700  player->SetReplaceRule( 1 , PLAYER_MSGTYPE_DATA, -1 , -1) ; 
    665701 
    666702  PlayerCc::BumperProxy pp(player,data->index); 
    … …  
    724760             
    725761        } 
     762#if DEBUG 
    726763      else{ 
    727764        yarp::os::Bottle& B = data->samgarPort->prepare();        // prepare the bottle/port 
    … …  
    732769        data->samgarPort->write(); 
    733770      } 
     771#endif 
    734772    } // while(1) 
    735773  return NULL; 
Note: See TracChangeset for help on using the changeset viewer.