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

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

m

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