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.

Sign up for GitHub

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

Jump to bottom

Implement cartesian plan cache #18

Merged
merged 9 commits into from
Dec 4, 2023
Merged

Implement cartesian plan cache #18

merged 9 commits into from
Dec 4, 2023

Conversation

methylDragon
Copy link
Collaborator

@methylDragon methylDragon commented Oct 18, 2023 •
edited
Loading

WARNING

 *   This cache does NOT support collision detection!
 *   Plans will be put into and fetched from the cache IGNORING collision.
 *   If your planning scene is expected to change between cache lookups, do NOT
 *   use this cache, fetched plans are likely to result in collision then.
 *
 *   To handle collisions this class will need to hash the planning scene world
 *   msg (after zeroing out std_msgs/Header timestamps and sequences) and do an
 *   appropriate lookup. (very out of scope)

Description

This PR builds on top of:

It implements and uses the plan cache for cartesian plans!

Cached plans appear in the move_group_cartesian_plan_cache collection in the db:
image

@methylDragon methylDragon requested a review from Yadunund October 18, 2023 04:09
@methylDragon methylDragon force-pushed the ch3/cartesian-plan-cache branch from 6f552c4 to e4c0aa1 Compare October 24, 2023 00:01
koonpeng
koonpeng requested changes Nov 8, 2023
View reviewed changes
Copy link
Member

@koonpeng koonpeng left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Same comments as #16 wrt warehouse_ros exceptions and lack of tests.

@methylDragon
Add cartesian plan caching interfaces
e4ec917 
Signed-off-by: methylDragon <methylDragon@gmail.com>
@methylDragon
Add construct_get_cartesian_plan_request
caada7e 
Signed-off-by: methylDragon <methylDragon@gmail.com>
@methylDragon
Add goal query and metadata
019dbfb 
Signed-off-by: methylDragon <methylDragon@gmail.com>
@methylDragon
Add start query and metadata
3b70b6e 
Signed-off-by: methylDragon <methylDragon@gmail.com>
@methylDragon
Implement top level cache ops
87fda73 
Signed-off-by: methylDragon <methylDragon@gmail.com>
@methylDragon
Use motion plan cache for cartesian plans
7d70524 
Signed-off-by: methylDragon <methylDragon@gmail.com>
@methylDragon
Allow mismatched plan frames since we coerce anyway
188215b 
Signed-off-by: methylDragon <methylDragon@gmail.com>
@methylDragon
Fix move bug
b2cda90 
Signed-off-by: methylDragon <methylDragon@gmail.com>
@methylDragon methylDragon force-pushed the ch3/cartesian-plan-cache branch from e4c0aa1 to b2cda90 Compare November 20, 2023 19:19
@methylDragon
Copy link
Collaborator Author

Will implement changes in a separate PR.

This was referenced Nov 20, 2023
Merged
Merged
koonpeng
koonpeng approved these changes Nov 27, 2023
View reviewed changes
Copy link
Member

@koonpeng koonpeng left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Pre-approving for when the tests are merged.

