Implement Full Schema using MoveIt2Servo #122
Conversation
|
Re. the following:
The app shows the user a live video stream, so assuming the user has at least a 0.5s delay before selecting a point on the screen, that delay is essentially there. |
| @@ -127,8 +218,8 @@ def create_tree( | |||
| wait_for_server_timeout_sec=0.0, | |||
| ), | |||
| # Get MoveIt2 Constraints | |||
| ComputeApproachConstraints( | |||
| name="ComputeApproachConstraints", | |||
| ComputeActionConstraints( | |||
There was a problem hiding this comment.
This could potentially block forever (e.g., if the tf or moveit lock is not available, or if the approach frame was not registered by the TF tree). Do we want to add a Timeout for that?
| # Docstring copied from @override | ||
|
|
||
| # Input Validation | ||
| if not self.blackboard_exists("stamped_msg") or not self.blackboard_exists( |
There was a problem hiding this comment.
Note that as of #124 , you can pass a list into blackboard_exists
| from ada_feeding.helpers import BlackboardKey | ||
| from ada_feeding.idioms import ( | ||
| pre_moveto_config, | ||
| scoped_behavior, | ||
| retry_call_ros_service, | ||
| ) | ||
| from ada_feeding.idioms.pre_moveto_config import set_parameter_response_all_success | ||
| from ada_feeding.trees import StartServoTree, StopServoTree | ||
| from ada_feeding.visitors import MoveToVisitor | ||
| from ada_feeding_msgs.action import AcquireFood | ||
| from ada_feeding_msgs.srv import AcquisitionSelect |
There was a problem hiding this comment.
Heads-up that some versions of pylint view imports from ada_feeding_msgs as "Third-party" and not as "Local." To address this, I'd recommend putting inputs from ada_feeding_msgs at the top of the "Local" block, to accommodate both versions of pylint.
| twist=Twist(), | ||
| ) | ||
|
|
||
| ### Define Tree Leaf Nodes |
There was a problem hiding this comment.
What was the motivation for defining these leaf nodes separately, but the other leaf nodes as part of the XML-like definition?
I think having these leaf nodes be separate is a bit confusing to read since it is unclear where variables like approach_thresh get written to the blackboard. Whereas if you define these leaf nodes in the tree structure, then readers would already have seen that Compute... outputs those blackboard variables.
There was a problem hiding this comment.
Incorporated into tree.
| ), # GraspRetry Sequence | ||
| ), # GraspRetryInfinite SuccessIsRunning | ||
| ), # GraspServoTimeout TimeoutFromBlackboard | ||
| ), # GraspSucceed FailureIsSuccess |
There was a problem hiding this comment.
This helps readability a lot -- thanks!
| name="GraspRetry", | ||
| memory=True, | ||
| children=[ | ||
| UpdateTimestamp( |
There was a problem hiding this comment.
I think we use the UpdateTimestamp + publishers.FromBlackboard sequence enough (4x in this file alone) that it makes sense to make it an idiom.
There was a problem hiding this comment.
I think I might just wrap this whole thing into a ServoMove Behavior anyway to more easily combine with MoveToVisitor, so that would encompass everything within Timeout(SuccessIsRunning(Sequence(...))) into a single value.
|
Updates tested again in sim. |
Description
Implement the full acquisition schema.
Testing procedure
Tested w/ Action 0 (i.e. just straight down and up).
pr_ros_controllers: Add parameter timer to check for outdated parameters pr_ros_controllers#30Observed occasional errors:
Before opening a pull request
python3 -m black .ada_feedingdirectory, run:pylint --recursive=y --rcfile=.pylintrc ..Before Merging
Squash & Merge