Skip to content

Commit cf5df22

Browse files
authored
Merge branch 'main' into topic-state-monitor
2 parents 2a0dd8f + 9dfca09 commit cf5df22

File tree

16 files changed

+558
-152
lines changed

16 files changed

+558
-152
lines changed

localization/yabloc/yabloc_common/CMakeLists.txt

+14-19
Original file line numberDiff line numberDiff line change
@@ -23,9 +23,6 @@ find_package(Sophus REQUIRED)
2323
# because it rewrite CMAKE_NO_SYSTEM_FROM_IMPORTED to TRUE.
2424
set(CMAKE_NO_SYSTEM_FROM_IMPORTED FALSE)
2525

26-
# glog
27-
find_package(glog REQUIRED)
28-
2926
# ===================================================
3027
# GeographicLib
3128
find_package(PkgConfig)
@@ -45,7 +42,10 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
4542
src/static_tf_subscriber.cpp
4643
src/extract_line_segments.cpp
4744
src/transform_line_segments.cpp
48-
src/color.cpp)
45+
src/color.cpp
46+
src/ground_server/ground_server_core.cpp
47+
src/ground_server/polygon_operation.cpp
48+
src/ll2_decomposer/ll2_decomposer_core.cpp)
4949
target_include_directories(
5050
${PROJECT_NAME} PUBLIC include
5151
)
@@ -63,23 +63,18 @@ target_link_libraries(${PROJECT_NAME} Geographic ${PCL_LIBRARIES} Sophus::Sophus
6363
# ===================================================
6464
# Executables
6565
# ground_server
66-
set(TARGET ground_server_node)
67-
ament_auto_add_executable(${TARGET}
68-
src/ground_server/ground_server_core.cpp
69-
src/ground_server/ground_server_node.cpp
70-
src/ground_server/polygon_operation.cpp)
71-
target_include_directories(${TARGET} PUBLIC include)
72-
target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
73-
target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus glog::glog)
66+
rclcpp_components_register_node(${PROJECT_NAME}
67+
PLUGIN "yabloc::ground_server::GroundServer"
68+
EXECUTABLE yabloc_ground_server_node
69+
EXECUTOR SingleThreadedExecutor
70+
)
7471

7572
# ll2_decomposer
76-
set(TARGET ll2_decomposer_node)
77-
ament_auto_add_executable(${TARGET}
78-
src/ll2_decomposer/ll2_decomposer_core.cpp
79-
src/ll2_decomposer/ll2_decomposer_node.cpp)
80-
target_include_directories(${TARGET} PUBLIC include)
81-
target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
82-
target_link_libraries(${TARGET} ${PCL_LIBRARIES})
73+
rclcpp_components_register_node(${PROJECT_NAME}
74+
PLUGIN "yabloc::ll2_decomposer::Ll2Decomposer"
75+
EXECUTABLE yabloc_ll2_decomposer_node
76+
EXECUTOR SingleThreadedExecutor
77+
)
8378

8479
# ===================================================
8580
ament_export_dependencies(PCL Sophus)

localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ class GroundServer : public rclcpp::Node
5757
using String = std_msgs::msg::String;
5858
using PointCloud2 = sensor_msgs::msg::PointCloud2;
5959
using Point = geometry_msgs::msg::Point;
60-
GroundServer();
60+
explicit GroundServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
6161

6262
private:
6363
const bool force_zero_tilt_;

localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@ class Ll2Decomposer : public rclcpp::Node
3939
using Marker = visualization_msgs::msg::Marker;
4040
using MarkerArray = visualization_msgs::msg::MarkerArray;
4141

42-
Ll2Decomposer();
42+
explicit Ll2Decomposer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
4343

4444
private:
4545
rclcpp::Publisher<Cloud2>::SharedPtr pub_road_marking_;

localization/yabloc/yabloc_common/launch/yabloc_common.launch.xml

+4-2
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,15 @@
11
<launch>
22
<arg name="input_particle_pose" default="/localization/pose_twist_fusion_filter/pose"/>
3+
<arg name="ground_server_param_path" default="$(find-pkg-share yabloc_common)/config/ground_server.param.yaml"/>
4+
<arg name="ll2_decomposer_param_path" default="$(find-pkg-share yabloc_common)/config/ll2_decomposer.param.yaml"/>
35

