feat(obstacle_cruise_planner): use motion utils #1381
Merged
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
Signed-off-by: yutaka <purewater0901@gmail.com>
Signed-off-by: yutaka <purewater0901@gmail.com>
Codecov Report
@@ Coverage Diff @@
## main #1381 +/- ##
========================================
- Coverage 9.92% 9.92% -0.01%
========================================
Files 1117 1117
Lines 77975 77987 +12
Branches 18118 18118
========================================
Hits 7742 7742
- Misses 62679 62691 +12
Partials 7554 7554
*This pull request uses carry forward flags. Click here to find out more.
Continue to review full report at Codecov.
|
| const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj.points); | ||
| if (zero_vel_idx) { | ||
| const auto wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( | ||
| planner_data.traj.points, vehicle_info_.max_longitudinal_offset_m, *zero_vel_idx); |
| for (size_t o_idx = zero_vel_idx; o_idx < output_traj.points.size(); ++o_idx) { | ||
| output_traj.points.at(o_idx).longitudinal_velocity_mps = 0.0; | ||
| const double zero_vel_dist = | ||
| closest_obstacle_dist - vehicle_info_.max_longitudinal_offset_m - feasible_margin_from_obstacle; |
Comment on lines
135
to
157
| const double zero_vel_dist = | ||
| closest_obstacle_dist - vehicle_info_.max_longitudinal_offset_m - feasible_margin_from_obstacle; | ||
| const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj.points); | ||
| if (zero_vel_idx) { | ||
| const auto wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( | ||
| planner_data.traj.points, vehicle_info_.max_longitudinal_offset_m, *zero_vel_idx); | ||
|
|
||
| // virtual wall marker for stop obstacle | ||
| const auto marker_pose = planner_data.traj.points.at(wall_idx).pose; | ||
| const auto markers = motion_utils::createStopVirtualWallMarker( | ||
| marker_pose, "obstacle stop", planner_data.current_time, 0); | ||
| tier4_autoware_utils::appendMarkerArray(markers, &debug_data.stop_wall_marker); | ||
| debug_data.obstacles_to_stop.push_back(*closest_stop_obstacle); | ||
|
|
||
| // Publish Stop Reason | ||
| const auto stop_pose = output_traj.points.at(*zero_vel_idx).pose; | ||
| const auto stop_reasons_msg = | ||
| makeStopReasonArray(planner_data.current_time, stop_pose, *closest_stop_obstacle); | ||
| stop_reasons_pub_->publish(stop_reasons_msg); | ||
|
|
||
| // Publish if ego vehicle collides with the obstacle with a limit acceleration | ||
| const auto stop_speed_exceeded_msg = | ||
| createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle); |
There was a problem hiding this comment.
use output_traj.points for wall_idx handling.
Signed-off-by: yutaka <purewater0901@gmail.com>
Signed-off-by: yutaka <purewater0901@gmail.com>
Signed-off-by: yutaka <purewater0901@gmail.com>
Signed-off-by: yutaka <purewater0901@gmail.com>
|
@takayuki5168 Please merge this PR after this fix |
|
@purewater0901 Resolve the conflicts please. |
Signed-off-by: yutaka <purewater0901@gmail.com>
|
fixed |
boyali
referenced
this pull request
in boyali/autoware.universe
Sep 28, 2022
boyali
referenced
this pull request
in boyali/autoware.universe
Oct 3, 2022
boyali
referenced
this pull request
in boyali/autoware.universe
Oct 3, 2022
yukke42
pushed a commit
to tzhong518/autoware.universe
that referenced
this pull request
Oct 14, 2022
boyali
referenced
this pull request
in boyali/autoware.universe
Oct 19, 2022
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.
Signed-off-by: yutaka purewater0901@gmail.com
Description
Use insert stop point from motion utils to get the accurate stop distance
Pre-review checklist for the PR author
The PR author must check the checkboxes below when creating the PR.
In-review checklist for the PR reviewers
The PR reviewers must check the checkboxes below before approval.
Post-review checklist for the PR author
The PR author must check the checkboxes below before merging.
After all checkboxes are checked, anyone who has write access can merge the PR.