Move ROS dependent files to 'src/ros_dependent'

localization/lidar_feature_localization/CMakeLists.txt

@@ -14,18 +14,19 @@ set(CMAKE_CXX_OUTPUT_EXTENSION_REPLACE ON)
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_auto REQUIRED)
 find_package(fmt REQUIRED)
+find_package(PCL REQUIRED)
 
 ament_auto_find_build_dependencies()
 
-set(INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
+set(INCLUDE_DIRS
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+  ${rotationlib_INCLUDE_DIRS})
 
 set(LIBRARY_SOURCE_FILES
   src/lib/algorithm.cpp
   src/lib/csv.cpp
   src/lib/eigen.cpp
-  src/lib/lib.cpp
   src/lib/pcl_utils.cpp
-  src/lib/ros_msg.cpp
   src/lib/random.cpp
   src/lib/stats.cpp)
 
@@ -54,11 +55,13 @@ set(EXTRACTION_SOURCE_FILES
   src/extraction/ring.cpp)
 
 set(ROS_DEPENDENT_SOURCE_FILES
-  src/ros_dependent/map_receiver.cpp)
+  src/ros_dependent/map_receiver.cpp
+  src/ros_dependent/ros_msg.cpp
+  src/ros_dependent/ring.cpp)
 
-ament_auto_add_library(library SHARED ${LIBRARY_SOURCE_FILES})
-ament_auto_add_library(localization SHARED ${LOCALIZATION_SOURCE_FILES})
-ament_auto_add_library(extraction SHARED ${EXTRACTION_SOURCE_FILES})
+add_library(library SHARED ${LIBRARY_SOURCE_FILES})
+add_library(localization SHARED ${LOCALIZATION_SOURCE_FILES})
+add_library(extraction SHARED ${EXTRACTION_SOURCE_FILES})
 ament_auto_add_library(ros_dependent SHARED ${ROS_DEPENDENT_SOURCE_FILES})
 
 target_include_directories(library PUBLIC ${INCLUDE_DIRS})
@@ -67,8 +70,8 @@ target_include_directories(extraction PUBLIC ${INCLUDE_DIRS})
 target_include_directories(ros_dependent PUBLIC ${INCLUDE_DIRS})
 
 target_link_libraries(library ${PCL_LIBRARIES} fmt)
-target_link_libraries(localization fmt)
-target_link_libraries(extraction fmt)
+target_link_libraries(localization ${PCL_LIBRARIES} fmt library)
+target_link_libraries(extraction ${PCL_LIBRARIES} fmt library)
 target_link_libraries(ros_dependent ${PCL_LIBRARIES})
 
 ament_auto_add_executable("lidar_feature_localization" app/localization.cpp)
@@ -184,7 +187,6 @@ if(BUILD_TESTING)
     test/lib/test_numeric.cpp
     test/lib/test_pcl_utils.cpp
     test/lib/test_random.cpp
-    test/lib/test_ros_msg.cpp
     test/lib/test_span.cpp
     test/lib/test_stats.cpp)
 
@@ -193,7 +195,9 @@ if(BUILD_TESTING)
   endforeach()
 
   set(ROS_DEPENDENT_TEST_FILES
-    test/ros_dependent/test_map_reciever.cpp)
+    test/ros_dependent/test_map_reciever.cpp
+    test/ros_dependent/test_ring.cpp
+    test/ros_dependent/test_ros_msg.cpp)
 
   foreach(filepath ${ROS_DEPENDENT_TEST_FILES})
     add_ros_dependent_testcase(${filepath})

localization/lidar_feature_localization/app/localization.cpp

@@ -48,11 +48,9 @@
 
 #include "lidar_feature_extraction/edge_surface_extraction.hpp"
 #include "lidar_feature_extraction/hyper_parameter.hpp"
-#include "lidar_feature_extraction/ring.hpp"
 
 #include "lidar_feature_library/point_type.hpp"
 #include "lidar_feature_library/qos.hpp"
-#include "lidar_feature_library/ros_msg.hpp"
 #include "lidar_feature_library/warning.hpp"
 
 #include "lidar_feature_localization/debug.hpp"
@@ -61,6 +59,9 @@
 #include "lidar_feature_localization/posevec.hpp"
 #include "lidar_feature_localization/stamp_sorted_objects.hpp"
 
+#include "ros_dependent/ring.hpp"
+#include "ros_dependent/ros_msg.hpp"
+
 using Odometry = nav_msgs::msg::Odometry;
 using PointCloud2 = sensor_msgs::msg::PointCloud2;
 using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;

localization/lidar_feature_localization/include/lidar_feature_extraction/label.hpp

@@ -38,7 +38,6 @@
 #include <vector>
 
 #include <boost/range/adaptor/reversed.hpp>
