source: flair-src/trunk/lib/FlairSensorActuator/src/HokuyoUTM30Lx.cpp @ 3

Last change on this file since 3 was 3, checked in by Sanahuja Guillaume, 6 years ago

sensoractuator

File size: 7.4 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5
6//  created:    2013/08/23
7//  filename:   HokuyoUTM30Lx.cpp
8//
9//  author:     Cesar Richard
10//              Copyright Heudiasyc UMR UTC/CNRS 7253
11//
12//  version:    $Id: $
13//
14//  purpose:    classe intégrant le telemetre laser Hokuyo UTM 30lx
15//
16//
17/*********************************************************************/
18
19#include "HokuyoUTM30Lx.h"
20#include <SerialPort.h>
21#include <FrameworkManager.h>
22#include <RangeFinderPlot.h>
23#include <cvmatrix.h>
24#include <Tab.h>
25#include <sstream>
26#include <iomanip>
27#include <string.h>
28#include <iostream>
29
30using namespace std;
31using namespace flair::core;
32using namespace flair::gui;
33
34namespace flair
35{
36namespace sensor
37{
38
39HokuyoUTM30Lx::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
52HokuyoUTM30Lx::~HokuyoUTM30Lx()
53{
54    delete main_tab;
55    SafeStop();
56    Join();
57}
58
59void 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        }
117    }
118    /** fin running loop **/
119    stopLaser();
120    WarnUponSwitches(false);
121}
122
123
124void 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
148bool 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
152void 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
165void HokuyoUTM30Lx::stopLaser(){
166    sendCommand("QT");
167}
168
169void HokuyoUTM30Lx::resetConfig(){
170    sendCommand("RS");
171}
172
173vector<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
180int 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];
185    }
186    value &= 0x3f;
187    value += 0x30;
188    return value;
189}
190
191float HokuyoUTM30Lx::decode2car(const char* data){
192    return ((data[0]-0x30)<<6)|((data[1]-0x30));
193}
194float HokuyoUTM30Lx::decode3car(const char* data){
195    return ((data[0]-0x30)<<12)|((data[1]-0x30)<<6)|((data[2]-0x30));
196}
197float HokuyoUTM30Lx::decode4car(const char* data){
198    return ((data[0]-0x30)<<18)|((data[1]-0x30)<<12)|((data[2]-0x30)<<6)|((data[3]-0x30));
199}
200
201string 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
212void 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    }
244cvmatrix* HokuyoUTM30Lx::getDatas(){
245    return output;
246}
247
248void HokuyoUTM30Lx::UseDefaultPlot(void)
249{
250    plot=new RangeFinderPlot(main_tab->NewRow(),"plot","x",-100,100,"y",-0,100,output,-45,225,output->Rows());
251}
252
253} // end namespace sensor
254} // end namespace framewor
Note: See TracBrowser for help on using the repository browser.