HokuyoLaserUnit.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::HokuyoLaserUnit
 * @author     Fabian Paus ( fabian dot paus at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "HokuyoLaserUnit.h"
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
#include <HokuyoLaserScannerDriver/urg_utils.h>
 
using namespace armarx;
 
HokuyoLaserUnit::HokuyoLaserUnit()
{
    addPlugin(heartbeat);
}
 
 
void
HokuyoLaserUnit::onInitComponent()
{
    offeringTopicFromProperty("RobotHealthTopicName");
    offeringTopicFromProperty("DebugObserverName");
 
    laserScannerListenerProxyName = getProperty<std::string>("LaserScannerListenerName").getValue();
    usingProxyFromProperty("LaserScannerListenerName");
    ARMARX_INFO << "Going to report to listener " << laserScannerListenerProxyName;
    updatePeriod = getProperty<int>("UpdatePeriod").getValue();
    angleOffset = getProperty<float>("AngleOffset").getValue();
 
    std::string deviceStrings = getProperty<std::string>("Devices").getValue();
    std::vector<std::string> splitDeviceStrings = Split(deviceStrings, ";");
    devices.clear();
    devices.reserve(splitDeviceStrings.size());
    for (std::string const& deviceString : splitDeviceStrings)
    {
        std::vector<std::string> deviceInfo = Split(deviceString, ",");
        if (deviceInfo.size() != 3)
        {
            ARMARX_WARNING << "Unexpected format for laser scanner device: " << deviceString
                           << " (split size: " << deviceInfo.size() << ")";
            continue;
        }
 
        try
        {
            int port = std::stoi(deviceInfo[1]);
 
            HokuyoLaserScanDevice& device = devices.emplace_back();
            device.ip = deviceInfo[0];
            device.port = port;
            device.frame = deviceInfo[2];
            device.angleOffset = angleOffset;
            device.connected = false;
 
            device.componentName = getName();
        }
        catch (std::exception const& ex)
        {
            ARMARX_WARNING << "Could not convert port to integer for laser scanner device "
                           << deviceString << " (port is " << deviceInfo[1] << ") : " << ex.what();
            continue;
        }
    }
}
 
void
HokuyoLaserUnit::onConnectComponent()
{
    ARMARX_TRACE;
 
    getProxyFromProperty(listenerPrx, "LaserScannerListenerName");
    debugObserver = getTopic<DebugObserverInterfacePrx>(
        getProperty<std::string>("DebugObserverName").getValue());
 
    connectedDevices.clear();
    for (HokuyoLaserScanDevice& device : devices)
    {
        if (device.task)
        {
            device.task->stop();
            device.task = nullptr;
        }
 
        // connecting devices for first time
        if (!device.reconnect())
        {
            ARMARX_WARNING << "Not starting task for laser scanner with IP: " << device.ip
                           << ", Port: " << device.port;
            continue;
        }
 
        ARMARX_CHECK_NOT_NULL(heartbeat);
        heartbeat->signUp(device.ip,
                          armarx::core::time::Duration::MilliSeconds(500),
                          armarx::core::time::Duration::MilliSeconds(800),
                          {"LaserScanner", "Localization"},
                          "HokuyoLaserScanDevice");
 
        LaserScannerInfo info;
        info.device = device.ip;
        info.frame = device.frame;
        int minStep = 0, maxStep = 0;
        urg_step_min_max(&device.urg, &minStep, &maxStep);
        info.minAngle = urg_step2rad(&device.urg, minStep);
        info.maxAngle = urg_step2rad(&device.urg, maxStep);
 
 
        int lengthDataSize = urg_max_data_size(&device.urg);
        info.stepSize = (info.maxAngle - info.minAngle) / (maxStep - minStep);
        device.lengthData.resize(lengthDataSize);
 
        device.listenerPrx = listenerPrx;
        device.robotHealthPlugin = heartbeat;
        device.debugObserver = debugObserver;
 
        connectedDevices.push_back(info);
 
        ARMARX_INFO << "Connected to " << device.ip << ", " << info.frame << ", " << info.minAngle
                    << ", " << info.maxAngle << ", " << info.stepSize;
 
        device.task = new RunningTask<HokuyoLaserScanDevice>(
            &device, &HokuyoLaserScanDevice::run, "HokuyoLaserScanUpdate_" + device.ip);
        device.task->start();
    }
}
 
void
HokuyoLaserUnit::onDisconnectComponent()
{
    for (HokuyoLaserScanDevice& device : devices)
    {
        if (device.task)
        {
            device.task->stop();
            device.task = nullptr;
        }
        if (device.connected)
        {
            urg_close(&device.urg);
            device.connected = false;
        }
    }
}
 
void
HokuyoLaserUnit::onExitComponent()
{
}
 
armarx::PropertyDefinitionsPtr
HokuyoLaserUnit::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new HokuyoLaserUnitPropertyDefinitions(getConfigIdentifier()));
}
 
std::string
HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const
{
    return laserScannerListenerProxyName;
}
 
