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

Last change on this file since 157 was 157, checked in by Sanahuja Guillaume, 5 years ago

iadded isready to iodevice:
avoid problem of imu not ready in ardrone2

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