feat(path_optimizer): additional failure logging and failure mode handling #1905
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
github-actions
bot
added
documentation
PanConChicharron
force-pushed
the
arjun.ram/RT0-33698
branch
2 times, most recently
from
ae2a9a3 to
1fcebfb
Compare
github-actions
bot
added
ci
component:planning
type:documentation
component:common
component:control
and removed
launch
planning
labels
PanConChicharron
force-pushed
the
arjun.ram/RT0-33698
branch
3 times, most recently
from
ca881f7 to
4f84b91
Compare
PanConChicharron
force-pushed
the
arjun.ram/RT0-33698
branch
2 times, most recently
from
0078eea to
56f8eb4
Compare
Comment on lines
404
to
407
| const bool apply_input_velocity = !(optimized_traj_failed || elapsed_time_over_three_seconds); | ||
| mrm_required_ = optimized_traj_failed && elapsed_time_over_three_seconds; | ||
|
|
||
| if (!apply_input_velocity) { |
There was a problem hiding this comment.
We can remove this part for the new velocity change. The reported issue will not happen since we have a short time condition to trigger MRM.
| return getPrevOptimizedTrajectory(p.traj_points); | ||
| } | ||
|
|
||
| if (enable_skip_optimization_) { | ||
| mrm_required_ = false; |
There was a problem hiding this comment.
Instead of inserting mrm_required_ = false; one by one, let's put it at the beginning of the optimizeTrajectory function only once.
| @@ -75,6 +78,7 @@ class PathOptimizer : public rclcpp::Node | |||
| bool enable_outside_drivable_area_stop_; | |||
| bool enable_skip_optimization_; | |||
| bool enable_reset_prev_optimization_; | |||
| bool mrm_required_{false}; | |||
There was a problem hiding this comment.
Publishing the diagnostics does not mean triggering the MRM since it depends on the setting of the system packages.
So, is_optimization_failed_ or something would be proper.
| @@ -301,6 +310,21 @@ bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const | |||
| return true; | |||
| } | |||
|
|
|||
| void PathOptimizer::onCheckPathOptimizationValid(diagnostic_updater::DiagnosticStatusWrapper & stat) | |||
| { | |||
| const std::string error_msg = "[Path Optimizer]: MRM not required"; | |||
There was a problem hiding this comment.
The same comment as above of "Publishing the diagnostics does not mean triggering the MRM since it depends on the setting of the system packages."
IMO, we should remove the word "MRM" from here, and just letting the latter module know that the path_optimizer has xxx kind of error would be enough.
| @@ -110,7 +183,7 @@ class MPTOptimizer | |||
| const TrajectoryParam & traj_param, const std::shared_ptr<DebugData> debug_data_ptr, | |||
| const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_); | |||
|
|
|||
| std::vector<TrajectoryPoint> optimizeTrajectory(const PlannerData & planner_data); | |||
| std::pair<std::vector<TrajectoryPoint>, int> optimizeTrajectory(const PlannerData & planner_data); | |||
There was a problem hiding this comment.
Instead of using std::pair with int, please use std::optional<std::vector>.
PanConChicharron
changed the title
PanConChicharron
force-pushed
the
arjun.ram/RT0-33698
branch
2 times, most recently
from
71b3f96 to
9b96851
Compare
Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>
PanConChicharron
force-pushed
the
arjun.ram/RT0-33698
branch
from
1624e39 to
50cc611
Compare
takayuki5168
approved these changes
Mar 18, 2025
|
Please retry analysis of this Pull-Request directly on SonarQube Cloud |
saka1-s
pushed a commit
that referenced
this pull request
Mar 24, 2025
…dling (#1905) * conditional timer - MRM trigger Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
saka1-s
pushed a commit
that referenced
this pull request
Mar 24, 2025
…dling (#1905) * conditional timer - MRM trigger Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Description
Returns the smoothed_points trajectory fed into MPT when the latter fails, triggers an emergency stop MRM after 3s.
Related links
Parent Issue:
How was this PR tested?
Notes for reviewers
None.
Interface changes
None.
Effects on system behavior
None.