@methylDragon
Plan cache code review fixes (sans unit tests) (#26)
66864b9 
* Remove query appending macro

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Default to warehouse_ros plugin if warehouse plugin isn't set

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Return and use init result

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add todo for catching exceptions

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Implement plan fetching with configurable key

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add comments for exact match tolerance

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Slightly refactor put plan

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Rename overwrite to delete_worse_plans

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Split out motion plan cache into its own library

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Sort constraints for reduced cardinality

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Rename util function

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add todo for is_diff

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add unit tests for motion plan cache (#28)

* Add count methods

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Enable shared from this for cache class

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add unit test build rules

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add tests for motion plan cache (but not cartesian)

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Fix bugs in cartesian caching

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add tests for cartesian plan cache

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Exit if a test fails

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Remove gtest import

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Remove enable_shared_from_this

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Only check for failure

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Test half in-tolerance

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Test different robot cache

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add force_cache_mode_execute_read_only (#29)

* Add force_cache_mode_execute_read_only

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add force_cache_mode_execute_read_only input port

Signed-off-by: methylDragon <methylDragon@gmail.com>

---------

Signed-off-by: methylDragon <methylDragon@gmail.com>

---------

Signed-off-by: methylDragon <methylDragon@gmail.com>

---------

Signed-off-by: methylDragon <methylDragon@gmail.com>
Yadunund
Yadunund approved these changes Dec 4, 2023
View reviewed changes
Copy link
Member

@Yadunund Yadunund left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

force_cache_mode_execute_read_only feels like a mouthful but we can iterate on this in the future. Thanks for this effort!

@Yadunund Yadunund merged commit e4d5dd6 into ch3/motion-plan-cache Dec 4, 2023
1 of 2 checks passed
@Yadunund Yadunund deleted the ch3/cartesian-plan-cache branch December 4, 2023 16:25
Yadunund pushed a commit that referenced this pull request Jan 22, 2024
@methylDragon
Implement motion plan cache (#16)
edce5e4 
* Add motion plan cache

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Switch to snake case for function names

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Print cache fetch time and key plans on planned execution time

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add overwrite_worse_plans param

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Fix plan fetching printout and don't recache fetched plans

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Update only use cached plans parameter name

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add copyright headers and motion_planner namespace

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Make motion_plan_cache a unique_ptr instead of shared_ptr

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Set numerical precision

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Use enum for planner database mode

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Rename cache methods to prepare for cartesian caching

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add very important warning to cache class

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Fix access after move bug

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Remove internal cache node

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Implement cartesian plan cache (#18)

* Add cartesian plan caching interfaces

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add construct_get_cartesian_plan_request

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add goal query and metadata

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add start query and metadata

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Implement top level cache ops

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Use motion plan cache for cartesian plans

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Allow mismatched plan frames since we coerce anyway

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Fix move bug

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Plan cache code review fixes (sans unit tests) (#26)

* Remove query appending macro

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Default to warehouse_ros plugin if warehouse plugin isn't set

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Return and use init result

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add todo for catching exceptions

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Implement plan fetching with configurable key

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add comments for exact match tolerance

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Slightly refactor put plan

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Rename overwrite to delete_worse_plans

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Split out motion plan cache into its own library

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Sort constraints for reduced cardinality

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Rename util function

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add todo for is_diff

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add unit tests for motion plan cache (#28)

* Add count methods

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Enable shared from this for cache class

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add unit test build rules

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add tests for motion plan cache (but not cartesian)

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Fix bugs in cartesian caching

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add tests for cartesian plan cache

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Exit if a test fails

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Remove gtest import

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Remove enable_shared_from_this

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Only check for failure

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Test half in-tolerance

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Test different robot cache

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add force_cache_mode_execute_read_only (#29)

* Add force_cache_mode_execute_read_only

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add force_cache_mode_execute_read_only input port

Signed-off-by: methylDragon <methylDragon@gmail.com>

---------

Signed-off-by: methylDragon <methylDragon@gmail.com>

---------

Signed-off-by: methylDragon <methylDragon@gmail.com>

---------

Signed-off-by: methylDragon <methylDragon@gmail.com>

---------

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Fix nullptr dereference

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Fix and add motion planner server tests (#30)

* Fix and add motion planner server tests

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add test deps

Signed-off-by: methylDragon <methylDragon@gmail.com>

* Add motion planner server test to CI

Signed-off-by: methylDragon <methylDragon@gmail.com>

---------

Signed-off-by: methylDragon <methylDragon@gmail.com>

---------

Signed-off-by: methylDragon <methylDragon@gmail.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@koonpeng koonpeng koonpeng approved these changes

@Yadunund Yadunund Yadunund approved these changes

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
3 participants
@methylDragon @Yadunund @koonpeng