Changeset 42 in pacpussensors for trunk/Sick
- Timestamp:
- Apr 1, 2014, 2:47:39 PM (11 years ago)
- Location:
- trunk/Sick
- Files:
-
- 9 edited
-
SickComponent.cpp (modified) (7 diffs)
-
SickComponent.h (modified) (1 diff)
-
SickLDMRSData.h (modified) (1 diff)
-
SickLDMRSSensor.cpp (modified) (15 diffs)
-
SickLDMRSSensor.h (modified) (1 diff)
-
SickLMSSensor.cpp (modified) (15 diffs)
-
SickLMSSensor.h (modified) (2 diffs)
-
SickSocket.cpp (modified) (7 diffs)
-
SickSocket.h (modified) (5 diffs)
Legend:
- Unmodified
- Added
- Removed
-
trunk/Sick/SickComponent.cpp
r37 r42 37 37 38 38 39 //////////////////////////////////////////////////////////////////////////40 /// Constructor.41 39 SickComponent::SickComponent(QString name) 42 40 : ComponentBase(name) … … 47 45 } 48 46 49 //////////////////////////////////////////////////////////////////////////50 /// Destructor.51 47 SickComponent::~SickComponent() 52 48 { … … 54 50 } 55 51 56 ////////////////////////////////////////////////////////////////////////// 57 /// Called by the ComponentManager to start the component 52 58 53 void SickComponent::startActivity() 59 54 { … … 63 58 } 64 59 65 ////////////////////////////////////////////////////////////////////////// 66 /// Called by the ComponentManager to stop the component 60 67 61 void SickComponent::stopActivity() 68 62 { … … 72 66 } 73 67 74 ////////////////////////////////////////////////////////////////////////// 75 /// Called by the ComponentManager to pass the XML parameters to the 76 /// component 68 77 69 ComponentBase::COMPONENT_CONFIGURATION SickComponent::configureComponent(XmlComponentConfig config) 78 70 { 79 71 /* 80 * <Sick Component sickldmrs_1="192.168.0.1:1202">72 * <Sick sickldmrs_0="192.168.0.1:2111" sicklms151_0="192.168.0.10:2111" sicklms511_0="192.168.1.50:2111"> 81 73 */ 82 // /Sick LD-MRS74 // Sick LD-MRS 83 75 int num = 0; 84 76 while (param.getProperty("sickldmrs_"+QString::number(num)) != QString::null){ … … 95 87 } 96 88 97 // /Sick LMS 15189 // Sick LMS 151 98 90 num = 0; 99 91 while (param.getProperty("sicklms151_"+QString::number(num)) != QString::null){ … … 110 102 } 111 103 104 // Sick LMS 511 112 105 num = 0; 113 106 while (param.getProperty("sicklms511_"+QString::number(num)) != QString::null){ -
trunk/Sick/SickComponent.h
r37 r42 70 70 * @param config XML file passed in order to configure the Sick Component. 71 71 * 72 * This function instanciate every sensor configured through the XML file. 73 * The XML file must be formated as expected by this function, see [annexe TODO]{TODO}. 72 * This function instanciate every sensors configured through the XML file. 73 * The XML file must be formated as expected by this function. 74 * Depending on used sensors and how many, you should define these three property in a "Sick" node (X must start from '0') : 75 * - sickldmrs_X 76 * - sicklms151_X 77 * - sicklms511_X 78 * 79 * For example, let's say we have two Sick LMS151, one LDMRS and one LMS511 : 80 * 81 * <tt> <Sick type="SickComponent" sickldmrs_0="192.168.0.1:2111" sicklms151_0="192.168.0.10:2111" sicklms151_1="192.168.0.11:2111" sicklms511_0="192.168.1.50:2111" /> </tt> 82 * 83 * Do not forget \c type="SickComponent". 74 84 */ 75 85 virtual ComponentBase::COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config); -
trunk/Sick/SickLDMRSData.h
r37 r42 135 135 u_int64_t timeStartFromSensor; //!< NTP time (creation of the message on sensor). 136 136 ScanHeader hScan; //!< General information about points recorded. @see ScanHeader 137 road_time_t time; //!< DBT timestamp.138 road_timerange_t timerange; //!< DBT timerange.137 // road_time_t time; //!< DBT timestamp. 138 // road_timerange_t timerange; //!< DBT timerange. 139 139 int32_t dataPos; //!< The position of the data in the binary file associated to the dbt file (utc file). 140 140 } SickLDMRS_dbt; -
trunk/Sick/SickLDMRSSensor.cpp
r37 r42 43 43 static const int SICKLDMRS_OBJECTDATA_TYPE = 0x2221; 44 44 45 ////////////////////////////////////////////////////////////////////////// 46 /// Constructor. 45 47 46 SickLDMRSSensor::SickLDMRSSensor(QObject *parent) 48 47 : ipaddr_("192.168.0.1"), … … 83 82 } 84 83 85 ////////////////////////////////////////////////////////////////////////// 86 /// Destructor. 84 87 85 SickLDMRSSensor::~SickLDMRSSensor() 88 86 { … … 91 89 } 92 90 93 ////////////////////////////////////////////////////////////////////////// 94 /// Called by the ComponentManager to start the component 91 95 92 void SickLDMRSSensor::startActivity() 96 93 { … … 104 101 105 102 try { 106 dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK , sizeof(SickLDMRS_dbt));103 dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK_LDMRS, sizeof(SickLDMRS_dbt)); 107 104 } catch (DbiteException & e) { 108 105 cerr << "error opening dbt file: "<< kSickDbtFileName << ", " << e.what() << endl; … … 136 133 } 137 134 138 ////////////////////////////////////////////////////////////////////////// 139 /// Called by the ComponentManager to stop the component 135 140 136 void SickLDMRSSensor::stopActivity() 141 137 { … … 161 157 162 158 163 //////////////////////////////////////////////////////////////////////////164 /// look for the position of the magic word in the packet.165 /// Return it if found else return -1166 159 u_int32_t SickLDMRSSensor::findMagicWord(const char * message, const unsigned length) 167 160 { … … 181 174 } 182 175 183 ////////////////////////////////////////////////////////////////////////// 184 /// Return the size of the message contained in the packet 185 /// You have to provide the position of the magic word in the packet 176 186 177 u_int32_t SickLDMRSSensor::getMessageSize(const char * message, const unsigned length, const long magicWordIndex) 187 178 { … … 198 189 } 199 190 200 ////////////////////////////////////////////////////////////////////////// 201 /// @param size the total size of the message including the 2 headers and the body 202 /// @param length the number of available bytes in the packet 191 203 192 bool SickLDMRSSensor::isMessageComplete(const unsigned length, const long size) 204 193 { … … 276 265 } 277 266 278 ////////////////////////////////////////////////////////////////////////// 279 /// this function is called when no complete message has been found 280 /// we set the flag previousData to true for the next processing stage 281 /// and we store the timestamp of the bytes acquisition 267 282 268 void SickLDMRSSensor::storePendingBytes(road_time_t time) 283 269 { … … 406 392 } 407 393 408 ////////////////////////////////////////////////////////////////////////// 409 /// write the data on the disk: 410 /// - complete the dbt file with msg.hData 411 /// - complete the binary data file with the alascaData.point 394 412 395 void SickLDMRSSensor::writeData(MessageLDMRS &msg) 413 396 { … … 419 402 entry.hScan = msg.hScan; 420 403 entry.dataPos = dataFile_.tellp(); // absolute position of pointer in UTC file 421 entry.time = msg.time;422 entry.timerange = msg.timerange;404 // entry.time = msg.time; 405 // entry.timerange = msg.timerange; 423 406 424 407 // write DBT … … 442 425 443 426 444 ///445 427 void SickLDMRSSensor::configure(){ 446 428 // Start measuring … … 451 433 452 434 453 //////////////////////////////////////////////////////////////////////////454 /// Event call by the SickSocket class when new data has been arrived455 /// on the network.456 435 void SickLDMRSSensor::customEvent(QEvent * e) 457 436 { … … 476 455 // write data on the disk 477 456 if (recording_) 478 writeData(msg List.front());457 writeData(msgToProcess); 479 458 480 459 #ifdef SICKLDMRS_SH_MEM 460 /// NOT TESTED ! 481 461 /// push data in shared memory 482 462 // First the scan info … … 499 479 } 500 480 501 // (malloced memory) ScanPoints[] or ScanObjects[] no longer needed502 free(msgToProcess.body);503 504 481 // removes the processed item of the list 505 482 msgList.pop_front(); -
trunk/Sick/SickLDMRSSensor.h
r37 r42 247 247 void writeData(MessageLDMRS &msg); 248 248 249 250 249 std::string kSickDbtFileName; //!< Name of the DBT file. 251 250 std::string kSickUtcFileName; //!< Name of the UTC file. -
trunk/Sick/SickLMSSensor.cpp
r37 r42 86 86 } 87 87 88 ////////////////////////////////////////////////////////////////////////// 89 /// Destructor. 88 90 89 SickLMSSensor::~SickLMSSensor() 91 90 { … … 94 93 } 95 94 96 ////////////////////////////////////////////////////////////////////////// 97 /// Called by the ComponentManager to start the component 95 98 96 void SickLMSSensor::startActivity() 99 97 { … … 143 141 144 142 try { 145 dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK , sizeof(SickLMS_dbt));143 dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK_LMS, sizeof(SickLMS_dbt)); 146 144 } catch (DbiteException & e) { 147 145 cerr << "error opening dbt file: "<< kSickDbtFileName << ", " << e.what() << endl; … … 174 172 } 175 173 176 ////////////////////////////////////////////////////////////////////////// 177 /// Called by the ComponentManager to stop the component 174 175 178 176 void SickLMSSensor::stopActivity() 179 177 { … … 200 198 } 201 199 202 ////////////////////////////////////////////////////////////////////////// 203 /// 200 204 201 void SickLMSSensor::configure(){ 205 202 // Start measuring … … 212 209 213 210 214 / // Data must be parsed in slot customEvent211 /* Data must be parsed in slot customEvent */ 215 212 void SickLMSSensor::askScanCfg(){ 216 213 char buf[100]; … … 222 219 223 220 224 ////////////////////////////////////////////////////////////////////////// 225 /// 221 226 222 int SickLMSSensor::isMessageComplete(const char* packets, unsigned int size) 227 223 { … … 234 230 235 231 236 ////////////////////////////////////////////////////////////////////////// 237 /// look for the position of the Start of Text character 238 /// Return it if found else return -1 232 239 233 int SickLMSSensor::findSTX(const char* packets, const unsigned int size ){ 240 234 int i = 0; … … 248 242 249 243 250 //////////////////////////////////////////////////////////////////////////251 /// this function is called when no complete message has been found252 /// we set the flag previousData to true for the next processing stage253 /// and we store the timestamp of the bytes acquisition254 244 void SickLMSSensor::storePendingBytes(road_time_t time) 255 245 { … … 261 251 } 262 252 263 ////////////////////////////////////////////////////////////////////////// 264 /// Analyse the ethernet packet received from the Sick LMS and try to find a 265 /// complete message (scan data message) 266 /// If a message has been found it is added at the end of the message list 267 /// else the pending bytes are stored to be analyzed by further icoming data 268 ////////////////////////////////////////////////////////////////////////// 253 269 254 void SickLMSSensor::reconstituteMessage(const char * packet, const int length, road_time_t time) 270 255 { … … 332 317 333 318 334 /**335 * \fn336 * \brief337 * \param338 * \param339 * \return340 */341 319 int SickLMSSensor::splitMessage(MessageLMS* message){ 342 320 … … 357 335 } 358 336 359 ////////////////////////////////////////////////////////////////////////// 360 /// Convert hexadecimal value stored as ASCII string to long 361 /// Warning : alpha characters must be in capital letters 337 338 362 339 long SickLMSSensor::xstol(std::string str){ 363 340 long ret = 0; … … 587 564 } 588 565 589 ////////////////////////////////////////////////////////////////////////// 590 /// write the data on the disk: 566 591 567 void SickLMSSensor::writeData(MessageLMS &msg) 592 568 { … … 595 571 entry.scanNumber = xstol(msg.splitMessage->at(7)); 596 572 entry.scannerStatus = xstol(msg.splitMessage->at(6)); 597 entry.time = msg.time; 598 entry.timerange = msg.timerange; 573 /* time in SickLMS_dbt is used uniquely when DBT file read */ 574 // entry.time = msg.time; 575 // entry.timerange = msg.timerange; 599 576 entry.scanFrequency = msg.data.scanFrequency; 600 577 entry.angleResolution = msg.data.angleResolution; 601 578 entry.startAngle = msg.data.startAngle; 579 580 // Initialisation 581 entry.dist_len1 = entry.dataPos_dist1 = 0; 582 entry.dist_len2 = entry.dataPos_dist2 = 0; 583 entry.dist_len3 = entry.dataPos_dist3 = 0; 584 entry.dist_len4 = entry.dataPos_dist4 = 0; 585 entry.dist_len5 = entry.dataPos_dist5 = 0; 586 entry.rssi_len1 = entry.dataPos_rssi1 = 0; 587 entry.rssi_len2 = entry.dataPos_rssi2 = 0; 602 588 603 589 if(msg.data.dist_len1){ … … 687 673 688 674 689 ////////////////////////////////////////////////////////////////////////// 690 /// Event call by the SickSocket class when new data has been arrived 691 /// on the network. 675 692 676 void SickLMSSensor::customEvent(QEvent * e) 693 677 { -
trunk/Sick/SickLMSSensor.h
r37 r42 116 116 * 117 117 * A message starts with a <STX> (0x02 in ASCII) char and ends with <ETX> (0x03 in ASCII). 118 * This function stores packets until a complete message is received. In this case, the 119 * message is added to the list of MessageLMS, msgList. 118 120 */ 119 121 void reconstituteMessage(const char * packet, const int length, road_time_t time); 120 122 121 // decoupe le message en scan122 123 /** 123 124 * @brief processScanData Parse information and process every needed values. … … 136 137 137 138 public Q_SLOTS: 138 //Fonction qui permet de trier les données reçues et de les enregistrer dans une structure point, puis dans un fichier binaire .dbt 139 /** 140 * @brief customEvent allows to receive the incoming data and store them into known structures. 141 * @param e Event that carries the Ethernet packets and receiving time. 142 */ 139 143 void customEvent(QEvent * e); 144 145 /** 146 * @brief Configure the object, not used for the moment. 147 */ 140 148 void configure(); 141 149 142 150 public: 143 SickSocket * S_socket; //on déclare pointeur de socket 144 // SickComponent * myParent; 151 /** 152 * @brief S_socket, used to receive and send data to the remote sensor. 153 */ 154 SickSocket * S_socket; 145 155 146 156 private: 147 // to recognize between several sensors157 /// Name is used to recognize between several sensors and store data into .dbt and utc files. 148 158 QString name_; 149 159 150 // The SickLMS IP or hostname160 /// The IP address of the remote Sick LDMRS sensor we are connected to. 151 161 QString ipaddr_; 152 162 153 // The SickLMS port163 /// The SickLDMRS port 154 164 int port_; 155 165 156 bool recording_; // Enable storing in DBT 166 /// Enable storing in DBT + UTC files 167 bool recording_; 157 168 158 169 scanCfg mainCfg; 159 170 171 /// Append new data into the private MessagePacket @b pendingBytes. 160 172 void storePendingBytes(road_time_t time); 161 void fillDataHeader(MessageLMS & msg); 162 void fillScanHeader(MessageLMS & msg); 173 174 /** Ask for scan configuration (frequency and angular resolution) . [See §5.2 of the documentation : telegrams]{TL_LMS1xx_5xx_TiM3xx_JEF300_JEF500_en_8014631_20120508.pdf} 175 Data is parsed in customEvent slot. 176 */ 163 177 void askScanCfg(); 164 178 179 /** 180 * @brief findSTX looks for the first character of the message <STX>. 181 * @param packets Raw data received. 182 * @param size Size of the raw data array (\c packets). 183 * @return Index of the <STX> character in \c packets. 184 */ 165 185 int findSTX(const char* packets, const unsigned int size ); 186 187 /** 188 * @brief splitMessage Split message into arrays of string to parse. 189 * @param message Contains raw data and is modified to store the vector of strings. 190 * @return Size of the vector. 191 */ 166 192 int splitMessage(MessageLMS* message); 193 194 /** 195 * @brief xstol convert hexadecimal values coded in ASCII to integer. 196 * @param str String to convert (Hexadecimal value) 197 * @return long integer value (to cast as needed) 198 */ 167 199 long xstol(std::string str); 168 200 169 std::string kSick MemoryName;170 std::string kSick DbtFileName;171 std::string kSickUtcFileName; 172 201 std::string kSickDbtFileName; //!< Name of the DBT file. 202 std::string kSickUtcFileName; //!< Name of the UTC file. 203 204 /// List of messages to process. 173 205 std::list<MessageLMS> msgList; 174 206 207 /// Received raw data is appended into this MessagePacket. 175 208 MessagePacket pendingBytes; 176 209 177 // write the data on the disk in the dbt file and the associated binary file 210 /** 211 * @brief Write data into .dbt + .utc files. 212 * @param msg 213 * 214 * SickLMS_dbt structure is filled and stored into the DBT file. 215 * The arrays of points' distance is then stored into the UTC file, depending on data received. 216 */ 178 217 void writeData(MessageLMS &msg); 179 218 180 // SickLMS_dbt dbtData_; 181 pacpus::DbiteFile dbtFile_; 182 std::ofstream dataFile_; 219 pacpus::DbiteFile dbtFile_; //!< DBT file. 220 std::ofstream dataFile_; //!< UTC file. 183 221 184 222 #ifdef SickLMS_SH_MEM -
trunk/Sick/SickSocket.cpp
r37 r42 23 23 DECLARE_STATIC_LOGGER("pacpus.base.SickSocket"); 24 24 25 ////////////////////////////////////////////////////////////////////////// 26 /// Constructor 25 27 26 SickSocket::SickSocket(AbstractSickSensor * parent) 28 27 : myParent(parent) … … 36 35 37 36 38 //////////////////////////////////////////////////////////////////////////39 /// Destructor40 37 SickSocket::~SickSocket() 41 38 { … … 43 40 } 44 41 45 ////////////////////////////////////////////////////////////////////////// 46 /// public slot 47 /// connection to server 42 48 43 void SickSocket::connectToServer(QString host, int port) 49 44 { … … 52 47 } 53 48 54 ////////////////////////////////////////////////////////////////////////// 55 /// protected slot 49 56 50 int SickSocket::socketConnected() 57 51 { … … 70 64 } 71 65 72 ////////////////////////////////////////////////////////////////////////// 73 /// protected slot 74 /// new incoming data to read 66 75 67 void SickSocket::socketReadyRead() 76 68 { … … 94 86 } 95 87 96 ////////////////////////////////////////////////////////////////////////// 97 /// protected slot 88 98 89 void SickSocket::socketError(QAbstractSocket::SocketError e) 99 90 { … … 101 92 } 102 93 103 ////////////////////////////////////////////////////////////////////////// 104 /// Send data to server via a text stream 94 105 95 void SickSocket::sendToServer(QString data) //a adapter aux données binaires 106 96 { -
trunk/Sick/SickSocket.h
r37 r42 3 3 // filename: SickSocket.h 4 4 // 5 // author: Gerald Dherbomez & Cyril Fougeray 5 // author: Gerald Dherbomez 6 // Modified by Cyril Fougeray 6 7 // Copyright Heudiasyc UMR UTC/CNRS 6599 7 8 // … … 31 32 class SickLMSSensor; 32 33 34 /** 35 * @brief The SickFrame class 36 */ 33 37 class SickFrame 34 38 { 35 39 public: 40 /** 41 * @brief SickFrame constructor 42 */ 36 43 SickFrame() 37 44 { … … 41 48 } 42 49 50 /// Destructor 43 51 ~SickFrame() 44 52 { … … 46 54 } 47 55 48 qint64 size; 49 road_time_t time; 50 char * msg; 56 qint64 size; //!< Size of incoming packet. 57 road_time_t time; //!< Time when packet is received. 58 char * msg; //!< Packet (raw data). 51 59 }; 52 60 53 // forward declaration 54 61 /** 62 * @brief The SickFrameEvent class 63 * QEvent that encapsulates packets. 64 */ 55 65 class SickFrameEvent 56 66 : public QEvent 57 67 { 58 68 public: 69 /// Constructor 59 70 SickFrameEvent() 60 71 : QEvent((QEvent::Type)(QEvent::User + 522)) 61 72 {} 62 73 74 /// Destructor 63 75 ~SickFrameEvent() 64 76 {} 65 77 78 /// Packet data 66 79 SickFrame * frame; 67 80 }; 68 81 82 /** 83 * @brief The SickSocket class 84 * Handles the ethernet connection with the remote sensor. 85 */ 69 86 class SickSocket 70 87 : public QObject … … 73 90 74 91 public: 92 /// Constructor 75 93 SickSocket(AbstractSickSensor * parent); 94 95 /// Destructor 76 96 ~SickSocket(); 77 97 78 98 public Q_SLOTS: 79 // enable the connection to the server99 /// Enable the connection to the server 80 100 void connectToServer(QString host, int port); 81 101 82 // avertit que le socket est connecté102 /// Warns about connection of the socket and launch configuration of the socket. 83 103 int socketConnected(); 84 104 85 / / appelé lorsque de nouvelles données sont arrivées sur le socket105 /** Called when incoming data is received. Create an event and send it to the sensor's handler. @see AbstractSickComponent */ 86 106 void socketReadyRead(); 87 107 88 // close the connection with the server108 /// Close the connection with the server 89 109 void closeSocket() { socket->close(); } 90 110 91 // fonction d'envoi des données au télémètre 111 /** 112 * @brief sendToServer Sends data to the remote lidar. 113 * @param data Data to be sent, translated in ASCII. 114 */ 92 115 void sendToServer(QString data); 93 116 Q_SIGNALS: 94 // signal emis pour demander la configuration du télémètre117 /// Asked for configuring sensor. 95 118 void configuration(); 96 119 97 120 protected slots: 98 99 // le socket est fermé par le serveur 121 /// Says to the user the connection is closed. 100 122 void socketConnectionClosed(); 101 123 102 // une erreur est survenue124 /// Warns the user an error occured 103 125 void socketError(QAbstractSocket::SocketError e); 104 126 105 127 private: 128 /// Socket 106 129 QTcpSocket *socket; 130 131 /// Mutex to use socket resource. 107 132 QMutex mutex; 108 133 134 /// Parent (contains slots to connect to in order to pass the received data). 109 135 AbstractSickSensor *myParent; 110 136 };
Note:
See TracChangeset
for help on using the changeset viewer.
