task_scene.py has a revampled and simplified API based on a revised config file format on how scenes are defined and the removal of the subscene concept. modify the create_array_marker function in rviz_tool that the arrow can indicate changes in pitch

Author: Andrew Lui

demos/gridscan/behaviours_gridscan.py

@@ -43,18 +43,20 @@ def __init__(self, name, arm_commander=None, scene=None):
         self.the_blackboard.register_key(key='tank', access=py_trees.common.Access.WRITE)        
         # attach robot agent
         self.arm_commander = arm_commander
-        self.the_scene = scene
+        self.the_scene:GridScanScene = scene
 
     # the concrete implementation that contains the logic of the simulated calibration
     def update(self):
         logger.info(f'SimCalibrate: found tank at pose: ')  
         # setup objects
-        for object_name in self.the_scene.list_object_names():
-            the_object = self.the_scene.get_object_config(object_name)
-            if the_object.type == 'box':
-                self.arm_commander.add_box_to_scene(object_name, the_object.dimensions, the_object.xyz, the_object.rpy, the_object.frame)  
-            if the_object.name == 'tank':
-                self.the_blackboard.set('tank', the_object)
+        for scene_name in self.the_scene.list_scene_names():
+            link_of_scene = self.the_scene.get_link_of_scene(scene_name)
+            if link_of_scene is None:
+                continue
+            if link_of_scene.type == 'box':
+                self.arm_commander.add_box_to_scene(scene_name, link_of_scene.dimensions, link_of_scene.xyz, link_of_scene.rpy, link_of_scene.parent_frame)  
+            if link_of_scene.name == 'tank':
+                self.the_blackboard.set('tank', link_of_scene)
         self.the_blackboard.task.result = '12'
         return Status.SUCCESS
 

demos/gridscan/task_scene.yaml

@@ -1,49 +1,23 @@
 ---
-scene:
-  named_poses: 
-    stow: [3.110, -0.5467, -2.692, -1.473, 1.570, 0.000]  # from base to wrist_3
-    home: [2.524, -1.241, -2.101, -1.370, 1.570, 0.000]  # from base to wrist_3
-  regions:
-    workspace: [-1.2, -0.97, -0.1, 1.5, 0.6, 1.2]  # min_x, min_y, min_z, max_x, max_y, max_z
-    inner: [-1.2, -0.3, 1.2, 0.6]  # min_x, min_y, max_x, max_y
-    work: [-1.2, -0.6, 1.2, -0.3]  # min_x, min_y, max_x, max_y
-    work3d: [-1.2, -0.6, 0.15, 1.2, -0.3, 0.6]  # min_x, min_y, min_z, max_x, max_y, max_z
-  objects:
-    tank:
+scenes:
+  root:
+    named_poses: 
+      stow: [3.110, -0.5467, -2.692, -1.473, 1.570, 0.000]  # from base to wrist_3
+      home: [2.524, -1.241, -2.101, -1.370, 1.570, 0.000]  # from base to wrist_3
+    regions:
+      workspace: [-1.2, -0.97, -0.1, 1.5, 0.6, 1.2]  # min_x, min_y, min_z, max_x, max_y, max_z
+      inner: [-1.2, -0.3, 1.2, 0.6]  # min_x, min_y, max_x, max_y
+      work: [-1.2, -0.6, 1.2, -0.3]  # min_x, min_y, max_x, max_y
+      work3d: [-1.2, -0.6, 0.15, 1.2, -0.3, 0.6]  # min_x, min_y, min_z, max_x, max_y, max_z
+
+  tank:
+    link:
       type: box
       model_file: null
       dimensions: [0.4, 1.8, 0.10]
       xyz: [0.0, -0.5, 0.05]
       rpy: [0, 0, 1.57]
       frame: null
-    side_1:
-      type: box
-      model_file: null
-      dimensions: [0.005, 1.8, 0.20]
-      xyz: [0.0, -0.3, 0.10]
-      rpy: [0, 0, 1.57]
-      frame: null
-    side_2:
-      type: box
-      model_file: null
-      dimensions: [0.005, 1.8, 0.20]
-      xyz: [0.0, -0.7, 0.10]
-      rpy: [0, 0, 1.57]
-    side_3:
-      type: box
-      model_file: null
-      dimensions: [0.4, 0.005, 0.20]
-      xyz: [-0.9, -0.5, 0.10]
-      rpy: [0, 0, 1.57]
-    side_4:
-      type: box
-      model_file: null
-      dimensions: [0.4, 0.005, 0.20]
-      xyz: [0.9, -0.5, 0.10]
-      rpy: [0, 0, 1.57]
-
-subscenes:
-  tank:
     bbox: [-0.2, -0.9, 0.2, 0.9]  # min_x, min_y, max_x, max_y
     positions:
       default: [null, null, 0.35]
@@ -85,6 +59,37 @@ subscenes:
       tile_y: 0
       origin_position: [-0.1, 0.45]
       ee_rotation: tank.rotations.beta
