feat(planning_validator): add validation for trajectory length and related tests #6195

TakaHoribe · 2024-01-28T15:39:15Z

Description

This PR improves planning validation.

Two validations are added:
- check trajectory forward length
- check longitudinal deviation
Add tests for all validation in the ros node level
Fix some bugs, and missing codes. (These are commented in github)

Now, the planning_validator can detect the invalid planning trajectory more correctly. For example, a very short path can be detected.

Before

planning_validator_before-2024-01-29_00.48.02.mp4

After

planning_validator_after-2024-01-29_00.29.33.mp4

Tests performed

run test in colcon
run psim and check if undesired behavior occurs (no error message on terminal)

Notes for reviewers

None

Interface changes

None

Effects on system behavior

The planning validation may will make more errors in diagnostics.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

I've confirmed the contribution guidelines.
The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

The PR follows the pull request guidelines.
The PR has been properly tested.
The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

There are no open discussions or they are tracked via tickets.
The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

Signed-off-by: Takamasa Horibe <[email protected]>

This reverts commit a360bd0.

TakaHoribe · 2024-01-28T15:40:12Z

planning/planning_validator/src/utils.cpp

@@ -296,7 +296,7 @@ std::pair<double, size_t> calcMaxSteeringRates(
    const auto steer_next = steering_array.at(i + 1);

    const auto steer_rate = (steer_next - steer_prev) / dt;
-    takeBigger(max_steering_rate, max_index, steer_rate, i);
+    takeBigger(max_steering_rate, max_index, std::abs(steer_rate), i);


TakaHoribe · 2024-01-28T15:40:26Z

planning/planning_validator/src/planning_validator.cpp

@@ -148,6 +152,20 @@ void PlanningValidator::setupDiag()
    setStatus(
      stat, validation_status_.is_valid_velocity_deviation, "velocity deviation is too large");
  });
