test(freespace_planner): add interface test (#3403)

* 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 freespace_planner 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> * modify file name Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * fix topic name Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * add empty route test Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * refactor 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> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
autowarefoundation · Apr 15, 2023 · c977ec8 · c977ec8
1 parent 79f7dc4
commit c977ec8
Show file tree

Hide file tree

Showing 4 changed files with 77 additions and 3 deletions.
diff --git a/planning/freespace_planner/CMakeLists.txt b/planning/freespace_planner/CMakeLists.txt
@@ -13,6 +13,15 @@ rclcpp_components_register_node(freespace_planner_node
   EXECUTABLE freespace_planner
 )
 
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+    test/test_${PROJECT_NAME}_node_interface.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}_node
+  )
+endif()
+
 ament_auto_package(
   INSTALL_TO_SHARE
   launch

diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml
@@ -21,6 +21,7 @@
   <depend>freespace_planning_algorithms</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>planning_test_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>route_handler</depend>
@@ -32,6 +33,7 @@
   <depend>vehicle_info_util</depend>
   <depend>visualization_msgs</depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp
@@ -431,9 +431,9 @@ void FreespacePlannerNode::onTimer()
   }
 
   // Get current pose
-  constexpr const char * vehicle_frame = "base_link";
-  current_pose_ = tier4_autoware_utils::transform2pose(
-    getTransform(occupancy_grid_->header.frame_id, vehicle_frame));
+  current_pose_.pose = odom_->pose.pose;
+  current_pose_.header = odom_->header;
+
   if (current_pose_.header.frame_id == "") {
     return;
   }

diff --git a/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp
@@ -0,0 +1,63 @@
+// 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 "freespace_planner/freespace_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 <vector>
+
+TEST(PlanningModuleInterfaceTest, testPlanningInterfaceWithVariousTrajectoryInput)
+{
+  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 freespace_planner_dir =
+    ament_index_cpp::get_package_share_directory("freespace_planner");
+
+  node_options.arguments(
+    {"--ros-args", "--params-file",
+     planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file",
+     freespace_planner_dir + "/config/freespace_planner.param.yaml"});
+
+  auto test_target_node = std::make_shared<freespace_planner::FreespacePlannerNode>(node_options);
+
+  // publish necessary topics from test_manager
+  test_manager->publishTF(test_target_node, "/tf");
+  test_manager->publishOdometry(test_target_node, "freespace_planner/input/odometry");
+  test_manager->publishOccupancyGrid(test_target_node, "freespace_planner/input/occupancy_grid");
+  test_manager->publishParkingScenario(test_target_node, "freespace_planner/input/scenario");
+
+  // set subscriber with topic name: freespace_planner → test_node_
+  test_manager->setRouteInputTopicName("freespace_planner/input/route");
+
+  // set freespace_planner's input topic name(this topic is changed to test node)
+  test_manager->setTrajectorySubscriber("freespace_planner/output/trajectory");
+
+  // test with normal route
+  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);
+}