+  side_1:
+    link:
+      type: box
+      model_file: null
+      dimensions: [0.005, 1.8, 0.20]
+      xyz: [0.0, -0.3, 0.10]
+      rpy: [0, 0, 1.57]
+      frame: null
+  side_2:
+    link:
+      type: box
+      model_file: null
+      dimensions: [0.005, 1.8, 0.20]
+      xyz: [0.0, -0.7, 0.10]
+      rpy: [0, 0, 1.57]
+  side_3:
+    link:
+      type: box
+      model_file: null
+      dimensions: [0.4, 0.005, 0.20]
+      xyz: [-0.9, -0.5, 0.10]
+      rpy: [0, 0, 1.57]
+  side_4:
+    link:
+      type: box
+      model_file: null
+      dimensions: [0.4, 0.005, 0.20]
+      xyz: [0.9, -0.5, 0.10]
+      rpy: [0, 0, 1.57]
+
+    
 
 
 

demos/gridscan/task_scene_gridscan.py

@@ -28,7 +28,7 @@ def __init__(self, scene_config_file:str = None):
         super(GridScanScene, self).__init__(scene_config_file)
         # -- prepare internal data structures for efficiency
         # tank configuration
-        self.tank_config = self.subscene_config['tank']
+        self.tank_config = self.scenes_branch['tank']
         # tile_step size
         self.step_size_x = self.tank_config['tile']['step_size_x']
         self.step_size_y = self.tank_config['tile']['step_size_y']

demos/gridscan/task_trees_manager_gridscan.py

@@ -189,7 +189,7 @@ def wrong_xy_at_tank(self) -> bool:
 
     def at_angle(self, rotation_pose) -> bool:
         current_xyzrpy = self.arm_commander.pose_in_frame_as_xyzrpy(reference_frame='tank')
-        rotation_rpy = self.the_scene.query_rotation_as_rpy(rotation_pose)
+        rotation_rpy = self.the_scene.query_config(rotation_pose)
         for index in range(len(rotation_rpy)):
             if rotation_rpy[index] is None:
                 continue

demos/pickndrop/task_scene.yaml

@@ -1,30 +1,24 @@
-scene:
-  named_poses: 
-    stow: [0.0, -1.244, 0.0, -2.949, 0.0, 1.704, 0.785] # from base
-    home: [0.0, -0.785, 0.0, -2.36, 0.0, 1.57, 0.785] # from base
-  regions:
-    workspace: [-0.5, -0.75, -0.1, 1.0, 0.5, 1.0]  # min_x, min_y, min_z, max_x, max_y, max_z
-    inner: [-0.5, -0.2, 1.0, 0.5]  # min_x, min_y, max_x, max_y
-  positions:
-    drop: [0.5, 0.3, 0.4]
-  objects:
-    the_table:
+scenes:
+  root:
+    named_poses: 
+      stow: [0.0, -1.244, 0.0, -2.949, 0.0, 1.704, 0.785] # from base
+      home: [0.0, -0.785, 0.0, -2.36, 0.0, 1.57, 0.785] # from base
+    regions:
+      workspace: [-0.5, -0.75, -0.1, 1.0, 0.5, 1.0]  # min_x, min_y, min_z, max_x, max_y, max_z
+      inner: [-0.5, -0.2, 1.0, 0.5]  # min_x, min_y, max_x, max_y
+    positions:
+      drop: [0.5, 0.3, 0.4]
+    gripper_offset: [0, 0, 0.12]
+    grip_prepare_offset: [0, 0, 0.20]
+        
+  the_table:
+    link:
+      parent_frame: root
       type: box
       model_file: null
       dimensions: [0.5, 0.2, 0.3]
       xyz: [0.2, -0.35, 0.15]
-      rpy: [0, 0, 0]
-    the_bin:
-      type: box
-      model_file: null
-      dimensions: [0.1, 0.1, 0.2]
-      xyz: [0.5, 0.3, 0.1]
-      rpy: [0, 0, 0]
-  gripper_offset: [0, 0, 0.12]
-  grip_prepare_offset: [0, 0, 0.20]
-      
-subscenes:
-  the_table:
+      rpy: [0, 0, 0]    
     positions:
       start: [0, 0, null]
       scan_z_level: [null, null, 0.45]
@@ -33,5 +27,12 @@ subscenes:
     scan_step_size: [0.05, 0.05]
 
   the_bin:
+    link:
+      parent_frame: root
+      type: box
+      model_file: null
+      dimensions: [0.1, 0.1, 0.2]
+      xyz: [0.5, 0.3, 0.1]
+      rpy: [0, 0, 0]    
     regions:
-      dropzone: [-0.05, -0.05, 0.05, 0.05]
+      dropzone: [-0.05, -0.05, 0.05, 0.05]

demos/pickndrop/task_trees_manager_pnd.py

@@ -121,14 +121,16 @@ def __init__(self, arm_commander:GeneralCommander, spin_period_ms:int=10):
     # Functions for the simulation (demo)
     def setup_objects(self):
         # setup objects
