Test refactoring

.github/workflows/build_and_test.yml

@@ -22,6 +22,10 @@ jobs:
         with:
           required-ros-distributions: foxy
 
+      - name: Concat build_depends.repos
+        run: |
+          curl -sSL https://raw.githubusercontent.com/tier4/AutowareArchitectureProposal.iv/ros2/build_depends.repos | sed '1d' >> build_depends.repos
+
       - name: Run action-ros-ci
         id: action_ros_ci_step
         uses: ros-tooling/[email protected]

ament_cmake_auto_gtest/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(ament_cmake_auto_gtest NONE)
+
+find_package(ament_cmake REQUIRED)
+
+ament_package(
+  CONFIG_EXTRAS "ament_cmake_auto_gtest-extras.cmake"
+)
+
+install(
+  DIRECTORY cmake
+  DESTINATION share/${PROJECT_NAME}
+)

ament_cmake_auto_gtest/README.md

@@ -0,0 +1,3 @@
+NOTE: This package is temporary and will be removed after the following pull requests have been merged.
+
+<https://github.com/ament/ament_cmake/pull/257>

ament_cmake_auto_gtest/ament_cmake_auto_gtest-extras.cmake

@@ -0,0 +1,20 @@
+# Copyright 2014 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+# copied from ament_cmake_auto/ament_cmake_auto-extras.cmake
+
+find_package(ament_cmake_auto QUIET REQUIRED)
+find_package(ament_cmake_gtest QUIET REQUIRED)
+
+include("${ament_cmake_auto_gtest_DIR}/ament_auto_add_gtest.cmake")

ament_cmake_auto_gtest/cmake/ament_auto_add_gtest.cmake

@@ -0,0 +1,116 @@
+# Copyright 2014 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+#
+# Add a gtest, automatically linking and including dependencies.
+#
+# Call add_executable(target ARGN), link it against the gtest libraries
+# and register the executable as a test.
+#
+# If gtest is not available the specified target is not being created and
+# therefore the target existence should be checked before being used.
+#
+#
+# All arguments of the CMake function ``add_executable()`` can be
+# used beside the custom arguments ``DIRECTORY`` and
+# ``NO_TARGET_LINK_LIBRARIES``.
+#
+# :param target: the name of the executable target
+# :type target: string
+# :param DIRECTORY: the directory to recursively glob for source
+#   files with the following extensions: c, cc, cpp, cxx
+# :type DIRECTORY: string
+# :param NO_TARGET_LINK_LIBRARIES: if set skip linking against
+#   ``${PROJECT_NAME}_LIBRARIES``
+# :type NO_TARGET_LINK_LIBRARIES: option
+# :param ARGN: the list of source files
+# :type ARGN: list of strings
+# :param RUNNER: the path to the test runner script
+#   (default: see ament_add_test).
+# :type RUNNER: string
+# :param TIMEOUT: the test timeout in seconds,
+#   default defined by ``ament_add_test()``
+# :type TIMEOUT: integer
+# :param WORKING_DIRECTORY: the working directory for invoking the
+#   executable in, default defined by ``ament_add_test()``
+# :type WORKING_DIRECTORY: string
+# :param SKIP_LINKING_MAIN_LIBRARIES: if set skip linking against the gtest
+#   main libraries
+# :type SKIP_LINKING_MAIN_LIBRARIES: option
+# :param SKIP_TEST: if set mark the test as being skipped
+# :type SKIP_TEST: option
+# :param ENV: list of env vars to set; listed as ``VAR=value``
+# :type ENV: list of strings
+# :param APPEND_ENV: list of env vars to append if already set, otherwise set;
+#   listed as ``VAR=value``
+# :type APPEND_ENV: list of strings
+# :param APPEND_LIBRARY_DIRS: list of library dirs to append to the appropriate
+#   OS specific env var, a la LD_LIBRARY_PATH
+# :type APPEND_LIBRARY_DIRS: list of strings
+#
+# @public
+#
+macro(ament_auto_add_gtest target)
+  cmake_parse_arguments(ARG
+    # ament_add_gtest flags
+    "SKIP_LINKING_MAIN_LIBRARIES;SKIP_TEST"
+    "RUNNER;TIMEOUT;WORKING_DIRECTORY"
+    "APPEND_ENV;APPEND_LIBRARY_DIRS;ENV"
+    # ament_add_executable flags
+    "WIN32;MACOSX_BUNDLE;EXCLUDE_FROM_ALL;NO_TARGET_LINK_LIBRARIES"
+    # ament_auto_add_gtest flags
+    "DIRECTORY"
+    ""
+    ${ARGN})
+  if(NOT ARG_DIRECTORY AND NOT ARG_UNPARSED_ARGUMENTS)
+    message(FATAL_ERROR "ament_auto_add_executable() called without any "
+      "source files and without a DIRECTORY argument")
+  endif()
+
+  set(_source_files "")
+  if(ARG_DIRECTORY)
+    # glob all source files
+    file(
+      GLOB_RECURSE
+      _source_files
+      RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}"
+      "${ARG_DIRECTORY}/*.c"
+      "${ARG_DIRECTORY}/*.cc"
+      "${ARG_DIRECTORY}/*.cpp"
+      "${ARG_DIRECTORY}/*.cxx"
+    )
+    if(NOT _source_files)
+      message(FATAL_ERROR "ament_auto_add_executable() no source files found "
+        "in directory '${CMAKE_CURRENT_SOURCE_DIR}/${ARG_DIRECTORY}'")
+    endif()
+  endif()
+
+  # parse again to "remove" custom arguments
+  cmake_parse_arguments(ARG "NO_TARGET_LINK_LIBRARIES" "DIRECTORY" "" ${ARGN})
+  ament_add_gtest("${target}" ${ARG_UNPARSED_ARGUMENTS} ${_source_files})
+
+  # add include directory of this package if it exists
+  if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/include")
+    target_include_directories("${target}" PUBLIC
+      "${CMAKE_CURRENT_SOURCE_DIR}/include")
+  endif()
+  # link against other libraries of this package
+  if(NOT ${PROJECT_NAME}_LIBRARIES STREQUAL "" AND
+      NOT ARG_NO_TARGET_LINK_LIBRARIES)
+    target_link_libraries("${target}" ${${PROJECT_NAME}_LIBRARIES})
+  endif()
+
+  # add exported information from found build dependencies
+  ament_target_dependencies(${target} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
+endmacro()

ament_cmake_auto_gtest/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>ament_cmake_auto_gtest</name>
+  <version>0.9.2</version>
+  <description>The auto-magic functions for ease to use of the ament buildsystem in CMake.</description>
+  <maintainer email="[email protected]">Dirk Thomas</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_gtest</buildtool_depend>
+
+  <buildtool_export_depend>ament_cmake_auto</buildtool_export_depend>
+  <buildtool_export_depend>ament_cmake_gtest</buildtool_export_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

obstacle_stop_planner_refine/CMakeLists.txt

@@ -7,38 +7,47 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_EXTENSIONS OFF)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic -O3)
+  add_compile_options(-Wall -Wextra -Wpedantic)
   add_compile_options(-Wno-unused-parameter)
 endif()
 
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
 find_package(Eigen3 REQUIRED)
