feat: add utils for pointcloud (#115)

* create geometry/conversion.hpp and pointcloud/transform_pointcloud.hpp

Signed-off-by: Autumn60 <[email protected]>

* add dummy_test for each namespaces

Signed-off-by: Autumn60 <[email protected]>

* pre-commit

Signed-off-by: Autumn60 <[email protected]>

---------

Signed-off-by: Autumn60 <[email protected]>

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()

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 <Eigen/Core>
+#include <tf2_eigen/tf2_eigen.hpp>
+
+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<float>().matrix();
+}
+
+Eigen::Matrix4f to_matrix4f(const geometry_msgs::msg::Transform & transform)
+{
+  Eigen::Affine3d affine;
+  tf2::fromMsg(to_pose(transform), affine);
+  return affine.cast<float>().matrix();
+}
+}  // namespace wheel_stuck_common_utils::geometry
+
+#endif  // WHEEL_STUCK_COMMON_UTILS__GEOMETRY__CONVERSION_HPP_

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 <Eigen/Core>
+
+#include <pcl/common/transforms.h>
+#include <pcl/point_cloud.h>
+
+namespace wheel_stuck_common_utils::pointcloud
+{
+template <typename PointT>
+void try_transform_pointcloud(
+  const pcl::PointCloud<PointT> & input, pcl::PointCloud<PointT> & output,
+  const Eigen::Affine3d & transform)
+{
+  pcl::transformPointCloud(input, output, transform);
+}
+
+template <typename PointT>
+pcl::PointCloud<PointT> transform_pointcloud(
+  const pcl::PointCloud<PointT> & input, const Eigen::Affine3d & transform)
+{
+  pcl::PointCloud<PointT> output;
+  pcl::transformPointCloud(input, output, transform);
+  return output;
+}
+}  // namespace wheel_stuck_common_utils::pointcloud
+
+#endif  // WHEEL_STUCK_COMMON_UTILS__POINTCLOUD__TRANSFORM_POINTCLOUD_HPP_

src/common/wheel_stuck_common_utils/package.xml

@@ -10,7 +10,12 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
+  <depend>geometry_msgs</depend>
+  <depend>pcl_ros</depend>
   <depend>rclcpp</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
 
   <test_depend>ament_lint_auto</test_depend>
 

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"

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"

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"