Skip to content

Commit 8071ca3

Browse files
WillYinglingWillYingling
authored
Add new objective to show adding collision object to planning scene
2 parents d07530c + 0870386 commit 8071ca3

File tree

4 files changed

+447
-122
lines changed

4 files changed

+447
-122
lines changed

‎src/lab_sim/objectives/addbottlestoplanningscene.xml‎

Lines changed: 154 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="AddBottlesToPlanningScene">
3+
<BehaviorTree
4+
ID="AddBottlesToPlanningScene"
5+
_description="Detects flasks using ML and adds them as collision objects to the planning scene"
6+
_favorite="false"
7+
>
8+
<Control ID="Sequence" name="TopLevelSequence">
9+
<SubTree ID="Clear Previous Obstacles" _collapsed="true" />
10+
<SubTree
11+
ID="Move to Waypoint"
12+
_collapsed="true"
13+
acceleration_scale_factor="1.0"
14+
controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
15+
controller_names="joint_trajectory_controller"
16+
joint_group_name="manipulator"
17+
keep_orientation="false"
18+
keep_orientation_tolerance="0.05"
19+
link_padding="0.01"
20+
seed="0"
21+
velocity_scale_factor="1.0"
22+
waypoint_name="Look at Flasks"
23+
/>
24+
<SubTree ID="Take Wrist Camera Snapshot" _collapsed="true" />
25+
<SubTree
26+
ID="Segment Image from Text Prompt Subtree"
27+
_collapsed="true"
28+
clip_model_path="models/clip.onnx"
29+
clipseg_model_path="models/clipseg.onnx"
30+
masks_visualization_topic="/masks_visualization"
31+
masks2d="{masks2d}"
32+
model_package="moveit_pro_clipseg"
33+
prompts="conical flask"
34+
threshold="0.15"
35+
image_topic_name="/wrist_camera/color"
36+
negative_prompts="box"
37+
erosion_size="10"
38+
/>
39+
<Action
40+
ID="GetPointCloud"
41+
message_out="{point_cloud}"
42+
publisher_timeout_sec="5.000000"
43+
timeout_sec="5.000000"
44+
topic_name="/wrist_camera/points"
45+
/>
46+
<Action
47+
ID="GetCameraInfo"
48+
message_out="{camera_info}"
49+
publisher_timeout_sec="5.000000"
50+
timeout_sec="5.000000"
51+
topic_name="/wrist_camera/camera_info"
52+
/>
53+
<Action
54+
ID="FindMaskedObjects"
55+
base_frame="grasp_link"
56+
camera_info="{camera_info}"
57+
detected_shapes="{detected_shapes}"
58+
face_separation_threshold="0.010000"
59+
masks2d="{masks2d}"
60+
minimum_face_area="0.000625"
61+
plane_inlier_threshold="0.010000"
62+
point_cloud="{point_cloud}"
63+
/>
64+
<Decorator
65+
ID="ForEach"
66+
index="{index}"
67+
out="{out}"
68+
vector_in="{detected_shapes}"
69+
>
70+
<Control ID="Sequence">
71+
<Control ID="Sequence">
72+
<Action
73+
ID="UnpackGraspableObjectMessage"
74+
grasp_info="{grasp_info}"
75+
graspable_object="{out}"
76+
object="{object}"
77+
object_color="{object_color}"
78+
/>
79+
<Action
80+
ID="UnpackCollisionObjectMessage"
81+
collision_object="{object}"
82+
header="{header}"
83+
id="{id}"
84+
mesh_poses="{mesh_poses}"
85+
meshes="{meshes}"
86+
operation="{operation}"
87+
plane_poses="{plane_poses}"
88+
planes="{planes}"
89+
pose="{pose}"
90+
primitive_poses="{primitive_poses}"
91+
primitives="{primitives}"
92+
subframe_names="{subframe_names}"
93+
subframe_poses="{subframe_poses}"
94+
type="{type}"
95+
/>
96+
<Action
97+
ID="UnpackPoseMessage"
98+
orientation="{orientation}"
99+
pose="{pose}"
100+
position="{position}"
101+
/>
102+
<Action
103+
ID="UnpackPointMessage"
104+
point="{position}"
105+
x="{x}"
106+
y="{y}"
107+
z="{z}"
108+
/>
109+
</Control>
110+
<Control ID="Sequence">
111+
<Action ID="ResetVector" vector="{vector}" />
112+
<Action
113+
ID="InsertInVector"
114+
index="0"
115+
output_vector="{vector}"
116+
element="{z}"
117+
input_vector="{vector}"
118+
/>
119+
<Action
120+
ID="InsertInVector"
121+
index="0"
122+
output_vector="{vector}"
123+
element="{y}"
124+
input_vector="{vector}"
125+
/>
126+
<Action
127+
ID="InsertInVector"
128+
index="0"
129+
output_vector="{vector}"
130+
element="{x}"
131+
input_vector="{vector}"
132+
/>
133+
</Control>
134+
<Action ID="Script" code="object_id:='bottle'..index" />
135+
<SubTree
136+
ID="AddCollisionBoxInFrontOfEndEffector"
137+
_collapsed="true"
138+
dimensions="0.15;0.15;0.15"
139+
offset="{vector}"
140+
object_id="{object_id}"
141+
/>
142+
</Control>
143+
</Decorator>
144+
</Control>
145+
</BehaviorTree>
146+
<TreeNodesModel>
147+
<SubTree ID="AddBottlesToPlanningScene">
148+
<MetadataFields>
149+
<Metadata runnable="true" />
150+
<Metadata subcategory="Perception - Planning Scene" />
151+
</MetadataFields>
152+
</SubTree>
153+
</TreeNodesModel>
154+
</root>

