test(behavior_path_planner): add interface test (autowarefoundation#3402

) * 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 (autowarefoundation#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> * change file name Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * change file name Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * modify CMake Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * refactor Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * add behavior_velocity_planner Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * add comments 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> 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>
KYabuuchi · Apr 14, 2023 · 9cfc440 · 9cfc440
1 parent 1d6d9ad
commit 9cfc440
Show file tree

Hide file tree

Showing 4 changed files with 138 additions and 14 deletions.
diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt
@@ -114,6 +114,12 @@ if(BUILD_TESTING)
     behavior_path_planner_node
   )
 
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}_node_interface
+    test/test_${PROJECT_NAME}_node_interface.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}_node_interface
+    behavior_path_planner_node
+  )
 endif()
 
 ament_auto_package(

diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
@@ -6,16 +6,17 @@
       resample_interval_for_output: 4.0                 # [m]
       detection_area_right_expand_dist: 0.0             # [m]
       detection_area_left_expand_dist: 1.0              # [m]
-      object_envelope_buffer: 0.3                       # [m]
+      drivable_area_right_bound_offset: 0.0             # [m]
+      drivable_area_left_bound_offset: 0.0              # [m]
 
       # avoidance module common setting
       enable_bound_clipping: false
       enable_avoidance_over_same_direction: true
       enable_avoidance_over_opposite_direction: true
       enable_update_path_when_object_is_gone: false
       enable_force_avoidance_for_stopped_vehicle: false
-      enable_safety_check: false
-      enable_yield_maneuver: false
+      enable_safety_check: true
+      enable_yield_maneuver: true
       disable_path_update: false
 
       # for debug
@@ -24,14 +25,46 @@
 
       # avoidance is performed for the object type with true
       target_object:
-        car: true
-        truck: true
-        bus: true
-        trailer: true
-        unknown: false
-        bicycle: false
-        motorcycle: false
-        pedestrian: false
+        car:
+          enable: true
+          envelope_buffer_margin: 0.3
+          safety_buffer_lateral: 0.7
+          safety_buffer_longitudinal: 0.0
+        truck:
+          enable: true
+          envelope_buffer_margin: 0.3
+          safety_buffer_lateral: 0.7
+          safety_buffer_longitudinal: 0.0
+        bus:
+          enable: true
+          envelope_buffer_margin: 0.3
+          safety_buffer_lateral: 0.7
+          safety_buffer_longitudinal: 0.0
+        trailer:
+          enable: true
+          envelope_buffer_margin: 0.3
+          safety_buffer_lateral: 0.7
+          safety_buffer_longitudinal: 0.0
+        unknown:
+          enable: false
+          envelope_buffer_margin: 0.3
+          safety_buffer_lateral: 0.7
+          safety_buffer_longitudinal: 0.0
+        bicycle:
+          enable: false
+          envelope_buffer_margin: 0.8
+          safety_buffer_lateral: 1.0
+          safety_buffer_longitudinal: 1.0
+        motorcycle:
+          enable: false
+          envelope_buffer_margin: 0.8
+          safety_buffer_lateral: 1.0
+          safety_buffer_longitudinal: 1.0
+        pedestrian:
+          enable: false
+          envelope_buffer_margin: 0.8
+          safety_buffer_lateral: 1.0
+          safety_buffer_longitudinal: 1.0
 
       # For target object filtering
       target_filtering:
@@ -64,16 +97,14 @@
         # avoidance lateral parameters
         lateral:
           lateral_collision_margin: 1.0                 # [m]
-          lateral_collision_safety_buffer: 0.7          # [m]
-          lateral_passable_safety_buffer: 0.0           # [m]
+          lateral_passable_safety_buffer: 1.0           # [m]
           road_shoulder_safety_margin: 0.3              # [m]
           avoidance_execution_lateral_threshold: 0.499
           max_right_shift_length: 5.0
           max_left_shift_length: 5.0
         # avoidance distance parameters
         longitudinal:
           prepare_time: 2.0                             # [s]
-          longitudinal_collision_safety_buffer: 0.0     # [m]
           min_prepare_distance: 1.0                     # [m]
           min_avoidance_distance: 10.0                  # [m]
           min_nominal_avoidance_speed: 7.0              # [m/s]

diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml
@@ -58,6 +58,7 @@
   <depend>magic_enum</depend>
   <depend>motion_utils</depend>
   <depend>perception_utils</depend>
+  <depend>planning_test_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>route_handler</depend>

diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp
@@ -0,0 +1,86 @@
+// 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 "behavior_path_planner/behavior_path_planner_node.hpp"
+#include "planning_interface_test_manager/planning_interface_test_manager.hpp"
+#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <vector>
+
+TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute)
+{
+  rclcpp::init(0, nullptr);
+
+  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 behavior_path_planner_dir =
+    ament_index_cpp::get_package_share_directory("behavior_path_planner");
+
+  node_options.append_parameter_override(
+    "bt_tree_config_path", behavior_path_planner_dir + "/config/behavior_path_planner_tree.xml");
+
+  test_utils::updateNodeOptions(
+    node_options, {planning_test_utils_dir + "/config/test_common.param.yaml",
+                   planning_test_utils_dir + "/config/test_nearest_search.param.yaml",
+                   planning_test_utils_dir + "/config/test_vehicle_info.param.yaml",
+                   behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml",
+                   behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml",
+                   behavior_path_planner_dir + "/config/scene_module_manager.param.yaml",
+                   behavior_path_planner_dir + "/config/avoidance/avoidance.param.yaml",
+                   behavior_path_planner_dir + "/config/lane_following/lane_following.param.yaml",
+                   behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml",
+                   behavior_path_planner_dir + "/config/pull_out/pull_out.param.yaml",
+                   behavior_path_planner_dir + "/config/pull_over/pull_over.param.yaml",
+                   behavior_path_planner_dir + "/config/avoidance_by_lc/avoidance_by_lc.param.yaml",
+                   behavior_path_planner_dir + "/config/side_shift/side_shift.param.yaml"});
+
+  auto test_target_node =
+    std::make_shared<behavior_path_planner::BehaviorPathPlannerNode>(node_options);
+
+  // publish necessary topics from test_manager
+  test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry");
+  test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel");
+  test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception");
+  test_manager->publishOccupancyGrid(
+    test_target_node, "behavior_path_planner/input/occupancy_grid_map");
+  test_manager->publishLaneDrivingScenario(
+    test_target_node, "behavior_path_planner/input/scenario");
+  test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map");
+  test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap");
+  test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state");
+  test_manager->publishLateralOffset(
+    test_target_node, "behavior_path_planner/input/lateral_offset");
+
+  // set subscriber with topic name: behavior_path_planner → test_node_
+  test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path");
+
+  // set behavior_path_planner's input topic name(this topic is changed to test node)
+  test_manager->setRouteInputTopicName("behavior_path_planner/input/route");
+
+  // test for normal trajectory
+  ASSERT_NO_THROW(test_manager->testWithBehaviorNominalRoute(test_target_node));
+  EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
+
+  // test with empty route
+  // test_manager->testWithAbnormalRoute(test_target_node);
+  // rclcpp::shutdown();
+}