move the tools modules to task_trees.tools to avoid potential name clashes when task trees work with other packages. remove demos from the module list in the setup.py file. Update rviz_tools.py to make it the same release as rviz_marker_tool which include publishing of MarkerArray

. Modified the BasicTask class and remove all pose specific parameters and functions to recover its intention as a generic task class.

Author: Andrew Lui

CMakeLists.txt

@@ -163,7 +163,7 @@ include_directories(
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
 catkin_install_python(PROGRAMS
-  demos/pushblock/demo.py
+  task_trees/demos/pushblock/demo.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
   )
 

demos/gridscan/behaviours_advanced.py

@@ -15,8 +15,8 @@
 from py_trees.common import Status
 # robot control module
 from task_scene_gridscan import GridScanScene
-from tools.logging_tools import logger
-from demos.gridscan.behaviours_gridscan import DoMoveTankGrid
+from task_trees.tools.logging_tools import logger
+from behaviours_gridscan import DoMoveTankGrid
 
 # -----------------------------------------------------------------------------------------------------
 # Advanced behaviours for illustrating the versatility of the ConditionalCommanderBehaviour base class

demos/gridscan/behaviours_gridscan.py

@@ -16,7 +16,7 @@
 # robot control module
 from task_trees.behaviours_move import SceneConditionalCommanderBehaviour
 from task_scene_gridscan import GridScanScene
-from tools.logging_tools import logger
+from task_trees.tools.logging_tools import logger
 
 # ----------------------------------------------------------------------
 # Custom Behaviour Classes

demos/gridscan/task_trees_manager_gridscan.py

@@ -23,11 +23,11 @@
 from task_trees.behaviours_base import *
 from task_trees.task_trees_manager import BasicTaskTreesManager, TaskTreesManager, BasicTask, TaskStates
 from task_trees.scene_to_rviz import SceneToRViz
-from demos.gridscan.behaviours_gridscan import SimCalibrate, DoMoveTankGrid
-from demos.gridscan.behaviours_advanced import DoMoveTankGridVisualCDROS
+from behaviours_gridscan import SimCalibrate, DoMoveTankGrid
+from behaviours_advanced import DoMoveTankGridVisualCDROS
 
-from tools.logging_tools import logger
-import tools.pose_tools as pose_tools
+from task_trees.tools.logging_tools import logger
+import task_trees.tools.pose_tools as pose_tools
 # -------------------------------------
 # Tasks specialized for this application 
 
@@ -55,6 +55,7 @@ def __init__(self, named_pose:str):
 class MoveGridPoseTask(BasicTask):
     def __init__(self, tile_x:int, tile_y:int, cell_x:int, cell_y:int): 
         super(MoveGridPoseTask, self).__init__([tile_x, tile_y, cell_x, cell_y])
+    
 
 # ----------------------------------------
 # The TaskManager specialized for this application
@@ -118,20 +119,20 @@ def query_grid_position_of_task(self):
         """
         if not self.the_blackboard.exists('task'):
             raise TypeError(f'unable to query logical pose due to no task has been submitted')
-        return self.the_blackboard.task.get_goal_as_logical_pose()
+        return self.the_blackboard.task.get_param()
 
     # return the logical rotation of the accepted task, or raise TypeError if no task is submitted   
     def query_logical_rpy_of_task(self):
         if not self.the_blackboard.exists('task'):
             raise TypeError(f'unable to query logical rotation due to no task has been submitted')            
-        grid_position = self.the_blackboard.task.get_goal_as_logical_pose()   
+        grid_position = self.the_blackboard.task.get_param()   
         rpy = self.the_scene.compute_rpy_from_grid_position(grid_position)
         return rpy
 
     # internal function for obtaining the logical pose of the current task as xyzrpy
     def _get_task_target_xyzrpy(self) -> list:
         if self.the_blackboard.exists('task'):
-            logical_pose = self.the_blackboard.task.get_goal_as_logical_pose()
+            logical_pose = self.the_blackboard.task.get_param()
             return self.the_scene.compute_xyzrpy_from_grid_position(logical_pose)
         return False
 

demos/pickndrop/behaviours_pnd.py

@@ -13,7 +13,7 @@
 # robot control module
 import arm_commander.tools.moveit_tools as moveit_tools
 from task_trees.behaviours_base import ConditionalBehaviour, ConditionalCommanderBehaviour
-from tools.logging_tools import logger
+from task_trees.tools.logging_tools import logger
 from scan_model import ScanRegionModel
 
 # ----------------------------------------------------------------------

demos/pickndrop/demo.py

@@ -18,7 +18,7 @@
 from arm_commander.commander_moveit import GeneralCommander
 from task_trees_manager_pnd import PNDTaskTreesManager, ScanTask, PickUpObjectTask, DropObjectTask
 from task_trees.states import TaskStates
-from tools.logging_tools import logger
+from task_trees.tools.logging_tools import logger
 # -- Test cases specialized for the PickNDrop Demo
 
 class DemoStates(Enum):

demos/pickndrop/task_trees_manager_pnd.py

@@ -22,8 +22,8 @@
 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
-from demos.pickndrop.behaviours_pnd import DoScanProgram
+import task_trees.tools.pose_tools as pose_tools
+from behaviours_pnd import DoScanProgram
 from scan_model import SingleLineScanModel, SteppingScanModel, FourCornersScanModel
 
 # -------------------------------------
@@ -52,6 +52,8 @@ class PickUpObjectTask(BasicTask):
     def __init__(self, xyz_world:list): 
         super(PickUpObjectTask, self).__init__()
         self.xyz = xyz_world
+    def get_xyz(self):
+        return self.xyz
 
 
 class DropObjectTask(BasicTask):
@@ -62,7 +64,9 @@ def __init__(self):
 class MovePoseTask(BasicTask):
     def __init__(self, xyz_world:list): 
         super(MovePoseTask, self).__init__()
-        self.xyz = xyz_world        
+        self.xyz = xyz_world
+    def get_xyz(self):
+        return self.xyz
 
 # ----------------------------------------
 # The TaskManager specialized for this application
@@ -161,7 +165,7 @@ def query_logical_pose_of_task(self):
         :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()
+            return self.the_blackboard.task.get_param()
         raise TypeError(f'unable to query logical pose due to no task has been submitted')
 
     # return the target position of the accepted task, or raise TypeError if no task is submitted

demos/pickndrop_estop/behaviours_pnd.py

@@ -0,0 +1,54 @@
+# 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__ = 'ak.lui@qut.edu.au'
+__status__ = 'Development'
+
+from py_trees.common import Status
+# robot control module
+import arm_commander.tools.moveit_tools as moveit_tools
+from task_trees.behaviours_base import ConditionalBehaviour, ConditionalCommanderBehaviour
+from task_trees.tools.logging_tools import logger
+from scan_model import ScanRegionModel
+
+# ----------------------------------------------------------------------
+# Behaviour Classes for the Pick-n-Drop robot arm manipulation application
+
+class DoScanProgram(ConditionalCommanderBehaviour):
+    """ This behaviour moves the end-effector over a planar region according toa program
+        The orientation of the end-effector is fixed.
+    """
+    def __init__(self, name, condition_fn=True, condition_policy=None, arm_commander=None, scan_program:ScanRegionModel=None):
+        """ the constructor, refers to the constructor ConditionalCommanderBehaviour for the description of the other parameters
+        :param scan_program: the scan program
+        :type scan_program: ScanRegionModel
+        """
+        super(DoScanProgram, self).__init__(name=name, condition_fn=condition_fn, condition_policy=condition_policy, arm_commander=arm_commander)
+        if scan_program is None:
+            logger.error(f'{__class__.__name__} ({self.name}): parameter (scan_program) is None -> fix the missing value at behaviour construction')
+            raise AssertionError(f'A parameter should not be None nor missing')         
+        self.scan_program:ScanRegionModel = scan_program
+    # the concrete implementation of the logic when the General Commander is READY        
+    def update_when_ready(self):
+        xyz = self.scan_program.get_next_xyz()
+        if xyz is None:
+            self.scan_program.reset()
+            xyz = self.scan_program.get_next_xyz()
+            
+        # a heck to test the use of orientation constraint
+        self.arm_commander.add_path_constraints(moveit_tools.create_path_orientation_constraint(
+            self.arm_commander.END_EFFECTOR_LINK, self.arm_commander.pose_in_frame('the_table'), 0.05, 0.05, 6.28))
+        # send the command to the General Commander in an asynchronous manner
+        self.arm_commander.move_to_position(x=xyz[0], y=xyz[1], z=xyz[2], cartesian=True, reference_frame='the_table', wait=False)
+        logger.info(f'DoScanProgram ({self.name}): started move to pose: {xyz} in reference frame "the_table"')   
+        return Status.RUNNING
+    # the concrete implementation of the logic when the command is completed    
+    def tidy_up(self):
+        super().tidy_up()
+        self.scan_program.done_current()
+        self.arm_commander.clear_path_constraints()

demos/pickndrop_estop/task_trees_manager_pnd.py

@@ -18,14 +18,14 @@
 
 # robot control module
 from arm_commander.commander_moveit import GeneralCommander, logger
-import tools.pose_tools as pose_tools
+import task_trees.tools.pose_tools as pose_tools
 
 from task_trees.behaviours_base import SimAttachObject, SimDetachObject
 from task_trees.behaviours_move import DoMoveNamedPose, DoMoveXYZ
 from task_trees.task_trees_manager import GuardedTaskTreesManager, BasicTask
 from task_trees.task_scene import Scene
 
-from demos.pickndrop.behaviours_pnd import DoScanProgram
+from behaviours_pnd import DoScanProgram
 from scan_model import SingleLineScanModel, SteppingScanModel, FourCornersScanModel
 
 # -------------------------------------
@@ -54,18 +54,20 @@ class PickUpObjectTask(BasicTask):
     def __init__(self, xyz_world:list): 
         super(PickUpObjectTask, self).__init__()
         self.xyz = xyz_world
-        
+    def get_xyz(self):
+        return self.xyz
 
 class DropObjectTask(BasicTask):
     def __init__(self): 
         super(DropObjectTask, self).__init__()
-        
 
 class MovePoseTask(BasicTask):
     def __init__(self, xyz_world:list): 
         super(MovePoseTask, self).__init__()
         self.xyz = xyz_world        
-
+    def get_xyz(self):
+        return self.xyz
+    
 # ----------------------------------------
 # The TaskManager specialized for this application
 class PNDTaskTreesManager(GuardedTaskTreesManager):
@@ -157,7 +159,7 @@ def query_logical_pose_of_task(self):
         :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()
+            return self.the_blackboard.task.get_param()
         raise TypeError(f'unable to query logical pose due to no task has been submitted')
 
     # return the target position of the accepted task, or raise TypeError if no task is submitted

demos/pushblock/task_trees_manager_pushblock.py

@@ -25,7 +25,7 @@
 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
+import task_trees.tools.pose_tools as pose_tools
 # -------------------------------------
 # Tasks specialized for the Pick-n-Drop 
 
@@ -140,7 +140,7 @@ def query_logical_pose_of_task(self):
         :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()
+            return self.the_blackboard.task.get_param()
         raise TypeError(f'unable to query logical pose due to no task has been submitted')
 
     def query_current_reference_frame(self):
@@ -155,15 +155,15 @@ def query_target_reference_frame(self):
         :rtype: str
         """
         if self.the_blackboard.exists('task'):
-            target = self.the_blackboard.task.get_goal_as_logical_pose()
+            target = self.the_blackboard.task.get_param()
             return target
         raise TypeError(f'unable to query the target due to no task has been submitted')                
 
     # return the ideal rotation of the end-effector for pushing to the target, which is derived from the target and the structure of the end-effector
     # defined in the config file
     def query_rotation_from_target(self):
         if self.the_blackboard.exists('task'):
-            target = self.the_blackboard.task.get_goal_as_logical_pose()
+            target = self.the_blackboard.task.get_param()
             rotation = self.the_scene.query_config(target + ".rotation")
             return rotation
         raise TypeError(f'unable to query the rotation due to no task has been submitted or the target area is wrong')