CPhidgetInterfaceKitProximitySensors.cpp

Go to the documentation of this file.

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

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

#include <mrpt/config/CConfigFileBase.h>
#include <mrpt/hwdrivers/CPhidgetInterfaceKitProximitySensors.h>
#include <mrpt/system/os.h>

#include <algorithm>
#include <iostream>

#if MRPT_HAS_PHIDGET
#include <phidget21.h>
#endif

using namespace mrpt::hwdrivers;
using namespace mrpt::obs;
using namespace mrpt::system;
using namespace std;

IMPLEMENTS_GENERIC_SENSOR(CPhidgetInterfaceKitProximitySensors, mrpt::hwdrivers)

/* -----------------------------------------------------
                Constructor
   ----------------------------------------------------- */
CPhidgetInterfaceKitProximitySensors::CPhidgetInterfaceKitProximitySensors()
    : mrpt::system::COutputLogger("CPhidgetInterfaceKitProximitySensors")

{
#if MRPT_HAS_PHIDGET
    m_carteInterfaceKit = new CPhidgetInterfaceKitHandle;
    *((CPhidgetInterfaceKitHandle*)m_carteInterfaceKit) = 0;
    m_sensorLabel = "PhidgetInterfaceKit";

    m_sensorIsPlugged.assign(8, false);
    m_minRange.assign(8, 0.1f);
    m_maxRange.assign(8, 0.8f);
    m_sensorPoses.resize(8);
    m_sensorType.assign(8, UNPLUGGED);

#else
    THROW_EXCEPTION(
        "MRPT Was compiled without the CPhidget support. Recompile MRPT to use "
        "this class");
#endif
}

/* -----------------------------------------------------
                loadConfig_sensorSpecific
   ----------------------------------------------------- */
void CPhidgetInterfaceKitProximitySensors::loadConfig_sensorSpecific(
    const mrpt::config::CConfigFileBase& configSource,
    const std::string& iniSection)
{
#if MRPT_HAS_PHIDGET
    if (!configSource.sectionExists(iniSection))
        THROW_EXCEPTION("Can't find section in configuration file");
    // looking for the board parameters.
    // process_rate = 100                       // Hz (common to all sensors)
    // serialNumber = 12345                     // The interface kit serial
    // number.
    m_process_rate =
        configSource.read_int(iniSection, string("process_rate"), 50);
    m_serialNumber =
        configSource.read_int(iniSection, string("serialNumber"), -1);
    bool display = configSource.read_bool(
        iniSection, string("displayRecapitulativeInformations"), false);

    // Looking for each sensor.

    for (int i = 1; i <= 8; i++)
    {
        string sensorNKeyName = format("sensor%d", i);
        string sensorType = configSource.read_string(
            iniSection, sensorNKeyName, string("UNPLUGGED"));
        if (sensorType != string("UNPLUGGED"))
        {
            // the sensor is plugged :
            // // check if the sensor type is supported.
            if (sensorType == string("EZ1"))
            {
                m_sensorType[i - 1] = EZ1;
                m_minRange[i - 1] = 0.15;  // meters
                m_maxRange[i - 1] = 6.45;  // meters
            }
            else if (sensorType == string("SHARP-30cm"))
            {
                m_sensorType[i - 1] = SHARP_30cm;
                m_minRange[i - 1] = 0.04;  // meters
                m_maxRange[i - 1] = 0.3;  // meters
            }
            else if (sensorType == string("SHARP-80cm"))
            {
                m_sensorType[i - 1] = SHARP_80cm;
                m_minRange[i - 1] = 0.06;  // meters
                m_maxRange[i - 1] = 0.8;  // meters
            }
            else
            {
                string err = format("Type of sensor %d is not supported", i);
                m_state = CGenericSensor::ssError;
                THROW_EXCEPTION(err);
            }
            m_sensorIsPlugged[i - 1] = true;
            // reading the sensor pose.
            string sensorNPoseX = format("pose%d_x", i);
            string sensorNPoseY = format("pose%d_y", i);
            string sensorNPoseZ = format("pose%d_z", i);
            string sensorNPoseYaw = format("pose%d_yaw", i);
            string sensorNPosePitch = format("pose%d_pitch", i);
            string sensorNPoseRoll = format("pose%d_roll", i);

            float x = configSource.read_float(iniSection, sensorNPoseX, 0.0);
            float y = configSource.read_float(iniSection, sensorNPoseY, 0.0);
            float z = configSource.read_float(iniSection, sensorNPoseZ, 0.0);
            float yaw =
                configSource.read_float(iniSection, sensorNPoseYaw, 0.0);
            float pitch =
                configSource.read_float(iniSection, sensorNPosePitch, 0.0);
            float roll =
                configSource.read_float(iniSection, sensorNPoseRoll, 0.0);

            m_sensorPoses[i - 1] =
                mrpt::poses::CPose3D(x, y, z, yaw, pitch, roll);
        }
    }
    if (display)
    {  // width = 80;
        cout.fill(' ');
        cout << "+-------------------------------------------------------------"
                "-----------------+"
             << endl;
        cout.width(79);
        cout << "|  Phidget interfaceKit board number : " << m_serialNumber;
        cout << "|" << endl;
        cout << "| Process rate : " << m_process_rate;
        cout << "|" << endl;
        cout << "+---------+---------------------+-----------------------------"
                "-----------------+"
             << endl;
        cout << "|    #    + Sensor type         | Sensor 3D pose              "
                "                 |"
             << endl;
        cout << "+---------+---------------------+-----------------------------"
                "-----------------+"
             << endl;
        for (int i = 0; i < 8; i++)
        {
            cout << "|";
            cout.width(9);
            cout << i + 1;
            cout << " |";
            cout.width(19);
            switch (m_sensorType[i])
            {
                case EZ1:
                    cout << "EZ1 |";
                    break;
                case SHARP_30cm:
                    cout << "SHARP_30cm |";
                    break;
                case SHARP_80cm:
                    cout << "SHARP_80cm |";
                    break;
                case UNPLUGGED:
                    cout << "UNPLUGGED |";
                    break;
            }
            cout.width(43);
            cout << m_sensorPoses[i];
            cout << "|" << endl;
        }
        cout << "+-------------------------------------------------------------"
                "-----------------+"
             << endl;
    }
#else
    MRPT_UNUSED_PARAM(configSource);
    MRPT_UNUSED_PARAM(iniSection);
#endif
}

