diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt index 10b694ce22cbd..3a429008c1b8e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt @@ -15,6 +15,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_crosswalk.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) endif() diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index 08bd8f91c7d71..dfa3e5c7f0864 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -21,6 +21,7 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_grid_map_utils autoware_internal_debug_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..7a875327d4dde --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 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 + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "crosswalk", "autoware::behavior_velocity_planner::CrosswalkModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt index 31900a23d00e5..a520b02a43c05 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt @@ -2,14 +2,6 @@ cmake_minimum_required(VERSION 3.14) project(autoware_behavior_velocity_planner) find_package(autoware_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -rosidl_generate_interfaces( - ${PROJECT_NAME} - "srv/LoadPlugin.srv" - "srv/UnloadPlugin.srv" - DEPENDENCIES -) autoware_package() @@ -24,15 +16,6 @@ rclcpp_components_register_node(${PROJECT_NAME}_lib EXECUTABLE ${PROJECT_NAME}_node ) -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(${PROJECT_NAME}_lib - ${PROJECT_NAME} "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}") -endif() - if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_node_interface.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index 4c1f10c355226..8a52ae70231d0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -21,10 +21,9 @@ #include #include -#include -#include #include +#include #include #include #include @@ -45,8 +44,6 @@ namespace autoware::behavior_velocity_planner { -using autoware_behavior_velocity_planner::srv::LoadPlugin; -using autoware_behavior_velocity_planner::srv::UnloadPlugin; using autoware_map_msgs::msg::LaneletMapBin; using tier4_planning_msgs::msg::VelocityLimit; @@ -125,13 +122,14 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node BehaviorVelocityPlannerManager planner_manager_; bool is_driving_forward_{true}; - rclcpp::Service::SharedPtr srv_load_plugin_; - rclcpp::Service::SharedPtr srv_unload_plugin_; + rclcpp::Service::SharedPtr srv_load_plugin_; + rclcpp::Service::SharedPtr srv_unload_plugin_; void onUnloadPlugin( - const UnloadPlugin::Request::SharedPtr request, - const UnloadPlugin::Response::SharedPtr response); + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response); void onLoadPlugin( - const LoadPlugin::Request::SharedPtr request, const LoadPlugin::Response::SharedPtr response); + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response); // mutex for planner_data_ std::mutex mutex_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index 04b4cf0328fd1..eca49a09c8a56 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -32,9 +32,8 @@ autoware_cmake eigen3_cmake_module - rosidl_default_generators - autoware_behavior_velocity_planner_common + autoware_internal_debug_msgs autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils @@ -60,15 +59,11 @@ tier4_v2x_msgs visualization_msgs - rosidl_default_runtime - ament_cmake_ros ament_lint_auto autoware_lint_common - rosidl_interface_packages - ament_cmake diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index 5f78e4c670b49..1e3e691076b8a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -70,9 +70,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio this->create_subscription( "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1)); - srv_load_plugin_ = create_service( + srv_load_plugin_ = create_service( "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2)); - srv_unload_plugin_ = create_service( + srv_unload_plugin_ = create_service( "~/service/unload_plugin", std::bind(&BehaviorVelocityPlannerNode::onUnloadPlugin, this, _1, _2)); @@ -112,19 +112,19 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio } void BehaviorVelocityPlannerNode::onLoadPlugin( - const LoadPlugin::Request::SharedPtr request, - [[maybe_unused]] const LoadPlugin::Response::SharedPtr response) + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + [[maybe_unused]] const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response) { std::unique_lock lk(mutex_); - planner_manager_.launchScenePlugin(*this, request->plugin_name); + planner_manager_.launchScenePlugin(*this, request->data); } void BehaviorVelocityPlannerNode::onUnloadPlugin( - const UnloadPlugin::Request::SharedPtr request, - [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response) + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + [[maybe_unused]] const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response) { std::unique_lock lk(mutex_); - planner_manager_.removeScenePlugin(*this, request->plugin_name); + planner_manager_.removeScenePlugin(*this, request->data); } void BehaviorVelocityPlannerNode::onParam() diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv deleted file mode 100644 index 7b9f08ab0ded8..0000000000000 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv +++ /dev/null @@ -1,3 +0,0 @@ -string plugin_name ---- -bool success diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv deleted file mode 100644 index 7b9f08ab0ded8..0000000000000 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv +++ /dev/null @@ -1,3 +0,0 @@ -string plugin_name ---- -bool success