Index: trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.cpp	(revision 3)
+++ trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.cpp	(revision 15)
@@ -32,221 +32,220 @@
 using namespace flair::gui;
 
-namespace flair
-{
-namespace sensor
-{
-
-HokuyoUTM30Lx::HokuyoUTM30Lx(const FrameworkManager* parent,string name,SerialPort *serialport,uint8_t priority) : LaserRangeFinder(parent,name), Thread(parent,name,priority)
-{
-    main_tab=new Tab(parent->GetTabWidget(),name);
-	cvmatrix_descriptor* desc=new cvmatrix_descriptor(1081,1);
-    output=new cvmatrix((IODevice*)this,desc,SignedIntegerType(16));
-
-    bufRetMut = new Mutex(reinterpret_cast<Object*>(this),(string)name+"BufRetMut");
-    sendingCmdMut = new Mutex(reinterpret_cast<Object*>(this),(string)name+"SendingCmdMut");
-
-    this->serialport=serialport;
-    serialport->SetRxTimeout(100000000);
-}
-
-HokuyoUTM30Lx::~HokuyoUTM30Lx()
-{
-    delete main_tab;
-    SafeStop();
-    Join();
-}
-
-void HokuyoUTM30Lx::Run(void)
-{
-	/** Debut init **/
-    char c,lastC;
-    int startStep;
-    bool mustDecode=false;
-    stringstream ss;
-    vector<string> ret;
-    lastC=c=0;
-
-	/** Fin init **/
-	resetConfig();
-	startLaser();
-	Printf("Laser started\r\n");
-
-    /** Debut running loop **/
-    WarnUponSwitches(true);
-
-    while(!ToBeStopped())
-    {
-        ss.clear();
-        ss.str("");
-        do{
-            lastC=c;
-            serialport->Read(&c,1);
-            ss << c;
-        }while(!(c=='\n'&&lastC=='\n'));
-        ret=explode(ss.str(),'\n');
-        if(!checkSum(ret.at(1)))
-            perror("Bad Checksum !");
-        if(ret.at(0).substr(0,2)=="MD")
-            mustDecode=true;
-
-        startStep=atoi(ret.at(0).substr(2,4).c_str());
-        ret.erase(ret.begin());
-        bufRetMut->GetMutex();
-        bufRet.push(ret.at(0).substr(0,2));
-        bufRetMut->ReleaseMutex();
-        if(mustDecode){
-            mustDecode=false;
-            ss.clear();
-            ss.str("");
-            do{
-                lastC=c;
-                serialport->Read(&c,1);
-                ss << c;
-            }while(!(c=='\n'&&lastC=='\n'));
-            ret=explode(ss.str(),'\n');
-            ret.erase(ret.begin());
-            ret.erase(ret.begin());
-            if(ret.at(0).size()==5)
-                if(!checkSum(ret.at(0)))
-                    perror("TimeStamp checksum error!");
-            ret.erase(ret.begin());
-            Printf("!post\r\n");
-            ret.pop_back();
-            decodeDatas(ret, startStep);
-        }
+namespace flair {
+namespace sensor {
+
+HokuyoUTM30Lx::HokuyoUTM30Lx(const FrameworkManager *parent, string name,
+                             SerialPort *serialport, uint8_t priority)
+    : LaserRangeFinder(parent, name), Thread(parent, name, priority) {
+  main_tab = new Tab(parent->GetTabWidget(), name);
+  cvmatrix_descriptor *desc = new cvmatrix_descriptor(1081, 1);
+  output = new cvmatrix((IODevice *)this, desc, SignedIntegerType(16));
+
+  bufRetMut =
+      new Mutex(reinterpret_cast<Object *>(this), (string)name + "BufRetMut");
+  sendingCmdMut = new Mutex(reinterpret_cast<Object *>(this),
+                            (string)name + "SendingCmdMut");
+
+  this->serialport = serialport;
+  serialport->SetRxTimeout(100000000);
+}
+
+HokuyoUTM30Lx::~HokuyoUTM30Lx() {
+  delete main_tab;
+  SafeStop();
+  Join();
+}
+
+void HokuyoUTM30Lx::Run(void) {
+  /** Debut init **/
+  char c, lastC;
+  int startStep;
+  bool mustDecode = false;
+  stringstream ss;
+  vector<string> ret;
+  lastC = c = 0;
+
+  /** Fin init **/
+  resetConfig();
+  startLaser();
+  Printf("Laser started\r\n");
+
+  /** Debut running loop **/
+  WarnUponSwitches(true);
+
+  while (!ToBeStopped()) {
+    ss.clear();
+    ss.str("");
+    do {
+      lastC = c;
+      serialport->Read(&c, 1);
+      ss << c;
+    } while (!(c == '\n' && lastC == '\n'));
+    ret = explode(ss.str(), '\n');
+    if (!checkSum(ret.at(1)))
+      perror("Bad Checksum !");
+    if (ret.at(0).substr(0, 2) == "MD")
+      mustDecode = true;
+
+    startStep = atoi(ret.at(0).substr(2, 4).c_str());
+    ret.erase(ret.begin());
+    bufRetMut->GetMutex();
+    bufRet.push(ret.at(0).substr(0, 2));
+    bufRetMut->ReleaseMutex();
+    if (mustDecode) {
+      mustDecode = false;
+      ss.clear();
+      ss.str("");
+      do {
+        lastC = c;
+        serialport->Read(&c, 1);
+        ss << c;
+      } while (!(c == '\n' && lastC == '\n'));
+      ret = explode(ss.str(), '\n');
+      ret.erase(ret.begin());
+      ret.erase(ret.begin());
+      if (ret.at(0).size() == 5)
+        if (!checkSum(ret.at(0)))
+          perror("TimeStamp checksum error!");
+      ret.erase(ret.begin());
+      Printf("!post\r\n");
+      ret.pop_back();
+      decodeDatas(ret, startStep);
     }
-    /** fin running loop **/
-    stopLaser();
-    WarnUponSwitches(false);
-}
-
-
-void HokuyoUTM30Lx::decodeDatas(vector<string> datas, int startStep){
-            stringstream ss;
-            for (unsigned i=0; i<datas.size(); i++){
-                if(datas.at(i).size()!=0){
-                    if(!checkSum(datas.at(i)))
-                        perror("Datas checksum error !");
-                    datas.at(i).erase(datas.at(i).end()-1);
-                    ss << datas.at(i);
-                }
-            }
-            output->GetMutex();
-            for(unsigned i=0;i<1080;i++){
-            //TODO: remettre -1 pour les points non lus
-                output->SetValueNoMutex(i,0,0);
-            }
-            for(unsigned i=0;i<ss.str().size();i+=3){
-                output->SetValueNoMutex(startStep+(i/3),0,decode3car(ss.str().substr(i,3).c_str()));
-            }
-            UpdateFrom(output);
-            ProcessUpdate(output);
-            output->ReleaseMutex();
-
-}
-
-bool HokuyoUTM30Lx::checkSum(string data){
-    return (char)encodeSum(data.substr(0,data.size()-1).c_str(),data.size()-1)==data.at(data.size()-1);
-}
-
-void HokuyoUTM30Lx::startLaser(){
-    string ret = sendCommand("BM");
-    #ifdef VERBOSE
-        if(ret == "00"){
-            cout << "BM: Alright !" << endl;
-        }else if(ret == "01"){
-            cout << "BM: Laser malfunction !" << endl;
-        }else if(ret == "02"){
-            cout << "BM: Laser already started !" << endl;
-        }
-    #endif
-}
-
-void HokuyoUTM30Lx::stopLaser(){
-    sendCommand("QT");
-}
-
-void HokuyoUTM30Lx::resetConfig(){
-    sendCommand("RS");
-}
-
-vector<string> HokuyoUTM30Lx::explode(const string str, char delimiter){
-    istringstream split(str);
-    vector<string> tokens;
-    for (string each; getline(split, each, delimiter); tokens.push_back(each));
-    return tokens;
-}
-
-int HokuyoUTM30Lx::encodeSum(const char* code, int byte) {
-    unsigned char value = 0;
-    int i;
-    for (i = 0; i < byte; ++i) {
-        value+=code[i];
+  }
+  /** fin running loop **/
+  stopLaser();
+  WarnUponSwitches(false);
+}
+
+void HokuyoUTM30Lx::decodeDatas(vector<string> datas, int startStep) {
+  stringstream ss;
+  for (unsigned i = 0; i < datas.size(); i++) {
+    if (datas.at(i).size() != 0) {
+      if (!checkSum(datas.at(i)))
+        perror("Datas checksum error !");
+      datas.at(i).erase(datas.at(i).end() - 1);
+      ss << datas.at(i);
     }
-    value &= 0x3f;
-    value += 0x30;
-    return value;
-}
-
-float HokuyoUTM30Lx::decode2car(const char* data){
-    return ((data[0]-0x30)<<6)|((data[1]-0x30));
-}
-float HokuyoUTM30Lx::decode3car(const char* data){
-    return ((data[0]-0x30)<<12)|((data[1]-0x30)<<6)|((data[2]-0x30));
-}
-float HokuyoUTM30Lx::decode4car(const char* data){
-    return ((data[0]-0x30)<<18)|((data[1]-0x30)<<12)|((data[2]-0x30)<<6)|((data[3]-0x30));
-}
-
-string HokuyoUTM30Lx::sendCommand(string command){
-    char c;
-    string ret="00";
-    c='\n';
-    sendingCmdMut->GetMutex();
-    serialport->Write(command.c_str(),command.size());
-    serialport->Write(&c,1);
-    sendingCmdMut->ReleaseMutex();
-    return ret;
-}
-
-void HokuyoUTM30Lx::getMesure(int startStep, int endStep,int clusterCount, int interval, int scanNumber){
-    stringstream ss;
-    string ret;
-    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;
-    ret = sendCommand(ss.str());
-    #ifdef VERBOSE
-        if(ret == "00"){
-            cout << "MD: Alright !" << endl;
-        }else if(ret == "01"){
-            cout << "MD: Laser malfunction !" << endl;
-        }else if(ret == "02"){
-            cout << "MD: End Step has non-numeric value !" << endl;
-        }else if(ret == "03"){
-            cout << "MD: Cluster Count has non-numeric value !" << endl;
-        }else if(ret == "04"){
-            cout << "MD: End Step is out of range !" << endl;
-        }else if(ret == "05"){
-            cout << "MD: End Step is smaller than Starting Step !" << endl;
-        }else if(ret == "06"){
-            cout << "MD: Scan Interval has non-numeric value !" << endl;
-        }else if(ret == "07"){
-            cout << "MD: Number of Scan has non-numeric value !" << endl;
-        }else if(ret == "98"){
-            cout << "MD: Resumption of process after confirming normal HokuyoUTM30Lx operation." << endl;
-        }else{
-            /* Concerne :
-            21~49 --- Processing stopped to verify the error.
-            50~97 --- Hardware trouble (such as laser, motor malfunctions etc.)*/
-            cout << "MD: Malfunction !" << endl;
-        }
-    #endif
-    }
-cvmatrix* HokuyoUTM30Lx::getDatas(){
-    return output;
-}
-
-void HokuyoUTM30Lx::UseDefaultPlot(void)
-{
-    plot=new RangeFinderPlot(main_tab->NewRow(),"plot","x",-100,100,"y",-0,100,output,-45,225,output->Rows());
+  }
+  output->GetMutex();
+  for (unsigned i = 0; i < 1080; i++) {
+    // TODO: remettre -1 pour les points non lus
+    output->SetValueNoMutex(i, 0, 0);
+  }
+  for (unsigned i = 0; i < ss.str().size(); i += 3) {
+    output->SetValueNoMutex(startStep + (i / 3), 0,
+                            decode3car(ss.str().substr(i, 3).c_str()));
+  }
+  UpdateFrom(output);
+  ProcessUpdate(output);
+  output->ReleaseMutex();
+}
+
+bool HokuyoUTM30Lx::checkSum(string data) {
+  return (char)encodeSum(data.substr(0, data.size() - 1).c_str(),
+                         data.size() - 1) == data.at(data.size() - 1);
+}
+
+void HokuyoUTM30Lx::startLaser() {
+  string ret = sendCommand("BM");
+#ifdef VERBOSE
+  if (ret == "00") {
+    cout << "BM: Alright !" << endl;
+  } else if (ret == "01") {
+    cout << "BM: Laser malfunction !" << endl;
+  } else if (ret == "02") {
+    cout << "BM: Laser already started !" << endl;
+  }
+#endif
+}
+
+void HokuyoUTM30Lx::stopLaser() { sendCommand("QT"); }
+
+void HokuyoUTM30Lx::resetConfig() { sendCommand("RS"); }
+
+vector<string> HokuyoUTM30Lx::explode(const string str, char delimiter) {
+  istringstream split(str);
+  vector<string> tokens;
+  for (string each; getline(split, each, delimiter); tokens.push_back(each))
+    ;
+  return tokens;
+}
+
+int HokuyoUTM30Lx::encodeSum(const char *code, int byte) {
+  unsigned char value = 0;
+  int i;
+  for (i = 0; i < byte; ++i) {
+    value += code[i];
+  }
+  value &= 0x3f;
+  value += 0x30;
+  return value;
+}
+
+float HokuyoUTM30Lx::decode2car(const char *data) {
+  return ((data[0] - 0x30) << 6) | ((data[1] - 0x30));
+}
+float HokuyoUTM30Lx::decode3car(const char *data) {
+  return ((data[0] - 0x30) << 12) | ((data[1] - 0x30) << 6) |
+         ((data[2] - 0x30));
+}
+float HokuyoUTM30Lx::decode4car(const char *data) {
+  return ((data[0] - 0x30) << 18) | ((data[1] - 0x30) << 12) |
+         ((data[2] - 0x30) << 6) | ((data[3] - 0x30));
+}
+
+string HokuyoUTM30Lx::sendCommand(string command) {
+  char c;
+  string ret = "00";
+  c = '\n';
+  sendingCmdMut->GetMutex();
+  serialport->Write(command.c_str(), command.size());
+  serialport->Write(&c, 1);
+  sendingCmdMut->ReleaseMutex();
+  return ret;
+}
+
+void HokuyoUTM30Lx::getMesure(int startStep, int endStep, int clusterCount,
+                              int interval, int scanNumber) {
+  stringstream ss;
+  string ret;
+  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;
+  ret = sendCommand(ss.str());
+#ifdef VERBOSE
+  if (ret == "00") {
+    cout << "MD: Alright !" << endl;
+  } else if (ret == "01") {
+    cout << "MD: Laser malfunction !" << endl;
+  } else if (ret == "02") {
+    cout << "MD: End Step has non-numeric value !" << endl;
+  } else if (ret == "03") {
+    cout << "MD: Cluster Count has non-numeric value !" << endl;
+  } else if (ret == "04") {
+    cout << "MD: End Step is out of range !" << endl;
+  } else if (ret == "05") {
+    cout << "MD: End Step is smaller than Starting Step !" << endl;
+  } else if (ret == "06") {
+    cout << "MD: Scan Interval has non-numeric value !" << endl;
+  } else if (ret == "07") {
+    cout << "MD: Number of Scan has non-numeric value !" << endl;
+  } else if (ret == "98") {
+    cout << "MD: Resumption of process after confirming normal HokuyoUTM30Lx "
+            "operation." << endl;
+  } else {
+    /* Concerne :
+    21~49 --- Processing stopped to verify the error.
+    50~97 --- Hardware trouble (such as laser, motor malfunctions etc.)*/
+    cout << "MD: Malfunction !" << endl;
+  }
+#endif
+}
+cvmatrix *HokuyoUTM30Lx::getDatas() { return output; }
+
+void HokuyoUTM30Lx::UseDefaultPlot(void) {
+  plot = new RangeFinderPlot(main_tab->NewRow(), "plot", "x", -100, 100, "y",
+                             -0, 100, output, -45, 225, output->Rows());
 }
 
