Autoware Planning Test Manager

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.

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.

Features

Confirmation of normal operation

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.

Robustness confirmation for special inputs

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.

(WIP)

Usage

TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
{
  rclcpp::init(0, nullptr);

  // instantiate test_manager with PlanningInterfaceTestManager type
  auto test_manager = std::make_shared<autoware::planning_test_manager::PlanningInterfaceTestManager>();

  // get package directories for necessary configuration files
  const auto autoware_test_utils_dir =
    ament_index_cpp::get_package_share_directory("autoware_test_utils");
  const auto target_node_dir =
    ament_index_cpp::get_package_share_directory("target_node");

  // set arguments to get the config file
  node_options.arguments(
    {"--ros-args", "--params-file",
     autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file",
     autoware_planning_validator_dir + "/config/planning_validator.param.yaml"});

  // instantiate the TargetNode with node_options
  auto test_target_node = std::make_shared<TargetNode>(node_options);

  // 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, "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

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)

Changelog for package autoware_planning_test_manager

1.0.0 (2025-03-31)

0.3.0 (2025-03-21)

• chore: fix versions in package.xml
• test(autoware_planning_test_manager): add tests (#273)
  • test(autoware_planning_test_manager): add tests

  * style(pre-commit): autofix ---------Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]\@users.noreply.github.com> Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>>

