LaserScannerPointCloudProvider.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    VisionX::ArmarXObjects::LaserScannerPointCloudProvider
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "LaserScannerPointCloudProvider.h"
#include "ArmarXCore/core/logging/Logging.h"
 
namespace armarx
{
 
    const std::size_t MAX_LASER_SCANNER_POINTS = 1024 * 10;
 
 
    void armarx::LaserScannerPointCloudProvider::onInitPointCloudProvider()
    {
        // usingProxy(getProperty<std::string>("LaserScannerUnitName").getValue());
        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
        resultCloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
 
        filter.setMeanK(50);
        filter.setStddevMulThresh(1.0);
 
    }
 
    void armarx::LaserScannerPointCloudProvider::onConnectPointCloudProvider()
    {
 
        robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
        // laserScanner = getProxy<LaserScannerUnitInterfacePrx>(getProperty<std::string>("LaserScannerUnitName").getValue());
 
        localRobot =  RemoteRobot::createLocalClone(robotStateComponent);
 
        // for (const LaserScannerInfo& info : laserScanner->getConnectedDevices())
        // {
        //      deviceToFrame[info.device] = info.frame;
        // }
 
    }
 
    void armarx::LaserScannerPointCloudProvider::onExitPointCloudProvider()
    {
    }
 
 
 
    void armarx::LaserScannerPointCloudProvider::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& scan, const TimestampBasePtr& timestamp, const Ice::Current& c)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud;
        cloud.points.reserve(scan.size());
 
        for (const LaserScanStep& l : scan)
        {
            pcl::PointXYZ p;
            p.x = -l.distance * std::sin(l.angle);
            p.y = l.distance * std::cos(l.angle);
            p.z = 0.0;
            cloud.points.push_back(p);
        }
 
        cloud.height = 1;
        cloud.width = cloud.points.size();
 
 
        std::unique_lock<std::mutex> lock(mutex, std::try_to_lock);
 
        if (!lock.owns_lock())
        {
            ARMARX_INFO << deactivateSpam(3) << "unable to process new laser scan";
            return;
        }
 
 
        RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
        // std::string frameName = deviceToFrame[device];
 
        const std::string& frameName = name;
 
        if (localRobot->hasRobotNode(frameName))
        {
            Eigen::Matrix4f transform = localRobot->getRobotNode(frameName)->getPoseInRootFrame();
            pcl::transformPointCloud(cloud, clouds[name], transform);
 
            resultCloud->clear();
            for (auto& kv : clouds)
            {
                const pcl::PointCloud<pcl::PointXYZ>& c = kv.second;
                (*resultCloud) += c;
            }
        }
        else
        {
            ARMARX_WARNING << deactivateSpam(5) << "Unknown frame for laser scanner: " << frameName;
        }
 
        if(resultCloud->empty())
        {
            ARMARX_INFO << deactivateSpam(10) << "Input point cloud is empty. Will not report anything.";
            return;
        }
 
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFiltered(new pcl::PointCloud<pcl::PointXYZ>());
        filter.setInputCloud(resultCloud);
        filter.filter(*cloudFiltered);
 
 
        ARMARX_DEBUG << "Publishing point cloud";
        providePointCloud(cloudFiltered);
    }
 
 
    armarx::PropertyDefinitionsPtr LaserScannerPointCloudProvider::createPropertyDefinitions()
    {
        auto def =  armarx::PropertyDefinitionsPtr(new LaserScannerPointCloudProviderPropertyDefinitions(
                        getConfigIdentifier()));
 
        def->topic<LaserScannerUnitListener>("LaserScans", "laser_scanner_topic", "Name of the laser scanner topic.");
 
        return def;
    }
 
 
    visionx::MetaPointCloudFormatPtr LaserScannerPointCloudProvider::getDefaultPointCloudFormat()
    {
        visionx::MetaPointCloudFormatPtr info = new visionx::MetaPointCloudFormat();
        info->capacity = MAX_LASER_SCANNER_POINTS * sizeof(visionx::Point3D);
        info->size = info->capacity;
        info->type = visionx::PointContentType::ePoints;
        return info;
    }
 
}