46
<!-- ground server -->
57
<arg name="output_height" default="height"/>
68
<arg name="output_ground" default="ground"/>
79
<arg name="output_ground_markers" default="ground_markers"/>
810
<arg name="output_ground_status" default="ground_status"/>
911
<arg name="output_near_cloud" default="near_cloud"/>
10-
<node name="ground_server" pkg="yabloc_common" exec="ground_server_node" output="screen" args="--ros-args --log-level warn">
12+
<node pkg="yabloc_common" exec="yabloc_ground_server_node" output="both">
1113
<param from="$(var ground_server_param_path)"/>
1214

1315
<remap from="~/input/vector_map" to="/map/vector_map"/>
@@ -25,7 +27,7 @@
2527
<arg name="output_ll2_bounding_box" default="ll2_bounding_box"/>
2628
<arg name="output_sign_board_marker" default="sign_board_marker"/>
2729

28-
<node name="ll2_decomposer" pkg="yabloc_common" exec="ll2_decomposer_node" output="screen" args="--ros-args --log-level warn">
30+
<node pkg="yabloc_common" exec="yabloc_ll2_decomposer_node" output="both">
2931
<param from="$(var ll2_decomposer_param_path)"/>
3032

3133
<remap from="~/input/vector_map" to="/map/vector_map"/>

localization/yabloc/yabloc_common/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -19,11 +19,11 @@
1919
<depend>autoware_auto_mapping_msgs</depend>
2020
<depend>cv_bridge</depend>
2121
<depend>geometry_msgs</depend>
22-
<depend>glog</depend>
2322
<depend>lanelet2_core</depend>
2423
<depend>lanelet2_extension</depend>
2524
<depend>pcl_conversions</depend>
2625
<depend>rclcpp</depend>
26+
<depend>rclcpp_components</depend>
2727
<depend>sensor_msgs</depend>
2828
<depend>signal_processing</depend>
2929
<depend>sophus</depend>

localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp

+5-2
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,8 @@
3030

3131
namespace yabloc::ground_server
3232
{
33-
GroundServer::GroundServer()
34-
: Node("ground_server"),
33+
GroundServer::GroundServer(const rclcpp::NodeOptions & options)
34+
: Node("ground_server", options),
3535
force_zero_tilt_(declare_parameter<bool>("force_zero_tilt")),
3636
R(declare_parameter<int>("R")),
3737
K(declare_parameter<int>("K"))
@@ -248,3 +248,6 @@ GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point)
248248
}
249249

250250
} // namespace yabloc::ground_server
251+
252+
#include <rclcpp_components/register_node_macro.hpp>
253+
RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::ground_server::GroundServer)

localization/yabloc/yabloc_common/src/ground_server/ground_server_node.cpp

-29
This file was deleted.

localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@
2424

2525
namespace yabloc::ll2_decomposer
2626
{
27-
Ll2Decomposer::Ll2Decomposer() : Node("ll2_to_image")
27+
Ll2Decomposer::Ll2Decomposer(const rclcpp::NodeOptions & options) : Node("ll2_to_image", options)
2828
{
2929
using std::placeholders::_1;
3030
const rclcpp::QoS latch_qos = rclcpp::QoS(10).transient_local();
@@ -263,3 +263,6 @@ void Ll2Decomposer::publish_additional_marker(const lanelet::LaneletMapPtr & lan
263263
}
264264

265265
} // namespace yabloc::ll2_decomposer
266+
267+
#include <rclcpp_components/register_node_macro.hpp>
268+
RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::ll2_decomposer::Ll2Decomposer)

localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_node.cpp

-23
This file was deleted.

localization/yabloc/yabloc_particle_filter/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
<buildtool_depend>rosidl_default_generators</buildtool_depend>
1919

2020
<depend>geometry_msgs</depend>
21+
<depend>glog</depend>
2122
<depend>rclcpp</depend>
2223
<depend>sensor_msgs</depend>
2324
<depend>sophus</depend>

planning/.pages

+1
Original file line numberDiff line numberDiff line change
@@ -80,5 +80,6 @@ nav:
8080
- 'About Planning Debug Tools': https://github.com/autowarefoundation/autoware_tools/tree/main/planning/planning_debug_tools
8181
- 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md
8282
- 'Planning Test Utils': planning/planning_test_utils
83+
- 'Planning Test Manager': planning/autoware_planning_test_manager
8384
- 'Planning Topic Converter': planning/planning_topic_converter
8485
- 'Planning Validator': planning/planning_validator