/* -----------------------------------------------------
                Initialize
   ----------------------------------------------------- */
void CPhidgetInterfaceKitProximitySensors::initialize()
{
#if MRPT_HAS_PHIDGET
    /*Try to connect to the interface kit board*/
    CPhidgetInterfaceKit_create(
        (CPhidgetInterfaceKitHandle*)m_carteInterfaceKit);
    CPhidget_open(*((CPhidgetHandle*)(m_carteInterfaceKit)), m_serialNumber);
    int err = CPhidget_waitForAttachment(
        *((CPhidgetHandle*)(m_carteInterfaceKit)),
        200);  // wait 200ms for board attachment.
    // if an error occur, "err" will be a > 0 value.
    if (err > 0)
    {
        m_state = CGenericSensor::ssError;
        THROW_EXCEPTION(
            "Can't find Phidget IK card, please check your serial number.");
    }
    // set frame rate
    /*int miliseconds =
    static_cast<int>(1000./d2f(m_process_rate)); for(int i = 0 ;
    i < 8 ; i++)
    {
        if(m_sensorIsPlugged[i])
        {
            int err =
    CPhidgetInterfaceKit_setDataRate(*((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)),
    i, miliseconds); if(err > 0)
            {
                string error = format("Can't set process rate to %d ms on
    channel %d of the Phidget IK Board.", miliseconds, i); m_state =
    CGenericSensor::ssError; THROW_EXCEPTION(error);
            }
        }
    }*/  // seems to be used only in the event based programming
                         // way.
    // compute (min/max) of (min/max) ranges.
    m_minOfMinRanges = *min_element(m_minRange.begin(), m_minRange.end());
    m_maxOfMaxRanges = *max_element(m_maxRange.begin(), m_maxRange.end());
    // driver is ready.
    m_state = CGenericSensor::ssWorking;
#endif
}

/* -----------------------------------------------------
                Destructor
   ----------------------------------------------------- */
CPhidgetInterfaceKitProximitySensors::~CPhidgetInterfaceKitProximitySensors()
{
#if MRPT_HAS_PHIDGET
    if (*((CPhidgetHandle*)m_carteInterfaceKit))
    {
        CPhidget_close(*((CPhidgetHandle*)(m_carteInterfaceKit)));
        CPhidget_delete(*((CPhidgetHandle*)(m_carteInterfaceKit)));
    }
#endif
}

/*-------------------------------------------------------------
                    doProcess
-------------------------------------------------------------*/
void CPhidgetInterfaceKitProximitySensors::doProcess()
{
    CObservationRange::Ptr obs = std::make_shared<CObservationRange>();

    try
    {
        getObservation(*obs);
        m_state = ssWorking;
        // if at least one data have been sensed :
        if (obs->sensedData.size() > 0)
        {
            appendObservation(obs);
        }
    }
    catch (...)
    {
        m_state = ssError;
        THROW_EXCEPTION("No observation received from the Phidget board!");
    }
}

/*-------------------------------------------------------------
                    getObservation
-------------------------------------------------------------*/
void CPhidgetInterfaceKitProximitySensors::getObservation(
    mrpt::obs::CObservationRange& obs)
{
#if MRPT_HAS_PHIDGET
    obs.timestamp = mrpt::system::getCurrentTime();
    obs.sensorLabel = m_sensorLabel;
    obs.minSensorDistance = m_minOfMinRanges;
    obs.maxSensorDistance = m_maxOfMaxRanges;
    obs.sensorConeApperture =
        DEG2RAD(2.0f);  // TODO : Adapt to real sensor cone apperture.
    obs.sensedData.clear();

    int sensorValue;
    for (int i = 0; i < 8; i++)
    {
        if (m_sensorIsPlugged[i])
        {
            mrpt::obs::CObservationRange::TMeasurement obsRange;
            int err = CPhidgetInterfaceKit_getSensorValue(
                *((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)), i,
                &sensorValue);
            if (err > 0)
            {
                string error(
                    "Error durring acquiering sensor value on channel : %d", i);
                THROW_EXCEPTION(error);
            }
            switch (m_sensorType[i])
            {
                case EZ1:
                    // TODO : find the conversion formula.
                    obsRange.sensedDistance = 1.0;
                    break;
                case SHARP_30cm:
                    obsRange.sensedDistance = 2076. / (d2f(sensorValue) - 11.);
                    break;
                case SHARP_80cm:
                    obsRange.sensedDistance =
                        4800. / (d2f(sensorValue) - 16.92);
                    break;
                default:
                    obsRange.sensedDistance = -1;
                    break;
            }

            obsRange.sensorID = i;
            obsRange.sensorPose =
                m_sensorPoses[i]
                    .asTPose();  // The pose of the IR sensor on the robot
            obs.sensedData.push_back(obsRange);
        }
    }
#else
    MRPT_UNUSED_PARAM(obs);
#endif
}