wip: add unit test

.gitignore

@@ -22,7 +22,8 @@ __pycache__/
 .settings/
 
 # Visual Studio Code
-.vscode/
+.vscode/*
+!.vscode/launch.json
 *.code-workspace
 
 # CLion

.vscode/launch.json

@@ -0,0 +1,61 @@
+{
+  // IntelliSense を使用して利用可能な属性を学べます。
+  // 既存の属性の説明をホバーして表示します。
+  // 詳細情報は次を確認してください: https://go.microsoft.com/fwlink/?linkid=830387
+  "version": "0.2.0",
+  "configurations": [
+    {
+      "name": "(gdb) obstacle_stop_planner_node_exe",
+      "type": "cppdbg",
+      "request": "attach",
+      "program": "${workspaceFolder}/install/lib/obstacle_stop_planner_nodes/obstacle_stop_planner_node_exe",
+      "processId": "${command:pickProcess}",
+      "MIMode": "gdb",
+      "setupCommands": [
+          {
+              "description": "gdb の再フォーマットを有効にする",
+              "text": "-enable-pretty-printing",
+              "ignoreFailures": true
+          }
+      ]
+    },
+    {
+      "name": "(gdb) gtest",
+      "type": "cppdbg",
+      "request": "launch",
+      "program": "build/obstacle_stop_planner_refine/test_obstacle_stop_planner",
+      "args": [],
+      "stopAtEntry": false,
+      "cwd": "${workspaceFolder}",
+      "environment": [],
+      "externalConsole": false,
+      "MIMode": "gdb",
+      "setupCommands": [
+          {
+              "description": "gdb の再フォーマットを有効にする",
+              "text": "-enable-pretty-printing",
+              "ignoreFailures": false
+          }
+      ]
+    },
+    {
+      "name": "(gdb) gtest node",
+      "type": "cppdbg",
+      "request": "launch",
+      "program": "build/obstacle_stop_planner_nodes/test_obstacle_stop_planner_node",
+      "args": [],
+      "stopAtEntry": false,
+      "cwd": "${workspaceFolder}",
+      "environment": [],
+      "externalConsole": false,
+      "MIMode": "gdb",
+      "setupCommands": [
+          {
+              "description": "gdb の再フォーマットを有効にする",
+              "text": "-enable-pretty-printing",
+              "ignoreFailures": false
+          }
+      ]
+    },
+  ]
+}

obstacle_stop_planner_nodes_refine/CMakeLists.txt

@@ -53,15 +53,21 @@ 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"
+  )
+
   # Unit tests
-  # set(TEST_SOURCES test/test_obstacle_stop_planner_node.cpp)
-  # set(TEST_OBSTACLE_STOP_PLANNER_NODE_EXE test_obstacle_stop_planner_node)
-  # ament_add_gtest(${TEST_OBSTACLE_STOP_PLANNER_NODE_EXE} ${TEST_SOURCES})
-  # target_link_libraries(${TEST_OBSTACLE_STOP_PLANNER_NODE_EXE} obstacle_stop_planner_node)
+  set(TEST_SOURCES test/test_obstacle_stop_planner_node.cpp)
+  set(TEST_OBSTACLE_STOP_PLANNER_NODE_EXE test_obstacle_stop_planner_node)
+  ament_add_gtest(${TEST_OBSTACLE_STOP_PLANNER_NODE_EXE} ${TEST_SOURCES})
+  target_link_libraries(${TEST_OBSTACLE_STOP_PLANNER_NODE_EXE} obstacle_stop_planner_node)
 endif()
 
 # ament package generation and installing
 ament_auto_package(
   INSTALL_TO_SHARE
     param
+    launch
 )

obstacle_stop_planner_refine/launch/obstacle_stop_planner.launch.xml → obstacle_stop_planner_nodes_refine/launch/obstacle_stop_planner.launch.xml

@@ -5,12 +5,12 @@
   <arg name="input_twist" default="/localization/twist" />
   <arg name="output_trajectory" default="output/trajectory" />
   <arg name="enable_slow_down" default="false" />
-  <arg name="param_file" default="$(find-pkg-share obstacle_stop_planner)/config/obstacle_stop_planner.yaml" />
-  <arg name="acc_param_file" default="$(find-pkg-share obstacle_stop_planner)/config/adaptive_cruise_control.yaml" />
-  <arg name="vehicle_param_file"/>
+  <arg name="param_file" default="$(find-pkg-share obstacle_stop_planner_nodes)/param/obstacle_stop_planner.yaml" />
+  <arg name="acc_param_file" default="$(find-pkg-share obstacle_stop_planner_nodes)/param/adaptive_cruise_control.yaml" />
+  <arg name="vehicle_param_file" default="$(find-pkg-share obstacle_stop_planner_nodes)/param/vehicle_info.yaml"/>
 
-  <node pkg="obstacle_stop_planner" exec="obstacle_stop_planner_node" name="obstacle_stop_planner" output="screen">
-    <param from="$(var vehicle_param_file)" />    
+  <node pkg="obstacle_stop_planner_nodes" exec="obstacle_stop_planner_node_exe" name="obstacle_stop_planner_node" output="screen">
+    <param from="$(var vehicle_param_file)" />
     <param from="$(var acc_param_file)" />
     <param from="$(var param_file)" />
     <remap from="output/stop_reason" to="/planning/scenario_planning/status/stop_reason" />

obstacle_stop_planner_nodes_refine/package.xml

@@ -18,6 +18,7 @@
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ros_testing</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

obstacle_stop_planner_nodes_refine/param/vehicle_info.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

obstacle_stop_planner_nodes_refine/test/obstacle_stop_planner_node_launch_test.py

@@ -0,0 +1,61 @@
+# Copyright 2021 the Autoware Foundation
+#
+# 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.
+#
+# Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+import os
+import unittest
+
+import launch_testing
+import pytest
+from ament_index_python import get_package_share_directory
+from launch.launch_description import LaunchDescription
+from launch_ros.actions import Node
+from launch_testing.actions import ReadyToTest
+from launch_testing.asserts import assertExitCodes
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    obstacle_stop_planner_node = Node(
+        package='obstacle_stop_planner_nodes',
+        executable='obstacle_stop_planner_node_exe',
+        namespace='test',
+        parameters=[
+            os.path.join(
+                get_package_share_directory('obstacle_stop_planner_nodes'),
+                'param/obstacle_stop_planner.yaml'),
+            os.path.join(
+                get_package_share_directory('obstacle_stop_planner_nodes'),
+                'param/adaptive_cruise_control.yaml'),
+            os.path.join(
+                get_package_share_directory('obstacle_stop_planner_nodes'),
+                'param/vehicle_info.yaml')
+        ])
+
+    context = {'obstacle_stop_planner_node': obstacle_stop_planner_node}
+
+    return LaunchDescription([
+        obstacle_stop_planner_node,
+        # Start tests right away - no need to wait for anything
+        ReadyToTest(),
+    ]), context
+
+
+@launch_testing.post_shutdown_test()
+class TestProcessOutput(unittest.TestCase):
+
+    def test_exit_code(self, proc_output, proc_info, obstacle_stop_planner_node):
+        # Check that process exits with code -15 code: termination request, sent to the program
+        assertExitCodes(proc_info, [-15], process=obstacle_stop_planner_node)

obstacle_stop_planner_nodes_refine/test/test_obstacle_stop_planner_node.cpp

@@ -0,0 +1,93 @@
+// Copyright 2020 The Autoware Foundation
+//
+// 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 <obstacle_stop_planner_nodes/obstacle_stop_planner_node.hpp>
+
+#include <memory>
+#include <chrono>
+
+#include "gtest/gtest.h"
+
+using sensor_msgs::msg::PointCloud2;
+using autoware_planning_msgs::msg::Trajectory;
+
+class ObstacleStopPlannerNodeTest : public ::testing::Test
+{
+public:
+  void SetUp() override
+  {
+    ASSERT_FALSE(rclcpp::ok());
+    rclcpp::init(0, nullptr);
+    ASSERT_TRUE(rclcpp::ok());
+
+    fake_node_ = std::make_shared<rclcpp::Node>("fake_node");
+
+    using PubAllocT = rclcpp::PublisherOptionsWithAllocator<std::allocator<void>>;
+    dummy_pointcloud_pub_ =
+      fake_node_->create_publisher<PointCloud2>(
+      "input/pointcloud", rclcpp::QoS{1}, PubAllocT{});
+    dummy_path_pub_ =
+      fake_node_->create_publisher<Trajectory>(
+      "input/trajectory", rclcpp::QoS{1}, PubAllocT{});
+    dummy_velocity_pub_ =
+      fake_node_->create_publisher<geometry_msgs::msg::TwistStamped>(
+      "input/twist", rclcpp::QoS{1}, PubAllocT{});
+    dummy_object_pub_ =
+      fake_node_->create_publisher<autoware_perception_msgs::msg::DynamicObjectArray>(
+      "input/objects", rclcpp::QoS{1}, PubAllocT{});
+    path_sub_ =
+      fake_node_->create_subscription<Trajectory>(
+      "output/trajectory", rclcpp::QoS{1},
+      [this](const Trajectory::SharedPtr msg)
+      {this->path_msg_ = *msg;});
+
+    rclcpp::NodeOptions node_options{};
+
+    planner_ = std::make_shared<obstacle_stop_planner_nodes::ObstacleStopPlannerNode>(node_options);
+  }
+
+  std::shared_ptr<obstacle_stop_planner_nodes::ObstacleStopPlannerNode> planner_;
+  rclcpp::Node::SharedPtr fake_node_{nullptr};
+  rclcpp::Publisher<PointCloud2>::SharedPtr dummy_pointcloud_pub_;
+  rclcpp::Publisher<Trajectory>::SharedPtr dummy_path_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr dummy_velocity_pub_;
+  rclcpp::Publisher<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr dummy_object_pub_;
+  rclcpp::Subscription<Trajectory>::SharedPtr path_sub_;
+  boost::optional<Trajectory> path_msg_;
+};
+
+TEST_F(ObstacleStopPlannerNodeTest, plan_simple_trajectory)
+{
+  using namespace std::chrono_literals;
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(fake_node_);
+  executor.add_node(planner_);
+
+  // TODO(KS): Publish data
+
+
+  using namespace std::chrono_literals;
+  rclcpp::WallRate rate(100ms);
+  while (rclcpp::ok()) {
+    executor.spin_some();
+    if (path_msg_.has_value()) {
+      break;
+    }
+    rate.sleep();
+  }
+
+  // Check data
+  ASSERT_EQ(0, 1);
+}

obstacle_stop_planner_refine/CMakeLists.txt

@@ -77,5 +77,4 @@ endif()
 ament_auto_package(
   INSTALL_TO_SHARE
   config
-  launch
 )

obstacle_stop_planner_refine/include/obstacle_stop_planner/control/adaptive_cruise_control.hpp

@@ -52,7 +52,7 @@ class AdaptiveCruiseController
     const Trajectory & input_trajectory);
 
 private:
-  rclcpp::Publisher<autoware_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr pub_debug_;
+  // rclcpp::Publisher<autoware_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr pub_debug_;
 
   rclcpp::Node * node_;
   /*