LaserScannerInfoSeq
HokuyoLaserUnit::getConnectedDevices(const Ice::Current&) const
{
    return connectedDevices;
}
 
bool
HokuyoLaserScanDevice::reconnect()
{
    if (connected)
    {
        ARMARX_INFO << "Disconnecting from laser scanner with IP " << ip;
        urg_close(&urg);
        connected = false;
    }
    ARMARX_INFO << "Reconnecting to " << ip << ":" << port;
    int ret = urg_open(&urg, URG_ETHERNET, ip.c_str(), port);
    connected = (ret == 0);
    if (!connected)
    {
        ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: " << ip
                       << ", Port: " << port << ", Error: " << ret << ")";
        return false;
    }
    ret = urg_start_measurement(&urg, URG_DISTANCE, URG_SCAN_INFINITY, 0);
    connected = (ret == 0);
    if (connected)
    {
        ARMARX_IMPORTANT << "Reconnected succesffully to " << ip << ":" << port;
        return true;
    }
    else
    {
        ARMARX_WARNING
            << "Could not start measurement for laser scanner device using URG driver (IP: " << ip
            << ", Port: " << port << ", Error: " << ret << ")";
        return false;
    }
}
 
void
HokuyoLaserScanDevice::run()
{
    ARMARX_TRACE;
    while (!task->isStopped())
    {
        ARMARX_INFO << deactivateSpam() << "Running update loop for laserscanner device " << ip;
 
        IceUtil::Time time_start = TimeUtil::GetTime();
 
        if (errorCounter > 10)
        {
            ARMARX_ERROR << "Device " << ip << " has too many consecutive errors!";
            // assume dead
            reconnect();
        }
        if (connected)
        {
            int lengthDataSize = urg_get_distance(&urg, lengthData.data(), nullptr);
            if (lengthDataSize < 0)
            {
                ARMARX_WARNING << deactivateSpam(1)
                               << "Could not get measurement for laser scanner (IP: " << ip
                               << ", Port: " << port << ", Error: " << lengthDataSize << ")";
                errorCounter++;
                continue;
            }
            IceUtil::Time time_measure = TimeUtil::GetTime();
            TimestampVariantPtr now(new TimestampVariant(time_measure));
 
            scan.clear();
            scan.reserve(lengthDataSize);
            for (int stepIndex = 0; stepIndex < lengthDataSize; ++stepIndex)
            {
                LaserScanStep step;
                long distance = lengthData[stepIndex];
                // TODO: Extract the min and max valid value for distance into parameters?
                if (distance >= 21 && distance <= 30000)
                {
                    step.angle =
                        angleOffset + (float)urg_index2rad(&urg, stepIndex); // Convert steps to rad
                    step.distance = (float)distance; // Data is already in mm
                    scan.push_back(step);
                }
            }
 
            IceUtil::Time time_update = TimeUtil::GetTime();
 
            errorCounter = 0;
 
            if (listenerPrx)
            {
 
                listenerPrx->reportSensorValues(ip, frame, scan, now);
            }
            else
            {
                ARMARX_WARNING << "No proxy available: IP: " << ip << ", Port: " << port;
            }
            IceUtil::Time time_proxyReport = TimeUtil::GetTime();
 
            if (robotHealthPlugin != nullptr)
            {
                robotHealthPlugin->heartbeatOnChannel(ip);
            }
            else
            {
                ARMARX_WARNING << "No robot health topic available: IP: " << ip
                               << ", Port: " << port;
            }
 
            IceUtil::Time time_topicHeartbeat = TimeUtil::GetTime();
 
            IceUtil::Time duration = time_topicHeartbeat - time_start;
 
            StringVariantBaseMap durations;
            durations["total_ms"] = new Variant(duration.toMilliSecondsDouble());
            durations["measure_ms"] =
                new Variant((time_measure - time_start).toMilliSecondsDouble());
            durations["update_ms"] =
                new Variant((time_update - time_measure).toMilliSecondsDouble());
            durations["proxy_report_ms"] =
                new Variant((time_proxyReport - time_update).toMilliSecondsDouble());
            durations["topic_health_ms"] =
                new Variant((time_topicHeartbeat - time_proxyReport).toMilliSecondsDouble());
            debugObserver->setDebugChannel(
                "LaserScannerDuration_" + simox::alg::replace_all(ip, ".", "_"), durations);
 
            if (duration.toSecondsDouble() > 0.1)
            {
                ARMARX_WARNING << "Laserscanner reports are slow"
                               << "Total time:     " << duration.toMilliSecondsDouble() << "ms\n"
                               << "Measure:   "
                               << (time_measure - time_start).toMilliSecondsDouble() << "ms \n"
                               << "Update:    "
                               << (time_update - time_measure).toMilliSecondsDouble() << "ms\n"
                               << "TopicSensor: "
                               << (time_proxyReport - time_update).toMilliSecondsDouble() << "ms\n"
                               << "TopicHeart:  "
                               << (time_topicHeartbeat - time_proxyReport).toMilliSecondsDouble()
                               << "ms\n";
            }
        }
    }
}