fixed the asset file loading issue for object mesh marker in scene_to…

…_rviz and add_object_to_scene in the arm_commander, both are accepting the same formats including absolute file path, relative package file path, and file, package, http uri. Added definition of custom reference frames under the branch "frames" and added _define_frames in the task trees manager. Added a demo program rviz_display for illustraing displaying markers in rviz. Added publishing period to persistent objects and pointcloud in rviz_tools.

demos/rviz_display/demo.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 - Andrew Kwok Fai LUI, 
+# Robotics and Autonomous Systems Group, REF, RI
+# and the Queensland University of Technology
+
+__author__ = 'Andrew Lui'
+__copyright__ = 'Copyright 2024'
+__license__ = 'GPL'
+__version__ = '1.0'
+__email__ = '[email protected]'
+__status__ = 'Development'
+
+import os, random
+import py_trees
+from py_trees.composites import Sequence, Parallel, Composite, Selector
+# robot control module
+from arm_commander.commander_moveit import GeneralCommander, logger
+from task_trees_manager_rviz import *
+
+if __name__=='__main__':
+    try:
+        arm_commander = GeneralCommander('panda_arm')
+        the_task_manager = RVizTaskTreesManager(arm_commander)
+
+        logger.info('rviz_display demo is running')
+        the_task_manager.spin()
+    except Exception as e:
+        logger.exception(e)
+
+
+

demos/rviz_display/task_scene.yaml

@@ -0,0 +1,27 @@
+---
+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
+  positions:
+    default_z: [null, null, 0.5]
+  objects:
+    teapot:
+      type: object
+      # model_file: file://localhost/home/qcr/arm_commander_ws/src/task_trees/demos/rviz_display/utah_teapot.stl
+      model_file: package://task_trees/demos/rviz_display/utah_teapot.stl
+      dimensions: [0.01, 0.01, 0.01]
+      xyz: [0.4, 0.20, 0.00]
+      rpy: [0, 0, 3.14]
+  frames:
+    area:
+      xyz: [0.4, -0.20, 0.00]
+      rpy: [3.14, 0, 3.14]
+subscenes:
+  area:
+    frames:
+      zone:
+        xyz: [0.4, -0.20, 0.00]
+        rpy: [3.14, 0, 3.14]
+
+

demos/rviz_display/task_trees_manager_rviz.py

