Changeset 42 in pacpussensors


Ignore:
Timestamp:
Apr 1, 2014, 2:47:39 PM (11 years ago)
Author:
cfougera
Message:

Last version

Location:
trunk
Files:
10 edited

Legend:

Unmodified
Added
Removed
  • trunk/Sick/SickComponent.cpp

    r37 r42  
    3737
    3838
    39 //////////////////////////////////////////////////////////////////////////
    40 /// Constructor.
    4139SickComponent::SickComponent(QString name)
    4240    : ComponentBase(name)
     
    4745}
    4846
    49 //////////////////////////////////////////////////////////////////////////
    50 /// Destructor.
    5147SickComponent::~SickComponent()
    5248{
     
    5450}
    5551
    56 //////////////////////////////////////////////////////////////////////////
    57 /// Called by the ComponentManager to start the component
     52
    5853void SickComponent::startActivity()
    5954{
     
    6358}
    6459
    65 //////////////////////////////////////////////////////////////////////////
    66 /// Called by the ComponentManager to stop the component
     60
    6761void SickComponent::stopActivity()
    6862{
     
    7266}
    7367
    74 //////////////////////////////////////////////////////////////////////////
    75 /// Called by the ComponentManager to pass the XML parameters to the
    76 /// component
     68
    7769ComponentBase::COMPONENT_CONFIGURATION SickComponent::configureComponent(XmlComponentConfig config)
    7870{
    7971    /*
    80      * <SickComponent 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">
    8173     */
    82     /// Sick LD-MRS
     74    // Sick LD-MRS
    8375    int num = 0;
    8476    while (param.getProperty("sickldmrs_"+QString::number(num)) != QString::null){
     
    9587    }
    9688
    97     /// Sick LMS 151
     89    // Sick LMS 151
    9890       num = 0;
    9991     while (param.getProperty("sicklms151_"+QString::number(num)) != QString::null){
     
    110102    }
    111103
     104     // Sick LMS 511
    112105     num = 0;
    113106   while (param.getProperty("sicklms511_"+QString::number(num)) != QString::null){
  • trunk/Sick/SickComponent.h

    r37 r42  
    7070     * @param config XML file passed in order to configure the Sick Component.
    7171     *
    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".
    7484     */
    7585    virtual ComponentBase::COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config);
  • trunk/Sick/SickLDMRSData.h

    r37 r42  
    135135    u_int64_t timeStartFromSensor;  //!< NTP time (creation of the message on sensor).
    136136    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.
    139139    int32_t dataPos;                //!< The position of the data in the binary file associated to the dbt file (utc file).
    140140} SickLDMRS_dbt;
  • trunk/Sick/SickLDMRSSensor.cpp

    r37 r42  
    4343static const int SICKLDMRS_OBJECTDATA_TYPE  = 0x2221;
    4444
    45 //////////////////////////////////////////////////////////////////////////
    46 /// Constructor.
     45
    4746SickLDMRSSensor::SickLDMRSSensor(QObject *parent)
    4847    : ipaddr_("192.168.0.1"),
     
    8382}
    8483
    85 //////////////////////////////////////////////////////////////////////////
    86 /// Destructor.
     84
    8785SickLDMRSSensor::~SickLDMRSSensor()
    8886{
     
    9189}
    9290
    93 //////////////////////////////////////////////////////////////////////////
    94 /// Called by the ComponentManager to start the component
     91
    9592void SickLDMRSSensor::startActivity()
    9693{
     
    104101
    105102        try {
    106             dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK, sizeof(SickLDMRS_dbt));
     103            dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK_LDMRS, sizeof(SickLDMRS_dbt));
    107104        } catch (DbiteException & e) {
    108105            cerr << "error opening dbt file: "<< kSickDbtFileName << ", " << e.what() << endl;
     
    136133}
    137134
    138 //////////////////////////////////////////////////////////////////////////
    139 /// Called by the ComponentManager to stop the component
     135
    140136void SickLDMRSSensor::stopActivity()
    141137{
     
    161157
    162158
    163 //////////////////////////////////////////////////////////////////////////
    164 /// look for the position of the magic word in the packet.
    165 /// Return it if found else return -1
    166159u_int32_t SickLDMRSSensor::findMagicWord(const char * message, const unsigned length)
    167160{
     
    181174}
    182175
    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
    186177u_int32_t SickLDMRSSensor::getMessageSize(const char * message, const unsigned length, const long magicWordIndex)
    187178{
     
    198189}
    199190
    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
    203192bool SickLDMRSSensor::isMessageComplete(const unsigned length, const long size)
    204193{
     
    276265}
    277266
    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
    282268void SickLDMRSSensor::storePendingBytes(road_time_t time)
    283269{
     
    406392}
    407393
    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
    412395void SickLDMRSSensor::writeData(MessageLDMRS &msg)
    413396{
     
    419402    entry.hScan = msg.hScan;
    420403    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;
    423406
    424407    // write DBT
     
    442425
    443426
    444 ///
    445427void SickLDMRSSensor::configure(){
    446428    // Start measuring
     
    451433
    452434
    453 //////////////////////////////////////////////////////////////////////////
    454 /// Event call by the SickSocket class when new data has been arrived
    455 /// on the network.
    456435void SickLDMRSSensor::customEvent(QEvent * e)
    457436{   
     
    476455            // write data on the disk
    477456            if (recording_)
    478                 writeData(msgList.front());
     457                writeData(msgToProcess);
    479458
    480459#ifdef SICKLDMRS_SH_MEM
     460            /// NOT TESTED !
    481461            /// push data in shared memory
    482462            // First the scan info
     
    499479        }
    500480
    501         // (malloced memory) ScanPoints[] or ScanObjects[] no longer needed
    502         free(msgToProcess.body);
    503 
    504481        // removes the processed item of the list
    505482        msgList.pop_front();
  • trunk/Sick/SickLDMRSSensor.h

    r37 r42  
    247247    void writeData(MessageLDMRS &msg);
    248248
    249 
    250249    std::string kSickDbtFileName; //!< Name of the DBT file.
    251250    std::string kSickUtcFileName; //!< Name of the UTC file.
  • trunk/Sick/SickLMSSensor.cpp

    r37 r42  
    8686}
    8787
    88 //////////////////////////////////////////////////////////////////////////
    89 /// Destructor.
     88
    9089SickLMSSensor::~SickLMSSensor()
    9190{
     
    9493}
    9594
    96 //////////////////////////////////////////////////////////////////////////
    97 /// Called by the ComponentManager to start the component
     95
    9896void SickLMSSensor::startActivity()
    9997{
     
    143141
    144142        try {
    145             dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK, sizeof(SickLMS_dbt));
     143            dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK_LMS, sizeof(SickLMS_dbt));
    146144        } catch (DbiteException & e) {
    147145            cerr << "error opening dbt file: "<< kSickDbtFileName << ", " << e.what() << endl;
     
    174172}
    175173
    176 //////////////////////////////////////////////////////////////////////////
    177 /// Called by the ComponentManager to stop the component
     174
     175
    178176void SickLMSSensor::stopActivity()
    179177{
     
    200198}
    201199
    202 //////////////////////////////////////////////////////////////////////////
    203 ///
     200
    204201void SickLMSSensor::configure(){
    205202    // Start measuring
     
    212209
    213210
    214 /// Data must be parsed in slot customEvent
     211/* Data must be parsed in slot customEvent */
    215212void SickLMSSensor::askScanCfg(){
    216213    char buf[100];
     
    222219
    223220
    224 //////////////////////////////////////////////////////////////////////////
    225 ///
     221
    226222int SickLMSSensor::isMessageComplete(const char* packets, unsigned int size)
    227223{
     
    234230
    235231
    236 //////////////////////////////////////////////////////////////////////////
    237 /// look for the position of the Start of Text character
    238 /// Return it if found else return -1
     232
    239233int SickLMSSensor::findSTX(const char* packets, const unsigned int size ){
    240234    int i = 0;
     
    248242
    249243
    250 //////////////////////////////////////////////////////////////////////////
    251 /// this function is called when no complete message has been found
    252 /// we set the flag previousData to true for the next processing stage
    253 /// and we store the timestamp of the bytes acquisition
    254244void SickLMSSensor::storePendingBytes(road_time_t time)
    255245{
     
    261251}
    262252
    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
    269254void SickLMSSensor::reconstituteMessage(const char * packet, const int length, road_time_t time)
    270255{
     
    332317
    333318
    334 /**
    335  * \fn
    336  * \brief
    337  * \param
    338  * \param
    339  * \return
    340  */
    341319int SickLMSSensor::splitMessage(MessageLMS* message){
    342320
     
    357335}
    358336
    359 //////////////////////////////////////////////////////////////////////////
    360 /// Convert hexadecimal value stored as ASCII string to long
    361 /// Warning : alpha characters must be in capital letters
     337
     338
    362339long SickLMSSensor::xstol(std::string str){
    363340    long ret = 0;
     
    587564}
    588565
    589 //////////////////////////////////////////////////////////////////////////
    590 /// write the data on the disk:
     566
    591567void SickLMSSensor::writeData(MessageLMS &msg)
    592568{
     
    595571    entry.scanNumber = xstol(msg.splitMessage->at(7));
    596572    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;
    599576    entry.scanFrequency = msg.data.scanFrequency;
    600577    entry.angleResolution = msg.data.angleResolution;
    601578    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;
    602588
    603589    if(msg.data.dist_len1){
     
    687673
    688674
    689 //////////////////////////////////////////////////////////////////////////
    690 /// Event call by the SickSocket class when new data has been arrived
    691 /// on the network.
     675
    692676void SickLMSSensor::customEvent(QEvent * e)
    693677{
  • trunk/Sick/SickLMSSensor.h

    r37 r42  
    116116     *
    117117     * 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.
    118120     */
    119121    void reconstituteMessage(const char * packet, const int length, road_time_t time);
    120122
    121     // decoupe le message en scan
    122123    /**
    123124     * @brief processScanData Parse information and process every needed values.
     
    136137
    137138public 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     */
    139143    void customEvent(QEvent * e);
     144
     145    /**
     146     * @brief Configure the object, not used for the moment.
     147     */
    140148    void configure();
    141149   
    142150public:
    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;
    145155
    146156private:
    147     // to recognize between several sensors
     157    /// Name is used to recognize between several sensors and store data into .dbt and utc files.
    148158    QString name_;
    149159
    150     // The SickLMS IP or hostname
     160    /// The IP address of the remote Sick LDMRS sensor we are connected to.
    151161    QString ipaddr_;
    152162
    153     // The SickLMS port
     163    /// The SickLDMRS port
    154164    int port_;
    155165
    156     bool recording_; // Enable storing in DBT
     166    /// Enable storing in DBT + UTC files
     167    bool recording_;
    157168
    158169    scanCfg mainCfg;
    159170
     171    /// Append new data into the private MessagePacket @b pendingBytes.
    160172    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    */
    163177    void askScanCfg();
    164178
     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     */
    165185    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     */
    166192    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     */
    167199    long xstol(std::string str);
    168200
    169     std::string kSickMemoryName;
    170     std::string kSickDbtFileName;
    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.
    173205    std::list<MessageLMS> msgList;
    174206
     207    /// Received raw data is appended into this MessagePacket.
    175208    MessagePacket pendingBytes;
    176209
    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     */
    178217    void writeData(MessageLMS &msg);
    179218
    180     // SickLMS_dbt dbtData_;
    181     pacpus::DbiteFile dbtFile_;
    182     std::ofstream dataFile_;
     219    pacpus::DbiteFile dbtFile_; //!< DBT file.
     220    std::ofstream dataFile_; //!< UTC file.
    183221
    184222#ifdef SickLMS_SH_MEM
  • trunk/Sick/SickSocket.cpp

    r37 r42  
    2323DECLARE_STATIC_LOGGER("pacpus.base.SickSocket");
    2424
    25 //////////////////////////////////////////////////////////////////////////
    26 /// Constructor
     25
    2726SickSocket::SickSocket(AbstractSickSensor * parent)
    2827    : myParent(parent)
     
    3635
    3736
    38 //////////////////////////////////////////////////////////////////////////
    39 /// Destructor
    4037SickSocket::~SickSocket()
    4138{
     
    4340}
    4441
    45 //////////////////////////////////////////////////////////////////////////
    46 /// public slot
    47 /// connection to server
     42
    4843void SickSocket::connectToServer(QString host, int port)
    4944{
     
    5247}
    5348
    54 //////////////////////////////////////////////////////////////////////////
    55 /// protected slot
     49
    5650int SickSocket::socketConnected()
    5751{
     
    7064}
    7165
    72 //////////////////////////////////////////////////////////////////////////
    73 /// protected slot
    74 /// new incoming data to read
     66
    7567void SickSocket::socketReadyRead()
    7668{   
     
    9486}
    9587
    96 //////////////////////////////////////////////////////////////////////////
    97 /// protected slot
     88
    9889void SickSocket::socketError(QAbstractSocket::SocketError e)
    9990{
     
    10192}
    10293
    103 //////////////////////////////////////////////////////////////////////////
    104 /// Send data to server via a text stream
     94
    10595void SickSocket::sendToServer(QString data) //a adapter aux données binaires
    10696{
  • trunk/Sick/SickSocket.h

    r37 r42  
    33//  filename:   SickSocket.h
    44//
    5 //  author:     Gerald Dherbomez & Cyril Fougeray
     5//  author:     Gerald Dherbomez
     6//              Modified by Cyril Fougeray
    67//              Copyright Heudiasyc UMR UTC/CNRS 6599
    78//
     
    3132class SickLMSSensor;
    3233
     34/**
     35 * @brief The SickFrame class
     36 */
    3337class SickFrame
    3438{
    3539public:
     40    /**
     41     * @brief SickFrame constructor
     42     */
    3643    SickFrame()
    3744    {
     
    4148    }
    4249   
     50    /// Destructor
    4351    ~SickFrame()
    4452    {
     
    4654    }
    4755   
    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).
    5159};
    5260
    53 // forward declaration
    54 
     61/**
     62 * @brief The SickFrameEvent class
     63 * QEvent that encapsulates packets.
     64 */
    5565class SickFrameEvent
    5666        : public QEvent
    5767{
    5868public:
     69    /// Constructor
    5970    SickFrameEvent()
    6071            : QEvent((QEvent::Type)(QEvent::User + 522))
    6172    {}
    6273   
     74    /// Destructor
    6375    ~SickFrameEvent()
    6476    {}
    6577   
     78    /// Packet data
    6679    SickFrame * frame;
    6780};
    6881
     82/**
     83 * @brief The SickSocket class
     84 * Handles the ethernet connection with the remote sensor.
     85 */
    6986class SickSocket
    7087        : public QObject
     
    7390 
    7491public:
     92    /// Constructor
    7593    SickSocket(AbstractSickSensor * parent);
     94
     95    /// Destructor
    7696    ~SickSocket();
    7797
    7898public Q_SLOTS:
    79     // enable the connection to the server
     99    /// Enable the connection to the server
    80100    void connectToServer(QString host, int port);
    81101
    82     //avertit que le socket est connecté
     102    /// Warns about connection of the socket and launch configuration of the socket.
    83103    int socketConnected();
    84104
    85     // appelé lorsque de nouvelles données sont arrivées sur le socket
     105    /** Called when incoming data is received. Create an event and send it to the sensor's handler. @see AbstractSickComponent */
    86106    void socketReadyRead(); 
    87107
    88     // close the connection with the server
     108    /// Close the connection with the server
    89109    void closeSocket() { socket->close(); }
    90110
    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     */
    92115    void sendToServer(QString data);
    93116Q_SIGNALS:
    94     // signal emis pour demander la configuration du télémètre
     117    /// Asked for configuring sensor.
    95118    void configuration();
    96119
    97120protected slots:
    98 
    99     // le socket est fermé par le serveur
     121    /// Says to the user the connection is closed.
    100122    void socketConnectionClosed();
    101123
    102     // une erreur est survenue
     124    /// Warns the user an error occured
    103125    void socketError(QAbstractSocket::SocketError e);
    104126
    105127private:
     128    /// Socket
    106129    QTcpSocket *socket;
     130
     131    /// Mutex to use socket resource.
    107132    QMutex mutex;
    108133
     134    /// Parent (contains slots to connect to in order to pass the received data).
    109135    AbstractSickSensor *myParent;
    110136};
  • trunk/StdDbtPlayerComponents/CMakeLists.txt

    r31 r42  
    2525        ImageViewer.h
    2626        StdDbtPlayerComponentsConfig.h
     27    DbtPlySickLMSManager.h
    2728)
    2829
     
    3536        DbtPlyImageManager.cpp
    3637        DbtPlyStereoManager.cpp
     38    DbtPlySickLMSManager.cpp
    3739        ImageViewer.cpp
    3840        ${HDRS}
     
    4749        DbtPlyImageManager.h
    4850    DbtPlyStereoManager.h
     51    DbtPlySickLMSManager.h
    4952        ImageViewer.h
    5053        ${PLUGIN_H}
Note: See TracChangeset for help on using the changeset viewer.