CBoardSonars.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"   // Precompiled headers

#include <mrpt/utils/utils_defs.h>
#include <mrpt/system/os.h>

#include <mrpt/hwdrivers/CBoardSonars.h>

using namespace mrpt::utils;
using namespace mrpt::hwdrivers;
using namespace std;

IMPLEMENTS_GENERIC_SENSOR(CBoardSonars,mrpt::hwdrivers)

/*-------------------------------------------------------------
                                                CBoardSonars
-------------------------------------------------------------*/
CBoardSonars::CBoardSonars()
{
        MRPT_START
        m_usbSerialNumber = "SONAR001";

        m_sensorLabel           = "SONAR1";

        m_gain = 6;
        m_maxRange = 4.0f;

        MRPT_END
}

/*-------------------------------------------------------------
                                                loadConfig_sensorSpecific
-------------------------------------------------------------*/
void  CBoardSonars::loadConfig_sensorSpecific(  const mrpt::utils::CConfigFileBase &configSource,
                                                                const std::string         &iniSection )
{
        MRPT_START

        std::vector<double> aux;                                                                                        // Auxiliar vector

        // Some parameters ...
        m_usbSerialNumber       = configSource.read_string(iniSection,"USB_serialNumber",m_usbSerialNumber,true);
        m_gain                          = configSource.read_int(iniSection,"gain",m_gain,true);
        m_maxRange                      = configSource.read_float(iniSection,"maxRange",m_maxRange,true);
        m_minTimeBetweenPings = configSource.read_float(iniSection,"minTimeBetweenPings",m_minTimeBetweenPings,true);
        // ----------------------------------------------------------------------------------------------------------------------
        ASSERT_( m_maxRange>0 && m_maxRange<=11 );
        ASSERT_( m_gain<=16 );


        // Sonar firing order ...
        configSource.read_vector( iniSection, "firingOrder", m_firingOrder, m_firingOrder, true );

        // ----------------------------------------------------------------------------------------------------------------------

        // Individual sonar gains ...
        configSource.read_vector( iniSection, "sonarGains", aux, aux, true );

        std::vector<int32_t>::iterator itSonar;
        std::vector<double>::iterator   itAux;
        for( itSonar = m_firingOrder.begin(), itAux = aux.begin(); itSonar != m_firingOrder.end(); ++itSonar, ++itAux )
                m_sonarGains[ *itSonar ] = *itAux;
        // ----------------------------------------------------------------------------------------------------------------------
        ASSERT_( aux.size() == m_firingOrder.size() );

        // Individual sonar poses
        aux.clear();
        for( itSonar = m_firingOrder.begin(); itSonar != m_firingOrder.end(); ++itSonar )
        {
                configSource.read_vector( iniSection, format("pose%i",*itSonar), aux, aux, true );      // Get the sonar poses
                m_sonarPoses[ *itSonar ] = mrpt::math::TPose3D( aux[0], aux[1], aux[2], DEG2RAD( (float)aux[3]), DEG2RAD( (float)aux[4]), DEG2RAD( (float)aux[5]) );
        }
        // ----------------------------------------------------------------------------------------------------------------------
        ASSERT_( m_sonarGains.size() == m_firingOrder.size() );

        MRPT_END
}

/*-------------------------------------------------------------
                                        queryFirmwareVersion
-------------------------------------------------------------*/
bool CBoardSonars::queryFirmwareVersion( string &out_firmwareVersion )
{
        try
        {
                utils::CMessage         msg,msgRx;

                // Try to connect to the device:
                if (!checkConnectionAndConnect())       return false;

                msg.type = 0x10;
                sendMessage(msg);

                if (receiveMessage(msgRx) )
                {
                        msgRx.getContentAsString( out_firmwareVersion );
                        return true;
                }
                else
                        return false;
        }
        catch(...)
        {
                Close();
                return false;
        }
}


