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 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 <[email protected]>

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

Signed-off-by: methylDragon <[email protected]>

* Return and use init result

Signed-off-by: methylDragon <[email protected]>

* Add todo for catching exceptions

Signed-off-by: methylDragon <[email protected]>

* Implement plan fetching with configurable key

Signed-off-by: methylDragon <[email protected]>

* Add comments for exact match tolerance

Signed-off-by: methylDragon <[email protected]>

* Slightly refactor put plan

Signed-off-by: methylDragon <[email protected]>

* Rename overwrite to delete_worse_plans

Signed-off-by: methylDragon <[email protected]>

* Split out motion plan cache into its own library

Signed-off-by: methylDragon <[email protected]>

* Sort constraints for reduced cardinality

Signed-off-by: methylDragon <[email protected]>

* Rename util function

Signed-off-by: methylDragon <[email protected]>

* Add todo for is_diff

Signed-off-by: methylDragon <[email protected]>

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

* Add count methods

Signed-off-by: methylDragon <[email protected]>

* Enable shared from this for cache class

Signed-off-by: methylDragon <[email protected]>

* Add unit test build rules

Signed-off-by: methylDragon <[email protected]>

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

Signed-off-by: methylDragon <[email protected]>

* Fix bugs in cartesian caching

Signed-off-by: methylDragon <[email protected]>

* Add tests for cartesian plan cache

Signed-off-by: methylDragon <[email protected]>

* Exit if a test fails

Signed-off-by: methylDragon <[email protected]>

* Remove gtest import

Signed-off-by: methylDragon <[email protected]>

* Remove enable_shared_from_this

Signed-off-by: methylDragon <[email protected]>

* Only check for failure

Signed-off-by: methylDragon <[email protected]>

* Test half in-tolerance

Signed-off-by: methylDragon <[email protected]>

* Test different robot cache

Signed-off-by: methylDragon <[email protected]>

* Add force_cache_mode_execute_read_only (#29)

* Add force_cache_mode_execute_read_only

Signed-off-by: methylDragon <[email protected]>

* Add force_cache_mode_execute_read_only input port

Signed-off-by: methylDragon <[email protected]>

---------

Signed-off-by: methylDragon <[email protected]>

---------

Signed-off-by: methylDragon <[email protected]>

---------

Signed-off-by: methylDragon <[email protected]>
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 <[email protected]>

* Switch to snake case for function names

Signed-off-by: methylDragon <[email protected]>

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

Signed-off-by: methylDragon <[email protected]>

* Add overwrite_worse_plans param

Signed-off-by: methylDragon <[email protected]>

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

Signed-off-by: methylDragon <[email protected]>

* Update only use cached plans parameter name

Signed-off-by: methylDragon <[email protected]>

* Add copyright headers and motion_planner namespace

Signed-off-by: methylDragon <[email protected]>

* Make motion_plan_cache a unique_ptr instead of shared_ptr

Signed-off-by: methylDragon <[email protected]>

* Set numerical precision

Signed-off-by: methylDragon <[email protected]>

* Use enum for planner database mode

Signed-off-by: methylDragon <[email protected]>

* Rename cache methods to prepare for cartesian caching

Signed-off-by: methylDragon <[email protected]>

* Add very important warning to cache class

Signed-off-by: methylDragon <[email protected]>

* Fix access after move bug

Signed-off-by: methylDragon <[email protected]>

* Remove internal cache node

Signed-off-by: methylDragon <[email protected]>

* Implement cartesian plan cache (#18)

* Add cartesian plan caching interfaces

Signed-off-by: methylDragon <[email protected]>

* Add construct_get_cartesian_plan_request

Signed-off-by: methylDragon <[email protected]>

* Add goal query and metadata

Signed-off-by: methylDragon <[email protected]>

* Add start query and metadata

Signed-off-by: methylDragon <[email protected]>

* Implement top level cache ops

Signed-off-by: methylDragon <[email protected]>

* Use motion plan cache for cartesian plans

Signed-off-by: methylDragon <[email protected]>

* Allow mismatched plan frames since we coerce anyway

Signed-off-by: methylDragon <[email protected]>

* Fix move bug

Signed-off-by: methylDragon <[email protected]>

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

* Remove query appending macro

Signed-off-by: methylDragon <[email protected]>

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

Signed-off-by: methylDragon <[email protected]>

* Return and use init result

Signed-off-by: methylDragon <[email protected]>

* Add todo for catching exceptions

Signed-off-by: methylDragon <[email protected]>

* Implement plan fetching with configurable key

Signed-off-by: methylDragon <[email protected]>

* Add comments for exact match tolerance

Signed-off-by: methylDragon <[email protected]>

* Slightly refactor put plan

Signed-off-by: methylDragon <[email protected]>

* Rename overwrite to delete_worse_plans

Signed-off-by: methylDragon <[email protected]>

* Split out motion plan cache into its own library

Signed-off-by: methylDragon <[email protected]>

* Sort constraints for reduced cardinality

Signed-off-by: methylDragon <[email protected]>

* Rename util function

Signed-off-by: methylDragon <[email protected]>

* Add todo for is_diff

Signed-off-by: methylDragon <[email protected]>

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

* Add count methods

Signed-off-by: methylDragon <[email protected]>

* Enable shared from this for cache class

Signed-off-by: methylDragon <[email protected]>

* Add unit test build rules

Signed-off-by: methylDragon <[email protected]>

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

Signed-off-by: methylDragon <[email protected]>

* Fix bugs in cartesian caching

Signed-off-by: methylDragon <[email protected]>

* Add tests for cartesian plan cache

Signed-off-by: methylDragon <[email protected]>

* Exit if a test fails

Signed-off-by: methylDragon <[email protected]>

* Remove gtest import

Signed-off-by: methylDragon <[email protected]>

* Remove enable_shared_from_this

Signed-off-by: methylDragon <[email protected]>

* Only check for failure

Signed-off-by: methylDragon <[email protected]>

* Test half in-tolerance

Signed-off-by: methylDragon <[email protected]>

* Test different robot cache

Signed-off-by: methylDragon <[email protected]>

* Add force_cache_mode_execute_read_only (#29)

* Add force_cache_mode_execute_read_only

Signed-off-by: methylDragon <[email protected]>

* Add force_cache_mode_execute_read_only input port

Signed-off-by: methylDragon <[email protected]>

---------

Signed-off-by: methylDragon <[email protected]>

---------

Signed-off-by: methylDragon <[email protected]>

---------

Signed-off-by: methylDragon <[email protected]>

---------

Signed-off-by: methylDragon <[email protected]>

* Fix nullptr dereference

Signed-off-by: methylDragon <[email protected]>

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

* Fix and add motion planner server tests

Signed-off-by: methylDragon <[email protected]>

* Add test deps

Signed-off-by: methylDragon <[email protected]>

* Add motion planner server test to CI

Signed-off-by: methylDragon <[email protected]>

---------

Signed-off-by: methylDragon <[email protected]>

---------

Signed-off-by: methylDragon <[email protected]>
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