Ignore:
Timestamp:
04/08/16 15:40:57 (8 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.cpp

    r3 r15  
    3232using namespace flair::gui;
    3333
    34 namespace flair
    35 {
    36 namespace sensor
    37 {
    38 
    39 HokuyoUTM30Lx::HokuyoUTM30Lx(const FrameworkManager* parent,string name,SerialPort *serialport,uint8_t priority) : LaserRangeFinder(parent,name), Thread(parent,name,priority)
    40 {
    41     main_tab=new Tab(parent->GetTabWidget(),name);
    42         cvmatrix_descriptor* desc=new cvmatrix_descriptor(1081,1);
    43     output=new cvmatrix((IODevice*)this,desc,SignedIntegerType(16));
    44 
    45     bufRetMut = new Mutex(reinterpret_cast<Object*>(this),(string)name+"BufRetMut");
    46     sendingCmdMut = new Mutex(reinterpret_cast<Object*>(this),(string)name+"SendingCmdMut");
    47 
    48     this->serialport=serialport;
    49     serialport->SetRxTimeout(100000000);
    50 }
    51 
    52 HokuyoUTM30Lx::~HokuyoUTM30Lx()
    53 {
    54     delete main_tab;
    55     SafeStop();
    56     Join();
    57 }
    58 
    59 void HokuyoUTM30Lx::Run(void)
    60 {
    61         /** Debut init **/
    62     char c,lastC;
    63     int startStep;
    64     bool mustDecode=false;
    65     stringstream ss;
    66     vector<string> ret;
    67     lastC=c=0;
    68 
    69         /** Fin init **/
    70         resetConfig();
    71         startLaser();
    72         Printf("Laser started\r\n");
    73 
    74     /** Debut running loop **/
    75     WarnUponSwitches(true);
    76 
    77     while(!ToBeStopped())
    78     {
    79         ss.clear();
    80         ss.str("");
    81         do{
    82             lastC=c;
    83             serialport->Read(&c,1);
    84             ss << c;
    85         }while(!(c=='\n'&&lastC=='\n'));
    86         ret=explode(ss.str(),'\n');
    87         if(!checkSum(ret.at(1)))
    88             perror("Bad Checksum !");
    89         if(ret.at(0).substr(0,2)=="MD")
    90             mustDecode=true;
    91 
    92         startStep=atoi(ret.at(0).substr(2,4).c_str());
    93         ret.erase(ret.begin());
    94         bufRetMut->GetMutex();
    95         bufRet.push(ret.at(0).substr(0,2));
    96         bufRetMut->ReleaseMutex();
    97         if(mustDecode){
    98             mustDecode=false;
    99             ss.clear();
    100             ss.str("");
    101             do{
    102                 lastC=c;
    103                 serialport->Read(&c,1);
    104                 ss << c;
    105             }while(!(c=='\n'&&lastC=='\n'));
    106             ret=explode(ss.str(),'\n');
    107             ret.erase(ret.begin());
    108             ret.erase(ret.begin());
    109             if(ret.at(0).size()==5)
    110                 if(!checkSum(ret.at(0)))
    111                     perror("TimeStamp checksum error!");
    112             ret.erase(ret.begin());
    113             Printf("!post\r\n");
    114             ret.pop_back();
    115             decodeDatas(ret, startStep);
    116         }
     34namespace flair {
     35namespace sensor {
     36
     37HokuyoUTM30Lx::HokuyoUTM30Lx(const FrameworkManager *parent, string name,
     38                             SerialPort *serialport, uint8_t priority)
     39    : LaserRangeFinder(parent, name), Thread(parent, name, priority) {
     40  main_tab = new Tab(parent->GetTabWidget(), name);
     41  cvmatrix_descriptor *desc = new cvmatrix_descriptor(1081, 1);
     42  output = new cvmatrix((IODevice *)this, desc, SignedIntegerType(16));
     43
     44  bufRetMut =
     45      new Mutex(reinterpret_cast<Object *>(this), (string)name + "BufRetMut");
     46  sendingCmdMut = new Mutex(reinterpret_cast<Object *>(this),
     47                            (string)name + "SendingCmdMut");
     48
     49  this->serialport = serialport;
     50  serialport->SetRxTimeout(100000000);
     51}
     52
     53HokuyoUTM30Lx::~HokuyoUTM30Lx() {
     54  delete main_tab;
     55  SafeStop();
     56  Join();
     57}
     58
     59void HokuyoUTM30Lx::Run(void) {
     60  /** Debut init **/
     61  char c, lastC;
     62  int startStep;
     63  bool mustDecode = false;
     64  stringstream ss;
     65  vector<string> ret;
     66  lastC = c = 0;
     67
     68  /** Fin init **/
     69  resetConfig();
     70  startLaser();
     71  Printf("Laser started\r\n");
     72
     73  /** Debut running loop **/
     74  WarnUponSwitches(true);
     75
     76  while (!ToBeStopped()) {
     77    ss.clear();
     78    ss.str("");
     79    do {
     80      lastC = c;
     81      serialport->Read(&c, 1);
     82      ss << c;
     83    } while (!(c == '\n' && lastC == '\n'));
     84    ret = explode(ss.str(), '\n');
     85    if (!checkSum(ret.at(1)))
     86      perror("Bad Checksum !");
     87    if (ret.at(0).substr(0, 2) == "MD")
     88      mustDecode = true;
     89
     90    startStep = atoi(ret.at(0).substr(2, 4).c_str());
     91    ret.erase(ret.begin());
     92    bufRetMut->GetMutex();
     93    bufRet.push(ret.at(0).substr(0, 2));
     94    bufRetMut->ReleaseMutex();
     95    if (mustDecode) {
     96      mustDecode = false;
     97      ss.clear();
     98      ss.str("");
     99      do {
     100        lastC = c;
     101        serialport->Read(&c, 1);
     102        ss << c;
     103      } while (!(c == '\n' && lastC == '\n'));
     104      ret = explode(ss.str(), '\n');
     105      ret.erase(ret.begin());
     106      ret.erase(ret.begin());
     107      if (ret.at(0).size() == 5)
     108        if (!checkSum(ret.at(0)))
     109          perror("TimeStamp checksum error!");
     110      ret.erase(ret.begin());
     111      Printf("!post\r\n");
     112      ret.pop_back();
     113      decodeDatas(ret, startStep);
    117114    }
    118     /** fin running loop **/
    119     stopLaser();
    120     WarnUponSwitches(false);
    121 }
    122 
    123 
    124 void HokuyoUTM30Lx::decodeDatas(vector<string> datas, int startStep){
    125             stringstream ss;
    126             for (unsigned i=0; i<datas.size(); i++){
    127                 if(datas.at(i).size()!=0){
    128                     if(!checkSum(datas.at(i)))
    129                         perror("Datas checksum error !");
    130                     datas.at(i).erase(datas.at(i).end()-1);
    131                     ss << datas.at(i);
    132                 }
    133             }
    134             output->GetMutex();
    135             for(unsigned i=0;i<1080;i++){
    136             //TODO: remettre -1 pour les points non lus
    137                 output->SetValueNoMutex(i,0,0);
    138             }
    139             for(unsigned i=0;i<ss.str().size();i+=3){
    140                 output->SetValueNoMutex(startStep+(i/3),0,decode3car(ss.str().substr(i,3).c_str()));
    141             }
    142             UpdateFrom(output);
    143             ProcessUpdate(output);
    144             output->ReleaseMutex();
    145 
    146 }
    147 
    148 bool HokuyoUTM30Lx::checkSum(string data){
    149     return (char)encodeSum(data.substr(0,data.size()-1).c_str(),data.size()-1)==data.at(data.size()-1);
    150 }
    151 
    152 void HokuyoUTM30Lx::startLaser(){
    153     string ret = sendCommand("BM");
    154     #ifdef VERBOSE
    155         if(ret == "00"){
    156             cout << "BM: Alright !" << endl;
    157         }else if(ret == "01"){
    158             cout << "BM: Laser malfunction !" << endl;
    159         }else if(ret == "02"){
    160             cout << "BM: Laser already started !" << endl;
    161         }
    162     #endif
    163 }
    164 
    165 void HokuyoUTM30Lx::stopLaser(){
    166     sendCommand("QT");
    167 }
    168 
    169 void HokuyoUTM30Lx::resetConfig(){
    170     sendCommand("RS");
    171 }
    172 
    173 vector<string> HokuyoUTM30Lx::explode(const string str, char delimiter){
    174     istringstream split(str);
    175     vector<string> tokens;
    176     for (string each; getline(split, each, delimiter); tokens.push_back(each));
    177     return tokens;
    178 }
    179 
    180 int HokuyoUTM30Lx::encodeSum(const char* code, int byte) {
    181     unsigned char value = 0;
    182     int i;
    183     for (i = 0; i < byte; ++i) {
    184         value+=code[i];
     115  }
     116  /** fin running loop **/
     117  stopLaser();
     118  WarnUponSwitches(false);
     119}
     120
     121void HokuyoUTM30Lx::decodeDatas(vector<string> datas, int startStep) {
     122  stringstream ss;
     123  for (unsigned i = 0; i < datas.size(); i++) {
     124    if (datas.at(i).size() != 0) {
     125      if (!checkSum(datas.at(i)))
     126        perror("Datas checksum error !");
     127      datas.at(i).erase(datas.at(i).end() - 1);
     128      ss << datas.at(i);
    185129    }
    186     value &= 0x3f;
    187     value += 0x30;
    188     return value;
    189 }
    190 
    191 float HokuyoUTM30Lx::decode2car(const char* data){
    192     return ((data[0]-0x30)<<6)|((data[1]-0x30));
    193 }
    194 float HokuyoUTM30Lx::decode3car(const char* data){
    195     return ((data[0]-0x30)<<12)|((data[1]-0x30)<<6)|((data[2]-0x30));
    196 }
    197 float HokuyoUTM30Lx::decode4car(const char* data){
    198     return ((data[0]-0x30)<<18)|((data[1]-0x30)<<12)|((data[2]-0x30)<<6)|((data[3]-0x30));
    199 }
    200 
    201 string HokuyoUTM30Lx::sendCommand(string command){
    202     char c;
    203     string ret="00";
    204     c='\n';
    205     sendingCmdMut->GetMutex();
    206     serialport->Write(command.c_str(),command.size());
    207     serialport->Write(&c,1);
    208     sendingCmdMut->ReleaseMutex();
    209     return ret;
    210 }
    211 
    212 void HokuyoUTM30Lx::getMesure(int startStep, int endStep,int clusterCount, int interval, int scanNumber){
    213     stringstream ss;
    214     string ret;
    215     ss << "MD" << std::setfill('0') << std::setw(4) << startStep << std::setw(4) << endStep << std::setw(2) << clusterCount << std::setw(1) << interval << std::setw(2) << scanNumber;
    216     ret = sendCommand(ss.str());
    217     #ifdef VERBOSE
    218         if(ret == "00"){
    219             cout << "MD: Alright !" << endl;
    220         }else if(ret == "01"){
    221             cout << "MD: Laser malfunction !" << endl;
    222         }else if(ret == "02"){
    223             cout << "MD: End Step has non-numeric value !" << endl;
    224         }else if(ret == "03"){
    225             cout << "MD: Cluster Count has non-numeric value !" << endl;
    226         }else if(ret == "04"){
    227             cout << "MD: End Step is out of range !" << endl;
    228         }else if(ret == "05"){
    229             cout << "MD: End Step is smaller than Starting Step !" << endl;
    230         }else if(ret == "06"){
    231             cout << "MD: Scan Interval has non-numeric value !" << endl;
    232         }else if(ret == "07"){
    233             cout << "MD: Number of Scan has non-numeric value !" << endl;
    234         }else if(ret == "98"){
    235             cout << "MD: Resumption of process after confirming normal HokuyoUTM30Lx operation." << endl;
    236         }else{
    237             /* Concerne :
    238             21~49 --- Processing stopped to verify the error.
    239             50~97 --- Hardware trouble (such as laser, motor malfunctions etc.)*/
    240             cout << "MD: Malfunction !" << endl;
    241         }
    242     #endif
    243     }
    244 cvmatrix* HokuyoUTM30Lx::getDatas(){
    245     return output;
    246 }
    247 
    248 void HokuyoUTM30Lx::UseDefaultPlot(void)
    249 {
    250     plot=new RangeFinderPlot(main_tab->NewRow(),"plot","x",-100,100,"y",-0,100,output,-45,225,output->Rows());
     130  }
     131  output->GetMutex();
     132  for (unsigned i = 0; i < 1080; i++) {
     133    // TODO: remettre -1 pour les points non lus
     134    output->SetValueNoMutex(i, 0, 0);
     135  }
     136  for (unsigned i = 0; i < ss.str().size(); i += 3) {
     137    output->SetValueNoMutex(startStep + (i / 3), 0,
     138                            decode3car(ss.str().substr(i, 3).c_str()));
     139  }
     140  UpdateFrom(output);
     141  ProcessUpdate(output);
     142  output->ReleaseMutex();
     143}
     144
     145bool HokuyoUTM30Lx::checkSum(string data) {
     146  return (char)encodeSum(data.substr(0, data.size() - 1).c_str(),
     147                         data.size() - 1) == data.at(data.size() - 1);
     148}
     149
     150void HokuyoUTM30Lx::startLaser() {
     151  string ret = sendCommand("BM");
     152#ifdef VERBOSE
     153  if (ret == "00") {
     154    cout << "BM: Alright !" << endl;
     155  } else if (ret == "01") {
     156    cout << "BM: Laser malfunction !" << endl;
     157  } else if (ret == "02") {
     158    cout << "BM: Laser already started !" << endl;
     159  }
     160#endif
     161}
     162
     163void HokuyoUTM30Lx::stopLaser() { sendCommand("QT"); }
     164
     165void HokuyoUTM30Lx::resetConfig() { sendCommand("RS"); }
     166
     167vector<string> HokuyoUTM30Lx::explode(const string str, char delimiter) {
     168  istringstream split(str);
     169  vector<string> tokens;
     170  for (string each; getline(split, each, delimiter); tokens.push_back(each))
     171    ;
     172  return tokens;
     173}
     174
     175int HokuyoUTM30Lx::encodeSum(const char *code, int byte) {
     176  unsigned char value = 0;
     177  int i;
     178  for (i = 0; i < byte; ++i) {
     179    value += code[i];
     180  }
     181  value &= 0x3f;
     182  value += 0x30;
     183  return value;
     184}
     185
     186float HokuyoUTM30Lx::decode2car(const char *data) {
     187  return ((data[0] - 0x30) << 6) | ((data[1] - 0x30));
     188}
     189float HokuyoUTM30Lx::decode3car(const char *data) {
     190  return ((data[0] - 0x30) << 12) | ((data[1] - 0x30) << 6) |
     191         ((data[2] - 0x30));
     192}
     193float HokuyoUTM30Lx::decode4car(const char *data) {
     194  return ((data[0] - 0x30) << 18) | ((data[1] - 0x30) << 12) |
     195         ((data[2] - 0x30) << 6) | ((data[3] - 0x30));
     196}
     197
     198string HokuyoUTM30Lx::sendCommand(string command) {
     199  char c;
     200  string ret = "00";
     201  c = '\n';
     202  sendingCmdMut->GetMutex();
     203  serialport->Write(command.c_str(), command.size());
     204  serialport->Write(&c, 1);
     205  sendingCmdMut->ReleaseMutex();
     206  return ret;
     207}
     208
     209void HokuyoUTM30Lx::getMesure(int startStep, int endStep, int clusterCount,
     210                              int interval, int scanNumber) {
     211  stringstream ss;
     212  string ret;
     213  ss << "MD" << std::setfill('0') << std::setw(4) << startStep << std::setw(4)
     214     << endStep << std::setw(2) << clusterCount << std::setw(1) << interval
     215     << std::setw(2) << scanNumber;
     216  ret = sendCommand(ss.str());
     217#ifdef VERBOSE
     218  if (ret == "00") {
     219    cout << "MD: Alright !" << endl;
     220  } else if (ret == "01") {
     221    cout << "MD: Laser malfunction !" << endl;
     222  } else if (ret == "02") {
     223    cout << "MD: End Step has non-numeric value !" << endl;
     224  } else if (ret == "03") {
     225    cout << "MD: Cluster Count has non-numeric value !" << endl;
     226  } else if (ret == "04") {
     227    cout << "MD: End Step is out of range !" << endl;
     228  } else if (ret == "05") {
     229    cout << "MD: End Step is smaller than Starting Step !" << endl;
     230  } else if (ret == "06") {
     231    cout << "MD: Scan Interval has non-numeric value !" << endl;
     232  } else if (ret == "07") {
     233    cout << "MD: Number of Scan has non-numeric value !" << endl;
     234  } else if (ret == "98") {
     235    cout << "MD: Resumption of process after confirming normal HokuyoUTM30Lx "
     236            "operation." << endl;
     237  } else {
     238    /* Concerne :
     239    21~49 --- Processing stopped to verify the error.
     240    50~97 --- Hardware trouble (such as laser, motor malfunctions etc.)*/
     241    cout << "MD: Malfunction !" << endl;
     242  }
     243#endif
     244}
     245cvmatrix *HokuyoUTM30Lx::getDatas() { return output; }
     246
     247void HokuyoUTM30Lx::UseDefaultPlot(void) {
     248  plot = new RangeFinderPlot(main_tab->NewRow(), "plot", "x", -100, 100, "y",
     249                             -0, 100, output, -45, 225, output->Rows());
    251250}
    252251
Note: See TracChangeset for help on using the changeset viewer.