-#include <rclcpp/rclcpp.hpp>
 
 #include "lidar_feature_extraction/algorithm.hpp"
 #include "lidar_feature_extraction/cloud_iterator.hpp"

localization/lidar_feature_localization/include/lidar_feature_extraction/ring.hpp

@@ -41,16 +41,11 @@
 #include <unordered_map>
 #include <vector>
 
-#include <sensor_msgs/msg/point_cloud2.hpp>
-#include <sensor_msgs/msg/point_field.hpp>
-
 #include "cloud_iterator.hpp"
 #include "iterator.hpp"
 #include "mapped_points.hpp"
 
 
-bool RingIsAvailable(const std::vector<sensor_msgs::msg::PointField> & fields);
-
 template<typename T>
 struct AHasSmallerPolarAngleThanB
 {

localization/lidar_feature_localization/include/lidar_feature_library/eigen.hpp

@@ -41,10 +41,6 @@
 #include "lidar_feature_library/matrix_types.hpp"
 
 
-Matrix6d GetEigenCovariance(const std::array<double, 36> & covariance);
-
-std::array<double, 36> FromEigenCovariance(const Matrix6d & covariance);
-
 template<int N>
 Eigen::MatrixXd VectorsToEigen(const std::vector<Eigen::Matrix<double, N, 1>> & vectors)
 {

localization/lidar_feature_localization/include/ros_dependent/ring.hpp

@@ -0,0 +1,38 @@
+// Copyright 2023 The Autoware Contributors, Takeshi Ishita
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the Autoware Contributors, Takeshi Ishita nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef ROS_DEPENDENT__RING_HPP_
+#define ROS_DEPENDENT__RING_HPP_
+
+#include <vector>
+
+#include <sensor_msgs/msg/point_field.hpp>
+
+bool RingIsAvailable(const std::vector<sensor_msgs::msg::PointField> & fields);
+
+#endif  // ROS_DEPENDENT__RING_HPP_

localization/lidar_feature_localization/include/lidar_feature_library/ros_msg.hpp → localization/lidar_feature_localization/include/ros_dependent/ros_msg.hpp

@@ -50,6 +50,9 @@
 
 #include "lidar_feature_library/matrix_types.hpp"
 
+Matrix6d GetEigenCovariance(const std::array<double, 36> & covariance);
+std::array<double, 36> FromEigenCovariance(const Matrix6d & covariance);
+
 template<typename T>
 sensor_msgs::msg::PointCloud2 ToRosMsg(const typename pcl::PointCloud<T>::Ptr & cloud_ptr)
 {

localization/lidar_feature_localization/src/extraction/ring.cpp

@@ -32,17 +32,6 @@
 
 #include "lidar_feature_extraction/ring.hpp"
 
-
-bool RingIsAvailable(const std::vector<sensor_msgs::msg::PointField> & fields)
-{
-  for (const auto & field : fields) {
-    if (field.name == "ring") {
-      return true;
-    }
-  }
-  return false;
-}
-
 void RemoveSparseRings(
   std::unordered_map<int, std::vector<int>> & rings,
   const int n_min_points)

localization/lidar_feature_localization/src/lib/eigen.cpp

@@ -32,28 +32,6 @@
 #include "lidar_feature_library/eigen.hpp"
 
 
-Matrix6d GetEigenCovariance(const std::array<double, 36> & covariance)
-{
-  Matrix6d matrix;
-  for (size_t i = 0; i < 6; i++) {
-    for (size_t j = 0; j < 6; j++) {
-      matrix(i, j) = covariance[i * 6 + j];
-    }
-  }
-  return matrix;
-}
-
-std::array<double, 36> FromEigenCovariance(const Matrix6d & covariance)
-{
-  std::array<double, 36> array;
-  for (size_t i = 0; i < 6; i++) {
-    for (size_t j = 0; j < 6; j++) {
-      array[i * 6 + j] = covariance(i, j);
-    }
-  }
-  return array;
-}
-
 Eigen::Isometry3d MakeIsometry3d(const Eigen::Quaterniond & q, const Eigen::Vector3d & t)
 {
   assert(std::abs(q.norm() - 1.0) < 1e-6);

localization/lidar_feature_localization/src/lib/lib.cpp → localization/lidar_feature_localization/src/ros_dependent/ring.cpp

@@ -1,4 +1,4 @@
-// Copyright 2022 Tixiao Shan, Takeshi Ishita
+// Copyright 2023 The Autoware Contributors, Takeshi Ishita
 //
 // Redistribution and use in source and binary forms, with or without
 // modification, are permitted provided that the following conditions are met:
@@ -10,7 +10,7 @@
 //      notice, this list of conditions and the following disclaimer in the
 //      documentation and/or other materials provided with the distribution.
 //
-//    * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its
+//    * Neither the name of the Autoware Contributors, Takeshi Ishita nor the names of its
 //      contributors may be used to endorse or promote products derived from
 //      this software without specific prior written permission.
 //
@@ -26,6 +26,15 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
-#include "lidar_feature_library/convert_point_cloud_type.hpp"
-#include "lidar_feature_library/downsample.hpp"
-#include "lidar_feature_library/ros_msg.hpp"
+#include "ros_dependent/ring.hpp"
+
+
+bool RingIsAvailable(const std::vector<sensor_msgs::msg::PointField> & fields)
+{
+  for (const auto & field : fields) {
+    if (field.name == "ring") {
+      return true;
+    }
+  }
+  return false;
+}

localization/lidar_feature_localization/src/lib/ros_msg.cpp → localization/lidar_feature_localization/src/ros_dependent/ros_msg.cpp

@@ -28,8 +28,30 @@
 
 #include <string>
 
-#include "lidar_feature_library/eigen.hpp"
-#include "lidar_feature_library/ros_msg.hpp"
+// #include "lidar_feature_library/eigen.hpp"
+#include "ros_dependent/ros_msg.hpp"
+
+Matrix6d GetEigenCovariance(const std::array<double, 36> & covariance)
+{
+  Matrix6d matrix;
+  for (size_t i = 0; i < 6; i++) {
+    for (size_t j = 0; j < 6; j++) {
+      matrix(i, j) = covariance[i * 6 + j];
+    }
+  }
+  return matrix;
+}
+
+std::array<double, 36> FromEigenCovariance(const Matrix6d & covariance)
+{
+  std::array<double, 36> array;
+  for (size_t i = 0; i < 6; i++) {
+    for (size_t j = 0; j < 6; j++) {
+      array[i * 6 + j] = covariance(i, j);
+    }
+  }
+  return array;
+}
 
 Eigen::Isometry3d GetIsometry3d(const geometry_msgs::msg::Pose & pose)
 {

localization/lidar_feature_localization/test/extraction/test_ring.cpp

@@ -128,23 +128,6 @@ TEST(AHasSmallerPolarAngleThanB, Random)
   }
 }
 
-TEST(Ring, RingIsAvailable)
-{
-  const std::vector<sensor_msgs::msg::PointField> with_ring = {
-    sensor_msgs::msg::PointField()
-    .set__name("intensity").set__offset(16).set__datatype(7).set__count(1),
-    sensor_msgs::msg::PointField()
-    .set__name("ring").set__offset(20).set__datatype(4).set__count(1)
-  };
-  EXPECT_TRUE(RingIsAvailable(with_ring));
-
-  const std::vector<sensor_msgs::msg::PointField> without_ring = {
-    sensor_msgs::msg::PointField()
-    .set__name("intensity").set__offset(16).set__datatype(7).set__count(1)
-  };
-  EXPECT_FALSE(RingIsAvailable(without_ring));
-}
-
 TEST(Ring, SortByAtan2)
 {
   std::vector<Point> points;

localization/lidar_feature_localization/test/lib/test_eigen.cpp

@@ -34,42 +34,6 @@
 #include "lidar_feature_library/eigen.hpp"
 
 
-TEST(RosMsg, GetEigenCovariance)
-{
-  const std::array<double, 36ul> covarinace = {
-    0, 1, 2, 3, 4, 5,
-    6, 7, 8, 9, 10, 11,
-    12, 13, 14, 15, 16, 17,
-    18, 19, 20, 21, 22, 23,
-    24, 25, 26, 27, 28, 29,
-    30, 31, 32, 33, 34, 35
-  };
-
-  const Matrix6d C = GetEigenCovariance(covarinace);
-  EXPECT_EQ(C(0, 0), 0);
-  EXPECT_EQ(C(2, 0), 12);
-  EXPECT_EQ(C(3, 4), 22);
-  EXPECT_EQ(C(5, 2), 32);
-}
-
-TEST(RosMsg, FromEigenCovariance)
-{
-  Matrix6d C;
-  C <<
-    0, 1, 2, 3, 4, 5,
-    6, 7, 8, 9, 10, 11,
-    12, 13, 14, 15, 16, 17,
-    18, 19, 20, 21, 22, 23,
-    24, 25, 26, 27, 28, 29,
-    30, 31, 32, 33, 34, 35;
-
-  const std::array<double, 36ul> covarinace = FromEigenCovariance(C);
-  EXPECT_EQ(covarinace[0 * 6 + 0], 0);
-  EXPECT_EQ(covarinace[2 * 6 + 0], 12);
-  EXPECT_EQ(covarinace[3 * 6 + 4], 22);
-  EXPECT_EQ(covarinace[5 * 6 + 2], 32);
-}
-
 TEST(Transform, MakeIsometry3d)
 {
   const Eigen::Quaterniond q = Eigen::Quaterniond(-1., 1., 1., 1).normalized();