Port spline interpolation to ros2. (#65)

autowarefoundation · Nov 2, 2020 · 78cc569 · 78cc569
1 parent b0682a9
commit 78cc569
Show file tree

Hide file tree

Showing 5 changed files with 23 additions and 50 deletions.
diff --git a/common/math/spline_interpolation/CMakeLists.txt b/common/math/spline_interpolation/CMakeLists.txt
@@ -1,43 +1,14 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(spline_interpolation)
 
-add_compile_options(-std=c++14)
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 14)
+endif()
 
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
-catkin_package(
-  INCLUDE_DIRS
-    include
-  LIBRARIES
-    spline_interpolation
-)
 
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-)
+ament_auto_add_library(spline_interpolation src/spline_interpolation.cpp)
 
-add_library(spline_interpolation src/spline_interpolation.cpp)
-
-target_link_libraries(spline_interpolation
-  ${catkin_LIBRARIES}
-)
-
-add_dependencies(spline_interpolation
-  ${catkin_EXPORTED_TARGETS}
-)
-
-install(
-  TARGETS
-    spline_interpolation
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-## Install project namespaced headers
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
+ament_auto_package()
diff --git a/common/math/spline_interpolation/COLCON_IGNORE b/common/math/spline_interpolation/COLCON_IGNORE
diff --git a/common/math/spline_interpolation/include/spline_interpolation/spline_interpolation.h b/common/math/spline_interpolation/include/spline_interpolation/spline_interpolation.h
@@ -22,7 +22,7 @@
 #include <numeric>
 #include <vector>
 
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 
 namespace spline_interpolation
 {

diff --git a/common/math/spline_interpolation/package.xml b/common/math/spline_interpolation/package.xml
@@ -1,15 +1,17 @@
 <?xml version="1.0"?>
-<package format="2">
-  <name>spline_interpolation</name>
-  <version>0.1.0</version>
-  <description>The spline interpolation package</description>
-  <maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
-  <author email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</author>
-
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+<name>spline_interpolation</name>
+<version>0.1.0</version>
+<description>The spline interpolation package</description>
+<maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
+<author email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</author>
   <license>Apache 2</license>
+<buildtool_depend>ament_cmake_auto</buildtool_depend>
 
-  <buildtool_depend>catkin</buildtool_depend>
+<depend>rclcpp</depend>
 
-  <depend>roscpp</depend>
-
+<export>
+  <build_type>ament_cmake</build_type>
+</export>
 </package>
diff --git a/common/math/spline_interpolation/src/spline_interpolation.cpp b/common/math/spline_interpolation/src/spline_interpolation.cpp
@@ -267,7 +267,7 @@ std::vector<double> PreconditionedConjugateGradient::solve() const
 
   for (size_t i = 1; i < x.size(); ++i) x[i] = x_in[i - 1];
 
-  if (num_iter == max_iter_) ROS_WARN("[interpolate (PCG)] unconverged!");
+  if (num_iter == max_iter_) RCLCPP_WARN(rclcpp::get_logger("PreconditionedConjugateGradient"), "[interpolate (PCG)] unconverged!");
   return x;
 }
 
@@ -309,7 +309,7 @@ std::vector<double> SOR::solve() const
     }
     ++num_iter;
   }
-  if (num_iter > max_iter_) ROS_WARN("[interpolate (SOR)] unconverged!");
+  if (num_iter > max_iter_) RCLCPP_WARN(rclcpp::get_logger("PreconditionedConjugateGradient"), "[interpolate (SOR)] unconverged!");
   return ans_next;
 }