diff --git a/localization/yabloc/yabloc_common/CMakeLists.txt b/localization/yabloc/yabloc_common/CMakeLists.txt index c8474a85ad963..ca2f28bae7cda 100644 --- a/localization/yabloc/yabloc_common/CMakeLists.txt +++ b/localization/yabloc/yabloc_common/CMakeLists.txt @@ -23,9 +23,6 @@ find_package(Sophus REQUIRED) # because it rewrite CMAKE_NO_SYSTEM_FROM_IMPORTED to TRUE. set(CMAKE_NO_SYSTEM_FROM_IMPORTED FALSE) -# glog -find_package(glog REQUIRED) - # =================================================== # GeographicLib find_package(PkgConfig) @@ -45,7 +42,10 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/static_tf_subscriber.cpp src/extract_line_segments.cpp src/transform_line_segments.cpp - src/color.cpp) + src/color.cpp + src/ground_server/ground_server_core.cpp + src/ground_server/polygon_operation.cpp + src/ll2_decomposer/ll2_decomposer_core.cpp) target_include_directories( ${PROJECT_NAME} PUBLIC include ) @@ -63,23 +63,18 @@ target_link_libraries(${PROJECT_NAME} Geographic ${PCL_LIBRARIES} Sophus::Sophus # =================================================== # Executables # ground_server -set(TARGET ground_server_node) -ament_auto_add_executable(${TARGET} - src/ground_server/ground_server_core.cpp - src/ground_server/ground_server_node.cpp - src/ground_server/polygon_operation.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus glog::glog) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::ground_server::GroundServer" + EXECUTABLE yabloc_ground_server_node + EXECUTOR SingleThreadedExecutor +) # ll2_decomposer -set(TARGET ll2_decomposer_node) -ament_auto_add_executable(${TARGET} - src/ll2_decomposer/ll2_decomposer_core.cpp - src/ll2_decomposer/ll2_decomposer_node.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::ll2_decomposer::Ll2Decomposer" + EXECUTABLE yabloc_ll2_decomposer_node + EXECUTOR SingleThreadedExecutor +) # =================================================== ament_export_dependencies(PCL Sophus) diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp index 154507f8195d9..4cc4128ad5a70 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp @@ -57,7 +57,7 @@ class GroundServer : public rclcpp::Node using String = std_msgs::msg::String; using PointCloud2 = sensor_msgs::msg::PointCloud2; using Point = geometry_msgs::msg::Point; - GroundServer(); + explicit GroundServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: const bool force_zero_tilt_; diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp index f6e6de6b38fdd..784b3997faffd 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp @@ -39,7 +39,7 @@ class Ll2Decomposer : public rclcpp::Node using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; - Ll2Decomposer(); + explicit Ll2Decomposer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: rclcpp::Publisher::SharedPtr pub_road_marking_; diff --git a/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml b/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml index 48299e880cc9c..29d3da71bce7f 100644 --- a/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml +++ b/localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml @@ -1,5 +1,7 @@ + + @@ -7,7 +9,7 @@ - + @@ -25,7 +27,7 @@ - + diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index 213057f428c38..caf044660257e 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -19,11 +19,11 @@ autoware_auto_mapping_msgs cv_bridge geometry_msgs - glog lanelet2_core lanelet2_extension pcl_conversions rclcpp + rclcpp_components sensor_msgs signal_processing sophus diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp index 749aa39318923..75d5f8e1f9cbc 100644 --- a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp +++ b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp @@ -30,8 +30,8 @@ namespace yabloc::ground_server { -GroundServer::GroundServer() -: Node("ground_server"), +GroundServer::GroundServer(const rclcpp::NodeOptions & options) +: Node("ground_server", options), force_zero_tilt_(declare_parameter("force_zero_tilt")), R(declare_parameter("R")), K(declare_parameter("K")) @@ -248,3 +248,6 @@ GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point) } } // namespace yabloc::ground_server + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::ground_server::GroundServer) diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp deleted file mode 100644 index 4e032ff72c998..0000000000000 --- a/localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// 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 "yabloc_common/ground_server/ground_server.hpp" - -#include - -int main(int argc, char ** argv) -{ - if (!google::IsGoogleLoggingInitialized()) { - google::InitGoogleLogging(argv[0]); - google::InstallFailureSignalHandler(); - } - - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); -} diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp index 5cf3e905e447d..df56f43b9287c 100644 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp +++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp @@ -24,7 +24,7 @@ namespace yabloc::ll2_decomposer { -Ll2Decomposer::Ll2Decomposer() : Node("ll2_to_image") +Ll2Decomposer::Ll2Decomposer(const rclcpp::NodeOptions & options) : Node("ll2_to_image", options) { using std::placeholders::_1; const rclcpp::QoS latch_qos = rclcpp::QoS(10).transient_local(); @@ -263,3 +263,6 @@ void Ll2Decomposer::publish_additional_marker(const lanelet::LaneletMapPtr & lan } } // namespace yabloc::ll2_decomposer + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::ll2_decomposer::Ll2Decomposer) diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp deleted file mode 100644 index 692f6a5da0913..0000000000000 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// 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 "yabloc_common/ll2_decomposer/ll2_decomposer.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index 5db4aa0c29e88..66c0d8af8865c 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -18,6 +18,7 @@ rosidl_default_generators geometry_msgs + glog rclcpp sensor_msgs sophus diff --git a/planning/.pages b/planning/.pages index e355639eb8da5..28a791801b805 100644 --- a/planning/.pages +++ b/planning/.pages @@ -80,5 +80,6 @@ nav: - 'About Planning Debug Tools': https://github.com/autowarefoundation/autoware_tools/tree/main/planning/planning_debug_tools - 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md - 'Planning Test Utils': planning/planning_test_utils + - 'Planning Test Manager': planning/autoware_planning_test_manager - 'Planning Topic Converter': planning/planning_topic_converter - 'Planning Validator': planning/planning_validator diff --git a/planning/autoware_planning_test_manager/README.md b/planning/autoware_planning_test_manager/README.md index 63f1bf53a4954..b72625be999dd 100644 --- a/planning/autoware_planning_test_manager/README.md +++ b/planning/autoware_planning_test_manager/README.md @@ -1,4 +1,4 @@ -# Planning Interface Test Manager +# Autoware Planning Test Manager ## Background diff --git a/planning/planning_test_utils/README.md b/planning/planning_test_utils/README.md index b72625be999dd..c064e5791a487 100644 --- a/planning/planning_test_utils/README.md +++ b/planning/planning_test_utils/README.md @@ -1,92 +1,40 @@ -# Autoware Planning Test Manager +# Test Utils ## Background -In each node of the planning module, when exceptional input, such as unusual routes or significantly deviated ego-position, is given, the node may not be prepared for such input and could crash. As a result, debugging node crashes can be time-consuming. For example, if an empty trajectory is given as input and it was not anticipated during implementation, the node might crash due to the unaddressed exceptional input when changes are merged, during scenario testing or while the system is running on an actual vehicle. +Several Autoware's components and modules have already adopted unit testing, so a common library to ease the process of writing unit tests is necessary. ## Purpose -The purpose is to provide a utility for implementing tests to ensure that node operates correctly when receiving exceptional input. By utilizing this utility and implementing tests for exceptional input, the purpose is to reduce bugs that are only discovered when actually running the system, by requiring measures for exceptional input before merging PRs. +The objective of the `test_utils` is to develop a unit testing library for the Autoware components. This library will include -## Features +- commonly used functions +- input/mock data parser +- maps for testing +- common routes and mock data for testing. -### Confirmation of normal operation +## Available Maps -For the test target node, confirm that the node operates correctly and publishes the required messages for subsequent nodes. To do this, test_node publish the necessary messages and confirm that the node's output is being published. +The following maps are available [here](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/planning_test_utils/test_map) -### Robustness confirmation for special inputs +### Common -After confirming normal operation, ensure that the test target node does not crash when given exceptional input. To do this, provide exceptional input from the test_node and confirm that the node does not crash. +The common map contains multiple types of usable inputs, including shoulder lanes, intersections, and some regulatory elements. The common map is named `lanelet2_map.osm` in the folder. -(WIP) +![common](./images/common.png) -## Usage +### 2 km Straight -```cpp +The 2 km straight lanelet map consists of two lanes that run in the same direction. The map is named `2km_test.osm`. -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) -{ - rclcpp::init(0, nullptr); +![two_km](./images/2km-test.png) - // instantiate test_manager with PlanningInterfaceTestManager type - auto test_manager = std::make_shared(); +The following illustrates the design of the map. - // get package directories for necessary configuration files - const auto planning_test_utils_dir = - ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto target_node_dir = - ament_index_cpp::get_package_share_directory("target_node"); +![straight_diagram](./images/2km-test.svg) - // set arguments to get the config file - 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"}); +## Example use cases - // instantiate the TargetNode with node_options - auto test_target_node = std::make_shared(node_options); +### Autoware Planning Test Manager - // publish the necessary topics from test_manager second argument is topic name - test_manager->publishOdometry(test_target_node, "/localization/kinematic_state"); - test_manager->publishMaxVelocity( - test_target_node, "motion_velocity_smoother/input/external_velocity_limit_mps"); - - // set scenario_selector's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("input/parking/trajectory"); - - // test with normal trajectory - ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); - - // make sure target_node is running - EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - - // test with trajectory input with empty/one point/overlapping point - ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); - - // shutdown ROS context - rclcpp::shutdown(); -} -``` - -## Implemented tests - -| Node | Test name | exceptional input | output | Exceptional input pattern | -| -------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | -| planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| motion_velocity_smoother | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_cruise_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_stop_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_velocity_limiter | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_avoidance_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| scenario_selector | NodeTestWithExceptionTrajectoryLaneDrivingMode NodeTestWithExceptionTrajectoryParkingMode | trajectory | scenario | Empty, single point, path with duplicate points for scenarios:LANEDRIVING and PARKING | -| freespace_planner | NodeTestWithExceptionRoute | route | trajectory | Empty route | -| behavior_path_planner | NodeTestWithExceptionRoute NodeTestWithOffTrackEgoPose | route | route odometry | Empty route Off-lane ego-position | -| behavior_velocity_planner | NodeTestWithExceptionPathWithLaneID | path_with_lane_id | path | Empty path | - -## Important Notes - -During test execution, when launching a node, parameters are loaded from the parameter file within each package. Therefore, when adding parameters, it is necessary to add the required parameters to the parameter file in the target node package. This is to prevent the node from being unable to launch if there are missing parameters when retrieving them from the parameter file during node launch. - -## Future extensions / Unimplemented parts - -(WIP) +The goal of the [Autoware Planning Test Manager](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_planning_test_manager/) is to test planning module nodes. The `PlanningInterfaceTestManager` class ([source code](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp)) creates wrapper functions based on the `test_utils` functions. diff --git a/planning/planning_test_utils/images/2km-test.png b/planning/planning_test_utils/images/2km-test.png new file mode 100644 index 0000000000000..297dc5a43865e Binary files /dev/null and b/planning/planning_test_utils/images/2km-test.png differ diff --git a/planning/planning_test_utils/images/2km-test.svg b/planning/planning_test_utils/images/2km-test.svg new file mode 100644 index 0000000000000..26632cdde9118 --- /dev/null +++ b/planning/planning_test_utils/images/2km-test.svg @@ -0,0 +1,505 @@ + + + + + + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 3.5 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 2000.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 1.75 meter +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (0.0, 0.0) +
+
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ 1000.0 meter +
+
+
+ + + + + + + + + + + + + + + + + +
+
+
+ 7.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 25.0 meter +
+
+
+ + + + + + + + + + + + +
+
+
+ pose +
+ (1000.0, -3.5) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (1000.0, 3.5) +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ pose +
+ (-1000.0, 3.5) +
+
+
+
+
+ + + + + + + diff --git a/planning/planning_test_utils/images/common.png b/planning/planning_test_utils/images/common.png new file mode 100644 index 0000000000000..340437b53c0f5 Binary files /dev/null and b/planning/planning_test_utils/images/common.png differ