+  d->add(ns + "distance_deviation", [&](auto & stat) {


Add missing diagnostic

TakaHoribe · 2024-01-28T15:41:42Z

planning/planning_validator/test/src/test_planning_validator_pubsub.cpp

@@ -55,7 +59,7 @@ class PubSubManager : public rclcpp::Node

 void spinSome(rclcpp::Node::SharedPtr node_ptr)
 {
-  for (int i = 0; i < 50; ++i) {
+  for (int i = 0; i < 10; ++i) {


Need to make sure this spin time is enough for CICD. This works well in my environment.

Signed-off-by: Takamasa Horibe <[email protected]>

SakodaShintaro · 2024-01-31T06:23:03Z

map/map_projection_loader/src/map_projection_loader.cpp

@@ -95,4 +95,5 @@ MapProjectionLoader::MapProjectionLoader() : Node("map_projection_loader")
  const auto adaptor = component_interface_utils::NodeAdaptor(this);
  adaptor.init_pub(publisher_);
  publisher_->publish(msg);
+  RCLCPP_WARN(get_logger(), "published map projection message");


Is this temporary code for debugging?
Or do you really want to merge it in the main branch as a WARN message?

@SakodaShintaro Sorry, this is just my mistake. This PR won't have any modification out of the planning_validator pkg.

Signed-off-by: Takamasa Horibe <[email protected]>

codecov · 2024-02-09T11:46:44Z

Codecov Report

Attention: 126 lines in your changes are missing coverage. Please review.

Comparison is base (1c54d43) 14.87% compared to head (465fbe8) 15.02%.
Report is 39 commits behind head on main.

Files	Patch %	Lines
...idator/test/src/test_planning_validator_pubsub.cpp	65.65%	1 Missing and 67 partials ⚠️
...idator/test/src/test_planning_validator_helper.cpp	59.75%	5 Missing and 28 partials ⚠️
...ning/planning_validator/src/planning_validator.cpp	68.33%	2 Missing and 17 partials ⚠️
...tor/test/src/test_planning_validator_functions.cpp	0.00%	0 Missing and 6 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6195      +/-   ##
==========================================
+ Coverage   14.87%   15.02%   +0.14%     
==========================================
  Files        1838     1838              
  Lines      126549   126843     +294     
  Branches    37958    38055      +97     
==========================================
+ Hits        18826    19057     +231     
+ Misses      86503    86492      -11     
- Partials    21220    21294      +74

Flag	Coverage Δ		*Carryforward flag
differential	`58.28% <63.58%> (?)`
total	`14.87% <ø> (-0.01%)`	⬇️	Carriedforward from 720abf3

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

kosuke55 · 2024-02-14T12:43:47Z

planning/planning_validator/src/planning_validator.cpp

+  std::cerr << "forward_length_required = " << forward_length_required
+            << ", forward_length = " << forward_length << std::endl;


is this needed?

Thank you! I removed it.

Signed-off-by: Takamasa Horibe <[email protected]>

kosuke55

LGTM!

…lated tests (autowarefoundation#6195) * tmp modify for smoother Signed-off-by: Takamasa Horibe <[email protected]> * improve planning validator Signed-off-by: Takamasa Horibe <[email protected]> * Revert "tmp modify for smoother" This reverts commit a360bd0. * remove undesired debug dependency Signed-off-by: Takamasa Horibe <[email protected]> * fix include guard Signed-off-by: Takamasa Horibe <[email protected]> * remove unintentional change Signed-off-by: Takamasa Horibe <[email protected]> * use acceleration to calculate required trajectory length Signed-off-by: Takamasa Horibe <[email protected]> * fix tests Signed-off-by: Takamasa Horibe <[email protected]> * remove debug comment Signed-off-by: Takamasa Horibe <[email protected]> --------- Signed-off-by: Takamasa Horibe <[email protected]>

TakaHoribe added 3 commits January 26, 2024 19:24

tmp modify for smoother

a360bd0

Signed-off-by: Takamasa Horibe <[email protected]>

improve planning validator

07c2171

Signed-off-by: Takamasa Horibe <[email protected]>

Revert "tmp modify for smoother"

25100bb

This reverts commit a360bd0.

TakaHoribe requested a review from kosuke55 as a code owner January 28, 2024 15:39

github-actions bot added the component:planning Route planning, decision-making, and navigation. (auto-assigned) label Jan 28, 2024

TakaHoribe commented Jan 28, 2024

View reviewed changes

TakaHoribe added 2 commits January 29, 2024 00:42

remove undesired debug dependency

5c2aca6

Signed-off-by: Takamasa Horibe <[email protected]>

fix include guard

0b931d1

Signed-off-by: Takamasa Horibe <[email protected]>

TakaHoribe requested review from anhnv3991, KYabuuchi, kminoda, Motsu-san, RyuYamamoto, SakodaShintaro, TaikiYamada4 and YamatoAndo as code owners January 31, 2024 04:53

github-actions bot added the component:map Map creation, storage, and loading. (auto-assigned) label Jan 31, 2024

SakodaShintaro reviewed Jan 31, 2024

View reviewed changes

TakaHoribe marked this pull request as draft February 9, 2024 04:58

TakaHoribe added 2 commits February 9, 2024 14:02

Merge remote-tracking branch 'awf/main' into update-planning-validator

9fe4e8c

remove unintentional change

081109d

Signed-off-by: Takamasa Horibe <[email protected]>

github-actions bot removed the component:map Map creation, storage, and loading. (auto-assigned) label Feb 9, 2024

YamatoAndo removed request for RyuYamamoto, anhnv3991, YamatoAndo, KYabuuchi, kminoda and Motsu-san February 9, 2024 05:10

YamatoAndo removed the request for review from TaikiYamada4 February 9, 2024 05:10

use acceleration to calculate required trajectory length

7a10058

Signed-off-by: Takamasa Horibe <[email protected]>

github-actions bot added the type:documentation Creating or refining documentation. (auto-assigned) label Feb 9, 2024

fix tests

720abf3

Signed-off-by: Takamasa Horibe <[email protected]>

TakaHoribe marked this pull request as ready for review February 9, 2024 11:00

TakaHoribe added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Feb 9, 2024

kosuke55 reviewed Feb 14, 2024

View reviewed changes

remove debug comment

465fbe8

Signed-off-by: Takamasa Horibe <[email protected]>

kosuke55 approved these changes Feb 14, 2024

View reviewed changes

TakaHoribe enabled auto-merge (squash) February 14, 2024 12:51

TakaHoribe merged commit b09bf92 into autowarefoundation:main Feb 14, 2024
22 of 26 checks passed

TakaHoribe deleted the update-planning-validator branch February 14, 2024 13:42

satoshi-ota mentioned this pull request Feb 14, 2024

fix(planning_validator): add missing params autowarefoundation/autoware_launch#876

Merged

4 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(planning_validator): add validation for trajectory length and related tests #6195

feat(planning_validator): add validation for trajectory length and related tests #6195

TakaHoribe commented Jan 28, 2024 •

edited

Loading

TakaHoribe Jan 28, 2024

TakaHoribe Jan 28, 2024

TakaHoribe Jan 28, 2024

SakodaShintaro Jan 31, 2024

TakaHoribe Feb 9, 2024

codecov bot commented Feb 9, 2024 •

edited

Loading

kosuke55 Feb 14, 2024

TakaHoribe Feb 14, 2024

kosuke55 left a comment

		std::cerr << "forward_length_required = " << forward_length_required
		<< ", forward_length = " << forward_length << std::endl;

feat(planning_validator): add validation for trajectory length and related tests #6195

feat(planning_validator): add validation for trajectory length and related tests #6195

Conversation

TakaHoribe commented Jan 28, 2024 • edited Loading

Description

Related links

Tests performed

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

TakaHoribe Jan 28, 2024

Choose a reason for hiding this comment

TakaHoribe Jan 28, 2024

Choose a reason for hiding this comment

TakaHoribe Jan 28, 2024

Choose a reason for hiding this comment

SakodaShintaro Jan 31, 2024

Choose a reason for hiding this comment

TakaHoribe Feb 9, 2024

Choose a reason for hiding this comment

codecov bot commented Feb 9, 2024 • edited Loading

Codecov Report

kosuke55 Feb 14, 2024

Choose a reason for hiding this comment

TakaHoribe Feb 14, 2024

Choose a reason for hiding this comment

kosuke55 left a comment

Choose a reason for hiding this comment

TakaHoribe commented Jan 28, 2024 •

edited

Loading

codecov bot commented Feb 9, 2024 •

edited

Loading