[3] | 1 | // %flair:license{
|
---|
[15] | 2 | // This file is part of the Flair framework distributed under the
|
---|
| 3 | // CECILL-C License, Version 1.0.
|
---|
[3] | 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>
|
---|
[214] | 23 | #include <Matrix.h>
|
---|
[3] | 24 | #include <Tab.h>
|
---|
| 25 | #include <sstream>
|
---|
| 26 | #include <iomanip>
|
---|
| 27 | #include <string.h>
|
---|
| 28 | #include <iostream>
|
---|
| 29 |
|
---|
| 30 | using namespace std;
|
---|
| 31 | using namespace flair::core;
|
---|
| 32 | using namespace flair::gui;
|
---|
| 33 |
|
---|
[15] | 34 | namespace flair {
|
---|
| 35 | namespace sensor {
|
---|
[3] | 36 |
|
---|
[137] | 37 | HokuyoUTM30Lx::HokuyoUTM30Lx(string name,
|
---|
[15] | 38 | SerialPort *serialport, uint8_t priority)
|
---|
[137] | 39 | : LaserRangeFinder(name), Thread(getFrameworkManager(), name, priority) {
|
---|
| 40 | main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
|
---|
[15] | 41 | cvmatrix_descriptor *desc = new cvmatrix_descriptor(1081, 1);
|
---|
[214] | 42 | output = new Matrix((IODevice *)this, desc, SignedIntegerType(16));
|
---|
[148] | 43 | delete desc;
|
---|
[3] | 44 |
|
---|
[15] | 45 | bufRetMut =
|
---|
| 46 | new Mutex(reinterpret_cast<Object *>(this), (string)name + "BufRetMut");
|
---|
| 47 | sendingCmdMut = new Mutex(reinterpret_cast<Object *>(this),
|
---|
| 48 | (string)name + "SendingCmdMut");
|
---|
[3] | 49 |
|
---|
[15] | 50 | this->serialport = serialport;
|
---|
| 51 | serialport->SetRxTimeout(100000000);
|
---|
[157] | 52 |
|
---|
| 53 | SetIsReady(true);
|
---|
[3] | 54 | }
|
---|
| 55 |
|
---|
[15] | 56 | HokuyoUTM30Lx::~HokuyoUTM30Lx() {
|
---|
| 57 | delete main_tab;
|
---|
| 58 | SafeStop();
|
---|
| 59 | Join();
|
---|
[3] | 60 | }
|
---|
| 61 |
|
---|
[15] | 62 | void 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;
|
---|
[3] | 70 |
|
---|
[15] | 71 | /** Fin init **/
|
---|
| 72 | resetConfig();
|
---|
| 73 | startLaser();
|
---|
| 74 | Printf("Laser started\r\n");
|
---|
[3] | 75 |
|
---|
[15] | 76 | /** Debut running loop **/
|
---|
| 77 | WarnUponSwitches(true);
|
---|
[3] | 78 |
|
---|
[15] | 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;
|
---|
[3] | 92 |
|
---|
[15] | 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);
|
---|
[3] | 117 | }
|
---|
[15] | 118 | }
|
---|
| 119 | /** fin running loop **/
|
---|
| 120 | stopLaser();
|
---|
| 121 | WarnUponSwitches(false);
|
---|
[3] | 122 | }
|
---|
| 123 |
|
---|
[15] | 124 | void 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();
|
---|
[3] | 146 | }
|
---|
| 147 |
|
---|
[15] | 148 | bool 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);
|
---|
[3] | 151 | }
|
---|
| 152 |
|
---|
[15] | 153 | void 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
|
---|
[3] | 164 | }
|
---|
| 165 |
|
---|
[15] | 166 | void HokuyoUTM30Lx::stopLaser() { sendCommand("QT"); }
|
---|
[3] | 167 |
|
---|
[15] | 168 | void HokuyoUTM30Lx::resetConfig() { sendCommand("RS"); }
|
---|
[3] | 169 |
|
---|
[15] | 170 | vector<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;
|
---|
[3] | 176 | }
|
---|
| 177 |
|
---|
[15] | 178 | int 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;
|
---|
[3] | 187 | }
|
---|
| 188 |
|
---|
[15] | 189 | float HokuyoUTM30Lx::decode2car(const char *data) {
|
---|
| 190 | return ((data[0] - 0x30) << 6) | ((data[1] - 0x30));
|
---|
[3] | 191 | }
|
---|
[15] | 192 | float HokuyoUTM30Lx::decode3car(const char *data) {
|
---|
| 193 | return ((data[0] - 0x30) << 12) | ((data[1] - 0x30) << 6) |
|
---|
| 194 | ((data[2] - 0x30));
|
---|
[3] | 195 | }
|
---|
[15] | 196 | float HokuyoUTM30Lx::decode4car(const char *data) {
|
---|
| 197 | return ((data[0] - 0x30) << 18) | ((data[1] - 0x30) << 12) |
|
---|
| 198 | ((data[2] - 0x30) << 6) | ((data[3] - 0x30));
|
---|
[3] | 199 | }
|
---|
| 200 |
|
---|
[15] | 201 | string 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;
|
---|
[3] | 210 | }
|
---|
| 211 |
|
---|
[15] | 212 | void 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
|
---|
[3] | 247 | }
|
---|
[214] | 248 | Matrix *HokuyoUTM30Lx::getDatas() { return output; }
|
---|
[3] | 249 |
|
---|
[15] | 250 | void HokuyoUTM30Lx::UseDefaultPlot(void) {
|
---|
| 251 | plot = new RangeFinderPlot(main_tab->NewRow(), "plot", "x", -100, 100, "y",
|
---|
| 252 | -0, 100, output, -45, 225, output->Rows());
|
---|
[3] | 253 | }
|
---|
| 254 |
|
---|
| 255 | } // end namespace sensor
|
---|
[170] | 256 | } // end namespace flair
|
---|