/*-------------------------------------------------------------
                                        checkConnectionAndConnect
-------------------------------------------------------------*/
bool CBoardSonars::sendConfigCommands()
{
        try
        {
                if (!isOpen())  return false;
                utils::CMessage         msg,msgRx;
                size_t                                  i;

                // Send cmd for firing order:
                // ----------------------------
                msg.type = 0x12;
                msg.content.resize(16);
                for (i=0;i<16;i++)
                {
                        if (i<m_firingOrder.size())
                                        msg.content[i] = m_firingOrder[i];
                        else    msg.content[i] = 0xFF;
                }
                sendMessage(msg);
                if (!receiveMessage(msgRx) ) return false;      // Error

                // Send cmd for gain:
                // ----------------------------
                //msg.type = 0x13;
                //msg.content.resize(1);
                //msg.content[0] = m_gain;
                //sendMessage(msg);
                //if (!receiveMessage(msgRx) ) return false;    // Error

                // Send cmd for set of gains:
                // ----------------------------
                msg.type = 0x16;
                msg.content.resize(16);
                for (i=0;i<16;i++)
                {
                        if( m_sonarGains.find(i) != m_sonarGains.end() )
                                        msg.content[i] = m_sonarGains[i];
                        else    msg.content[i] = 0xFF;
                }
                sendMessage(msg);
                if (!receiveMessage(msgRx) ) return false;      // Error

                // Send cmd for max range:
                // ----------------------------
                msg.type = 0x14;
                msg.content.resize(1);
                msg.content[0] = (int)((m_maxRange/0.043f) - 1);
                sendMessage(msg);
                if (!receiveMessage(msgRx) ) return false;      // Error

                // Send cmd for max range:
                // ----------------------------
                msg.type = 0x15;
                msg.content.resize(2);
                uint16_t  T = (uint16_t)(m_minTimeBetweenPings * 1000.0f);
                msg.content[0] = T >> 8;
                msg.content[1] = T & 0x00FF;
                sendMessage(msg);
                if (!receiveMessage(msgRx) ) return false;      // Error

                return true;
        }
        catch(...)
        {
                // Error opening device:
                Close();
                return false;
        }
}

/*-------------------------------------------------------------
                                        getObservation
-------------------------------------------------------------*/
bool CBoardSonars::getObservation( mrpt::obs::CObservationRange &obs )
{
        try
        {
                obs.sensorLabel = m_sensorLabel;
                obs.timestamp = mrpt::system::getCurrentTime();
                obs.minSensorDistance = 0.04f;
                obs.maxSensorDistance = m_maxRange;
                obs.sensorConeApperture = DEG2RAD(30.0f);
                obs.sensedData.clear();
                mrpt::obs::CObservationRange::TMeasurement obsRange;

                utils::CMessage         msg,msgRx;

                // Try to connect to the device:
                if (!checkConnectionAndConnect())       return false;

                msg.type = 0x11;
                sendMessage(msg);

                if (receiveMessage(msgRx) )
                {
                        if ( msgRx.content.empty() )
                                return false;

                        // For each sensor:
                        ASSERT_((msgRx.content.size() % 2) == 0);
                        vector<uint16_t>        data(msgRx.content.size() / 2);
                        memcpy( &data[0], &msgRx.content[0], msgRx.content.size() );

                        for (size_t i=0;i<data.size() / 2;i++)
                        {
                                uint16_t        sonar_idx      = data[2*i + 0];
                                uint16_t        sonar_range_cm = data[2*i + 1];
                                if ( sonar_range_cm!=0xFFFF && sonar_idx<16 )
                                {
                                        obsRange.sensorID = sonar_idx;
                                        obsRange.sensorPose = m_sonarPoses[sonar_idx]; // mrpt::poses::CPose3D(); // sonar_idx
                                        obsRange.sensedDistance = sonar_range_cm * 0.01f;
                                        obs.sensedData.push_back( obsRange );
                                }
                        }
                        return true;
                }
                else
                        return false;
        }
        catch(...)
        {
                Close();
                return false;
        }
}

/*-------------------------------------------------------------
                                        programI2CAddress
-------------------------------------------------------------*/
bool CBoardSonars::programI2CAddress( uint8_t currentAddress, uint8_t newAddress )
{
        try
        {
                utils::CMessage         msg,msgRx;

                // Try to connect to the device:
                if (!checkConnectionAndConnect())       return false;

                msg.type = 0x20;
                msg.content.resize(2);
                msg.content[0] = currentAddress;
                msg.content[1] = newAddress;
                sendMessage(msg);

                mrpt::system::sleep(10);

                return receiveMessage(msgRx);
        }
        catch(...)
        {
                Close();
                return false;
        }
}

/*-------------------------------------------------------------
                                        checkConnectionAndConnect
-------------------------------------------------------------*/
bool CBoardSonars::checkConnectionAndConnect()
{
        if (isOpen())
                return true;

        try
        {
                OpenBySerialNumber( m_usbSerialNumber );
                mrpt::system::sleep(10);
                Purge();
                mrpt::system::sleep(10);
                SetLatencyTimer(1);
                SetTimeouts(300,100);

                return sendConfigCommands();
        }
        catch(...)
        {
                // Error opening device:
                Close();
                return false;
        }
}

/*-------------------------------------------------------------
                                        doProcess
-------------------------------------------------------------*/
void CBoardSonars::doProcess()
{
        mrpt::obs::CObservationRangePtr obs = mrpt::obs::CObservationRange::Create();
        if (getObservation( *obs ))
                appendObservation( obs );
}