Skip to content

Commit 86ff341

Browse files
author
Taylor Kessler Faulkner
committed
Prevent motion to resting config from colliding with hospital table
1 parent 3328f12 commit 86ff341

File tree

2 files changed

+91
-54
lines changed

2 files changed

+91
-54
lines changed

‎ada_feeding/ada_feeding/trees/acquire_food_tree.py

Lines changed: 78 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
from overrides import override
1616
import py_trees
1717
from py_trees.blackboard import Blackboard
18+
from py_trees.behaviours import Success
1819
import py_trees_ros
1920
from rcl_interfaces.srv import SetParameters
2021
import rclpy
@@ -162,14 +163,14 @@ def create_tree(
162163
name + "RemoveWheelchairWall",
163164
),
164165
workers=[
165-
py_trees_ros.service_clients.FromConstant(
166-
name="ClearOctomap",
167-
service_name="/clear_octomap",
168-
service_type=Empty,
169-
service_request=Empty.Request(),
170-
# Default fail if service is down
171-
wait_for_server_timeout_sec=0.0,
172-
),
166+
# py_trees_ros.service_clients.FromConstant(
167+
# name="ClearOctomap",
168+
# service_name="/clear_octomap",
169+
# service_type=Empty,
170+
# service_request=Empty.Request(),
171+
# # Default fail if service is down
172+
# wait_for_server_timeout_sec=0.0,
173+
# ),
173174
MoveIt2JointConstraint(
174175
name="RestingConstraint",
175176
ns=name,
@@ -323,7 +324,8 @@ def create_tree(
323324
) # End RecoverySequence
324325

325326
def move_above_plan(
326-
flip_food_frame: bool = False, action: Optional[BlackboardKey] = None,
327+
flip_food_frame: bool = False,
328+
action: Optional[BlackboardKey] = None,
327329
) -> py_trees.behaviour.Behaviour:
328330
return py_trees.composites.Sequence(
329331
name="MoveAbovePlanningSeq",
@@ -380,7 +382,8 @@ def move_above_plan(
380382
inputs={
381383
"action_select_response": BlackboardKey(
382384
"action_response"
383-
), "action": action,
385+
),
386+
"action": action,
384387
# Default move_above_dist_m = 0.05
385388
# Default food_frame_id = "food"
386389
# Default approach_frame_id = "approach"
@@ -469,43 +472,77 @@ def move_above_plan(
469472
scoped_behavior(
470473
name="TableCollision",
471474
# Set Approach F/T Thresh
472-
pre_behavior=ToggleCollisionObject(
473-
name="AllowTable",
474-
ns=name,
475-
inputs={
476-
"collision_object_ids": ["table"],
477-
"allow": True,
478-
},
475+
pre_behavior=(
476+
Success()
477+
# ToggleCollisionObject(
478+
# name="AllowTable",
479+
# ns=name,
480+
# inputs={
481+
# "collision_object_ids": ["table"],
482+
# "allow": True,
483+
# },
484+
# )
479485
),
480-
post_behavior=ToggleCollisionObject(
481-
name="DisallowTable",
482-
ns=name,
483-
inputs={
484-
"collision_object_ids": ["table"],
485-
"allow": False,
486-
},
486+
post_behavior=(
487+
Success()
488+
# ToggleCollisionObject(
489+
# name="DisallowTable",
490+
# ns=name,
491+
# inputs={
492+
# "collision_object_ids": ["table"],
493+
# "allow": False,
494+
# },
495+
# )
487496
),
488497
on_preempt_timeout=5.0,
489498
# Starts a new Sequence w/ Memory internally
490499
workers=[
491500
scoped_behavior(
492501
name="OctomapCollision",
493502
# Set Approach F/T Thresh
494-
pre_behavior=ToggleCollisionObject(
495-
name="AllowOctomap",
496-
ns=name,
497-
inputs={
498-
"collision_object_ids": ["<octomap>"],
499-
"allow": True,
500-
},
503+
pre_behavior=py_trees.composites.Sequence(
504+
name="AllowOctomapAndTable",
505+
memory=True,
506+
children=[
507+
ToggleCollisionObject(
508+
name="AllowOctomap",
509+
ns=name,
510+
inputs={
511+
"collision_object_ids": ["<octomap>"],
512+
"allow": True,
513+
},
514+
),
515+
ToggleCollisionObject(
516+
name="AllowTable",
517+
ns=name,
518+
inputs={
519+
"collision_object_ids": ["table"],
520+
"allow": True,
521+
},
522+
),
523+
],
501524
),
502-
post_behavior=ToggleCollisionObject(
503-
name="DisallowOctomap",
504-
ns=name,
505-
inputs={
506-
"collision_object_ids": ["<octomap>"],
507-
"allow": False,
508-
},
525+
post_behavior=py_trees.composites.Sequence(
526+
name="DisallowOctomapAndTable",
527+
memory=True,
528+
children=[
529+
ToggleCollisionObject(
530+
name="DisallowOctomap",
531+
ns=name,
532+
inputs={
533+
"collision_object_ids": ["<octomap>"],
534+
"allow": False,
535+
},
536+
),
537+
ToggleCollisionObject(
538+
name="DisallowTable",
539+
ns=name,
540+
inputs={
541+
"collision_object_ids": ["table"],
542+
"allow": False,
543+
},
544+
),
545+
],
509546
),
510547
on_preempt_timeout=5.0,
511548
workers=[
@@ -522,8 +559,8 @@ def move_above_plan(
522559
name="BackupFlipFoodFrameSel",
523560
memory=True,
524561
children=[
525-
move_above_plan(False),
526-
move_above_plan(True, BlackboardKey("action")),
562+
move_above_plan(True),
563+
move_above_plan(False, BlackboardKey("action")),
527564
],
528565
),
529566
MoveIt2Execute(
@@ -849,7 +886,7 @@ def move_above_plan(
849886
), # OctomapCollision
850887
]
851888
+ resting_position_behaviors, # End TableCollision.workers
852-
), # End TableCollision
889+
), # End Success # TableCollision
853890
], # End root_seq.children
854891
) # End root_seq
855892

‎ada_feeding/config/ada_feeding_action_servers_custom.yaml

Lines changed: 13 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -8,12 +8,12 @@ ada_feeding_action_servers:
88
- hospital_table_mount_1
99
hospital_table_mount_1:
1010
AcquireFood.tree_kwargs.resting_joint_positions:
11-
- -1.17223908965
12-
- 4.070232398326571
13-
- 1.1134910875797221
14-
- -1.5986870802590785
15-
- -1.5705609033545338
16-
- 0.5052628782035897
11+
- -1.1717344206197549
12+
- 3.8349623560423542
13+
- 1.143531544675816
14+
- -1.4583640571583674
15+
- -1.8237450981853311
16+
- 7.026948142726954
1717
MoveAbovePlate.tree_kwargs.joint_positions:
1818
- -0.19797034068
1919
- 3.6562432079254257
@@ -31,12 +31,12 @@ ada_feeding_action_servers:
3131
- -0.6768258670154677
3232
- 0.1146851200873633
3333
MoveToRestingPosition.tree_kwargs.goal_configuration:
34-
- -1.17223908965
35-
- 4.070232398326571
36-
- 1.1134910875797221
37-
- -1.5986870802590785
38-
- -1.5705609033545338
39-
- 0.5052628782035897
34+
- -1.1717344206197549
35+
- 3.8349623560423542
36+
- 1.143531544675816
37+
- -1.4583640571583674
38+
- -1.8237450981853311
39+
- 7.026948142726954
4040
MoveToStagingConfiguration.tree_kwargs.goal_configuration:
4141
- -0.5394174198411514
4242
- 4.294121954324551
@@ -45,7 +45,7 @@ ada_feeding_action_servers:
4545
- -2.1827671763194583
4646
- 1.6106314909061383
4747
planning_scene_namespace_to_use: bedside
48-
namespace_to_use: default
48+
namespace_to_use: hospital_table_mount_1
4949
side_staging_1:
5050
AcquireFood.tree_kwargs.resting_joint_positions:
5151
- -0.8849039817349507

0 commit comments

Comments
 (0)