@@ -0,0 +1,128 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 - Andrew Kwok Fai LUI, 
+# Robotics and Autonomous Systems Group, REF, RI
+# and the Queensland University of Technology
+
+__author__ = 'Andrew Lui'
+__copyright__ = 'Copyright 2024'
+__license__ = 'GPL'
+__version__ = '1.0'
+__email__ = '[email protected]'
+__status__ = 'Development'
+
+import os, math, random
+import py_trees
+from py_trees.composites import Sequence, Parallel, Composite, Selector
+
+# robot control module
+from arm_commander.commander_moveit import GeneralCommander, logger
+import arm_commander.tools.moveit_tools as moveit_tools
+
+from task_trees.states import TaskStates
+from task_trees.behaviours_base import SimAttachObject, SimDetachObject, PrintPosesInFrame, PrintPose
+from task_trees.behaviours_move import DoMoveNamedPose, DoMoveXYZ, DoMoveXYZRPY, DoRotate
+from task_trees.task_trees_manager import TaskTreesManager, BasicTask
+from task_trees.task_scene import Scene
+from task_trees.scene_to_rviz import SceneToRViz
+import tools.pose_tools as pose_tools
+# -------------------------------------
+# Tasks specialized for this demo
+
+# A sequence of behaviours for moving the arm to one of the named pose 
+class MoveNamedPoseTask(BasicTask):
+    def __init__(self, named_pose:str):
+        """ the constructor
+        :param named_pose: one of the named pose defined in the configuration file
+        :type named_pose: str
+        """
+        if named_pose is None or type(named_pose) is not str or len(named_pose) == 0:
+            logger.error(f'{__class__.__name__}: parameter (named_pose) is not an non-empty string -> fix the missing value at behaviour construction')
+            raise AssertionError(f'A parameter is invalid')  
+        super(MoveNamedPoseTask, self).__init__(named_pose)  
+
+# ----------------------------------------
+# The TaskManager specialized for this application
+class RVizTaskTreesManager(TaskTreesManager):
+    """ This is a subclass specialized for the pick-n-drop application
+    """
+    def __init__(self, arm_commander:GeneralCommander, spin_period_ms:int=10):
+        """ the constructor
+        :param arm_commander: a general commander for a particular arm manipulator 
+        :type arm_commander: GeneralCommander
+        :param spin_period_ms: the tick_tock period, defaults to 10 seconds
+        :type spin_period_ms: int, optional
+        """
+        super(RVizTaskTreesManager, self).__init__(arm_commander)
+        # load the task scene
+        config_file = os.path.join(os.path.dirname(__file__), 'task_scene.yaml')
+        self.the_scene = Scene(config_file)
+        # setup visualization in rviz
+        self.scene_to_rviz = SceneToRViz(self.the_scene, arm_commander.get_world_reference_frame(), False)
+        self.scene_to_rviz.display_object('objects.teapot')
+
+        # setup the robotic manipulation platform through the commander
+        self.arm_commander.abort_move(wait=True)
+        self.arm_commander.reset_world()
+        bird_object = self.the_scene.get_object_config('teapot')
+        self.arm_commander.add_object_to_scene('teapot_another', bird_object.model_file, bird_object.dimensions, 
+                                bird_object.xyz, bird_object.rpy, reference_frame='area')
+        # self.arm_commander.set_workspace_walls(*(self.the_scene.query_config('regions.workspace')))
+        # setup name poses
+        self._define_named_poses(self.the_scene)
+        self._define_custom_frames(self.the_scene)
+
+        # setup the blackboard and the two blackboard keys 'seen_object'
+        self.the_blackboard.register_key(key='the_object', access=py_trees.common.Access.WRITE)      
+
+        # build and install the behavior tree
+        self._set_initialize_branch(self.create_initialize_branch())
+        self._add_task_branch(self.create_move_namedpose_task_branch(), MoveNamedPoseTask)
+
+        # install and unleash the behaviour tree
+        self._install_bt_and_spin(self.bt, spin_period_ms)
+
+    # return the logical pose of the accepted task, or raise TypeError if no task is submitted
+    def query_logical_pose_of_task(self):
+        """ return the goal, which is a logical pose, of the current task
+        :raises TypeError: no task has been submitted
+        :return: the logical pose
+        :rtype: unspecified as defined by the specific subclass of BasicTask
+        """
+        if self.the_blackboard.exists('task'):
+            return self.the_blackboard.task.get_goal_as_logical_pose()
+        raise TypeError(f'unable to query logical pose due to no task has been submitted')
+
+    # -------------------------------------------------
+    # --- create the behaviour tree and its branches
+
+    def create_move_namedpose_task_branch(self) -> Composite:
+        # - the branch that executes the task MoveNamedPoseTask
+        move_named_pose_branch = py_trees.decorators.Timeout(
+            duration=60,
+            name='move_named_pose_branch_timeout',
+            child=py_trees.composites.Sequence(
+                'move_named_pose_branch',
+                memory=True,
+                children=[
+                    DoMoveNamedPose('move_home_first', True, arm_commander=self.arm_commander, scene=self.the_scene, named_pose='named_poses.home'),
+                    DoMoveNamedPose('move_task_pose', True, arm_commander=self.arm_commander, scene=self.the_scene, named_pose=self.query_logical_pose_of_task),                    
+                    ],
+            ),
+        )
+        return move_named_pose_branch
+
+    def create_initialize_branch(self) -> py_trees.decorators.OneShot:
+        init_action_branch = py_trees.composites.Sequence(
+            'init_action_branch',
+            memory=True,
+            children = [
+                DoMoveNamedPose('init_move_home', True,
+                                        arm_commander=self.arm_commander, scene=self.the_scene, named_pose='named_poses.home'),               
+            ]
+        )
+        return init_action_branch
+
+
+
+

docs/source/SCENE_SUPPORT.md

@@ -17,7 +17,9 @@ A scene configuration file is divided into the root scene and custom subscene. T
 scene:
   ... variables of the root scene go here
     objects: 
-      ... optional definitions of objects and reference frames
+      ... optional definitions of objects (which are also reference frames)
+    frames:
+      ... optional definitions of reference frames
     named_poses:
       ... optional definitions of named poses
 
@@ -31,14 +33,15 @@ subscenes:
 
 ### Optional Branches Recognized by the Task Tree Manager
 
-The key `objects` under the root scene is recognized by the task tree manager `TaskTreesManager` class as definitions of collision objects and custom reference frames. In the task trees, collision objects are also recognized as custom reference frames. 
+The named branches of `objects`, `frames`, and `named_poses` have dedicated functions in the scene configuration.
 
-Similarly, the key `named_poses` under the root scene is also recognized by the task tree manager as the named poses adding to the arm commander.
+- The key `objects` under the root scene is recognized by the task tree manager `TaskTreesManager` class as definitions of collision objects. In the task trees, collision objects are also recognized as custom reference frames. 
+- The key `frames` under the root scene is recognized as definitions of custom reference frames. 
+- The key`named_poses` under the root scene is also recognized by the task tree manager as the named poses adding to the arm commander.
 
-Both of these branches are optional.
-
-The `TaskTreesManager` class offers the functions `_define_objects` and `_define_named_poses` for defining the objects and named poses according to the scene configuration.
+The presence of these branches are optional.
 
+The `TaskTreesManager` class offers the functions `_define_objects`, `_define_frames` and `_define_named_poses` for adding the objects, the custom reference frames, and named poses defined in the scene configuration.
 
 ### Specifying Configurations in the Root Scene and Subscenes
 
