test(planning_validator): add interface test (#3406)

* add planning_interface_test_manager class

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add counter function

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add interface test for motion_velocity_smoother

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add param in default_motion_velocity_smoother,param.yaml

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* successfully launch target node

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* successfully build the test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add test for test_motion_velocity_smoother

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add abnormal trajectory test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete unnecessary part

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* style(pre-commit): autofix

* style(pre-commit): autofix

* run pre-commit

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add declaration

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* Refactor callback functions for standardization

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refacotoring

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactored

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* Refactor functions into a single template function

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* Refactor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* style(pre-commit): autofix

* refactor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* relete unnecessary definition

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* Revert "delete unnecessary definition"

This reverts commit 6cd13f8.

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete unnecessary definition

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete unnecessary definition

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete module dependent part

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete unnecessary arg

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* apply motion_velocity_smoother change

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add interface test for obstacle stop planner

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* run pre-commit

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add test fot obstacle_cruise_planner

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add test fot planning_vaildator

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add interface test for freespace_planner_node

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add interface test for obstacle_stop_planner with slow down

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add part of interface test for freespace

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* change package name

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* apply change of package name

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* fix build error

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp commit for debug

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add planning interface test manager for scenario selector

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* update parameter

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp for behavior_path_planner

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add param file

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add test free space planner module

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* update map

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add test for behavior path planner

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* pass freespace test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* pass freespace test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* update map

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* update map detection area

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add no stopping area

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* for print debug

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* update param

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add param file

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor(behavior_path_planner): remove unnecessary functions (#3271)

* refactor(behavior_path_planner): remove unnecessary functions

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

---------

Signed-off-by: yutaka <purewater0901@gmail.com>

* Revert "temp"

This reverts commit b82805e.

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* suppress build error (-Werror=pedantic)

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add interface test for behavior_velocity_planner

* build success

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add param

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* pass test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* pass test

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* build success

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* update param

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete param file

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* Resolve differences outside of parameter files

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* update test manager

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* update test manager

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* upload cloud map

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* fix typo

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor utils

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor utils

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor test_manager

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* modify test for obstacle cruise planner

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* modify test for obstacle_stop_planner

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* revert parameter change

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add obstacle_avoidance_planner

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* Revert "add obstacle_avoidance_planner"

This reverts commit efb5d50.

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add planning_validator

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* add rosidl_default_generators in CMake

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* Revert "add rosidl_default_generators in CMake"

This reverts commit 7f87b0a.

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete unnecessary files

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* refactor(behavior_velocity_planner::intersection): organize param intersection (#3409)

* reorganize intersection param for readability

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* reorganized PlannerParam

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

---------

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* refactor(lane_following): remove duplicated reference path generation process (#3398)

* feat(utils): add new function

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(behavior_path_planner): use utils function

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(planning_test_utils): fix build error (#3410)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(pointcloud_preprocessor): fix broken logic (#3122)

`if (a && b, c)` is the same as `if (c)` not `if (a && b && c)` !!

Signed-off-by: Vincent Richard <vincent.francois.richard@gmail.com>

* refactor(crop_box_filter): remove always true condition (#3245)

* refactor(crop_box_filter): remove always true condition

tf_input_frame_ can't be empty in crop box filter.

Signed-off-by: Vincent Richard <richard-v@macnica.co.jp>

* style(pre-commit): autofix

---------

Signed-off-by: Vincent Richard <richard-v@macnica.co.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

* feat(pointcloud_preprocessor): add data layout check utils (#3306)

* feat(pointcloud_preprocessor): add data layout check utils

When input cloud data layout is compatible filters can copy data faster.

Signed-off-by: Vincent Richard <richard-v@macnica.co.jp>

* style(pre-commit): autofix

* add "name" field check

cleaning

Signed-off-by: Vincent Richard <richard-v@macnica.co.jp>

---------

Signed-off-by: Vincent Richard <richard-v@macnica.co.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

* update cmake minimum version

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Signed-off-by: yutaka <purewater0901@gmail.com>
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Signed-off-by: Vincent Richard <vincent.francois.richard@gmail.com>
Signed-off-by: Vincent Richard <richard-v@macnica.co.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com>
Co-authored-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: Vincent Richard <vincent.francois.richard@gmail.com>

planning/planning_validator/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.14)
+cmake_minimum_required(VERSION 3.22)
 project(planning_validator)
 
 find_package(autoware_cmake REQUIRED)
@@ -51,6 +51,8 @@ if(BUILD_TESTING)
     test/src/test_planning_validator_functions.cpp
     test/src/test_planning_validator_helper.cpp
     test/src/test_planning_validator_pubsub.cpp
+    test/src/test_${PROJECT_NAME}_node_interface.cpp
+
   )
   ament_target_dependencies(test_planning_validator
     rclcpp

planning/planning_validator/package.xml

@@ -20,6 +20,7 @@
   <depend>geometry_msgs</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
+  <depend>planning_test_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tier4_autoware_utils</depend>

planning/planning_validator/test/src/test_planning_validator_node_interface.cpp

@@ -0,0 +1,58 @@
+// Copyright 2023 Tier IV, 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.
+
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "planning_interface_test_manager/planning_interface_test_manager.hpp"
+#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp"
+#include "planning_validator/planning_validator.hpp"
+
+#include <gtest/gtest.h>
+
+#include <vector>
+
+TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
+{
+  auto test_manager = std::make_shared<planning_test_utils::PlanningInterfaceTestManager>();
+
+  auto node_options = rclcpp::NodeOptions{};
+
+  const auto planning_test_utils_dir =
+    ament_index_cpp::get_package_share_directory("planning_test_utils");
+
+  const auto planning_validator_dir =
+    ament_index_cpp::get_package_share_directory("planning_validator");
+
+  node_options.arguments(
+    {"--ros-args", "--params-file",
+     planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file",
+     planning_validator_dir + "/config/planning_validator.param.yaml"});
+
+  auto test_target_node = std::make_shared<planning_validator::PlanningValidator>(node_options);
+
+  // publish necessary topics from test_manager
+  test_manager->publishOdometry(test_target_node, "planning_validator/input/kinematics");
+
+  // set subscriber with topic name: planning_validator → test_node_
+  test_manager->setTrajectorySubscriber("planning_validator/output/trajectory");
+
+  // set planning_validator's input topic name(this topic is changed to test node)
+  test_manager->setTrajectoryInputTopicName("planning_validator/input/trajectory");
+
+  // test for normal trajectory
+  ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node));
+  EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
+
+  // test for trajectory with empty/one point/overlapping point
+  test_manager->testWithAbnormalTrajectory(test_target_node);
+}