• feat: adaptation to ROS nodes guidelines about directory structure (#272)
• feat(autoware_path_generator): function to smooth the path (#227) * feat: function to smooth the route (see below) Description: This commit is kind of feature porting from [autoware.universe]{.title-ref} as follows * Import [PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection]{.title-ref} from the following [autoware.universe]{.title-ref} code https://github.com/autowarefoundation/autoware.universe/blob/a0816b7e3e35fbe822fefbb9c9a8132365608b49/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp#L74-L104
  • Also import all related functions from the [autoware.universe]{.title-ref} side
  • style(pre-commit): autofix
  • bugs: fix remaining conflicts

  * Update planning/autoware_path_generator/src/utils.cpp Co-authored-by: Kosuke Takeuchi <<kosuke.tnp@gmail.com>> * Update planning/autoware_path_generator/src/utils.cpp Co-authored-by: Kosuke Takeuchi <<kosuke.tnp@gmail.com>>

  • refactor: as follows
  • Enhance error handlings
  • Remove unused variables
  • Simplify the code
  • Improve readability a little bit
  • style(pre-commit): autofix
  • refactor: enhance error handling
  • style(pre-commit): autofix
  • bug: fix wrong function declaration in header
  • bug: fix wrong point index calculation
  • bug: remove meaningless comment
  • This comment is wrote because of my misunderstanding
  • fix: apply [pre-commit]{.title-ref}
  • fix: smooth path before cropping trajectory points
  • bug: fix shadow variable
  • bug: fix missing parameters for [autoware_path_generator]{.title-ref}
  • bug: fix by cpplint
  • style(pre-commit): autofix
  • bug: apply missing fix proposed by cpplint
  • style(pre-commit): autofix
  • bug: [autoware_test_utils]{.title-ref} should be in the [test_depend]{.title-ref}
  • fix(autoware_path_generator): add maintainer and author
  • style(pre-commit): autofix
  • fix: by pre-commit
  • Sorry, I was forgetting to do this on my local env.
  • fix: smooth path only when a goal point is included
  • bug: do error handling
  • style(pre-commit): autofix
  • bug: fix wrong distance calculation
  • The goal position is generally separate from the path points
  • fix: remove sanity check temporary as following reasons
  • CI (especially unit tests) fails due to this sanity check

  * As this is out of scope for this PR, we will fix the bug where the start and end are reversed in another PR

  • refactor: fix complexity
  • We should start from the simple one
  • Then we can add the necessary optimization later
  • bug: missing fixes in the include header
  • bug: inconsistent function declaration
  • The type of returned value and arguments were wrong

  * Update planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp Co-authored-by: Kosuke Takeuchi <<kosuke.tnp@gmail.com>> * Update planning/autoware_path_generator/src/node.cpp Co-authored-by: Kosuke Takeuchi <<kosuke.tnp@gmail.com>> * Update planning/autoware_path_generator/src/utils.cpp Co-authored-by: Kosuke Takeuchi <<kosuke.tnp@gmail.com>> * Update planning/autoware_path_generator/src/utils.cpp Co-authored-by: Kosuke Takeuchi <<kosuke.tnp@gmail.com>>

  • style(pre-commit): autofix
  • fix: apply comment in the following PR
  • https://github.com/autowarefoundation/autoware.core/pull/227#discussion_r1986045016
  • fix: sorry, I was missing one comment to be applied
  • style(pre-commit): autofix
  • bug: fix wrong goal point interpolation
  • feat: add test case (goal on left side)
  • bug: fix as follows
  • Prevent name duplication (path_up_to_just_before_pre_goal)
  • Fix missing left/right bound
  • Goal must have zero velocity
  • Improve readability
  • Other minor fixes
  • bug: fix duplicated zero velocity set
  • Zero velocity is set after the removed lines by this commit
  • feat: add one test case (goal on left side)

  * Update planning/autoware_path_generator/src/utils.cpp Co-authored-by: Kosuke Takeuchi <<kosuke.tnp@gmail.com>>

  • fix: apply comment from reviewer
  • fix(package.xml): update maintainer for the following packages
  • [autoware_planning_test_manager]{.title-ref}
  • [autoware_test_utils]{.title-ref}

  * Update planning/autoware_path_generator/src/node.cpp Co-authored-by: Kosuke Takeuchi <<kosuke.tnp@gmail.com>> * Update planning/autoware_path_generator/src/utils.cpp Co-authored-by: Mitsuhiro Sakamoto <<50359861+mitukou1109@users.noreply.github.com>> * Update planning/autoware_path_generator/src/utils.cpp Co-authored-by: Mitsuhiro Sakamoto <<50359861+mitukou1109@users.noreply.github.com>>

  • bug: fix missing header in the path
  • This finally causes an issue that the vehicle cannot engage
  • bug: fix an issue that smooth connection does not work
  • refactor: simplify code
  • bug: fix wrong pose at the goal (see below)

  * If we return nullopt here, the original path whose goal position is located at the center line is used. * Unless far from the goal point, the path becomes smoothed one whose goal position is located at the side of road correctly. * But as the goal approaches very closely, the goal position is shifted from smoothed one to the original one

  • Thus, the goal pose finally becomes wrong due to the goal position shift
  • refactor: no need this line here
  • style(pre-commit): autofix
  • bug: fix so we follow the provided review comments
  • bug: sorry, this is unsaved fix, ...
  • cosmetic: fix wrong comment
  • bug: unused function [get_goal_lanelet()]{.title-ref} remaining
  • bug: carefully handle the pre goal velocity
  • It seems zero pre goal velocity makes scenario fail
  • We need to insert appropriate velocity for pre goal

  * Update planning/autoware_path_generator/src/utils.cpp Co-authored-by: Kosuke Takeuchi <<kosuke.tnp@gmail.com>> * Update planning/autoware_path_generator/src/utils.cpp Co-authored-by: Kosuke Takeuchi <<kosuke.tnp@gmail.com>> * style(pre-commit): autofix ---------Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]\@users.noreply.github.com> Co-authored-by: Kosuke Takeuchi <<kosuke.tnp@gmail.com>> Co-authored-by: Mitsuhiro Sakamoto <<50359861+mitukou1109@users.noreply.github.com>>

• feat(autoware_planning_test_manager): porting autoware_planning_test_manager package from autoware.universe (#203)
• Contributors: Junya Sasaki, NorahXiong, mitsudome-r