‎src/lab_sim/objectives/addcollisionboxinfrontofendeffector.xml‎

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root
3+
BTCPP_format="4"
4+
main_tree_to_execute="AddCollisionBoxInFrontOfEndEffector"
5+
>
6+
<!--//////////-->
7+
<BehaviorTree
8+
ID="AddCollisionBoxInFrontOfEndEffector"
9+
_description="Adds a collision box in front of the end effector with the specified ID, dimensions and offset."
10+
_favorite="false"
11+
name=""
12+
>
13+
<Control ID="Sequence" name="TopLevelSequence">
14+
<Action
15+
ID="CreateStampedPose"
16+
reference_frame="grasp_link"
17+
stamped_pose="{stamped_pose}"
18+
position_xyz="{offset}"
19+
/>
20+
<Action
21+
ID="CreateSolidPrimitiveBox"
22+
solid_primitive="{solid_primitive}"
23+
dimensions="{dimensions}"
24+
/>
25+
<Action
26+
ID="CreateCollisionObjectFromSolidPrimitive"
27+
collision_object="{collision_object}"
28+
object_id="{object_id}"
29+
pose="{stamped_pose}"
30+
solid_primitive="{solid_primitive}"
31+
/>
32+
<SubTree
33+
ID="AddCollisionObjectToPlanningScene"
34+
_collapsed="false"
35+
collision_object="{collision_object}"
36+
pose="{stamped_pose}"
37+
object_id="{object_id}"
38+
/>
39+
</Control>
40+
</BehaviorTree>
41+
<TreeNodesModel>
42+
<SubTree ID="AddCollisionBoxInFrontOfEndEffector">
43+
<MetadataFields>
44+
<Metadata runnable="false" />
45+
<Metadata subcategory="Perception - Planning Scene" />
46+
</MetadataFields>
47+
<inout_port
48+
name="dimensions"
49+
default="0.3;0.3;0.3"
50+
type="std::vector&lt;double, std::allocator&lt;double&gt; &gt;"
51+
/>
52+
<inout_port name="object_id" default="" type="std::string" />
53+
<inout_port
54+
name="offset"
55+
default="0;0;0.1"
56+
type="std::vector&lt;double, std::allocator&lt;double&gt; &gt;"
57+
/>
58+
</SubTree>
59+
</TreeNodesModel>
60+
</root>

‎src/lab_sim/objectives/addcollisionobjecttoplanningscene.xml‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
<?xml version="1.0" encoding="UTF-8" ?>
2+
<root BTCPP_format="4" main_tree_to_execute="AddCollisionObjectToPlanningScene">
3+
<!--//////////-->
4+
<BehaviorTree
5+
ID="AddCollisionObjectToPlanningScene"
6+
_description="Adds a collision box to the planning scene with the specified ID and pose."
7+
>
8+
<Control ID="Sequence">
9+
<Action
10+
ID="CreateGraspableObject"
11+
cuboid_object="{cuboid_object}"
12+
dx="0.100000"
13+
dy="0.100000"
14+
dz="0.100000"
15+
generate_front_face="false"
16+
generate_side_faces="false"
17+
generate_top_face="false"
18+
object_id="{object_id}"
19+
pose="{pose}"
20+
/>
21+
<Action
22+
ID="UnpackGraspableObjectMessage"
23+
grasp_info="{grasp_info}"
24+
graspable_object="{cuboid_object}"
25+
object="{object}"
26+
object_color="{object_color}"
27+
/>
28+
<Action
29+
ID="CreateGraspableObjectMessage"
30+
grasp_info="{grasp_info}"
31+
graspable_object="{graspable_object}"
32+
object="{collision_object}"
33+
object_color="{object_color}"
34+
/>
35+
<Action
36+
ID="ModifyObjectInPlanningScene"
37+
apply_planning_scene_service="/apply_planning_scene"
38+
object="{graspable_object}"
39+
/>
40+
<Action
41+
ID="GetCurrentPlanningScene"
42+
planning_scene_msg="{planning_scene}"
43+
/>
44+
<Action
45+
ID="IsCollisionObjectInPlanningScene"
46+
planning_scene="{planning_scene}"
47+
collision_object_name="{object_id}"
48+
/>
49+
</Control>
50+
</BehaviorTree>
51+
<TreeNodesModel>
52+
<SubTree ID="AddCollisionObjectToPlanningScene">
53+
<MetadataFields>
54+
<Metadata runnable="false" />
55+
<Metadata subcategory="Perception - Planning Scene" />
56+
</MetadataFields>
57+
<inout_port name="collision_object" default="{collision_object}" />
58+
<inout_port name="object_id" default="" type="std::string">
59+
ID of collision object in the planning scene
60+
</inout_port>
61+
<inout_port
62+
name="pose"
63+
default="{stamped_pose}"
64+
type="geometry_msgs::msg::PoseStamped_&lt;std::allocator&lt;void&gt; &gt;"
65+
/>
66+
</SubTree>
67+
</TreeNodesModel>
68+
</root>

0 commit comments

Comments
 (0)