-        for object_name in self.the_scene.list_object_names():
-            the_object = self.the_scene.get_object_config(object_name)
-            if the_object.type == 'box':
-                self.arm_commander.add_box_to_scene(object_name, the_object.dimensions, the_object.xyz, the_object.rpy)    
+        for scene_name in self.the_scene.list_scene_names():
+            the_link = self.the_scene.get_link_of_scene(scene_name)
+            if the_link is None:
+                continue
+            if the_link.type == 'box':
+                self.arm_commander.add_box_to_scene(scene_name, the_link.dimensions, the_link.xyz, the_link.rpy)    
 
     # Generate a new ball on the table
     def generate_a_ball(self):
-        the_table = self.the_scene.get_object_config('the_table')
+        the_table = self.the_scene.get_link_of_scene('the_table')
         x, y = the_table.dimensions[0] * random.random() * 0.9, the_table.dimensions[1] * random.random() * 0.9 # position in the box frame
         z = the_table.xyz[2] + the_table.dimensions[2] / 2 + 0.035
         xyz = [x, y, z]

demos/pickndrop_estop/task_scene.yaml

@@ -1,30 +1,24 @@
-scene:
-  named_poses: 
-    stow: [0.0, -1.244, 0.0, -2.949, 0.0, 1.704, 0.785] # from base
-    home: [0.0, -0.785, 0.0, -2.36, 0.0, 1.57, 0.785] # from base
-  regions:
-    workspace: [-0.5, -0.75, -0.1, 1.0, 0.5, 1.0]  # min_x, min_y, min_z, max_x, max_y, max_z
-    inner: [-0.5, -0.2, 1.0, 0.5]  # min_x, min_y, max_x, max_y
-  positions:
-    drop: [0.5, 0.3, 0.4]
-  objects:
-    the_table:
+scenes:
+  root:
+    named_poses: 
+      stow: [0.0, -1.244, 0.0, -2.949, 0.0, 1.704, 0.785] # from base
+      home: [0.0, -0.785, 0.0, -2.36, 0.0, 1.57, 0.785] # from base
+    regions:
+      workspace: [-0.5, -0.75, -0.1, 1.0, 0.5, 1.0]  # min_x, min_y, min_z, max_x, max_y, max_z
+      inner: [-0.5, -0.2, 1.0, 0.5]  # min_x, min_y, max_x, max_y
+    positions:
+      drop: [0.5, 0.3, 0.4]
+    gripper_offset: [0, 0, 0.12]
+    grip_prepare_offset: [0, 0, 0.20]
+        
+  the_table:
+    link:
+      parent_frame: root
       type: box
       model_file: null
       dimensions: [0.5, 0.2, 0.3]
-      xyz: [0.2, -0.3, 0.15]
-      rpy: [0, 0, 0]
-    the_bin:
-      type: box
-      model_file: null
-      dimensions: [0.1, 0.1, 0.2]
-      xyz: [0.5, 0.3, 0.1]
-      rpy: [0, 0, 0]
-  gripper_offset: [0, 0, 0.12]
-  grip_prepare_offset: [0, 0, 0.20]
-      
-subscenes:
-  the_table:
+      xyz: [0.2, -0.35, 0.15]
+      rpy: [0, 0, 0]    
     positions:
       start: [0, 0, null]
       scan_z_level: [null, null, 0.45]
@@ -33,5 +27,12 @@ subscenes:
     scan_step_size: [0.05, 0.05]
 
   the_bin:
+    link:
+      parent_frame: root
+      type: box
+      model_file: null
+      dimensions: [0.1, 0.1, 0.2]
+      xyz: [0.5, 0.3, 0.1]
+      rpy: [0, 0, 0]    
     regions:
-      dropzone: [-0.05, -0.05, 0.05, 0.05]
+      dropzone: [-0.05, -0.05, 0.05, 0.05]

demos/pickndrop_estop/task_trees_manager_pnd.py

@@ -115,14 +115,16 @@ def __init__(self, arm_commander:GeneralCommander, spin_period_ms:int=10):
     # Functions for the simulation (demo)
     def setup_objects(self):
         # setup objects
-        for object_name in self.the_scene.list_object_names():
-            the_object = self.the_scene.get_object_config(object_name)
-            if the_object.type == 'box':
-                self.arm_commander.add_box_to_scene(object_name, the_object.dimensions, the_object.xyz, the_object.rpy) 
+        for scene_name in self.the_scene.list_scene_names():
+            the_link = self.the_scene.get_link_of_scene(scene_name)
+            if the_link is None:
+                continue
+            if the_link.type == 'box':
+                self.arm_commander.add_box_to_scene(scene_name, the_link.dimensions, the_link.xyz, the_link.rpy)  
 
     # Generate a new ball on the table
     def generate_a_ball(self):
-        the_table = self.the_scene.get_object_config('the_table')
+        the_table = self.the_scene.get_link_of_scene('the_table')
         x, y = the_table.dimensions[0] * random.random() * 0.9, the_table.dimensions[1] * random.random() * 0.9 # position in the box frame
         z = the_table.xyz[2] + the_table.dimensions[2] / 2 + 0.035
         xyz = [x, y, z]