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

Full review #2

Closed
wants to merge 251 commits into from
Closed
Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
251 commits
Select commit Hold shift + click to select a range
f0ea20f
Robustify against negative dt values
Timple Aug 3, 2021
a8f45bd
Merge pull request #1 from nobleo/bugfix/HARVEY-812-negative-dt
Timple Aug 3, 2021
727ad5f
Make init-vel threshold tunable so users can pick a value for their a…
Aug 16, 2021
81b78b9
Merge pull request #3 from nobleo/fix/init_vel_threshold_tunable
MCFurry Aug 16, 2021
86e2dcf
2.16.0
Timple Aug 20, 2021
c6edd9c
Merge pull request #4 from nobleo/release/2.16.0
Timple Aug 21, 2021
503603d
Enable industrial CI
Timple Aug 23, 2021
5bb319c
See if noetic fails as well
Timple Aug 23, 2021
388bc15
Relax the criterium a bit
Timple Aug 23, 2021
709a2d2
Merge pull request #5 from nobleo/industrial-ci
Timple Sep 22, 2021
aed0080
Fix backwards cancel bug and add test for it
Dec 1, 2021
ada695d
Fix test!?
Dec 1, 2021
00202b4
Merge pull request #7 from nobleo/fix/HARVEY-855-cancel-on-backward-turn
MCFurry Dec 3, 2021
18146c1
Fix acceleration/deceleration switching in reverse driving
Jan 20, 2022
fa60109
Add ugly test for checking forward and reverse acceleration/decelerat…
Jan 20, 2022
8876ba8
Slight test improvements
Jan 20, 2022
c26e5cf
Change accel/decel check in test
Jan 25, 2022
39c0a10
Try fix test?
Jan 25, 2022
bdf0057
This seems to fix the tests?
Jan 26, 2022
6bb3f8b
using sign() function here is not necessary...
Jan 26, 2022
e6f4fcb
sign() function not needed here
Jan 27, 2022
1812b07
Cleanup unused dependencies
Rayman Jan 28, 2022
12dc7f0
Fix compile warnings
Rayman Jan 28, 2022
916de0f
Merge pull request #13 from nobleo/fix/compile-warnings
Rayman Jan 28, 2022
39b4b70
Cleanup header files
Rayman Jan 28, 2022
47806e2
fix: Use target_x_vel to determine collision look ahead direction
PaulVerhoeckx Feb 1, 2022
c088ff1
fix: Prevent configuring a collision check resolution of zero
PaulVerhoeckx Feb 1, 2022
77eca58
fix: Prevent dividing by zero when collision_look_ahead_distance is zero
PaulVerhoeckx Feb 1, 2022
a522def
Merge pull request #18 from nobleo/fix/prevent-devisions-by-zero
PaulVerhoeckx Feb 1, 2022
b255c38
Merge pull request #11 from nobleo/fix/HARVEY-939-acc-dec-switched-in…
MCFurry Feb 1, 2022
45d2bb3
fix: Use absolute largest velocity to determine collision look ahead …
PaulVerhoeckx Feb 3, 2022
9c12e68
Merge pull request #14 from nobleo/refactor/cleanup-headers
Rayman Feb 7, 2022
a0f0c64
Merge pull request #15 from nobleo/fix/collision-look-ahead-length-of…
PaulVerhoeckx Feb 9, 2022
e82ce1a
fix: check predicted footprints for LETHAL_OBSTACLE instead of INSCRI…
rokusottervanger Feb 2, 2022
05ee28a
fix: make velocity scaledown consider costmap only at predicted base_…
rokusottervanger Feb 2, 2022
3cc2b53
test: make tracking error assessment configurable
rokusottervanger Feb 3, 2022
12f8c95
test: disregard final tracking error in lethal obstacle test
rokusottervanger Feb 3, 2022
45f3092
Refactor visualization to a separate class
Rayman Jan 28, 2022
b1f969c
Merge pull request #23 from nobleo/refactor/visualization
Rayman Feb 14, 2022
53c1f5e
Fixed signed/unsigned comparisons.
lewie-donckers Feb 9, 2022
d1d5f47
Stricter compiler settings.
lewie-donckers Feb 9, 2022
fae6229
fixup! Refactor visualization to a separate class
Rayman Feb 14, 2022
f976942
Let the test fail if exceptions occur in vis_cb
Rayman Feb 14, 2022
9698b4e
Merge pull request #25 from nobleo/cleanup/fix-signed-unsigned-compar…
lewie-donckers Feb 15, 2022
f603fec
Reworked to add_compile_options().
lewie-donckers Feb 15, 2022
1495813
Merge pull request #26 from nobleo/cleanup/stricter-compiler-settings
lewie-donckers Feb 15, 2022
36cacd9
Switched to enum classes.
lewie-donckers Feb 9, 2022
1b54e83
Forgot pragma once.
lewie-donckers Feb 15, 2022
a992823
Replaced macros with constants.
lewie-donckers Feb 9, 2022
bdd6340
Merge pull request #27 from nobleo/cleanup/switched-to-enum-classes
lewie-donckers Feb 15, 2022
fe6ba23
Reworked formatting.
lewie-donckers Feb 15, 2022
717ea77
Merge pull request #24 from nobleo/fix/index-error
Rayman Feb 15, 2022
14b5e5e
Merge pull request #28 from nobleo/cleanup/replaced-macros-with-const…
lewie-donckers Feb 15, 2022
20e290b
Quaternion of carrotTF was not initialized, set to proper value
Feb 15, 2022
74f8080
Moved constants to reduce scope.
lewie-donckers Feb 9, 2022
fb96a3d
Merge pull request #29 from nobleo/cleanup/reduced-scope-of-constants
lewie-donckers Feb 16, 2022
2039749
Moved helper function.
lewie-donckers Feb 9, 2022
ce62ab5
Merge pull request #30 from nobleo/cleanup/reduced-scope-of-helper-fu…
lewie-donckers Feb 16, 2022
e7fa49a
Replaced C-style arrays.
lewie-donckers Feb 9, 2022
5706086
Fully initialize at once
Feb 16, 2022
931900d
Merge pull request #31 from nobleo/cleanup/replaced-c-style-arrays
lewie-donckers Feb 16, 2022
2cd5400
Remove unsused member variables.
lewie-donckers Feb 9, 2022
733488f
Mark overriden member functions as such.
lewie-donckers Feb 9, 2022
2008bd6
Distinguish between system and normal headers.
lewie-donckers Feb 9, 2022
00b4b43
Even stricter compiler settings.
lewie-donckers Feb 9, 2022
2611f8b
Replaced bind with lambda.
lewie-donckers Feb 9, 2022
6dfab7e
Merge pull request #32 from nobleo/cleanup/removed-unused-member-vari…
lewie-donckers Feb 16, 2022
7afa0d7
Merge pull request #34 from nobleo/cleanup/mark-overridden-functions
lewie-donckers Feb 16, 2022
ce5559e
Merge pull request #36 from nobleo/cleanup/distinguish-between-system…
lewie-donckers Feb 16, 2022
b5c1391
Merge pull request #47 from nobleo/fix/tf2_tf_not_properly_initialized
MCFurry Feb 16, 2022
e8f1b80
Merge pull request #37 from nobleo/cleanup/even-stricter-compiler-set…
lewie-donckers Feb 16, 2022
c03098f
Merge pull request #39 from nobleo/cleanup/replaced-bind-with-lambda
lewie-donckers Feb 16, 2022
7f18225
Replaced member variables with locals.
lewie-donckers Feb 9, 2022
e107cb9
Replaced member variable with local.
lewie-donckers Feb 9, 2022
4cadb68
Replaced member variables with constants.
lewie-donckers Feb 9, 2022
7841bdc
Ignore Python cache files.
lewie-donckers Feb 9, 2022
941217e
Reworked by moving gitignore.
lewie-donckers Feb 16, 2022
982d02c
Switched to std::clamp().
lewie-donckers Feb 9, 2022
aa1196e
Merge pull request #42 from nobleo/cleanup/replaced-member-variables-…
lewie-donckers Feb 16, 2022
25b3c7a
Merge pull request #43 from nobleo/cleanup/replaced-another-member-va…
lewie-donckers Feb 16, 2022
c2e5cdb
Merge pull request #45 from nobleo/cleanup/ignore-python-cache-files
lewie-donckers Feb 16, 2022
38a6b98
Merge pull request #44 from nobleo/cleanup/replaced-member-variables-…
lewie-donckers Feb 16, 2022
6396a14
Merge pull request #46 from nobleo/feature/use-std-clamp
lewie-donckers Feb 16, 2022
6b2264f
Explicitly disabled copy/move.
lewie-donckers Feb 9, 2022
b0a26ef
Reworked by switching to Boost.
lewie-donckers Feb 16, 2022
d1b3003
Removed dead code.
lewie-donckers Feb 9, 2022
120ffb6
Remove dead code.
lewie-donckers Feb 9, 2022
1cf7b91
Fixed signed/unsigned conversion warnings.
lewie-donckers Feb 10, 2022
1c32dda
Reworked to keep old behavior.
lewie-donckers Feb 16, 2022
6027dbf
Ensure all members are initialized.
lewie-donckers Feb 10, 2022
a76bf4e
Merge pull request #41 from nobleo/cleanup/explicitly-disable-copy-move
lewie-donckers Feb 17, 2022
b09a16e
Merge pull request #50 from nobleo/cleanup/removed-dead-code
lewie-donckers Feb 17, 2022
639ca65
Merge pull request #51 from nobleo/cleanup/removed-more-dead-code
lewie-donckers Feb 17, 2022
5c57eaf
Merge pull request #52 from nobleo/cleanup/fix-clang-tidy-signed-unsi…
lewie-donckers Feb 17, 2022
4dfa410
Reworked to different defaults.
lewie-donckers Feb 17, 2022
8cc7cf7
Merge pull request #53 from nobleo/cleanup/initialize-all-members
lewie-donckers Feb 17, 2022
17e5ddd
Fix possible unsigned underflow
Rayman Feb 17, 2022
3e85956
Added clang-tidy and fixed all warnings.
lewie-donckers Feb 16, 2022
c97e697
Reworked to use const& for big types.
lewie-donckers Feb 17, 2022
3867198
Reworked to exclude uninitialized variables.
lewie-donckers Feb 17, 2022
a9b2340
Use std::exchange().
lewie-donckers Feb 14, 2022
d9afec6
Added FifoArray abstraction.
lewie-donckers Feb 10, 2022
5c3178f
Added filtered error tracker abstraction.
lewie-donckers Feb 10, 2022
1f7a674
Reworked by renaming to SecondOrderLowpass.
lewie-donckers Feb 17, 2022
c218440
Merge pull request #55 from nobleo/cleanup/clang-tidy
lewie-donckers Feb 17, 2022
c68441d
Merge pull request #56 from nobleo/cleanup/use-std-exchange
lewie-donckers Feb 17, 2022
ee40fe0
Const locals when possible.
lewie-donckers Feb 11, 2022
367d68f
Replaced helper function sign().
lewie-donckers Feb 16, 2022
41ac8bb
Added to_transform() helper.
lewie-donckers Feb 17, 2022
4afeb4c
Merge pull request #68 from nobleo/fix/possible-unsigned-underflow
Rayman Feb 18, 2022
d45ff62
Merge pull request #57 from nobleo/cleanup/add-fifo-array-abstraction
lewie-donckers Feb 21, 2022
0bb2c08
Merge pull request #58 from nobleo/cleanup/add-filtered-error-tracker…
lewie-donckers Feb 21, 2022
abb0127
Merge pull request #60 from nobleo/cleanup/const-local-variables
lewie-donckers Feb 21, 2022
322e85b
Merge pull request #61 from nobleo/cleanup/replaced-helper-function
lewie-donckers Feb 21, 2022
8162623
Merge pull request #62 from nobleo/cleanup/added-to-transform-helper
lewie-donckers Feb 21, 2022
4d80e50
Moved distSquared() helpers.
lewie-donckers Feb 17, 2022
f267837
Reworked by switching to distance2().
lewie-donckers Feb 21, 2022
6b0b435
Merge pull request #63 from nobleo/cleanup/moved-dist-squared-helpers
lewie-donckers Feb 21, 2022
5224fa1
Added is_pose_angle_obtuse() helper.
lewie-donckers Feb 17, 2022
c033dc8
Removed config duplicate members.
lewie-donckers Feb 14, 2022
35a3250
Used ROS2 clang-format on all files.
lewie-donckers Feb 17, 2022
6f154fe
Cleaned up includes.
lewie-donckers Feb 9, 2022
07f8346
Merge pull request #64 from nobleo/cleanup/added-is-obtuse-helper
lewie-donckers Feb 21, 2022
a2d772a
Merge pull request #66 from nobleo/cleanup/removed-config-duplicate-m…
lewie-donckers Feb 21, 2022
a11a446
Merge pull request #67 from nobleo/cleanup/clang-format
lewie-donckers Feb 21, 2022
338f321
Merge pull request #40 from nobleo/cleanup/cleanup-includes
lewie-donckers Feb 21, 2022
cf7e1b2
Extracted filter_plan() function.
lewie-donckers Feb 21, 2022
b491f68
Added unittests for FifoArray.
lewie-donckers Feb 21, 2022
3e46d5f
Reworked by moving test source files.
lewie-donckers Feb 21, 2022
70a6b86
Enable VERBOSE_TESTS
Rayman Feb 21, 2022
ec8c881
Merge pull request #73 from nobleo/cleanup/extract-function-filter-plan
lewie-donckers Feb 22, 2022
feeeb15
Merge pull request #74 from nobleo/cleanup/unittest_fifo_array
lewie-donckers Feb 22, 2022
5dacfef
Merge pull request #77 from nobleo/feature/verbose-tests
Rayman Feb 22, 2022
c33c2eb
Enable roslint
Rayman Feb 22, 2022
c7e973a
Disable whitespace/braces check
Rayman Feb 22, 2022
32f9d89
Fix roslint
Rayman Feb 22, 2022
61c6cc8
Merge pull request #78 from nobleo/fix/enable-roslint
Rayman Feb 22, 2022
9fc9c64
Refactored output parameters of update.
lewie-donckers Feb 23, 2022
fb18c46
Refactored output parameters of distToSegmentSquared().
lewie-donckers Feb 23, 2022
2f38340
Refactored distToSegmentSquared().
lewie-donckers Feb 23, 2022
5c81ea7
Refactored findPositionOnPlan().
lewie-donckers Feb 23, 2022
27794c5
Reworked by adding documentation.
lewie-donckers Feb 24, 2022
029012c
Reworked by switching to std::clamp().
lewie-donckers Feb 24, 2022
60febdb
Merge pull request #84 from nobleo/cleanup/refactored-output-paramete…
lewie-donckers Feb 24, 2022
0b0aa98
Merge pull request #85 from nobleo/cleanup/refactored-output-paramete…
lewie-donckers Feb 24, 2022
292baa9
Merge pull request #91 from nobleo/cleanup/refactored-distToSegmentSq…
lewie-donckers Feb 24, 2022
3a50bc1
Merge pull request #92 from nobleo/cleanup/refactored-findPositionOnPlan
lewie-donckers Feb 24, 2022
dad418d
Refactored mandatory ptr parameter to reference.
lewie-donckers Feb 21, 2022
edb060d
Merge pull request #76 from nobleo/cleanup/refactored-mandatory-ptr-p…
lewie-donckers Feb 24, 2022
05e3c26
Split loop into 3.
lewie-donckers Feb 23, 2022
a97a7f6
Replaced some loops with algorithms.
lewie-donckers Feb 23, 2022
eadc778
Extracted deltas_of_plan().
lewie-donckers Feb 24, 2022
61bd1c1
Keep RosLint happy.
lewie-donckers Feb 24, 2022
9553a44
Extracted distances_to_goal().
lewie-donckers Feb 24, 2022
727444b
Extracted inverse_turning_radiuses().
lewie-donckers Feb 24, 2022
f3c81ec
Keep RosLint happy.
lewie-donckers Feb 24, 2022
454f586
Refactored setTricycleModel().
lewie-donckers Feb 24, 2022
3226551
Refactored setPlan().
lewie-donckers Feb 24, 2022
c060190
Refactored findPositionOnPlan().
lewie-donckers Feb 24, 2022
02c0e8b
Refactored last Controller functions().
lewie-donckers Feb 24, 2022
591d815
Removed unnecessary conversions.
lewie-donckers Feb 24, 2022
79a04e8
Removed global_plan_ member.
lewie-donckers Feb 24, 2022
f75275f
Merge pull request #95 from nobleo/cleanup/refactored-setPlan
lewie-donckers Feb 28, 2022
c72feae
Merge pull request #96 from nobleo/cleanup/extracted-setPlan-calculat…
lewie-donckers Feb 28, 2022
056f450
Merge pull request #98 from nobleo/cleanup/extracted-setPlan-calculat…
lewie-donckers Feb 28, 2022
74833d5
Merge pull request #99 from nobleo/cleanup/extracted-setPlan-calculat…
lewie-donckers Feb 28, 2022
dd16487
Moved visualization.
lewie-donckers Feb 28, 2022
2a0762e
Reworked by renaming local variable.
lewie-donckers Feb 28, 2022
d2bcbbd
Keep clang-tidy happy.
lewie-donckers Feb 28, 2022
5f3c36e
Merge pull request #109 from nobleo/cleanup/calculations-clang-tidy-fix
lewie-donckers Feb 28, 2022
8366354
2.18.0
Timple Mar 1, 2022
fea81ec
Merge branch 'main' into fix/controller-is-too-conservative-in-its-co…
rokusottervanger Mar 1, 2022
68ab94d
Merge pull request #100 from nobleo/fix/97-refactored-setTricycleModel
lewie-donckers Mar 2, 2022
8478161
Merge pull request #101 from nobleo/fix/97-refactored-setPlan
lewie-donckers Mar 2, 2022
e9b5da2
Merge pull request #102 from nobleo/fix/97-refactored-findPositionOnPlan
lewie-donckers Mar 2, 2022
64f450d
Merge pull request #103 from nobleo/fix/97-refactored-update
lewie-donckers Mar 2, 2022
9185c3b
Merge pull request #104 from nobleo/fix/97-removed-unnecessary-conver…
lewie-donckers Mar 2, 2022
0601627
Merge pull request #105 from nobleo/fix/97-removed-global-plan-member
lewie-donckers Mar 2, 2022
ff653a6
Merge pull request #106 from nobleo/fix/38-move-visualization
lewie-donckers Mar 3, 2022
2e9447e
Refactor collision visualization functions
Rayman Mar 2, 2022
6bd6587
Remove unnecessary marker ids
Rayman Mar 3, 2022
7c24fc5
Refactor std::transform calls into a separate function
Rayman Mar 3, 2022
33b0262
Simplified the calculation of goal direction.
lewie-donckers Feb 28, 2022
c70762c
Reworked to separate function.
lewie-donckers Mar 3, 2022
c8da2a2
Switched to old GTest syntax for melodic.
lewie-donckers Mar 3, 2022
9e329e8
Fix for melodic that is compatible with noetic.
lewie-donckers Mar 3, 2022
1a77a2f
Disable nav_core::BaseLocalPlanner support.
lewie-donckers Feb 28, 2022
5b0f1b8
Removed unnecessary nav_core references.
lewie-donckers Feb 28, 2022
25305c3
Controller no longer changes plan.
lewie-donckers Feb 28, 2022
b649534
Reworked by changing logging to error.
lewie-donckers Mar 2, 2022
5aad0ec
Replaced data member with local.
lewie-donckers Mar 1, 2022
efcda2c
Removed NOLINT.
lewie-donckers Mar 1, 2022
16acb5e
Refactored output parameter.
lewie-donckers Mar 1, 2022
c615c89
Reworked by adding comment.
lewie-donckers Mar 3, 2022
ed9caa3
Refactor to use color names
Rayman Mar 4, 2022
a09706b
Merge pull request #118 from nobleo/fix/38-move-visualization-refactored
Rayman Mar 7, 2022
b986bc9
Merge pull request #108 from nobleo/fix/82-simplify-goal-direction-ca…
lewie-donckers Mar 7, 2022
00009ed
Merge pull request #110 from nobleo/fix/90-disable-base-local-planner…
lewie-donckers Mar 7, 2022
0101afd
Merge pull request #111 from nobleo/fix/86-controller-no-longer-chang…
lewie-donckers Mar 7, 2022
08b7a12
Merge pull request #113 from nobleo/cleanup/planner-replaced-data-mem…
lewie-donckers Mar 7, 2022
22e3cb0
Merge pull request #114 from nobleo/cleanup/removed-nolint
lewie-donckers Mar 7, 2022
3173444
Merge pull request #115 from nobleo/cleanup/planner-refactor-output-p…
lewie-donckers Mar 7, 2022
660ff55
Merge branch 'main' into fix/controller-is-too-conservative-in-its-co…
rokusottervanger Mar 7, 2022
6ceb953
Moved distSquared and added tests.
lewie-donckers Mar 8, 2022
b9067ef
Made distToSegmentSquared static.
lewie-donckers Mar 8, 2022
fb79ba8
Merge pull request #20 from nobleo/fix/controller-is-too-conservative…
rokusottervanger Mar 8, 2022
8dde3a8
Clarify init_vel_max_diff check
Timple Feb 15, 2022
dac1ff0
Rokus comments
Timple Mar 9, 2022
c13602f
Reworked by renaming data member.
lewie-donckers Mar 14, 2022
9d12eda
Refactor name and output of distToSegmentSquared.
lewie-donckers Mar 8, 2022
d196881
Move, refactor and test closestPointOnSegment().
lewie-donckers Mar 10, 2022
8ab5a25
Reworked by renaming function.
lewie-donckers Mar 14, 2022
b576661
Merge pull request #130 from nobleo/refactor/122-move-distSquared
lewie-donckers Mar 14, 2022
f04dcb5
Merge pull request #131 from nobleo/refactor/122-make-distToSegmentSq…
lewie-donckers Mar 14, 2022
a7d16ca
Merge pull request #132 from nobleo/refactor/122-rename-distToSegment…
lewie-donckers Mar 14, 2022
04d8458
Merge pull request #133 from nobleo/refactor/122-move-test-closestPoi…
lewie-donckers Mar 14, 2022
ff5bb45
Fix lowpass & refactor filter structure
Rayman Feb 25, 2022
d5fdccb
Enable feedforward_ang during testing for lower error
Rayman Mar 3, 2022
f63223e
Add integral filter abstraction
Rayman Mar 4, 2022
b485259
Add reset() and configure() tests
Rayman Mar 7, 2022
d424e22
Increase timeout to reduce test flakeyness
Rayman Mar 7, 2022
3ada333
Merge pull request #119 from nobleo/fix/lowpass
Rayman Mar 14, 2022
b084218
Cleanup Controller::update().
lewie-donckers Mar 15, 2022
be0176d
Replaced path pose parameter with return value.
lewie-donckers Mar 15, 2022
e9701f3
Made Controller::findPositionOnPlan() const.
lewie-donckers Mar 15, 2022
9d4b602
Replaced distance_to_goal_ data member with local.
lewie-donckers Mar 15, 2022
b7e143c
Moved last visited pose index to find result.
lewie-donckers Mar 15, 2022
3ac6955
Replaced controller_state parameter of findPositionOnPlan().
lewie-donckers Mar 15, 2022
cd11f02
Replaced Controller::getControllerState().
lewie-donckers Mar 15, 2022
a9db24d
Renamed position to pose for findPositionOnPlan.
lewie-donckers Mar 17, 2022
c0e7d15
Merge pull request #135 from nobleo/fix/123-cleanup-update
lewie-donckers Mar 17, 2022
13aad0a
Merge pull request #136 from nobleo/fix/123-replace-path-pose-parameter
lewie-donckers Mar 17, 2022
536a557
Merge pull request #137 from nobleo/fix/123-made-findPositionOnPlan-c…
lewie-donckers Mar 17, 2022
0e68580
Merge pull request #138 from nobleo/fix/123-replaced-distance-to-goal…
lewie-donckers Mar 17, 2022
010f296
Merge pull request #139 from nobleo/fix/123-moved-last-visited-pose-i…
lewie-donckers Mar 17, 2022
746ccbe
Merge pull request #140 from nobleo/fix/123-replaced-controller-state…
lewie-donckers Mar 17, 2022
497e790
Merge pull request #144 from nobleo/fix/123-renamed-position-to-pose
lewie-donckers Mar 17, 2022
dbe631d
Merge pull request #141 from nobleo/fix/124-replace-getControllerState
lewie-donckers Mar 17, 2022
e10e6b0
Move getControlPointPose to a separate function & test
Rayman Mar 21, 2022
e3bd43c
Simplify closestPoseOnSegment math
Rayman Mar 21, 2022
d5198be
fix: Use absolute target velocity to calculate end phase distance
PaulVerhoeckx Mar 22, 2022
ec82e90
fix: Enforce positive end phase time and simplify end phase distance
PaulVerhoeckx Mar 22, 2022
8f26afc
Merge pull request #150 from nobleo/fix/end-phase-distance-for-drivin…
PaulVerhoeckx Mar 22, 2022
1cd31ea
Merge pull request #148 from nobleo/refactor/closest-pose-on-segment
Rayman Mar 24, 2022
4d51759
Refactor projectedCollisionCost to input-output function
Timple Mar 24, 2022
1512fbc
Make projectedCollisionCost static
Timple Mar 24, 2022
28d90cc
Factored out creation of projected footprint
Timple Mar 25, 2022
826bd16
2.19.0
Timple Mar 7, 2022
c0e598d
Relicense the code under Apache-2.0
Rayman Jan 6, 2023
750f8c1
Merge pull request #157 from nobleo/relicense-apache-v2
Rayman Jan 10, 2023
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
10 changes: 6 additions & 4 deletions include/path_tracking_pid/controller.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -73,8 +73,9 @@ class Controller : private boost::noncopyable
* @param current Where is the robot now?
* @param odom_twist Robot odometry
* @param global_plan Plan to follow
* @return whether the plan was successfully updated or not
*/
void setPlan(
bool setPlan(
const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist,
const std::vector<tf2::Transform> & global_plan);

Expand All @@ -85,12 +86,14 @@ class Controller : private boost::noncopyable
* @param tf_base_to_steered_wheel Where is the steered wheel now?
* @param steering_odom_twist Steered wheel odometry
* @param global_plan Plan to follow
* @return whether the plan was successfully updated or not
*/
void setPlan(
bool setPlan(
const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist,
const tf2::Transform & tf_base_to_steered_wheel,
const geometry_msgs::Twist & steering_odom_twist,
const std::vector<tf2::Transform> & global_plan);

/**
* Find position on plan by looking at the surroundings of last known pose.
* @param current Where is the robot now?
Expand Down Expand Up @@ -137,8 +140,7 @@ class Controller : private boost::noncopyable
* @return Update result
*/
UpdateResult update_with_limits(
const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist,
ros::Duration dt);
const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist, ros::Duration dt);

/**
* Perform prediction steps on the lateral error and return a reduced velocity that stays within bounds
Expand Down
54 changes: 27 additions & 27 deletions src/controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,33 +43,27 @@ bool is_pose_angle_obtuse(
return distSquared(prev, next) > (distSquared(prev, cur) + distSquared(cur, next));
}

// Filters the given plan. The first and last poses are always accepted (if they exist).
// Intermediate poses are only accepted if the angle (with respect to the previous and next poses)
// is obtuse.
std::vector<tf2::Transform> filter_plan(const std::vector<tf2::Transform> & plan)
/**
* Checks the given plan. The first and last poses are always accepted (if they exist). Intermediate
* poses are only accepted if the angle (with respect to the previous and next poses) is obtuse.
*
* @param[in] plan Plan to check.
* @return True if all poses in the plan are accepted. False otherwise.
*/
bool check_plan(const std::vector<tf2::Transform> & plan)
{
const auto plan_size = plan.size();
auto result = std::vector<tf2::Transform>{};
result.reserve(plan.size());

if (plan_size > 0) {
result.push_back(plan.front());
}

for (int pose_idx = 1; pose_idx < static_cast<int>(plan_size) - 1; ++pose_idx) {
const auto & prev_pose = plan[pose_idx - 1];
const auto & pose = plan[pose_idx];
const auto & next_pose = plan[pose_idx + 1];
if (is_pose_angle_obtuse(prev_pose, pose, next_pose)) {
result.push_back(pose);
if (!is_pose_angle_obtuse(prev_pose, pose, next_pose)) {
return false;
}
}

if (plan_size > 1) {
result.push_back(plan.back());
}

return result;
return true;
}

} // namespace
Expand Down Expand Up @@ -150,19 +144,19 @@ TricycleSteeringCmdVel Controller::computeTricycleModelInverseKinematics(
return steering_cmd_vel;
}

void Controller::setPlan(
bool Controller::setPlan(
const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist,
const std::vector<tf2::Transform> & global_plan)
{
ROS_DEBUG("TrackingPidLocalPlanner::setPlan(%zu)", global_plan.size());

global_plan_tf_ = filter_plan(global_plan);
if (global_plan_tf_.size() != global_plan.size()) {
ROS_WARN(
"Not all poses of path are used since not all poses were in the expected direction of the "
"path!");
if (!check_plan(global_plan)) {
ROS_ERROR("Rejected plan because not all poses were in the expected direction of the path!");
return false;
}

global_plan_tf_ = global_plan;

if (!config_.track_base_link) {
// Add carrot length to plan using goal pose (we assume the last pose contains correct angle)
tf2::Transform carrotTF(
Expand Down Expand Up @@ -226,17 +220,23 @@ void Controller::setPlan(
}
controller_state_.end_phase_enabled = false;
controller_state_.end_reached = false;

return true;
}

void Controller::setPlan(
bool Controller::setPlan(
const tf2::Transform & current_tf, const geometry_msgs::Twist & odom_twist,
const tf2::Transform & tf_base_to_steered_wheel,
const geometry_msgs::Twist & /* steering_odom_twist */,
const std::vector<tf2::Transform> & global_plan)
{
setPlan(current_tf, odom_twist, global_plan);
controller_state_.previous_steering_angle = tf2::getYaw(tf_base_to_steered_wheel.getRotation());
// TODO(clopez) use steering_odom_twist to check if setpoint is being followed
const auto result = setPlan(current_tf, odom_twist, global_plan);

if (result) {
controller_state_.previous_steering_angle = tf2::getYaw(tf_base_to_steered_wheel.getRotation());
}

return result;
}

Controller::DistToSegmentSquaredResult Controller::distToSegmentSquared(
Expand Down
18 changes: 11 additions & 7 deletions src/path_tracking_pid_local_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -188,14 +188,18 @@ bool TrackingPidLocalPlanner::setPlan(const std::vector<geometry_msgs::PoseStamp

// TODO(clopez): subscribe to steered wheel odom
geometry_msgs::Twist steering_odom_twist;
pid_controller_.setPlan(
tf2_convert<tf2::Transform>(tfCurPoseStamped_.transform), latest_odom_.twist.twist,
tf2_convert<tf2::Transform>(tf_base_to_steered_wheel_stamped_.transform), steering_odom_twist,
convert_plan(global_plan_map_frame));
if (!pid_controller_.setPlan(
tf2_convert<tf2::Transform>(tfCurPoseStamped_.transform), latest_odom_.twist.twist,
tf2_convert<tf2::Transform>(tf_base_to_steered_wheel_stamped_.transform),
steering_odom_twist, convert_plan(global_plan_map_frame))) {
return false;
}
} else {
pid_controller_.setPlan(
tf2_convert<tf2::Transform>(tfCurPoseStamped_.transform), latest_odom_.twist.twist,
convert_plan(global_plan_map_frame));
if (!pid_controller_.setPlan(
tf2_convert<tf2::Transform>(tfCurPoseStamped_.transform), latest_odom_.twist.twist,
convert_plan(global_plan_map_frame))) {
return false;
}
}

pid_controller_.setEnabled(true);
Expand Down