diff --git a/source/RobotAPI/libraries/aron/core/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
index 3e43781ec6ae4cff3ff33ded8965deec84cd833c..6a81faa502be6c0a667d66949d5bfa67624900c7 100644
--- a/source/RobotAPI/libraries/aron/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
@@ -5,9 +5,6 @@ armarx_set_target("Library: ${LIB_NAME}")
find_package(Eigen3 QUIET)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-if(Eigen3_FOUND)
- include_directories(${Eigen3_INCLUDE_DIR})
-endif()
find_package(Simox QUIET)
armarx_build_if(Simox_FOUND "Simox not available")
@@ -15,14 +12,21 @@ if(Simox_FOUND)
include_directories(${Simox_INCLUDE_DIR})
endif()
+find_package(PCL QUIET COMPONENTS io common)
+armarx_build_if(PCL_FOUND "PCL not available")
+
set(LIBS
ArmarXCoreInterfaces
ArmarXCore
RobotAPIInterfaces
cppgen
-
SimoxUtility
+
+ #${Simox_LIBS}
+ # System libraries
+ #Eigen3::Eigen
+ # PCLInterface
)
set(LIB_FILES
@@ -218,7 +222,17 @@ set(LIB_HEADERS
codegenerator/typeReader/xml/Reader.h
)
+
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+if(PCL_FOUND)
+target_include_directories("${LIB_NAME}"
+ SYSTEM PUBLIC
+ "${PCL_INCLUDE_DIRS}"
+)
+endif()
+
+
# add unit tests
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
index 1ba373de5aaaf56fb7062da4134792991b80ac47..1d4c51c5480309d096420ef8af6e8af2176c428e 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
@@ -24,6 +24,8 @@
// Header
#include "PCLPointCloud.h"
+#include <RobotAPI/libraries/core/pcl_point_types.h>
+
namespace armarx::aron::cppcodegenerator::serializer
{
@@ -37,6 +39,7 @@ namespace armarx::aron::cppcodegenerator::serializer
{"PointXYZRGBL", {"pcl::PointXYZRGBL", 32}},
{"PointXYZRGBA", {"pcl::PointXYZRGBA", 32}},
{"PointXYZHSV", {"pcl::PointXYZHSV", 32}},
+ {"PointPolar", {"pcl::PointPolar", 16}},
};
// constructors
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp
index 77485c5fdb3b8ce814366b0b73492d249235ab5c..cf56e483356695bf2b54a747e36c4d758e7f1a56 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.cpp
@@ -29,20 +29,38 @@
// ArmarX
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Data.h>
#include <RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h>
+
+namespace fs = std::filesystem;
+
namespace armarx::aron::xmltypereader
{
- void Reader::parseFile(const std::string& filename)
+ void Reader::parseFile(const std::string& _filename)
{
- RapidXmlReaderPtr reader = RapidXmlReader::FromFile(filename);
- parse(reader);
+ std::string filename = _filename;
+ // Handle C++ style includes like "<path/to/file>".
+ if (!filename.empty() && filename.front() == '<' && filename.back() == '>')
+ {
+ filename = filename.substr(1, filename.size() - 2);
+ }
+ parseFile(std::filesystem::path(filename));
}
- void Reader::parseFile(const std::filesystem::path& file)
+ void Reader::parseFile(const std::filesystem::path& _file)
{
+ fs::path file = _file;
+ if (!file.empty() && file.is_relative())
+ {
+ if (std::optional<fs::path> resolved = detail::resolveRelativePackagePath(file))
+ {
+ file = resolved.value();
+ }
+ }
+
RapidXmlReaderPtr reader = RapidXmlReader::FromFile(file.string());
parse(reader);
}
@@ -185,4 +203,30 @@ namespace armarx::aron::xmltypereader
return typenavigator::IntEnumNavigator::DynamicCastAndCheck(factory.create(node, Path()));
}
+
+
+ std::optional<fs::path> detail::resolveRelativePackagePath(const fs::path& path)
+ {
+ const std::string package = *path.begin();
+ armarx::CMakePackageFinder finder(package);
+ if (finder.packageFound())
+ {
+ for (const std::string& includePath : finder.getIncludePathList())
+ {
+ fs::path absPath = includePath / path;
+ if (fs::is_regular_file(absPath))
+ {
+ return absPath;
+ }
+ }
+ return std::nullopt;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
}
+
+
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h
index 0801465160e06ddbd0ebfc78b57d8b8bb97eafcd..e19083a9aa2718792c705ec41267a4e7f50d2cc8 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/typeReader/xml/Reader.h
@@ -24,8 +24,10 @@
#pragma once
// STD/STL
-#include <memory>
#include <filesystem>
+#include <memory>
+#include <optional>
+
// Base Class
#include <RobotAPI/libraries/aron/core/codegenerator/typeReader/Reader.h>
@@ -63,4 +65,10 @@ namespace armarx::aron::xmltypereader
private:
ReaderFactory factory;
};
+
+
+ namespace detail
+ {
+ std::optional<std::filesystem::path> resolveRelativePackagePath(const std::filesystem::path& path);
+ }
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h
index 2f62f1b579b5e84b3acc93aa7a72d70329492736..a5e7f249f322d246cc3ce5af1a2f510904b472f6 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/ndarray/PCLPointCloud.h
@@ -79,7 +79,8 @@ namespace armarx::aron::typenavigator
{"PointXYZRGB", {"pcl::PointXYZRGB"}},
{"PointXYZRGBL", {"pcl::PointXYZRGBL"}},
{"PointXYZRGBA", {"pcl::PointXYZRGBA"}},
- {"PointXYZHSV", {"pcl::PointXYZHSV"}}
+ {"PointXYZHSV", {"pcl::PointXYZHSV"}},
+ {"PointPolar", {"pcl::PointPolar"}}
};
const unsigned int ACCEPTED_DIMENSION_MIN_SIZE = 2;
const unsigned int ACCEPTED_DIMENSION_MAX_SIZE = 2;
diff --git a/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
index 7eeffdbb49db4b321b77ed47fa1e58361d538094..de44d5a1d28a8148139265d1a18a6667c1c4a697 100644
--- a/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
@@ -40,20 +40,22 @@ armarx_enable_aron_file_generation_for_target(
TARGET_NAME
${TEST_NAME}
ARON_FILES
- xmls/HumanPoseTest.xml
+ # xmls/BaseClass.xml
+ # xmls/DerivedClassTest.xml
xmls/DictTest.xml
+ xmls/EigenMatrixTest.xml
+ xmls/EigenQuaternionTest.xml
+ xmls/EnumTest.xml
+ xmls/HumanPoseTest.xml
+ xmls/IVTCByteImageTest.xml
xmls/ListTest.xml
xmls/NaturalIKTest.xml
xmls/ObjectTest.xml
- xmls/PrimitiveTest.xml
- xmls/IVTCByteImageTest.xml
- xmls/EigenMatrixTest.xml
- xmls/EigenQuaternionTest.xml
xmls/OpenCVMatTest.xml
- xmls/PCLPointCloudTest.xml
- xmls/PositionTest.xml
xmls/OrientationTest.xml
+ xmls/PCLPointCloudTest.xml
xmls/PoseTest.xml
- xmls/EnumTest.xml
+ xmls/PositionTest.xml
+ xmls/PrimitiveTest.xml
#ENABLE_DEBUG_INFO
)
diff --git a/source/RobotAPI/libraries/core/pcl_point_types.h b/source/RobotAPI/libraries/core/pcl_point_types.h
new file mode 100644
index 0000000000000000000000000000000000000000..e610f72fed7ac5051c8ec43a842b2f60e811281d
--- /dev/null
+++ b/source/RobotAPI/libraries/core/pcl_point_types.h
@@ -0,0 +1,63 @@
+/*
+ * 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::Core
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#define PCL_NO_PRECOMPILE
+#include <pcl/pcl_macros.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+// #include <pcl/memory.h>
+
+#include <Eigen/Core>
+
+namespace pcl
+{
+
+ /**
+ * @brief Point for polar coordinates
+ *
+ */
+ struct PointPolar
+ {
+ // PCL_ADD_POINT4D; // preferred way of adding a XYZ+padding
+
+ //! radial distance
+ float r;
+
+ //! angle
+ float phi;
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ // PCL_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
+ } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment
+
+
+
+} // namespace pcl
+
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::PointPolar,
+ (float, r, r)
+ (float, phi, phi)
+ )
\ No newline at end of file