planning/autoware_planning_test_manager/README.md

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
# Planning Interface Test Manager
1+
# Autoware Planning Test Manager
22

33
## Background
44

+20-72
Original file line numberDiff line numberDiff line change
@@ -1,92 +1,40 @@
1-
# Autoware Planning Test Manager
1+
# Test Utils
22

33
## Background
44

5-
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.
5+
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.
66

77
## Purpose
88

9-
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.
9+
The objective of the `test_utils` is to develop a unit testing library for the Autoware components. This library will include
1010

11-
## Features
11+
- commonly used functions
12+
- input/mock data parser
13+
- maps for testing
14+
- common routes and mock data for testing.
1215

13-
### Confirmation of normal operation
16+
## Available Maps
1417

15-
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.
18+
The following maps are available [here](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/planning_test_utils/test_map)
1619

17-
### Robustness confirmation for special inputs
20+
### Common
1821

19-
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.
22+
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.
2023

21-
(WIP)
24+
![common](./images/common.png)
2225

23-
## Usage
26+
### 2 km Straight
2427

25-
```cpp
28+
The 2 km straight lanelet map consists of two lanes that run in the same direction. The map is named `2km_test.osm`.
2629

27-
TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
28-
{
29-
rclcpp::init(0, nullptr);
30+
![two_km](./images/2km-test.png)
3031

31-
// instantiate test_manager with PlanningInterfaceTestManager type
32-
auto test_manager = std::make_shared<planning_test_utils::PlanningInterfaceTestManager>();
32+
The following illustrates the design of the map.
3333

34-
// get package directories for necessary configuration files
35-
const auto planning_test_utils_dir =
36-
ament_index_cpp::get_package_share_directory("planning_test_utils");
37-
const auto target_node_dir =
38-
ament_index_cpp::get_package_share_directory("target_node");
34+
![straight_diagram](./images/2km-test.svg)
3935

40-
// set arguments to get the config file
41-
node_options.arguments(
42-
{"--ros-args", "--params-file",
43-
planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file",
44-
planning_validator_dir + "/config/planning_validator.param.yaml"});
36+
## Example use cases
4537

46-
// instantiate the TargetNode with node_options
47-
auto test_target_node = std::make_shared<TargetNode>(node_options);
38+
### Autoware Planning Test Manager
4839

49-
// publish the necessary topics from test_manager second argument is topic name
50-
test_manager->publishOdometry(test_target_node, "/localization/kinematic_state");
51-
test_manager->publishMaxVelocity(
52-
test_target_node, "motion_velocity_smoother/input/external_velocity_limit_mps");
53-
54-
// set scenario_selector's input topic name(this topic is changed to test node)
55-
test_manager->setTrajectoryInputTopicName("input/parking/trajectory");
56-
57-
// test with normal trajectory
58-
ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node));
59-
60-
// make sure target_node is running
61-
EXPECT_GE(test_manager->getReceivedTopicNum(), 1);
62-
63-
// test with trajectory input with empty/one point/overlapping point
64-
ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node));
65-
66-
// shutdown ROS context
67-
rclcpp::shutdown();
68-
}
69-
```
70-
71-
## Implemented tests
72-
73-
| Node | Test name | exceptional input | output | Exceptional input pattern |
74-
| -------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- |
75-
| planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points |
76-
| motion_velocity_smoother | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points |
77-
| obstacle_cruise_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points |
78-
| obstacle_stop_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points |
79-
| obstacle_velocity_limiter | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points |
80-
| obstacle_avoidance_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points |
81-
| scenario_selector | NodeTestWithExceptionTrajectoryLaneDrivingMode NodeTestWithExceptionTrajectoryParkingMode | trajectory | scenario | Empty, single point, path with duplicate points for scenarios:LANEDRIVING and PARKING |
82-
| freespace_planner | NodeTestWithExceptionRoute | route | trajectory | Empty route |
83-
| behavior_path_planner | NodeTestWithExceptionRoute NodeTestWithOffTrackEgoPose | route | route odometry | Empty route Off-lane ego-position |
84-
| behavior_velocity_planner | NodeTestWithExceptionPathWithLaneID | path_with_lane_id | path | Empty path |
85-
86-
## Important Notes
87-
88-
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.
89-
90-
## Future extensions / Unimplemented parts
91-
92-
(WIP)
40+
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.
5.45 KB
Loading

0 commit comments

Comments
 (0)