Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(behavior_path_sampling_module): add sampling based planner #6131

Merged
Changes from 1 commit
Commits
Show all changes
115 commits
Select commit Hold shift + click to select a range
88460a6
first commit: add only necessary bpp code for template
danielsanchezaran Sep 22, 2023
449aa8b
change name of file
danielsanchezaran Sep 22, 2023
e06ad6b
delete more unrelated code
danielsanchezaran Sep 25, 2023
5c72414
refactor
danielsanchezaran Sep 25, 2023
32b6ecc
fix manager
danielsanchezaran Sep 26, 2023
edafb15
rebase
danielsanchezaran Sep 26, 2023
271985c
Copy sampling-based planner to behavior path planner
danielsanchezaran Sep 26, 2023
ea55095
fix include paths
danielsanchezaran Sep 26, 2023
e30778b
rebase
danielsanchezaran Sep 27, 2023
e60648a
eliminate unused code
danielsanchezaran Sep 27, 2023
45be300
delete repeated code
danielsanchezaran Sep 27, 2023
ee2ccc3
add dependencies for bezier and frenet planners
danielsanchezaran Sep 27, 2023
73db8e6
[WIP] Made a simple implementation of behavior planning
danielsanchezaran Sep 28, 2023
c45536e
[WIP] added comments on making drivable area
danielsanchezaran Sep 28, 2023
7ab7916
Just adding functions to test
danielsanchezaran Sep 29, 2023
b1eb340
[WIP] Implement Frenet Planner
danielsanchezaran Sep 29, 2023
3ce2eb1
eliminate unused code
danielsanchezaran Sep 29, 2023
69b23d7
WIP add debug marker generation
danielsanchezaran Oct 3, 2023
cfe4fca
Comment out for debugging
danielsanchezaran Oct 3, 2023
4f99495
return prev drivable area (temp)
danielsanchezaran Oct 6, 2023
aed6cbb
fixes to compile after rebase
danielsanchezaran Nov 7, 2023
e37b183
WIP update sampling planner param structure to equal behav planner
danielsanchezaran Nov 8, 2023
0b381de
Updated param handling
danielsanchezaran Nov 9, 2023
77d60ac
changed names of internal_variable to match changes
danielsanchezaran Nov 9, 2023
0a873eb
partially solve markers not clearing
danielsanchezaran Nov 9, 2023
fbe64d7
add param update functionality
danielsanchezaran Nov 14, 2023
439c4a0
WIP transform frenet to pathwithlaneid
danielsanchezaran Nov 15, 2023
315adae
set frenet path as output
danielsanchezaran Nov 15, 2023
41d1583
Added pruning to select the best frenet path
danielsanchezaran Nov 15, 2023
2b096ab
Initialize vehicle info
danielsanchezaran Nov 16, 2023
14ccb9c
calculate properly right and left bound for drivable area check
danielsanchezaran Nov 16, 2023
b384a01
remove debug prints and increase vehicle margin, should be param
danielsanchezaran Nov 16, 2023
5d5f5d6
param changes for driving in small lanes
danielsanchezaran Nov 20, 2023
c040f96
WIP add drivable area expansion from LC
danielsanchezaran Nov 22, 2023
937caa2
add drivable area expansion
danielsanchezaran Nov 22, 2023
5d93c2e
add driveable area
danielsanchezaran Nov 22, 2023
ca7b5ee
Make the points on the path have the same z as goal
danielsanchezaran Nov 23, 2023
2dea7c7
remove print, changes
danielsanchezaran Nov 24, 2023
c40bc01
WIP add prev sampling path to calculation
danielsanchezaran Nov 24, 2023
ce930d0
WIP constraints handler
danielsanchezaran Nov 27, 2023
d656659
Add modifiable hard constraints checking function
danielsanchezaran Nov 28, 2023
5157cb4
Add modifiable soft constraints checking function
danielsanchezaran Nov 28, 2023
f1755b0
Add costs for distance to goal and curvature
danielsanchezaran Nov 28, 2023
ac78e75
take out todo-> solved
danielsanchezaran Nov 28, 2023
51635a4
Added normalized constraints with ref path speed
danielsanchezaran Nov 29, 2023
21fc93f
(WIP)isExecution requested update to not execute
danielsanchezaran Nov 30, 2023
389beb5
refactor: move getInitialState to utils
danielsanchezaran Nov 30, 2023
4eaf28d
refactor: move some functions to utils, get rid of velocity req in ge…
danielsanchezaran Nov 30, 2023
f5046ac
made curvature soft constraint depend on distance to goal
danielsanchezaran Nov 30, 2023
727e2f8
Add prev path extension
danielsanchezaran Nov 30, 2023
d066a0a
add calculation of initial lateral velocity and acceleration
maxime-clem Nov 30, 2023
3c4b0e9
add calculation of initial lateral velocity and acceleration to path …
danielsanchezaran Dec 1, 2023
31992bd
WIP Add poses to path to get centerline distance and other stuff
danielsanchezaran Dec 4, 2023
8466bc8
clear info_marker_ to prevent performance issues
maxime-clem Dec 1, 2023
349d116
solve dependency issues
danielsanchezaran Dec 4, 2023
be59647
Add cost to avg. distance to centerline
danielsanchezaran Dec 4, 2023
6491e15
added arc lenght based extension limit
danielsanchezaran Dec 4, 2023
edea1bc
Add running and success conditions, add dist to soft const
danielsanchezaran Dec 5, 2023
0dfdaeb
update success transition
danielsanchezaran Dec 5, 2023
6810e31
Solve bug with goal not being in any current lanelet
danielsanchezaran Dec 5, 2023
e7694dd
Add todo comment
danielsanchezaran Dec 5, 2023
f26ad38
Adjust to centerline cost
danielsanchezaran Dec 6, 2023
f5a2eb6
update soft costs
danielsanchezaran Dec 6, 2023
2bc08ce
tuning
danielsanchezaran Dec 6, 2023
db3c8b5
add reference path change after sampling planner Success (which might…
danielsanchezaran Dec 6, 2023
2bfa077
Added soft constraints weights as parameter to easily tune w/ rqt
danielsanchezaran Dec 7, 2023
003e51b
improve performance by computing arc coordinates before soft constrai…
danielsanchezaran Dec 8, 2023
bf2ffed
temp
danielsanchezaran Dec 12, 2023
ce8cf65
temp
danielsanchezaran Dec 12, 2023
ec03bc0
deleted unusused
danielsanchezaran Dec 12, 2023
50a6dc3
delete unused
danielsanchezaran Dec 12, 2023
f47544a
add plugin export macro
danielsanchezaran Dec 12, 2023
11ee054
fix launch file
danielsanchezaran Dec 12, 2023
93a29f1
WIP still not launching sampling planner plugin
danielsanchezaran Dec 12, 2023
d6aed0f
solve problem of plugin insertion (duplicated files)
danielsanchezaran Dec 13, 2023
2ab53f6
partly fix issue with PathwithLaneID not having laneids at the first …
danielsanchezaran Dec 13, 2023
28def35
Modify PreviousOutput path since it is no longer a shared ptr
danielsanchezaran Dec 19, 2023
3dca5de
Added new change root lanelet request override
danielsanchezaran Dec 27, 2023
3d1adb1
WIP update collision detection to use rtree
danielsanchezaran Dec 27, 2023
9890006
fix bug with index
danielsanchezaran Dec 27, 2023
848c47f
Add rtree for collision checking
danielsanchezaran Dec 27, 2023
ab7d10a
refine soft constraints use remaining length of path max curv and nor…
danielsanchezaran Dec 27, 2023
e69485d
Add sanity check and delete unused code
danielsanchezaran Jan 4, 2024
eb321f7
change success transit function to be more accurate
danielsanchezaran Jan 4, 2024
10b38f9
refactor
danielsanchezaran Jan 4, 2024
0b72e9e
solve bug for path.extend with 0 poses
danielsanchezaran Jan 5, 2024
c59a3e4
add hard check for empty paths
danielsanchezaran Jan 5, 2024
18ea6fc
fix private current_state usage
danielsanchezaran Jan 19, 2024
dc11430
Add path reuse at different lenghts
danielsanchezaran Jan 19, 2024
05701bb
delete old comments use param for path reuse
danielsanchezaran Jan 22, 2024
50e799c
light refactoring
danielsanchezaran Jan 22, 2024
efc87fb
pre-commit changes
danielsanchezaran Jan 22, 2024
9071672
pre-commit add dependency
danielsanchezaran Jan 22, 2024
8e6b364
delete unused dependencies
danielsanchezaran Jan 29, 2024
f564a43
change constraints evaluation to return vectors
danielsanchezaran Jan 29, 2024
1d906e6
use tier4 autoware utils function to calc quaternion
danielsanchezaran Jan 29, 2024
7e5b83d
refactor, use autoware utils
danielsanchezaran Jan 29, 2024
7d9d6f6
update comment
danielsanchezaran Jan 29, 2024
9917778
Add documentation
danielsanchezaran Jan 29, 2024
4bd0b0a
pre-commit changes
danielsanchezaran Jan 29, 2024
86d291a
delete unused dependencies and repeated args
danielsanchezaran Jan 31, 2024
534001b
update copyright and fix magic numbers
danielsanchezaran Jan 31, 2024
0a5ac84
delete unused header
danielsanchezaran Jan 31, 2024
312fe57
refactoring
danielsanchezaran Jan 31, 2024
57487d0
remove unused dependency
danielsanchezaran Jan 31, 2024
55e6039
update copyright and dependency
danielsanchezaran Feb 1, 2024
1504293
update calcBound to work properly
danielsanchezaran Feb 1, 2024
fc7ba1d
solve problem with drivable area
danielsanchezaran Feb 1, 2024
24efc6f
remove forced false
danielsanchezaran Feb 1, 2024
565ccdd
solve calc bound problem
danielsanchezaran Feb 2, 2024
a7faeb6
fix compatibility with updates to bpp
danielsanchezaran Feb 2, 2024
5283a03
remove cerr print
danielsanchezaran Feb 2, 2024
29a6f0f
solve bugs when merging with lane
danielsanchezaran Feb 2, 2024
83985cd
solve issue of sbp not activating
danielsanchezaran Feb 2, 2024
4b87d15
remove unused commented code
danielsanchezaran Feb 5, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
fix private current_state usage
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
danielsanchezaran committed Feb 6, 2024
commit 18ea6fc124052dd249b9ad52f54b14ecc71595d9
Original file line number Diff line number Diff line change
@@ -93,7 +93,10 @@ class SamplingPlannerModule : public SceneModuleInterface
BehaviorModuleOutput plan() override;
CandidateOutput planCandidate() const override;
void updateData() override;
bool isRootLaneletToBeUpdated() const override { return current_state_ == ModuleStatus::SUCCESS; }
bool isRootLaneletToBeUpdated() const override
{
return getCurrentStatus() == ModuleStatus::SUCCESS;
}

void updateModuleParams(const std::any & parameters) override
{
Original file line number Diff line number Diff line change
@@ -24,7 +24,6 @@ using tier4_autoware_utils::calcDistance2d;
using tier4_autoware_utils::calcOffsetPose;
using tier4_autoware_utils::getPoint;
using tier4_autoware_utils::Point2d;
using utils::toPath;

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;