Skip to content

Commit

Permalink
Merge branch 'main' into topic-state-monitor
Browse files Browse the repository at this point in the history
  • Loading branch information
karishma1911 authored May 28, 2024
2 parents 2a0dd8f + 9dfca09 commit cf5df22
Show file tree
Hide file tree
Showing 16 changed files with 558 additions and 152 deletions.
33 changes: 14 additions & 19 deletions localization/yabloc/yabloc_common/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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
)
Expand All @@ -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)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<Cloud2>::SharedPtr pub_road_marking_;
Expand Down
Original file line number Diff line number Diff line change
@@ -1,13 +1,15 @@
<launch>
<arg name="input_particle_pose" default="/localization/pose_twist_fusion_filter/pose"/>
<arg name="ground_server_param_path" default="$(find-pkg-share yabloc_common)/config/ground_server.param.yaml"/>
<arg name="ll2_decomposer_param_path" default="$(find-pkg-share yabloc_common)/config/ll2_decomposer.param.yaml"/>

<!-- ground server -->
<arg name="output_height" default="height"/>
<arg name="output_ground" default="ground"/>
<arg name="output_ground_markers" default="ground_markers"/>
<arg name="output_ground_status" default="ground_status"/>
<arg name="output_near_cloud" default="near_cloud"/>
<node name="ground_server" pkg="yabloc_common" exec="ground_server_node" output="screen" args="--ros-args --log-level warn">
<node pkg="yabloc_common" exec="yabloc_ground_server_node" output="both">
<param from="$(var ground_server_param_path)"/>

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

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

<remap from="~/input/vector_map" to="/map/vector_map"/>
Expand Down
2 changes: 1 addition & 1 deletion localization/yabloc/yabloc_common/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -19,11 +19,11 @@
<depend>autoware_auto_mapping_msgs</depend>
<depend>cv_bridge</depend>
<depend>geometry_msgs</depend>
<depend>glog</depend>
<depend>lanelet2_core</depend>
<depend>lanelet2_extension</depend>
<depend>pcl_conversions</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>sensor_msgs</depend>
<depend>signal_processing</depend>
<depend>sophus</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<bool>("force_zero_tilt")),
R(declare_parameter<int>("R")),
K(declare_parameter<int>("K"))
Expand Down Expand Up @@ -248,3 +248,6 @@ GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point)
}

} // namespace yabloc::ground_server

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::ground_server::GroundServer)

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -263,3 +263,6 @@ void Ll2Decomposer::publish_additional_marker(const lanelet::LaneletMapPtr & lan
}

} // namespace yabloc::ll2_decomposer

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::ll2_decomposer::Ll2Decomposer)

This file was deleted.

1 change: 1 addition & 0 deletions localization/yabloc/yabloc_particle_filter/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
<buildtool_depend>rosidl_default_generators</buildtool_depend>

<depend>geometry_msgs</depend>
<depend>glog</depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>sophus</depend>
Expand Down
1 change: 1 addition & 0 deletions planning/.pages
Original file line number Diff line number Diff line change
Expand Up @@ -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
2 changes: 1 addition & 1 deletion planning/autoware_planning_test_manager/README.md
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
# Planning Interface Test Manager
# Autoware Planning Test Manager

## Background

Expand Down
92 changes: 20 additions & 72 deletions planning/planning_test_utils/README.md
Original file line number Diff line number Diff line change
@@ -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<planning_test_utils::PlanningInterfaceTestManager>();
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<TargetNode>(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.
Binary file added planning/planning_test_utils/images/2km-test.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading

0 comments on commit cf5df22

Please sign in to comment.