From 52ccd2b01a5bae3d4513ef96d7fa22a0135a5e5b Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Mon, 5 Aug 2024 23:08:41 +0900 Subject: [PATCH] feat: add utils for pointcloud (#115) * create geometry/conversion.hpp and pointcloud/transform_pointcloud.hpp Signed-off-by: Autumn60 * add dummy_test for each namespaces Signed-off-by: Autumn60 * pre-commit Signed-off-by: Autumn60 --------- Signed-off-by: Autumn60 --- .../wheel_stuck_common_utils/CMakeLists.txt | 2 + .../geometry/conversion.hpp | 49 +++++++++++++++++++ .../pointcloud/transform_pointcloud.hpp | 43 ++++++++++++++++ .../wheel_stuck_common_utils/package.xml | 5 ++ .../test/geometry/dummy_test.cpp | 15 ++++++ .../test/pointcloud/dummy_test.cpp | 15 ++++++ .../test/ros/dummy_test.cpp | 16 ++++++ 7 files changed, 145 insertions(+) create mode 100644 src/common/wheel_stuck_common_utils/include/wheel_stuck_common_utils/geometry/conversion.hpp create mode 100644 src/common/wheel_stuck_common_utils/include/wheel_stuck_common_utils/pointcloud/transform_pointcloud.hpp create mode 100644 src/common/wheel_stuck_common_utils/test/geometry/dummy_test.cpp create mode 100644 src/common/wheel_stuck_common_utils/test/pointcloud/dummy_test.cpp create mode 100644 src/common/wheel_stuck_common_utils/test/ros/dummy_test.cpp diff --git a/src/common/wheel_stuck_common_utils/CMakeLists.txt b/src/common/wheel_stuck_common_utils/CMakeLists.txt index 3266b8d..8bf3ad3 100644 --- a/src/common/wheel_stuck_common_utils/CMakeLists.txt +++ b/src/common/wheel_stuck_common_utils/CMakeLists.txt @@ -14,6 +14,8 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() +include_directories(include) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/src/common/wheel_stuck_common_utils/include/wheel_stuck_common_utils/geometry/conversion.hpp b/src/common/wheel_stuck_common_utils/include/wheel_stuck_common_utils/geometry/conversion.hpp new file mode 100644 index 0000000..30046e5 --- /dev/null +++ b/src/common/wheel_stuck_common_utils/include/wheel_stuck_common_utils/geometry/conversion.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 Fool Stuck Engineers +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef WHEEL_STUCK_COMMON_UTILS__GEOMETRY__CONVERSION_HPP_ +#define WHEEL_STUCK_COMMON_UTILS__GEOMETRY__CONVERSION_HPP_ + +#include +#include + +namespace wheel_stuck_common_utils::geometry +{ + +geometry_msgs::msg::Pose to_pose(const geometry_msgs::msg::Transform & transform) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = transform.translation.x; + pose.position.y = transform.translation.y; + pose.position.z = transform.translation.z; + pose.orientation = transform.rotation; + return pose; +} + +Eigen::Matrix4f to_matrix4f(const geometry_msgs::msg::Pose & pose) +{ + Eigen::Affine3d affine; + tf2::fromMsg(pose, affine); + return affine.cast().matrix(); +} + +Eigen::Matrix4f to_matrix4f(const geometry_msgs::msg::Transform & transform) +{ + Eigen::Affine3d affine; + tf2::fromMsg(to_pose(transform), affine); + return affine.cast().matrix(); +} +} // namespace wheel_stuck_common_utils::geometry + +#endif // WHEEL_STUCK_COMMON_UTILS__GEOMETRY__CONVERSION_HPP_ diff --git a/src/common/wheel_stuck_common_utils/include/wheel_stuck_common_utils/pointcloud/transform_pointcloud.hpp b/src/common/wheel_stuck_common_utils/include/wheel_stuck_common_utils/pointcloud/transform_pointcloud.hpp new file mode 100644 index 0000000..01a00d7 --- /dev/null +++ b/src/common/wheel_stuck_common_utils/include/wheel_stuck_common_utils/pointcloud/transform_pointcloud.hpp @@ -0,0 +1,43 @@ +// Copyright 2024 Fool Stuck Engineers +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef WHEEL_STUCK_COMMON_UTILS__POINTCLOUD__TRANSFORM_POINTCLOUD_HPP_ +#define WHEEL_STUCK_COMMON_UTILS__POINTCLOUD__TRANSFORM_POINTCLOUD_HPP_ + +#include + +#include +#include + +namespace wheel_stuck_common_utils::pointcloud +{ +template +void try_transform_pointcloud( + const pcl::PointCloud & input, pcl::PointCloud & output, + const Eigen::Affine3d & transform) +{ + pcl::transformPointCloud(input, output, transform); +} + +template +pcl::PointCloud transform_pointcloud( + const pcl::PointCloud & input, const Eigen::Affine3d & transform) +{ + pcl::PointCloud output; + pcl::transformPointCloud(input, output, transform); + return output; +} +} // namespace wheel_stuck_common_utils::pointcloud + +#endif // WHEEL_STUCK_COMMON_UTILS__POINTCLOUD__TRANSFORM_POINTCLOUD_HPP_ diff --git a/src/common/wheel_stuck_common_utils/package.xml b/src/common/wheel_stuck_common_utils/package.xml index 413c301..36a8d53 100644 --- a/src/common/wheel_stuck_common_utils/package.xml +++ b/src/common/wheel_stuck_common_utils/package.xml @@ -10,7 +10,12 @@ ament_cmake_auto + geometry_msgs + pcl_ros rclcpp + tf2_eigen + tf2_geometry_msgs + tf2_ros ament_lint_auto diff --git a/src/common/wheel_stuck_common_utils/test/geometry/dummy_test.cpp b/src/common/wheel_stuck_common_utils/test/geometry/dummy_test.cpp new file mode 100644 index 0000000..dbddc37 --- /dev/null +++ b/src/common/wheel_stuck_common_utils/test/geometry/dummy_test.cpp @@ -0,0 +1,15 @@ +// Copyright 2024 Fool Stuck Engineers +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "wheel_stuck_common_utils/geometry/conversion.hpp" diff --git a/src/common/wheel_stuck_common_utils/test/pointcloud/dummy_test.cpp b/src/common/wheel_stuck_common_utils/test/pointcloud/dummy_test.cpp new file mode 100644 index 0000000..922ce14 --- /dev/null +++ b/src/common/wheel_stuck_common_utils/test/pointcloud/dummy_test.cpp @@ -0,0 +1,15 @@ +// Copyright 2024 Fool Stuck Engineers +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "wheel_stuck_common_utils/pointcloud/transform_pointcloud.hpp" diff --git a/src/common/wheel_stuck_common_utils/test/ros/dummy_test.cpp b/src/common/wheel_stuck_common_utils/test/ros/dummy_test.cpp new file mode 100644 index 0000000..b93d50a --- /dev/null +++ b/src/common/wheel_stuck_common_utils/test/ros/dummy_test.cpp @@ -0,0 +1,16 @@ +// Copyright 2024 Fool Stuck Engineers +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "wheel_stuck_common_utils/ros/function_timer.hpp" +#include "wheel_stuck_common_utils/ros/no_callback_subscription.hpp"