PointCloud.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
 * Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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/>.
 *
 * @author     Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
// Header
#include "PointCloud.h"
 
#include <SimoxUtility/meta/type_name.h>
 
namespace armarx::aron::codegenerator::cpp::generator
{
    const std::map<type::pointcloud::VoxelType, std::tuple<std::string, int, std::string>>
        VoxelType2Cpp = {
            // see http://docs.ros.org/en/groovy/api/pcl/html/point__types_8hpp_source.html
            {type::pointcloud::VoxelType::POINT_XYZ,
             {"pcl::PointXYZ", 16, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZ"}},
            {type::pointcloud::VoxelType::POINT_XYZI,
             {"pcl::PointXYZI", 32, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZI"}},
            {type::pointcloud::VoxelType::POINT_XYZL,
             {"pcl::PointXYZL", 32, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZL"}},
            {type::pointcloud::VoxelType::POINT_XYZRGB,
             {"pcl::PointXYZRGB", 32, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZRGB"}},
            {type::pointcloud::VoxelType::POINT_XYZRGBL,
             {"pcl::PointXYZRGBL",
              32,
              "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZRGBL"}},
            {type::pointcloud::VoxelType::POINT_XYZRGBA,
             {"pcl::PointXYZRGBA",
              32,
              "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZRGBA"}},
            {type::pointcloud::VoxelType::POINT_XYZHSV,
             {"pcl::PointXYZHSV",
              32,
              "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZHSV"}}};
 
    // constructors
    PointCloud::PointCloud(const type::PointCloud& n) :
        detail::NDArrayGenerator<type::PointCloud, PointCloud>(
            "pcl::PointCloud<" + std::get<0>(VoxelType2Cpp.at(n.getVoxelType())) + ">",
            "pcl::PointCloud<" + std::get<0>(VoxelType2Cpp.at(n.getVoxelType())) + ">",
            simox::meta::get_type_name<data::dto::NDArray>(),
            simox::meta::get_type_name<type::dto::PointCloud>(),
            n)
    {
    }
 
    std::vector<std::string>
    PointCloud::getRequiredIncludes() const
    {
        return {"<pcl/point_cloud.h>", "<pcl/point_types.h>"};
    }
 
    std::pair<std::vector<std::pair<std::string, std::string>>, bool>
    PointCloud::getCtorInitializers(const std::string& name) const
    {
        /** for now we do not have any default values
        if (type.getDefaultValue() == aron::type::pointcloud::default_value::DEFAULT ||
            type.getDefaultValue() == aron::type::pointcloud::default_value::ZEROS)
        {
            return {{{name, getInstantiatedCppTypename() + "::Zero()"}}, true};
        }
        else if (type.getDefaultValue() == aron::type::pointcloud::default_value::ONES)
        {
            return {{{name, getInstantiatedCppTypename() + "::One()"}}, true};
        }
        else 
        */
 
        if (type.getDefaultValue() == aron::type::pointcloud::default_value::DEFAULT ||
            type.getDefaultValue() == aron::type::pointcloud::default_value::ZEROS)
        {
            std::cerr << "Currently ARON does not support default values for pointclouds. Ignoring "
                         "your request";
        }
        else if (type.getDefaultValue() == aron::type::pointcloud::default_value::ONES)
        {
            std::cerr << "Currently ARON does not support default values for pointclouds. Ignoring "
                         "your request";
        }
 
        if (type.getMaybe() == type::Maybe::OPTIONAL)
        {
            return {{{name, "std::nullopt"}}, true};
        }
        return {{}, false};
    }
 
    CppBlockPtr
    PointCloud::getResetSoftBlock(const std::string& cppAccessor) const
    {
        CppBlockPtr block_if_data = std::make_shared<CppBlock>();
        block_if_data->addLine(cppAccessor + " = " + getFullInstantiatedCppTypenameGenerator() +
                               "(" + cppAccessor + nextEl() + "width, " + cppAccessor + nextEl() +
                               "height);");
        return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
    }
 
    CppBlockPtr
    PointCloud::getWriteTypeBlock(const std::string& typeAccessor,
                                  const std::string& cppAccessor,
                                  const Path& p,
                                  std::string& variantAccessor) const
    {
        CppBlockPtr b = std::make_shared<CppBlock>();
        std::string escaped_accessor = EscapeAccessor(cppAccessor);
        variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
 
        b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writePointCloud(" +
                   std::get<2>(VoxelType2Cpp.at(type.getVoxelType())) + ", " + "\"" +
                   type.getDefaultValue() + "\", " +
                   conversion::Maybe2CppString.at(type.getMaybe()) + ", " + "armarx::aron::Path(" +
                   ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
                   "})); // of " + cppAccessor);
        return b;
    }
 
    CppBlockPtr
    PointCloud::getWriteBlock(const std::string& cppAccessor,
                              const Path& p,
                              std::string& variantAccessor) const
    {
        CppBlockPtr block_if_data = std::make_shared<CppBlock>();
        std::string escaped_accessor = EscapeAccessor(cppAccessor);
        variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
 
        block_if_data->addLine(
            variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({ static_cast<int>(" +
            cppAccessor + nextEl() + "width), static_cast<int>(" + cppAccessor + nextEl() +
            "height), " + std::to_string(std::get<1>(VoxelType2Cpp.at(type.getVoxelType()))) +
            "}, " + "\"" + std::get<0>(VoxelType2Cpp.at(type.getVoxelType())) + "\", " +
            "reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() +
            "points.data()), " + "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
            simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
 
        return resolveMaybeWriteBlock(block_if_data, cppAccessor);
    }
 
    CppBlockPtr
    PointCloud::getReadBlock(const std::string& cppAccessor,
                             const std::string& variantAccessor) const
    {
        CppBlockPtr block_if_data = std::make_shared<CppBlock>();
        std::string escaped_accessor = EscapeAccessor(cppAccessor);
        std::string type = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_typeAsString";
        std::string dims = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_shape";
        std::string data = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_data";
 
        block_if_data->addLine("std::string " + type + ";");
        block_if_data->addLine("std::vector<int> " + dims + ";");
        block_if_data->addLine("std::vector<unsigned char> " + data + ";");
        block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray(" + variantAccessor +
                               ", " + dims + ", " + type + ", " + data + "); // of " + cppAccessor);
 
        block_if_data->addLine(
            "ARMARX_CHECK_AND_THROW(" + dims +
            ".size() == 3, ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"Received "
            "wrong dimensions for member '" +
            cppAccessor + "'.\"));");
        block_if_data->addLine("ARMARX_CHECK_AND_THROW(" + type + " == \"" +
                               std::get<0>(VoxelType2Cpp.at(this->type.getVoxelType())) +
                               "\", ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, "
                               "\"Received wrong type for member '" +
                               cppAccessor + "'.\"));");
 
        // assign is not availablePCL for PCL 1.8
        // block_if_data->addLine(cppAccessor + nextEl() + "assign(" + dims + "[0], " + dims + "[1], " + std::get<0>(VoxelType2Cpp.at(this->type.getVoxelType())) + "());");
        block_if_data->addLine(cppAccessor + nextEl() + "clear();");
        // resize(width, height , value) is not available either
        // block_if_data->addLine(cppAccessor + nextEl() + "resize(" + dims + "[0], " + dims + "[1], " + std::get<0>(VoxelType2Cpp.at(this->type.getVoxelType())) + "()));");
        block_if_data->addLine(cppAccessor + nextEl() + "resize(" + dims + "[0] * " + dims +
                               "[1]);");
        block_if_data->addLine(cppAccessor + nextEl() + "width = " + dims + "[0];");
        block_if_data->addLine(cppAccessor + nextEl() + "height = " + dims + "[1];");
 
        block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor +
                               nextEl() + "points.data()), " + data + ".data(), " + data +
                               ".size());");
        return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
    }
 
    CppBlockPtr
    PointCloud::getEqualsBlock(const std::string& accessor,
                               const std::string& otherInstanceAccessor) const
    {
        CppBlockPtr block_if_data = std::make_shared<CppBlock>();
        block_if_data->addLine("if (" + accessor + nextEl() + "width != " + otherInstanceAccessor +
                               nextEl() + "width || " + accessor + nextEl() +
                               "height != " + otherInstanceAccessor + nextEl() + "height)");
        block_if_data->addLineAsBlock("return false;");
 
        //block_if_data->addLine("if (" + accessor + nextEl() + "points != " + otherInstanceAccessor + nextEl() + "points)");
        //block_if_data->addLine("\t return false;");
        return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
    }
} // namespace armarx::aron::codegenerator::cpp::generator