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

Last change on this file since 15 was 15, checked in by Bayard Gildas, 6 years ago

sources reformatted with flair-format-dir script

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(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);
114    }
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);
129    }
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());
250}
251
252} // end namespace sensor
253} // end namespace framewor
Note: See TracBrowser for help on using the repository browser.