-find_package(OpenCV REQUIRED)
-find_package(PCL REQUIRED)
-
-ament_auto_add_executable(obstacle_stop_planner_node
-  src/debug_marker.cpp
-  src/node.cpp
-  src/main.cpp
-  src/adaptive_cruise_control.cpp
-)
 
-target_include_directories(obstacle_stop_planner_node
-  PUBLIC
-    ${OpenCV_INCLUDE_DIRS}
-    ${PCL_INCLUDE_DIRS}
+file(GLOB_RECURSE OBSTACLE_STOP_PLANNER_NODES_NODE_SRC
+  src/*
+)
+file(GLOB_RECURSE OBSTACLE_STOP_PLANNER_NODES_NODE_HEADERS
+  include/obstacle_stop_planner/*
 )
 
-target_link_libraries(obstacle_stop_planner_node
-    ${OpenCV_LIBRARIES}
-    ${PCL_LIBRARIES}
+# generate component node library
+ament_auto_add_library(obstacle_stop_planner_node SHARED
+  ${OBSTACLE_STOP_PLANNER_NODES_NODE_SRC}
+  ${OBSTACLE_STOP_PLANNER_NODES_NODE_HEADERS}
+)
+rclcpp_components_register_node(obstacle_stop_planner_node
+  PLUGIN "obstacle_stop_planner::ObstacleStopPlannerNode"
+  EXECUTABLE obstacle_stop_planner_node_exe
 )
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  add_ros_test(
+    test/obstacle_stop_planner_node_launch_test.py
+    TIMEOUT "30"
+  )
+
+  find_package(ament_cmake_auto_gtest REQUIRED)
+  file(GLOB_RECURSE test_files test/**/*.cpp)
+  ament_auto_add_gtest(test_obstacle_stop_planner
+    test/test_obstacle_stop_planner.test
+    ${test_files}
+  )
 endif()
 
 ament_auto_package(

obstacle_stop_planner_refine/config/vehicle_info.param.yaml

@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+    wheel_radius: 0.39
+    wheel_width: 0.42
+    wheel_base: 2.74 # between front wheel center and rear wheel center
+    wheel_tread: 1.63 # between left wheel center and right wheel center
+    front_overhang: 1.0 # between front wheel center and vehicle front
+    rear_overhang: 1.03 # between rear wheel center and vehicle rear
+    left_overhang: 0.1 # between left wheel center and vehicle left
+    right_overhang: 0.1 # between right wheel center and vehicle right
+    vehicle_height: 2.5