@@ -68,10 +71,12 @@ scene:
       frame: area_1
 subscenes:
 ```
-The scene configuration file below defines one subscene called 'area', in which several configuration keys are defined.
+A subscene is firstly a namespace for queries and key references. It should also be the reference frame to which the poses are defined.  The subscene name is usually either the name of a reference frame or a collision object. 
+
+The scene configuration file below defines one subscene called `area_1`, in which several configuration keys are defined. Note that `area_1` is a reference frame (a side effect of defining a collision object). Normally, pose keys such as `area_1.positions.start` and their subscene `area_1` are used together in using the move behaviours.
 ```
 subscenes:
-  area:
+  area_1:
     positions:
       hover: [null, null, 0.40]
       down: [null, null, 0.27]
@@ -83,13 +88,13 @@ subscenes:
 
 ### Collision Objects and Custom Reference Frames
 
-The collision object definition comprises the following mandatory keys headed by the name of the object.
+Collision object definitions comprise the following mandatory and optional keys headed by the name of the object.
 - `type`: `box`, `sphere`, or `object`
-- `model_file`: the path to the mesh file if the type is `object`
+- `model_file`: the path to the mesh file if the type is `object`, which supports full file path, package relative file path, the `file`, `package` and `http` uri.
 - `dimensions`: a list of length, width, and height for a `box`, the radius for a `sphere`, and the scaling factor as a list of 3 numbers for an `object`.
 - `xyz`: the position of the object
 - `rpy`: the orientation of the object  
-- `frame`: the reference frame against which the object pose is measured, where null implies the world frame. This field is optional.
+- `frame`: the reference frame for the object pose, where `null` implies the world frame. This field is optional.
 
 Every collision object will project its position and rotation as its own custom reference frames. The following defines the object named `area_1` and the fields are populated. Note that the null frame is default to the world frame.
 ```
@@ -101,7 +106,14 @@ Every collision object will project its position and rotation as its own custom
       rpy: [0, 0, 3.14]
       frame: null
 ```
-A collision object can be defined under a subscene as well. The default frame is that subscene.
+Collision objects can be defined under a subscene as well. If `frame` is not given or its value is `null`, the subscene becomes the default frame.
+
+Custom reference frame definitions comprise the following mandatory and optional keys headed by the name of the frame.
+- `xyz`: the position of the object
+- `rpy`: the orientation of the object  
+- `parent` or `frame`: the reference frame for the object pose, where `null` implies the world frame. This field is optional.
+
+Similarly, custom reference frames can be defined under a subscene. The default frame is the parent subscene.
 
 ### Loading and Querying the Scene Configuration File
 

task_trees/scene_to_rviz.py

@@ -20,11 +20,12 @@
 from tools.logging_tools import logger
 from tools.rviz_tools import RvizVisualizer
 from tools.pose_tools import pose_to_xyzq, list_to_xyzq
+from tools.rospkg_tools import PackageFile
 
 class SceneToRViz(RvizVisualizer):
     """ The helper class for managing visualization of scene configs in rviz
     """
-    def __init__(self, the_scene:Scene, base_frame:str=None, publish_objects_transform=True):
+    def __init__(self, the_scene:Scene, base_frame:str=None, publish_objects_transform=True, pub_period:float=0.1):
         # check if a ROS node has been initialized 
         node_uri = rospy.get_node_uri()
         if node_uri is None:
@@ -49,7 +50,7 @@ def __init__(self, the_scene:Scene, base_frame:str=None, publish_objects_transfo
         # setup custom transforms
         self.custom_transform_object_dict = defaultdict(lambda: None)
         # setup timer
-        self.timer_transform = rospy.Timer(rospy.Duration(0.1), self._cb_timer_transform)
+        self.timer_transform = rospy.Timer(rospy.Duration(pub_period), self._cb_timer_transform)
 
     # internal function: for publishing the transform regularly
     def _cb_timer_transform(self, event):
@@ -241,9 +242,14 @@ def display_object(self, config_key:str, rgba=[1.0, 0.0, 0.0, 0.8]) -> None:
         elif the_object.type == 'sphere':
             marker = rviz_tools.create_sphere_marker(f'object.{config_key}', 0, the_object.xyz, reference_frame, the_object.dimensions, rgba=rgba)
             self.add_persistent_marker(marker)  
-        elif the_object.type == 'object':      
+        elif the_object.type == 'object': 
+            try:
+                model_file = PackageFile.resolve_to_file_or_http_uri(the_object.model_file)
+            except Exception as ex:
+                logger.warning(f'SceneToRViz (display_objects): Invalid model_file for object ({the_object.model_file}): {ex}')
+                return
             xyzrpy = the_object.xyz + the_object.rpy        
-            marker = rviz_tools.create_mesh_marker(f'object.{config_key}', 0, the_object.model_file, xyzrpy, reference_frame, the_object.dimensions, rgba=rgba)
+            marker = rviz_tools.create_mesh_marker(f'object.{config_key}', 0, model_file, xyzrpy, reference_frame, the_object.dimensions, rgba=rgba)
             self.add_persistent_marker(marker) 
         else:
             logger.warning(f'SceneToRViz (display_objects): Invalid object type {the_object.type} of the key {config_key}')