CGillAnemometer.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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/hwdrivers/CGillAnemometer.h>
#include <mrpt/system/datetime.h>

#include <iostream>
#include <iterator>
#include <sstream>
#include <thread>

using namespace std;
using namespace mrpt::hwdrivers;

IMPLEMENTS_GENERIC_SENSOR(CGillAnemometer, mrpt::hwdrivers)

/* -----------------------------------------------------
                Constructor
   ----------------------------------------------------- */
CGillAnemometer::CGillAnemometer() { m_sensorLabel = "WINDSONIC"; }
/* -----------------------------------------------------
                loadConfig_sensorSpecific
   ----------------------------------------------------- */
void CGillAnemometer::loadConfig_sensorSpecific(
    const mrpt::config::CConfigFileBase& configSource,
    const std::string& iniSection)
{
#ifdef _WIN32
    com_port =
        configSource.read_string(iniSection, "COM_port_WIN", "COM1", true);
#else
    com_port =
        configSource.read_string(iniSection, "COM_port_LIN", "/dev/tty0", true);
#endif

    com_bauds = configSource.read_int(iniSection, "COM_baudRate", 9600, false);

    pose_x = configSource.read_float(iniSection, "pose_x", 0, true);
    pose_y = configSource.read_float(iniSection, "pose_y", 0, true);
    pose_z = configSource.read_float(iniSection, "pose_z", 0, true);
    pose_roll = configSource.read_float(iniSection, "pose_roll", 0, true);
    pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, true);
    pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, true);
}

/* -----------------------------------------------------
                tryToOpenTheCOM
----------------------------------------------------- */
bool CGillAnemometer::tryToOpenTheCOM()
{
    if (COM.isOpen()) return true;  // Already open

    if (m_verbose)
        cout << "[CGillAnemometer] Opening " << com_port << " @ " << com_bauds
             << endl;

    try
    {
        COM.open(com_port);
        COM.setConfig(com_bauds, 0, 8, 1);
        COM.setTimeouts(50, 1, 100, 1, 20);
        COM.purgeBuffers();

        return true;  // All OK!
    }
    catch (std::exception& e)
    {
        std::cerr << "[CGillAnemometer::tryToOpenTheCOM] Error opening or "
                     "configuring the serial port:"
                  << std::endl
                  << e.what();
        COM.close();
        return false;
    }
    catch (...)
    {
        std::cerr << "[CGillAnemometer::tryToOpenTheCOM] Error opening or "
                     "configuring the serial port."
                  << std::endl;
        COM.close();
        return false;
    }
}

/* -----------------------------------------------------
                doProcess
----------------------------------------------------- */
void CGillAnemometer::doProcess()
{
    // Is the COM open?
    if (!tryToOpenTheCOM())
    {
        m_state = ssError;
        printf("ERROR: No observation received from the Anemometer!\n");
        THROW_EXCEPTION("Cannot open the serial port");
    }

    mrpt::obs::CObservationWindSensor::Ptr obsPtr =
        mrpt::make_aligned_shared<mrpt::obs::CObservationWindSensor>();
    bool have_reading = false;
    std::string wind_reading;
    bool time_out = false;

    try
    {
        while (!have_reading)
        {
            // Read info into string: (default is Polar continuous)
            // format = <STX>Q, dir, speed, units, status, <ETX>
            // Q      -> Sensor identifier
            // dir    -> 3 digits (0-359)
            // speed  -> %3.2f
            // units  -> M (m/s), K (km/h)
            // status -> 00 = ok, else is an Error Code

            wind_reading = COM.ReadString(500, &time_out);
            if (time_out)
            {
                cout << "[CGillAnemometer] " << com_port << " @ " << com_bauds
                     << " - measurement Timed-Out" << endl;
                std::this_thread::sleep_for(10ms);
            }
            else
                have_reading = true;
        }

        // parse format = <STX>Q, dir, speed, units, status, <ETX>
        std::deque<std::string> list;
        mrpt::system::tokenize(wind_reading, ",", list);
        if (list.size() == 6)
        {
            // Status
            int status = atoi(list.at(4).c_str());
            if (status == 0)
            {
                // Units
                std::string s_units = list.at(3);
                // Module
                std::string s_speed = list.at(2);
                if (s_units == "M")
                    obsPtr->speed = atof(s_speed.c_str());
                else if (s_units == "K")
                    obsPtr->speed = atof(s_speed.c_str()) * 1000 / 3600;
                else
                {
                    printf(
                        "ERROR: WindSonic measurement units not supported: "
                        "%s\n",
                        s_units.c_str());
                    obsPtr->speed = 0.0;
                }
                // angle
                std::string s_direction = list.at(1);
                obsPtr->direction = atof(s_direction.c_str());

                // Prepare observation
                obsPtr->sensorLabel = m_sensorLabel;
                obsPtr->timestamp = mrpt::system::getCurrentTime();
                obsPtr->sensorPoseOnRobot = mrpt::poses::CPose3D(
                    pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll);
                appendObservation(obsPtr);
            }
            else
                printf("ERROR: WindSonic error code %u\n", status);
        }
        else if (list.size() == 5)  // when there is no wind, the sensor does
        // not provide a wind direction
        {
            // Status
            int status = atoi(list.at(3).c_str());
            if (status == 0)
            {
                // Units
                std::string s_units = list.at(2);
                // module
                std::string s_speed = list.at(1);
                if (s_units == "M")
                    obsPtr->speed = atof(s_speed.c_str());
                else if (s_units == "K")
                    obsPtr->speed = atof(s_speed.c_str()) * 1000 / 3600;
                else
                {
                    printf(
                        "ERROR: WindSonic measurement units not supported: "
                        "%s\n",
                        s_units.c_str());
                    obsPtr->speed = 0.0;
                }
                // Angle
                obsPtr->direction = 0.0;

                // Prepare observation
                obsPtr->sensorLabel = m_sensorLabel;
                obsPtr->timestamp = mrpt::system::getCurrentTime();
                obsPtr->sensorPoseOnRobot = mrpt::poses::CPose3D(
                    pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll);
                appendObservation(obsPtr);
            }
            else
                printf("ERROR: WindSonic error code %u\n", status);
        }
        else
        {
            printf(
                "ERROR: Windsonic reading incorrect format: %s [%u]\n",
                wind_reading.c_str(), static_cast<unsigned int>(list.size()));
        }
    }
    catch (std::exception& e)
    {
        std::cerr << "[CGillAnemometer::doProcess] Error:" << std::endl
                  << e.what();
    }
    catch (...)
    {
        std::cerr << "[CGillAnemometer::doProcess] Unknown Error" << std::endl;
    }
}