From 78cc569d85491a9d4417636c9ce991b35ea0c7f6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Yunus=20Emre=20=C3=87al=C4=B1=C5=9Fkan?=
 <yunus.ec@gmail.com>
Date: Mon, 2 Nov 2020 07:25:47 +0100
Subject: [PATCH] Port spline interpolation to ros2. (#65)

---
 .../math/spline_interpolation/CMakeLists.txt  | 45 ++++---------------
 .../math/spline_interpolation/COLCON_IGNORE   |  0
 .../spline_interpolation.h                    |  2 +-
 common/math/spline_interpolation/package.xml  | 22 ++++-----
 .../src/spline_interpolation.cpp              |  4 +-
 5 files changed, 23 insertions(+), 50 deletions(-)
 delete mode 100644 common/math/spline_interpolation/COLCON_IGNORE

diff --git a/common/math/spline_interpolation/CMakeLists.txt b/common/math/spline_interpolation/CMakeLists.txt
index 264bd5da07b78..a482ea70a2fed 100644
--- 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()
\ No newline at end of file
diff --git a/common/math/spline_interpolation/COLCON_IGNORE b/common/math/spline_interpolation/COLCON_IGNORE
deleted file mode 100644
index e69de29bb2d1d..0000000000000
diff --git a/common/math/spline_interpolation/include/spline_interpolation/spline_interpolation.h b/common/math/spline_interpolation/include/spline_interpolation/spline_interpolation.h
index 41b98392b480c..3d4d850160896 100644
--- 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
index d1e0490876a81..dc4febdbc1ab8 100644
--- 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
index 78b3e4e9ccd65..834b6c40c16b4 100644
--- 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;
 }