Twist stamped migration#4031

Closed

SteveMacenski wants to merge 38 commits intomainfrom

twist_stamped_migration

Member

SteveMacenski commented Jan 4, 2024

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #1)
Primary OS tested on	Ubuntu,
Robotic platform tested on	CI
Does this PR contain AI generated software?	None

Ryanf55 and others added 30 commits

December 7, 2023 18:33


          Add TwistStamped to controller_server via TwistPublisher util

ede0c73

* Add a new util class for publishing either Twist or TwistStamped
* Add a new parameter for selecting to stamp the twist data
* Consume TwistPublisher in nav2_controller

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Fix small issues

a2ec1a5

* Unused variable
* Incorrect doxygen

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Remove stored node and assert

356640d

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Add tests for node

e75f865

* Facing timeout even though it does the same thing as velocity smoother test

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Add missing spin call to solve timeout

fdaf98e

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Fix copyright (me instead of intel)

ce295c3

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Add full test coverage with subscriber

5c63d59

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Remove unused rclcpp fixture

bbb1b23

* Can't use it due to needing to join the pub thread after rclcpp shuts down

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Use TwistStamped in nav2_behaviors

c6119b8

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Use TwistStamped in collision monitor node

a4c2ffb

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Add TwistStamped readme updates to velocity smoother

50a90ea

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Add TwistSubscriber implementation

d8ff35c

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Fix syntax errors

8c2c37f

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Use TwistSubscriber in test_velocity_smoother

5449cf0

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Use TwistSubscriber in assisted_teleop

47b6e77

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Use TwistSubscriber in collision monitor node

78b3018

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Use TwistSubscriber in velocity smoother

9306ce5

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Remove unused code

7f349be

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          add timestamp and frame_id to TwistStamped message

0f23c80


          Add missing utility include

ef67626

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Document TwistPublisher and TwistSubscriber usage

22732b2

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Use pass-by-reference

ff36e3f

* Instead of std::move(std::unique_ptr<TwistStamped>)

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Finish twist subscriber tests

de9a695

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Add other constructor and docs

00ba082

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Merge branch 'main' into 1594-twist-stamped-publisher

d806e1c


          Fix linter issues

5895d33

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Manually fix paren alignment

e65fed5

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Remove GSoC reference

d074e00

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Document twist bool param in README

b94181e

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Handle twistPublisher in collision monitor

0e70ab3

* Implement behavior in the stamped callback
* Unstamped callback calls the stamped callback
* Switch to unique pointer for publisher

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

Ryanf55 and others added 7 commits

December 22, 2023 22:25


          Convert to using TwistStamped interally

50cc292

* Use incoming twistStamped timestamp if available
* Convert all internal representations to use TwistStamped

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Remove nav2_util usage instructions

c9b4e9b

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Remove unused Twist only subscriber

83d3b64

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          More linter fixes

5fbf249

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Prefer working with unique_ptr for cmd_vel

03ec283

* This makes it easier to switch to std::move instead of dereference on
  publish

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>


          Completing twist stamped migration

e6c019f


          shared to unique ptr

da53aa2

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

SteveMacenski requested a review from jwallace42

January 4, 2024 01:13

Contributor

Ryanf55 commented Jan 4, 2024 •

edited

Loading

The additional changes look great. Any reason I shouldn't just cherry-pick them onto the original PR? Due to some unforeseen circumstances, I'll need 1 week, perhaps 2, before I am able to build+test this with ArduPilot. Given the previous level of testing and CI, I'd be happy to merge as-is.

Member Author

SteveMacenski commented Jan 4, 2024

Any reason I shouldn't just cherry-pick them onto the original PR?

I figured this was easier, but if you want to continue to take the lead, I'm perfectly happy with that. I'd want you to test before merging as the primary consumer of enabling the parameters

codecov bot commented Jan 4, 2024 •

edited

Loading

Codecov Report

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

Comparison is base (e2c8555) 90.15% compared to head (da53aa2) 90.13%.
Report is 4 commits behind head on main.

❗ Current head da53aa2 differs from pull request most recent head 2e311c2. Consider uploading reports for the commit 2e311c2 to get more accurate results

Files	Patch %	Lines
nav2_behaviors/plugins/assisted_teleop.cpp	78.57%	3 Missing ⚠️
...2_collision_monitor/src/collision_monitor_node.cpp	88.23%	2 Missing ⚠️
nav2_util/include/nav2_util/twist_publisher.hpp	92.30%	2 Missing ⚠️
nav2_velocity_smoother/src/velocity_smoother.cpp	93.54%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #4031      +/-   ##
==========================================
- Coverage   90.15%   90.13%   -0.03%     
==========================================
  Files         415      417       +2     
  Lines       18593    18645      +52     
==========================================
+ Hits        16763    16805      +42     
- Misses       1830     1840      +10

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

jwallace42 reviewed

View reviewed changes

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp Show resolved Hide resolved

nav2_behaviors/plugins/assisted_teleop.cpp Show resolved Hide resolved

nav2_behaviors/plugins/spin.cpp Show resolved Hide resolved

nav2_collision_monitor/src/collision_monitor_node.cpp Show resolved Hide resolved


          twist add stamps and properly propogated

2e311c2

Member Author

SteveMacenski commented Jan 4, 2024

Nice catch, looking back over it, the original had a number of issues where the proper stamps weren't stored and reused, but incorrectly restamped

jwallace42 approved these changes

View reviewed changes

Contributor

jwallace42 left a comment

LGTM!

Contributor

Ryanf55 commented Jan 8, 2024

I've cherry picked these changes back to the original PR. I'll report back with ArduPilot test results soon.

Member Author

SteveMacenski commented Jan 8, 2024

No rush - take it easy Ryan

Ryanf55 mentioned this pull request

Add TwistStamped support via TwistPublisher and TwistSubscriber #3775

Closed

7 tasks

Ryanf55 mentioned this pull request

1594 twist stamped publisher #4077

Merged

SteveMacenski closed this

SteveMacenski deleted the twist_stamped_migration branch

February 8, 2024 03:15

SteveMacenski mentioned this pull request

45-50% performance improvement in MPPI controller using Eigen library for computation. #4621

Merged

7 tasks

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet