diff --git a/CMakeLists.txt b/CMakeLists.txt index e377b2a..ebaf2fa 100644 --- a/CMakeLists.txt +++ b/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} ) diff --git a/demos/gridscan/behaviours_advanced.py b/demos/gridscan/behaviours_advanced.py index 9cdd3bd..0bee935 100644 --- a/demos/gridscan/behaviours_advanced.py +++ b/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 diff --git a/demos/gridscan/behaviours_gridscan.py b/demos/gridscan/behaviours_gridscan.py index 3bc989f..9471867 100644 --- a/demos/gridscan/behaviours_gridscan.py +++ b/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 @@ -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 diff --git a/demos/gridscan/task_scene.yaml b/demos/gridscan/task_scene.yaml index 7440177..508f6f3 100644 --- a/demos/gridscan/task_scene.yaml +++ b/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] + + diff --git a/demos/gridscan/task_scene_gridscan.py b/demos/gridscan/task_scene_gridscan.py index f53547c..3c966ad 100644 --- a/demos/gridscan/task_scene_gridscan.py +++ b/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'] diff --git a/demos/gridscan/task_trees_manager_gridscan.py b/demos/gridscan/task_trees_manager_gridscan.py index ee52753..8729480 100644 --- a/demos/gridscan/task_trees_manager_gridscan.py +++ b/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 @@ -188,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 diff --git a/demos/pickndrop/behaviours_pnd.py b/demos/pickndrop/behaviours_pnd.py index 4daf101..cb39d5e 100644 --- a/demos/pickndrop/behaviours_pnd.py +++ b/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 # ---------------------------------------------------------------------- diff --git a/demos/pickndrop/demo.py b/demos/pickndrop/demo.py index 216224a..46597a4 100644 --- a/demos/pickndrop/demo.py +++ b/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): diff --git a/demos/pickndrop/task_scene.yaml b/demos/pickndrop/task_scene.yaml index 326d0c9..7a0ee29 100644 --- a/demos/pickndrop/task_scene.yaml +++ b/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] \ No newline at end of file diff --git a/demos/pickndrop/task_trees_manager_pnd.py b/demos/pickndrop/task_trees_manager_pnd.py index 63e5145..50f05db 100644 --- a/demos/pickndrop/task_trees_manager_pnd.py +++ b/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 @@ -117,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] @@ -161,7 +167,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 diff --git a/demos/pickndrop_estop/behaviours_pnd.py b/demos/pickndrop_estop/behaviours_pnd.py new file mode 100644 index 0000000..cb39d5e --- /dev/null +++ b/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() diff --git a/demos/pickndrop_estop/task_scene.yaml b/demos/pickndrop_estop/task_scene.yaml index 995573f..7a0ee29 100644 --- a/demos/pickndrop_estop/task_scene.yaml +++ b/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] \ No newline at end of file diff --git a/demos/pickndrop_estop/task_trees_manager_pnd.py b/demos/pickndrop_estop/task_trees_manager_pnd.py index f717280..f711636 100644 --- a/demos/pickndrop_estop/task_trees_manager_pnd.py +++ b/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): @@ -113,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] @@ -157,7 +161,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 diff --git a/demos/pushblock/task_scene.yaml b/demos/pushblock/task_scene.yaml index e1ba483..51f4c99 100644 --- a/demos/pushblock/task_scene.yaml +++ b/demos/pushblock/task_scene.yaml @@ -1,76 +1,89 @@ --- -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.75, 0.30, 0.5] # min_x, min_y, max_x, max_y - rotations: - # alpha: [3.14, null, 0.78] - # beta: [3.14, null, -0.78] - alpha: [3.14, 0, -0.785] - beta: [3.14, 0, 2.358] - push_block: - dimensions: [0.05, 0.05, 0.05] - xyz: [0.0, 0.025, 0.151] - rpy: [0, 0, 0] - objects: - block_1: +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.75, 0.30, 0.5] # min_x, min_y, max_x, max_y + rotations: + # alpha: [3.14, null, 0.78] + # beta: [3.14, null, -0.78] + alpha: [3.14, 0, -0.785] + beta: [3.14, 0, 2.358] + push_block: + dimensions: [0.05, 0.05, 0.05] + xyz: [0.0, 0.025, 0.151] + rpy: [0, 0, 0] + + block_1: + link: type: box model_file: null dimensions: [0.10, 0.10, 0.35] xyz: [0.4, -0.10, 0.175] rpy: [0, 0, 0] - block_2: + block_2: + link: type: box model_file: null dimensions: [0.10, 0.10, 0.35] xyz: [0.6, -0.10, 0.175] rpy: [0, 0, 0] - block_3: + block_3: + link: type: box model_file: null dimensions: [0.10, 0.10, 0.35] xyz: [0.6, 0.10, 0.175] rpy: [0, 0, 0] - block_4: + block_4: + link: type: box model_file: null dimensions: [0.10, 0.10, 0.35] xyz: [0.4, 0.10, 0.175] rpy: [0, 0, 0] - area_1: + area_1: + rotation: rotations.alpha + link: type: box model_file: null dimensions: [0.1, 0.1, 0.25] xyz: [0.4, 0.0, 0.125] rpy: [0, 0, 1.57] - area_2: + area_2: + rotation: rotations.beta + link: type: box model_file: null dimensions: [0.1, 0.1, 0.25] xyz: [0.50, -0.10, 0.125] rpy: [0, 0, 3.14] - area_3: + area_3: + rotation: rotations.beta + link: type: box model_file: null dimensions: [0.1, 0.1, 0.25] xyz: [0.6, 0.0, 0.125] rpy: [0, 0, -1.57] - area_4: + area_4: + rotation: rotations.alpha + link: type: box model_file: null dimensions: [0.1, 0.1, 0.25] xyz: [0.50, 0.10, 0.125] rpy: [0, 0, 0] - area_5: + area_5: + link: type: box model_file: null dimensions: [0.1, 0.1, 0.25] xyz: [0.50, 0.0, 0.125] rpy: [0, 0, 0] -subscenes: area: positions: hover: [null, null, 0.40] @@ -79,12 +92,5 @@ subscenes: centre: [0, -0.015, null] # -0.01 restart: [0, -0.185, null] end: [0, -0.06, null] - area_1: - rotation: rotations.alpha - area_2: - rotation: rotations.beta - area_3: - rotation: rotations.beta - area_4: - rotation: rotations.alpha + diff --git a/demos/pushblock/task_trees_manager_pushblock.py b/demos/pushblock/task_trees_manager_pushblock.py index 77ee515..9fa6fc8 100644 --- a/demos/pushblock/task_trees_manager_pushblock.py +++ b/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 @@ -83,8 +83,8 @@ def __init__(self, arm_commander:GeneralCommander, spin_period_ms:int=10): # setup visualization in rviz self.scene_to_rviz = SceneToRViz(self.the_scene, arm_commander.get_world_reference_frame(), False) self.scene_to_rviz.display_bbox_regions('regions', rgba=[1.0, 0.0, 1.0, 0.2]) - object_area1 = self.the_scene.get_object_config('area_1') - self.scene_to_rviz.add_custom_transform('area', object_area1.xyz, object_area1.rpy, arm_commander.get_world_reference_frame()) + link_area1 = self.the_scene.get_link_of_scene('area_1') + self.scene_to_rviz.add_custom_transform('area', link_area1.xyz, link_area1.rpy, arm_commander.get_world_reference_frame()) self.scene_to_rviz.display_positions('area.positions', rgba=[1.0, 1.0, 0.0, 0.8]) # setup the robotic manipulation platform through the commander self.arm_commander.abort_move(wait=True) @@ -112,7 +112,7 @@ def setup_objects(self): # 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) - self._define_objects(self.the_scene, 'box') + self._define_links(self.the_scene, 'box') def generate_the_object(self): self.the_object = dict() @@ -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,7 +155,7 @@ 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') @@ -163,7 +163,7 @@ def query_target_reference_frame(self): # 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') @@ -197,19 +197,19 @@ def in_a_region(self, logical_region) -> bool: return pose_tools.in_region((current_pose.pose.position.x, current_pose.pose.position.y,), self.the_scene.query_config(logical_region)) def over_an_object(self, object_name) -> bool: - the_object = self.the_scene.get_object_config(object_name) - the_object_position_as_bbox = [the_object.xyz[0] - the_object.dimensions[0] / 2, the_object.xyz[1] - the_object.dimensions[1] / 2, - the_object.xyz[0] + the_object.dimensions[0] / 2, the_object.xyz[1] + the_object.dimensions[1] / 2] + the_link = self.the_scene.get_link_of_scene(object_name) + the_object_position_as_bbox = [the_link.xyz[0] - the_link.dimensions[0] / 2, the_link.xyz[1] - the_link.dimensions[1] / 2, + the_link.xyz[0] + the_link.dimensions[0] / 2, the_link.xyz[1] + the_link.dimensions[1] / 2] current_pose = self.arm_commander.pose_of_robot_link() return pose_tools.in_region((current_pose.pose.position.x, current_pose.pose.position.y,), the_object_position_as_bbox) def on_or_above_z(self, position) -> bool: current_pose = self.arm_commander.pose_of_robot_link() - return current_pose.pose.position.z > self.the_scene.query_position_as_xyz(position)[2] - 0.05 + return current_pose.pose.position.z > self.the_scene.query_config(position)[2] - 0.05 def below_z(self, position) -> bool: current_pose = self.arm_commander.pose_of_robot_link() - return current_pose.pose.position.z < self.the_scene.query_position_as_xyz(position)[2] - 0.05 + return current_pose.pose.position.z < self.the_scene.query_config(position)[2] - 0.05 def wrong_restart_position(self) -> bool: target = self.query_target_reference_frame() diff --git a/demos/pushblock_ros/ros_demo.py b/demos/pushblock_ros/ros_demo.py index 76e2f57..683d1c2 100644 --- a/demos/pushblock_ros/ros_demo.py +++ b/demos/pushblock_ros/ros_demo.py @@ -15,7 +15,7 @@ import rospy, actionlib from actionlib_msgs.msg import GoalStatus from task_trees.msg import TaskAction, TaskGoal, TaskFeedback, TaskResult -from tools.logging_tools import logger +from task_trees.tools.logging_tools import logger # -- Test cases specialized for the Push Block Demo class PushBlockROSClientDemo(): diff --git a/demos/pushblock_ros/ros_server.py b/demos/pushblock_ros/ros_server.py index f7606f4..79e72ae 100644 --- a/demos/pushblock_ros/ros_server.py +++ b/demos/pushblock_ros/ros_server.py @@ -17,7 +17,7 @@ from arm_commander.commander_moveit import GeneralCommander, logger from task_trees.msg import TaskAction, TaskGoal, TaskResult from task_trees.states import COMPLETION_STATES -from demos.pushblock.task_trees_manager_pushblock import * +from task_trees.demos.pushblock.task_trees_manager_pushblock import * class PushBlockROSServer(): """ The server side of the application program for the PushBlock Demo that implements the behaviour tree diff --git a/demos/pushblock_ros/task_scene.yaml b/demos/pushblock_ros/task_scene.yaml index 22e6492..51f4c99 100644 --- a/demos/pushblock_ros/task_scene.yaml +++ b/demos/pushblock_ros/task_scene.yaml @@ -1,78 +1,89 @@ -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.75, 0.30, 0.5] # min_x, min_y, max_x, max_y - positions: - drop: [0.5, 0.3, 0.4] - rotations: - # alpha: [3.14, null, 0.78] - # beta: [3.14, null, -0.78] - alpha: [3.14, 0, -0.785] - beta: [3.14, 0, 2.358] - push_block: - dimensions: [0.05, 0.05, 0.05] - xyz: [0.0, 0.025, 0.151] - rpy: [0, 0, 0] - objects: - block_1: +--- +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.75, 0.30, 0.5] # min_x, min_y, max_x, max_y + rotations: + # alpha: [3.14, null, 0.78] + # beta: [3.14, null, -0.78] + alpha: [3.14, 0, -0.785] + beta: [3.14, 0, 2.358] + push_block: + dimensions: [0.05, 0.05, 0.05] + xyz: [0.0, 0.025, 0.151] + rpy: [0, 0, 0] + + block_1: + link: type: box model_file: null dimensions: [0.10, 0.10, 0.35] xyz: [0.4, -0.10, 0.175] rpy: [0, 0, 0] - block_2: + block_2: + link: type: box model_file: null dimensions: [0.10, 0.10, 0.35] xyz: [0.6, -0.10, 0.175] rpy: [0, 0, 0] - block_3: + block_3: + link: type: box model_file: null dimensions: [0.10, 0.10, 0.35] xyz: [0.6, 0.10, 0.175] rpy: [0, 0, 0] - block_4: + block_4: + link: type: box model_file: null dimensions: [0.10, 0.10, 0.35] xyz: [0.4, 0.10, 0.175] rpy: [0, 0, 0] - area_1: + area_1: + rotation: rotations.alpha + link: type: box model_file: null dimensions: [0.1, 0.1, 0.25] xyz: [0.4, 0.0, 0.125] rpy: [0, 0, 1.57] - area_2: + area_2: + rotation: rotations.beta + link: type: box model_file: null dimensions: [0.1, 0.1, 0.25] xyz: [0.50, -0.10, 0.125] rpy: [0, 0, 3.14] - area_3: + area_3: + rotation: rotations.beta + link: type: box model_file: null dimensions: [0.1, 0.1, 0.25] xyz: [0.6, 0.0, 0.125] rpy: [0, 0, -1.57] - area_4: + area_4: + rotation: rotations.alpha + link: type: box model_file: null dimensions: [0.1, 0.1, 0.25] xyz: [0.50, 0.10, 0.125] rpy: [0, 0, 0] - area_5: + area_5: + link: type: box model_file: null dimensions: [0.1, 0.1, 0.25] xyz: [0.50, 0.0, 0.125] - rpy: [0, 0, 0] - -subscenes: + rpy: [0, 0, 0] area: positions: hover: [null, null, 0.40] @@ -81,12 +92,5 @@ subscenes: centre: [0, -0.015, null] # -0.01 restart: [0, -0.185, null] end: [0, -0.06, null] - area_1: - rotation: rotations.alpha - area_2: - rotation: rotations.beta - area_3: - rotation: rotations.beta - area_4: - rotation: rotations.alpha + diff --git a/demos/pytrees_moves_scene/move_scene_1.py b/demos/pytrees_moves_scene/move_scene_1.py index ed77d55..d7550ca 100644 --- a/demos/pytrees_moves_scene/move_scene_1.py +++ b/demos/pytrees_moves_scene/move_scene_1.py @@ -40,7 +40,7 @@ def __init__(self, arm_commander:GeneralCommander, spin_period_ms:int=10): self.arm_commander.wait_for_ready_to_move() self.the_scene = Scene(os.path.join(os.path.dirname(__file__), 'task_scene.yaml')) # setup name poses - self.named_poses = self.the_scene.keys_of_config('named_poses') + self.named_poses = self.the_scene.key_list_under_config_key('named_poses') for pose_name in self.named_poses: pose_name = 'named_poses.' + pose_name self.arm_commander.add_named_pose(pose_name, self.the_scene.query_config(pose_name)) diff --git a/demos/pytrees_moves_scene/move_scene_4.py b/demos/pytrees_moves_scene/move_scene_4.py index 5a88cf7..92abd92 100644 --- a/demos/pytrees_moves_scene/move_scene_4.py +++ b/demos/pytrees_moves_scene/move_scene_4.py @@ -44,16 +44,18 @@ def __init__(self, arm_commander:GeneralCommander, spin_period_ms:int=10): # load scene config self.the_scene = Scene(os.path.join(os.path.dirname(__file__), 'task_scene_4.yaml')) # setup name poses - self.named_poses = self.the_scene.keys_of_config('named_poses') + self.named_poses = self.the_scene.key_list_under_config_key('named_poses') for pose_name in self.named_poses: pose_name = 'named_poses.' + pose_name self.arm_commander.add_named_pose(pose_name, self.the_scene.query_config(pose_name)) # 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_of_scene = self.the_scene.get_link_of_scene(scene_name) + if the_link_of_scene is None: + continue + if the_link_of_scene.type == 'box': + self.arm_commander.add_box_to_scene(scene_name, the_link_of_scene.dimensions, the_link_of_scene.xyz, the_link_of_scene.rpy) # build the behaviour tree self.move_branch = self.create_move_branch() diff --git a/demos/pytrees_moves_scene/move_scene_5.py b/demos/pytrees_moves_scene/move_scene_5.py index ae82f44..e24edb6 100644 --- a/demos/pytrees_moves_scene/move_scene_5.py +++ b/demos/pytrees_moves_scene/move_scene_5.py @@ -43,16 +43,18 @@ def __init__(self, arm_commander:GeneralCommander, spin_period_ms:int=10): # load scene config self.the_scene = Scene(os.path.join(os.path.dirname(__file__), 'task_scene_4.yaml')) # setup name poses - self.named_poses = self.the_scene.keys_of_config('named_poses') + self.named_poses = self.the_scene.key_list_under_config_key('named_poses') for pose_name in self.named_poses: pose_name = 'named_poses.' + pose_name self.arm_commander.add_named_pose(pose_name, self.the_scene.query_config(pose_name)) # 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_of_scene = self.the_scene.get_link_of_scene(scene_name) + if the_link_of_scene is None: + continue + if the_link_of_scene.type == 'box': + self.arm_commander.add_box_to_scene(scene_name, the_link_of_scene.dimensions, the_link_of_scene.xyz, the_link_of_scene.rpy) # build the behaviour tree self.move_branch = self.create_move_branch() diff --git a/demos/pytrees_moves_scene/task_scene.yaml b/demos/pytrees_moves_scene/task_scene.yaml index 4ee0105..63b15e3 100644 --- a/demos/pytrees_moves_scene/task_scene.yaml +++ b/demos/pytrees_moves_scene/task_scene.yaml @@ -1,9 +1,9 @@ --- -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 +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 -subscenes: diff --git a/demos/pytrees_moves_scene/task_scene_2.yaml b/demos/pytrees_moves_scene/task_scene_2.yaml index ef15b3d..73e4cbe 100644 --- a/demos/pytrees_moves_scene/task_scene_2.yaml +++ b/demos/pytrees_moves_scene/task_scene_2.yaml @@ -1,11 +1,12 @@ --- -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: - start: [0.4, 0.0, 0.2] - end: [0.4, 0.0, 0.6] -subscenes: +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 + positions: + start: [0.4, 0.0, 0.2] + end: [0.4, 0.0, 0.6] + diff --git a/demos/pytrees_moves_scene/task_scene_3.yaml b/demos/pytrees_moves_scene/task_scene_3.yaml index 580e780..8f4e516 100644 --- a/demos/pytrees_moves_scene/task_scene_3.yaml +++ b/demos/pytrees_moves_scene/task_scene_3.yaml @@ -1,13 +1,14 @@ --- -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_x: [0.3, null, null] - init: [0.307, 0.0, 0.588] - rotations: - alpha: [3.139, 0.0, -0.785] -subscenes: +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 + positions: + default_x: [0.3, null, null] + init: [0.307, 0.0, 0.588] + rotations: + alpha: [3.139, 0.0, -0.785] + diff --git a/demos/pytrees_moves_scene/task_scene_4.yaml b/demos/pytrees_moves_scene/task_scene_4.yaml index 1088ead..96fd663 100644 --- a/demos/pytrees_moves_scene/task_scene_4.yaml +++ b/demos/pytrees_moves_scene/task_scene_4.yaml @@ -1,22 +1,24 @@ --- -scene: - named_poses: - 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: - area_1: +scenes: + root: + named_poses: + home: [0.0, -0.785, 0.0, -2.36, 0.0, 1.57, 0.785] # from base + positions: + default_z: [null, null, 0.5] + area_1: + link: + parent_frame: root type: box model_file: null dimensions: [0.2, 0.2, 0.01] xyz: [0.4, 0.20, 0.00] rpy: [0, 0, 3.14] - area_2: + area_2: + link: + parent_frame: root type: box model_file: null dimensions: [0.2, 0.2, 0.01] xyz: [0.4, -0.20, 0.00] rpy: [0, 0, 3.14] -subscenes: - diff --git a/demos/rviz_display/task_scene.yaml b/demos/rviz_display/task_scene.yaml index 0786b4b..062592e 100644 --- a/demos/rviz_display/task_scene.yaml +++ b/demos/rviz_display/task_scene.yaml @@ -1,27 +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 - positions: - default_z: [null, null, 0.5] - objects: - teapot: +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 + positions: + default_z: [null, null, 0.5] + + teapot: + link: 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: + area: + link: + type: frame 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] - + diff --git a/demos/rviz_display/task_trees_manager_rviz.py b/demos/rviz_display/task_trees_manager_rviz.py index fb94d0a..f451009 100644 --- a/demos/rviz_display/task_trees_manager_rviz.py +++ b/demos/rviz_display/task_trees_manager_rviz.py @@ -17,15 +17,11 @@ # 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 @@ -44,7 +40,7 @@ def __init__(self, named_pose:str): # ---------------------------------------- # The TaskManager specialized for this application class RVizTaskTreesManager(TaskTreesManager): - """ This is a subclass specialized for the pick-n-drop application + """ This is a subclass specialized for the rviz demo """ def __init__(self, arm_commander:GeneralCommander, spin_period_ms:int=10): """ the constructor @@ -59,14 +55,14 @@ def __init__(self, arm_commander:GeneralCommander, spin_period_ms:int=10): 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') + self.scene_to_rviz.display_link('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') + teapot_object = self.the_scene.get_link_of_scene('teapot') + self.arm_commander.add_object_to_scene('teapot_another', teapot_object.model_file, teapot_object.dimensions, + teapot_object.xyz, teapot_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) @@ -90,7 +86,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') # ------------------------------------------------- diff --git a/demos/simple_moves_scene/move_scene_4.py b/demos/simple_moves_scene/move_scene_4.py index 4467fa5..bb2f69d 100644 --- a/demos/simple_moves_scene/move_scene_4.py +++ b/demos/simple_moves_scene/move_scene_4.py @@ -43,7 +43,7 @@ def __init__(self, arm_commander:GeneralCommander, spin_period_ms:int=10): # setup name poses self._define_named_poses(self.the_scene) # setup objects - self._define_objects(self.the_scene) + self._define_links(self.the_scene) # build and install the behavior tree self._set_initialize_branch(self.create_init_branch()) diff --git a/demos/simple_moves_scene/move_scene_5.py b/demos/simple_moves_scene/move_scene_5.py index 2da9775..4a1e802 100644 --- a/demos/simple_moves_scene/move_scene_5.py +++ b/demos/simple_moves_scene/move_scene_5.py @@ -43,7 +43,7 @@ def __init__(self, arm_commander:GeneralCommander, spin_period_ms:int=10): # setup name poses self._define_named_poses(self.the_scene) # setup objects - self._define_objects(self.the_scene) + self._define_links(self.the_scene) # build and install the behavior tree self._set_initialize_branch(self.create_init_branch()) self._add_priority_branch(self.create_move_branch()) diff --git a/demos/simple_moves_scene/task_scene.yaml b/demos/simple_moves_scene/task_scene.yaml index 4ee0105..cee2223 100644 --- a/demos/simple_moves_scene/task_scene.yaml +++ b/demos/simple_moves_scene/task_scene.yaml @@ -1,9 +1,9 @@ --- -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 - -subscenes: +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 + diff --git a/demos/simple_moves_scene/task_scene_2.yaml b/demos/simple_moves_scene/task_scene_2.yaml index c3df9d6..9d7a9a7 100644 --- a/demos/simple_moves_scene/task_scene_2.yaml +++ b/demos/simple_moves_scene/task_scene_2.yaml @@ -1,11 +1,12 @@ --- -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: - start: [0.3, 0.0, 0.2] - end: [0.3, 0.0, 0.6] -subscenes: +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 + positions: + start: [0.3, 0.0, 0.2] + end: [0.3, 0.0, 0.6] + diff --git a/demos/simple_moves_scene/task_scene_3.yaml b/demos/simple_moves_scene/task_scene_3.yaml index 580e780..8f4e516 100644 --- a/demos/simple_moves_scene/task_scene_3.yaml +++ b/demos/simple_moves_scene/task_scene_3.yaml @@ -1,13 +1,14 @@ --- -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_x: [0.3, null, null] - init: [0.307, 0.0, 0.588] - rotations: - alpha: [3.139, 0.0, -0.785] -subscenes: +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 + positions: + default_x: [0.3, null, null] + init: [0.307, 0.0, 0.588] + rotations: + alpha: [3.139, 0.0, -0.785] + diff --git a/demos/simple_moves_scene/task_scene_4.yaml b/demos/simple_moves_scene/task_scene_4.yaml index 1088ead..96fd663 100644 --- a/demos/simple_moves_scene/task_scene_4.yaml +++ b/demos/simple_moves_scene/task_scene_4.yaml @@ -1,22 +1,24 @@ --- -scene: - named_poses: - 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: - area_1: +scenes: + root: + named_poses: + home: [0.0, -0.785, 0.0, -2.36, 0.0, 1.57, 0.785] # from base + positions: + default_z: [null, null, 0.5] + area_1: + link: + parent_frame: root type: box model_file: null dimensions: [0.2, 0.2, 0.01] xyz: [0.4, 0.20, 0.00] rpy: [0, 0, 3.14] - area_2: + area_2: + link: + parent_frame: root type: box model_file: null dimensions: [0.2, 0.2, 0.01] xyz: [0.4, -0.20, 0.00] rpy: [0, 0, 3.14] -subscenes: - diff --git a/demos/simple_moves_scene/task_scene_6.yaml b/demos/simple_moves_scene/task_scene_6.yaml index 01dfc4d..b851ede 100644 --- a/demos/simple_moves_scene/task_scene_6.yaml +++ b/demos/simple_moves_scene/task_scene_6.yaml @@ -1,13 +1,14 @@ --- -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: - start: [null, -0.3, null] - end: [null, 0.3, null] - near: [0.4, null, null] - far: [0.6, null, null] -subscenes: +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 + positions: + start: [null, -0.3, null] + end: [null, 0.3, null] + near: [0.4, null, null] + far: [0.6, null, null] + diff --git a/demos/task_moves/task_move_4_gui.py b/demos/task_moves/task_move_4_gui.py index 51438c8..8643742 100644 --- a/demos/task_moves/task_move_4_gui.py +++ b/demos/task_moves/task_move_4_gui.py @@ -15,7 +15,7 @@ from graphics import * import rospy from std_msgs.msg import Int8 -from tools.logging_tools import logger +from task_trees.tools.logging_tools import logger # Require the python Tkinter module # sudo apt install python3-tk diff --git a/docs/source/SCENE_SUPPORT.md b/docs/source/SCENE_SUPPORT.md index d8954fb..72c1c50 100644 --- a/docs/source/SCENE_SUPPORT.md +++ b/docs/source/SCENE_SUPPORT.md @@ -10,73 +10,72 @@ The key enabling component of the logical scene support is **scene configuration ### Basic Structure of a Scene Configuration File -A scene configuration file is divided into the root scene and custom subscene. The significance betwwen the root scene and the subscenes is the namespace of their configuration key-value pairs. All key-value defined under the root scene do have an empty namespace, while the variables under a subscene requires the subscene name as the namespace. The top level keys must be `scene` and `subscenes`, and under `subscenes` keys representing names of subscenes can be added. In the following example, `sub_scene_1` and `sub_scene_2` are names of custom subscenes. +A scene configuration file can be divided into one or more scenes. The first scene must be known as `root`. The scene is the top-level identifier for refering to all the resource keys in the configuration file. Each scene defines its own reference frame and optionally an visual object attached. A scene without a specified visual object is called a frame-type scene or simply known as a frame. + +The significance betwwen the root scene and the other scenes is the namespace of their configuration key-value pairs. All key-value defined under the root scene may be replaced with an empty namespace, while the variables under a subscene requires the subscene name as the namespace. + +The configuration file must starts with the key `scenes`, under which each scene can be added by the name as the head the scene branch. In the following example, `root` is the mandatory scene, and `scene_1` and `scene_2` are names of custom scenes. ```diff --- -scene: +scenes: + root: ... variables of the root scene go here - objects: - ... optional definitions of objects (which are also reference frames) - frames: - ... optional definitions of reference frames + named_poses: ... optional definitions of named poses -subscenes: - sub_scene_1: - ... variables of the subscene sub_scene_1 go here + scene_1: + link: + ... pose definitions of scene (which are also reference frames) + ... variables of the subscene scene_1 go here - sub_scene_2: - ... variables of the subscene sub_scene_2 go here + scene_2: + ... variables of the subscene scene_2 go here ``` -### Optional Branches Recognized by the Task Tree Manager - -The named branches of `objects`, `frames`, and `named_poses` have dedicated functions in the scene configuration. - -- 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. - -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 +### Specifying Configurations in the Root Scene and Other Scenes Consider the example `task_scene_4.yaml` [source code](../../demos/simple_moves_scene/task_scene_4.yaml) below. In this example, all configuration keys are under the root scene. No subscene is defined. ``` --- -scene: - named_poses: - 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: - area_1: +scenes: + root: + named_poses: + home: [0.0, -0.785, 0.0, -2.36, 0.0, 1.57, 0.785] # from base + positions: + default_z: [null, null, 0.5] + area_1: + link: + parent_frame: root type: box model_file: null dimensions: [0.2, 0.2, 0.01] xyz: [0.4, 0.20, 0.00] rpy: [0, 0, 3.14] - frame: null - area_2: + area_2: + link: + parent_frame: root type: box model_file: null dimensions: [0.2, 0.2, 0.01] xyz: [0.4, -0.20, 0.00] rpy: [0, 0, 3.14] - frame: area_1 -subscenes: ``` -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. +A scene is firstly a namespace for queries and key references. It should also be the reference frame to which the poses are defined. The scene name is both the name of a reference frame and, if defined, 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. +The scene configuration file below defines one scene 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 scene (referene frame) `area_1` are used together in using the move behaviours. ``` -subscenes: +scenes: area_1: + link: + parent_frame: root + type: box + model_file: null + dimensions: [0.2, 0.2, 0.01] + xyz: [0.4, 0.20, 0.00] + rpy: [0, 0, 3.14] positions: hover: [null, null, 0.40] down: [null, null, 0.27] @@ -86,7 +85,9 @@ subscenes: end: [0, -0.06, null] ``` -### Collision Objects and Custom Reference Frames +### The `link` Branch: Collision Objects and Custom Reference Frames + +The link of a scene specifies a reference frame and optionally the collision object. Collision object definitions comprise the following mandatory and optional keys headed by the name of the object. - `type`: `box`, `sphere`, or `object` @@ -94,11 +95,13 @@ Collision object definitions comprise the following mandatory and optional keys - `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 for the object pose, where `null` implies the world frame. This field is optional. +- `parent_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. +Every collision object will project its position and rotation as its own custom reference frames. The following defines the scene named `area_1` which is also the name of the reference frame. In particular, the type `box` indicaates that it is a collision object as well. Note that the null frame is default to the base/world frame. The pose parameter + (xyz and rpy) is the transform from `area_1` to the base/world frame. ``` - area_1: + area_1: + link: type: box model_file: null dimensions: [0.2, 0.2, 0.01] @@ -106,14 +109,15 @@ Every collision object will project its position and rotation as its own custom rpy: [0, 0, 3.14] frame: null ``` -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. +Collision objects can be defined based on the reference of another scene. If `parent_frame` is not given or its value is `null`, the scene 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. +- `type`: `frame` +- `xyz`: the position of the transform +- `rpy`: the orientation of the transform +- `parent_frame` or `frame`: the reference frame for the custom frame, 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. +Similarly, custom reference frames can be defined with another scene as the parent frame. The default frame is the root scene. ### Loading and Querying the Scene Configuration File @@ -126,13 +130,13 @@ from task_trees.task_scene import Scene ``` The `Scene` class gives every configuration key a unique reference. The reference is a dot-separated string made up of the scene namespace followed by keys of the ancestors of a key-value configuration. For example, the reference `named_poses.home` refers to the `home` child of the `named_poses` node under the root scene (therefore the empty namespace). A configuration key under a subscene has a prepended namespace. For example, `area.positions.start` refers to the value `[0, 0.11, null]` in the above example. -The following code snippet demonstrates the functions `keys_of_config` and `query_config` for listing the children keys and querying for the value of a configuration reference. +The following code snippet demonstrates the functions `key_list_under_config_key` and `query_config` for listing the children keys and querying for the value of a configuration reference. ``` self.the_scene = Scene(os.path.join(os.path.dirname(__file__), 'task_scene.yaml')) # setup name poses - self.named_poses = self.the_scene.keys_of_config('named_poses') + self.named_poses = self.the_scene.key_list_under_config_key('named_poses') for pose_name in self.named_poses: pose_name = 'named_poses.' + pose_name self.arm_commander.add_named_pose(pose_name, self.the_scene.query_config(pose_name)) @@ -147,6 +151,16 @@ In the following compositional move target example, the missing x and y componen DoMoveXYZ('move_xy', True, arm_commander=self.arm_commander, scene=self.the_scene, target_xyz=['positions.default_z', self.generate_random_xyz], reference_frame='area_2'), ``` +### Branches Recognized by the Task Tree Manager + +The named branches of `link`, and `named_poses` have dedicated functions in the scene configuration. + +- The key `link` under a scene with the `type` equals to `box`, `sphere`, and `object` 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 type of `frames` under the key `link` of a 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. + +The `TaskTreesManager` class offers the functions `_define_links`, `_define_frames` and `_define_named_poses` for adding the scenes/collision objects, the custom reference frames, and named poses defined in the scene configuration. + ### Internal Logical References @@ -154,30 +168,58 @@ Subclasses of `SceneConditionalCommanderBehaviour` accepts internal references w The following part of a configuration file is extracted from `task_scene.yaml` [source code](../../demos//pushblock/task_scene.yaml) under `/demos/pushblock`. ``` -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.75, 0.30, 0.5] # min_x, min_y, max_x, max_y - positions: - drop: [0.5, 0.3, 0.4] - rotations: - alpha: [3.14, 0, -0.785] - beta: [3.14, 0, 2.358] +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.75, 0.30, 0.5] # min_x, min_y, max_x, max_y + rotations: + # alpha: [3.14, null, 0.78] + # beta: [3.14, null, -0.78] + alpha: [3.14, 0, -0.785] + beta: [3.14, 0, 2.358] + push_block: + dimensions: [0.05, 0.05, 0.05] + xyz: [0.0, 0.025, 0.151] + rpy: [0, 0, 0] ... -subscenes: area_1: rotation: rotations.alpha + link: + type: box + model_file: null + dimensions: [0.1, 0.1, 0.25] + xyz: [0.4, 0.0, 0.125] + rpy: [0, 0, 1.57] area_2: rotation: rotations.beta + link: + type: box + model_file: null + dimensions: [0.1, 0.1, 0.25] + xyz: [0.50, -0.10, 0.125] + rpy: [0, 0, 3.14] area_3: rotation: rotations.beta + link: + type: box + model_file: null + dimensions: [0.1, 0.1, 0.25] + xyz: [0.6, 0.0, 0.125] + rpy: [0, 0, -1.57] area_4: rotation: rotations.alpha + link: + type: box + model_file: null + dimensions: [0.1, 0.1, 0.25] + xyz: [0.50, 0.10, 0.125] + rpy: [0, 0, 0] ``` -The key `rotation` of the four subscenes are mapped to either `rotation.alpha` or `rotation.beta`. For example, in resolving the key `area_1.rotation`, if the value is a string, `SceneConditionalCommanderBehaviour` attempts to resolve it once more using the scene configuration. +The key `rotation` of the four area scenes are mapped to either `rotation.alpha` or `rotation.beta`. For example, in resolving the key `area_1.rotation`, if the value is a string, `SceneConditionalCommanderBehaviour` attempts to resolve it once more using the scene configuration. The internal logical reference feature improves the readability and maintainability of the configuration file. @@ -188,15 +230,16 @@ The functions provided by the `Scene` class are listed below. | Functions | Remarks | | --- | --- | | `query_config(self, name:str, default=None)` | The `name` is in the format of dot separated reference | -| `query_position_as_xyz(self, name, default=None)` | Alias of `query_config` for backward compatability | -| `query_rotation_as_rpy(self, name, default=None)` | Alias of `query_config` for backward compatability | -| `get_scene_config(self)` | Return the root scene config in the config file as a dict | -| `get_subscene_config(self, name)`| Return the config of a subscene as a dict | | `exists_config(self, name:str)` | Return True if the configuration name exists | -| `keys_of_config(self, name:str)` | Return the children of a configuration reference as a list of keys | -| `len_of_list_config(self, name:str)` | Return the number of children of a list typed configuration reference | -| `list_object_names(self)` | Return the names of the defined objects as a list | -| `get_object_config(self, object_name: str)` | Return the configuration of an object given its name | +| `key_list_under_config_key(self, key:str)` | Return the children keys of a configuration reference as a list | +| `len_of_key_list_under_config_key(self, key:str)` | Return the number of children under a list-type configuration reference | +| `list_scene_names(self)` | Return the names of the defined scenes as a list | +| `get_scene_config_as_dict(self, scene_name)` | Return the scene config branch in the config file as a dict | +| `get_link_of_scene(self, scene_name)`| Return the link of the scene as a LinkConfig | +| `list_frame_names(self)` | Return the names of the defined frame-type scenes as a list | +| `get_frame_config_as_dict(self, frame_name)` | Return the frame-type scene config branch in the config file as a dict | +| `get_link_of_frame(self, frame_name)`| Return the link of the frame as a LinkConfig | +| `get_named_poses_of_root_as_dict(self, name)`| Return the config of a subscene as a dict | Refer to the API Reference for more details about these functions. @@ -212,22 +255,24 @@ The function `query_config` allows the query of any configuration key using a do For example, tke query key `positions.start` refers to a configuration key under the root scene. ``` -scene: - positions: - start: [1, 2, 3] - end: [2, 3, 5] +scenes: + root: + positions: + start: [1, 2, 3] + end: [2, 3, 5] ``` On the other hand, the query key `grid.1.rotation.alpha` matches the following structure, and it refers to the value [0, 0, 1.57]. The `1` refers to the index `1` of the list-like child under `grid`. ``` -scene: - grid: - - rotation: - alpha: [3.14, 0, 0] - - rotation: - alpha: [0, 0, 1.57] +scenes: + root: + grid: + - rotation: + alpha: [3.14, 0, 0] + - rotation: + alpha: [0, 0, 1.57] ``` -To refer to a configuration key under a subscene, the query key should start with the subscene name. Given the following configuration yaml file. +To refer to a configuration key under a non-root scene, the query key should start with the scene name. Given the following configuration yaml file. ``` subscenes: the_tank: @@ -242,7 +287,6 @@ The query key `the_tank.grid.0.rotation.alpha` refers to the value `[3.14, 0, 0] Application developers can design a suitable configuration structure and use the function `query_config` to rerieve configuration values. - ### Author Dr Andrew Lui, Senior Research Engineer
diff --git a/setup.py b/setup.py index fdacc5c..5253ea7 100644 --- a/setup.py +++ b/setup.py @@ -7,7 +7,7 @@ # fetch values from package.xml setup_args = generate_distutils_setup( - packages=['task_trees', 'demos', 'tools'], + packages=['task_trees'], package_dir={'': '.'}, python_requires='>=3.9', install_requires=req_packages diff --git a/task_trees/behaviours_base.py b/task_trees/behaviours_base.py index 683b47c..335d0ec 100644 --- a/task_trees/behaviours_base.py +++ b/task_trees/behaviours_base.py @@ -20,7 +20,8 @@ from arm_commander.commander_moveit import GeneralCommander, GeneralCommanderStates from task_trees.states import TaskStates from task_trees.task_scene import Scene -from tools.logging_tools import logger +from task_trees.tools.logging_tools import logger + # ----------------------------------------------------------------- # The base behaviours as an extension to PyTrees for working # with other components in a robot arm manipulation application @@ -234,9 +235,12 @@ def update(self): return Status.SUCCESS elif self.commander_state == GeneralCommanderStates.ABORTED: logger.error(f'ConditionalCommanderBehaviour handles task aborted') - if self.the_blackboard.exists('task'): - self.the_blackboard.set('task.state', TaskStates.ABORTED) - self.tidy_up() + # NOTE: the update enables the task trees to report pytree FAILURE but the task status can stay the same + # This allows the task status is WORKING and so other parts of the behaviour tree may deal with the arm commander abort signal + # such as retry or trying another branch + # if self.the_blackboard.exists('task'): + # self.the_blackboard.set('task.state', TaskStates.ABORTED) + # self.tidy_up() return Status.FAILURE elif self.commander_state == GeneralCommanderStates.ERROR: logger.error(f'ConditionalCommanderBehaviour handles task error') diff --git a/task_trees/behaviours_move.py b/task_trees/behaviours_move.py index 1be15b3..9a2890f 100644 --- a/task_trees/behaviours_move.py +++ b/task_trees/behaviours_move.py @@ -14,7 +14,7 @@ from geometry_msgs.msg import Pose, PoseStamped # robot control module from task_trees.behaviours_base import ConditionalCommanderBehaviour, SceneConditionalCommanderBehaviour, PrintPose -from tools.logging_tools import logger +from task_trees.tools.logging_tools import logger # ---------------------------------------------------------------------- # Move Behaviour Classes for robot arm manipulation applications @@ -201,8 +201,8 @@ def update_when_ready(self): xyz = self.compute_physical_target(self.target_xyz, None, default_target=current_xyz[:3]) rpy = self.compute_physical_target(self.target_rpy, None, default_target=current_xyz[3:]) else: - xyz = self.compute_physical_target(self.target_xyz, self.the_scene.query_position_as_xyz, default_target=current_xyz[:3]) - rpy = self.compute_physical_target(self.target_rpy, self.the_scene.query_rotation_as_rpy, default_target=current_xyz[3:]) + xyz = self.compute_physical_target(self.target_xyz, self.the_scene.query_config, default_target=current_xyz[:3]) + rpy = self.compute_physical_target(self.target_rpy, self.the_scene.query_config, default_target=current_xyz[3:]) if xyz is None: logger.error(f'DoMoveXYZRPY ({self.name}): invalid position xyz parameter {xyz}') return Status.FAILURE @@ -295,7 +295,7 @@ def update_when_ready(self): if self.the_scene is None: target_rpy = self.compute_physical_target(self.target_rpy) else: - target_rpy = self.compute_physical_target(self.target_rpy, self.the_scene.query_rotation_as_rpy) + target_rpy = self.compute_physical_target(self.target_rpy, self.the_scene.query_config) if self.target_rpy is None or target_rpy is None: logger.error(f'DoRotate ({self.name}): invalid target_rpy parameter {self.target_rpy}') diff --git a/task_trees/behaviours_move_sense.py b/task_trees/behaviours_move_sense.py index 8e69d62..73430cc 100644 --- a/task_trees/behaviours_move_sense.py +++ b/task_trees/behaviours_move_sense.py @@ -15,7 +15,7 @@ # robot control module from task_trees.behaviours_base import ConditionalBehaviour from task_trees.behaviours_move import DoMoveXYZ -from tools.logging_tools import logger +from task_trees.tools.logging_tools import logger # ----------------------------------------------------------------------------------------------------- # Advanced behaviours for illustrating the use of a ROS message topic to alert an imminent risk, such # as collision, and to abort the move behaviour diff --git a/task_trees/demos/pushblock/demo.py b/task_trees/demos/pushblock/demo.py new file mode 100644 index 0000000..32e3cce --- /dev/null +++ b/task_trees/demos/pushblock/demo.py @@ -0,0 +1,119 @@ +#!/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__ = 'ak.lui@qut.edu.au' +__status__ = 'Development' + +import random, sys, signal, time +import py_trees +# robot control module +from arm_commander.commander_moveit import GeneralCommander, logger +from task_trees.demos.pushblock.task_trees_manager_pushblock import * + +# -- Test cases specialized for the Push Block Demo +# -- NOTE: setup so that it can be installed as a rosrun demo program +class PushBlockDemoApplication(): + """ The application program for the PushBlock Demo + """ + def __init__(self): + signal.signal(signal.SIGINT, self.stop) + self.the_blackboard = py_trees.blackboard.Client() + self.the_blackboard.register_key(key='the_object', access=py_trees.common.Access.READ) + + self.arm_commander = GeneralCommander('panda_arm') + self.the_task_manager = PushBlockTaskTreesManager(self.arm_commander) + # self.the_task_manager.display_tree() + self._run_demo() + self.the_task_manager.spin() + + def stop(self, *args, **kwargs): + logger.info('stop signal received') + self.the_task_manager.shutdown() + sys.exit(0) + + def _run_test(self): + task_manager = self.the_task_manager + logger.info(f'=== Test 1') + logger.info(f'=== Submit a MoveNamedPoseTask (stow)') + task_manager.submit_task(the_task:=MoveNamedPoseTask('named_poses.stow')) + the_task.wait_for_completion() + logger.info(f'=== Submit a MoveNamedPoseTask (home)') + task_manager.submit_task(the_task:=MoveNamedPoseTask('named_poses.home')) + the_task.wait_for_completion() + logger.info(f'=== Test Finished') + + def _run_push_test(self): + task_manager = self.the_task_manager + the_task = None + logger.info(f'=== PushBlock Demo Started') + logger.info(f'=== Submit a MoveNamedPoseTask (home)') + task_manager.submit_task(the_task:=MoveNamedPoseTask('named_poses.home')) + the_task.wait_for_completion() + # generate the block for simulation + task_manager.generate_the_object() + # test push blocks to different areas + logger.info(f'=== Submit a PushBlockTask (2)') + task_manager.submit_task(the_task:=PushBlockTask('area_2')) + the_task.wait_for_completion() + logger.info(f'=== Submit a PushBlockTask (4)') + task_manager.submit_task(the_task:=PushBlockTask('area_4')) + the_task.wait_for_completion() + logger.info(f'=== Submit a PushBlockTask (3)') + task_manager.submit_task(the_task:=PushBlockTask('area_3')) + the_task.wait_for_completion() + logger.info(f'=== Submit a PushBlockTask (1)') + task_manager.submit_task(the_task:=PushBlockTask('area_1')) + the_task.wait_for_completion() + + def _run_demo(self): + task_manager = self.the_task_manager + the_task = None + logger.info(f'=== PushBlock Demo Started') + logger.info(f'=== Submit a MoveNamedPoseTask (home)') + task_manager.submit_task(the_task:=MoveNamedPoseTask('named_poses.home')) + the_task.wait_for_completion() + # generate the block for simulation + task_manager.setup_objects() + task_manager.generate_the_object() + # initialize variables + is_random = False + while True: + if not is_random: + while True: + target = input('Enter the area (1, 2, 3, 4), go home (H), or run random demo (R): ') + if target in ['1', '2', '3', '4', 'H', 'R']: + break + time.sleep(0.1) + if target == 'R': + is_random = True + if target == 'H': + task_manager.submit_task(the_task:=MoveNamedPoseTask('named_poses.home')) + the_task.wait_for_completion() + continue + if is_random: + target = random.randint(1, 4) + area = f'area_{target}' + if self.the_blackboard.the_object['area'] == area: + if not is_random: + logger.info(f'The area "{target}" is where the block is already located') + continue + logger.info(f'=== Submit a PushBlockTask ({area})') + task_manager.submit_task(the_task:=PushBlockTask(area)) + the_task.wait_for_completion() + +if __name__=='__main__': + # rospy.init_node('run_pnd_task_manager_demo', anonymous=False) + try: + PushBlockDemoApplication() + logger.info('pnd_task_manager_demo is running') + except Exception as e: + logger.exception(e) + + diff --git a/task_trees/demos/pushblock/docs/DemoPushBlock1.gif b/task_trees/demos/pushblock/docs/DemoPushBlock1.gif new file mode 100644 index 0000000..e90ed90 Binary files /dev/null and b/task_trees/demos/pushblock/docs/DemoPushBlock1.gif differ diff --git a/task_trees/demos/pushblock/docs/PushBlockDesign.png b/task_trees/demos/pushblock/docs/PushBlockDesign.png new file mode 100644 index 0000000..a1b94d9 Binary files /dev/null and b/task_trees/demos/pushblock/docs/PushBlockDesign.png differ diff --git a/task_trees/demos/pushblock/docs/PushBlockReferenceFrames.png b/task_trees/demos/pushblock/docs/PushBlockReferenceFrames.png new file mode 100644 index 0000000..03537d2 Binary files /dev/null and b/task_trees/demos/pushblock/docs/PushBlockReferenceFrames.png differ diff --git a/task_trees/demos/pushblock/task_scene.yaml b/task_trees/demos/pushblock/task_scene.yaml new file mode 100644 index 0000000..51f4c99 --- /dev/null +++ b/task_trees/demos/pushblock/task_scene.yaml @@ -0,0 +1,96 @@ +--- +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.75, 0.30, 0.5] # min_x, min_y, max_x, max_y + rotations: + # alpha: [3.14, null, 0.78] + # beta: [3.14, null, -0.78] + alpha: [3.14, 0, -0.785] + beta: [3.14, 0, 2.358] + push_block: + dimensions: [0.05, 0.05, 0.05] + xyz: [0.0, 0.025, 0.151] + rpy: [0, 0, 0] + + block_1: + link: + type: box + model_file: null + dimensions: [0.10, 0.10, 0.35] + xyz: [0.4, -0.10, 0.175] + rpy: [0, 0, 0] + block_2: + link: + type: box + model_file: null + dimensions: [0.10, 0.10, 0.35] + xyz: [0.6, -0.10, 0.175] + rpy: [0, 0, 0] + block_3: + link: + type: box + model_file: null + dimensions: [0.10, 0.10, 0.35] + xyz: [0.6, 0.10, 0.175] + rpy: [0, 0, 0] + block_4: + link: + type: box + model_file: null + dimensions: [0.10, 0.10, 0.35] + xyz: [0.4, 0.10, 0.175] + rpy: [0, 0, 0] + area_1: + rotation: rotations.alpha + link: + type: box + model_file: null + dimensions: [0.1, 0.1, 0.25] + xyz: [0.4, 0.0, 0.125] + rpy: [0, 0, 1.57] + area_2: + rotation: rotations.beta + link: + type: box + model_file: null + dimensions: [0.1, 0.1, 0.25] + xyz: [0.50, -0.10, 0.125] + rpy: [0, 0, 3.14] + area_3: + rotation: rotations.beta + link: + type: box + model_file: null + dimensions: [0.1, 0.1, 0.25] + xyz: [0.6, 0.0, 0.125] + rpy: [0, 0, -1.57] + area_4: + rotation: rotations.alpha + link: + type: box + model_file: null + dimensions: [0.1, 0.1, 0.25] + xyz: [0.50, 0.10, 0.125] + rpy: [0, 0, 0] + area_5: + link: + type: box + model_file: null + dimensions: [0.1, 0.1, 0.25] + xyz: [0.50, 0.0, 0.125] + rpy: [0, 0, 0] + area: + positions: + hover: [null, null, 0.40] + down: [null, null, 0.27] + start: [0, 0.11, null] + centre: [0, -0.015, null] # -0.01 + restart: [0, -0.185, null] + end: [0, -0.06, null] + + diff --git a/task_trees/demos/pushblock/task_trees_manager_pushblock.py b/task_trees/demos/pushblock/task_trees_manager_pushblock.py new file mode 100644 index 0000000..9fa6fc8 --- /dev/null +++ b/task_trees/demos/pushblock/task_trees_manager_pushblock.py @@ -0,0 +1,334 @@ +#!/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__ = 'ak.lui@qut.edu.au' +__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 task_trees.tools.pose_tools as pose_tools +# ------------------------------------- +# Tasks specialized for the Pick-n-Drop + +# 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) + +# A sequence of behaviours for moving the arm to one of the named pose +class PushBlockTask(BasicTask): + def __init__(self, area:str): + """ the constructor + :param area: one of the named pose defined in the configuration file + :type area: str + """ + super(PushBlockTask, self).__init__(area) + if not (area is not None or type(area) is not str or not area.startswith('area_')): + logger.error(f'{__class__.__name__}: parameter (area) is not a string starts with "area_" -> fix the value at behaviour construction') + raise AssertionError(f'A parameter is invalid') + +class MoveAreaTask(BasicTask): + def __init__(self, area:str): + """ the constructor + :param area: one of the named pose defined in the configuration file + :type area: str + """ + super(MoveAreaTask, self).__init__(area) + if not (area is not None or type(area) is not str or not area.startswith('area_')): + logger.error(f'{__class__.__name__}: parameter (area) is not a string starts with "area_" -> fix the value at behaviour construction') + raise AssertionError(f'A parameter is invalid') + +# ---------------------------------------- +# The TaskManager specialized for this application +class PushBlockTaskTreesManager(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(PushBlockTaskTreesManager, 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_bbox_regions('regions', rgba=[1.0, 0.0, 1.0, 0.2]) + link_area1 = self.the_scene.get_link_of_scene('area_1') + self.scene_to_rviz.add_custom_transform('area', link_area1.xyz, link_area1.rpy, arm_commander.get_world_reference_frame()) + self.scene_to_rviz.display_positions('area.positions', rgba=[1.0, 1.0, 0.0, 0.8]) + # setup the robotic manipulation platform through the commander + self.arm_commander.abort_move(wait=True) + self.arm_commander.reset_world() + # self.arm_commander.set_workspace_walls(*(self.the_scene.query_config('regions.workspace'))) + # setup name poses + self._define_named_poses(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) + self._add_task_branch(self.create_push_block_task_branch(), PushBlockTask) + + # install and unleash the behaviour tree + self._install_bt_and_spin(self.bt, spin_period_ms) + + # ----------------------------------------- + # 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, the_object.frame) + self._define_links(self.the_scene, 'box') + + def generate_the_object(self): + self.the_object = dict() + self.the_blackboard.the_object = self.the_object + the_block_config = self.the_scene.query_config('push_block') + self.the_object['area'] = 'area_1' + # transform the pose + xyzrpy = the_block_config['xyz'] + the_block_config['rpy'] + the_pose = pose_tools.list_to_pose_stamped(xyzrpy, self.the_object['area']) + the_pose = self.arm_commander.transform_pose(the_pose, self.arm_commander.WORLD_REFERENCE_LINK) + xyzrpy_in_world = pose_tools.pose_to_xyzrpy(the_pose.pose) + self.arm_commander.add_box_to_scene('the_object', the_block_config['dimensions'], xyzrpy_in_world[:3], xyzrpy_in_world[3:]) + + # ----------------------------------------- + # Functions for the behaviour tree + def post_fn_update_area(self): + target = self.query_target_reference_frame() + self.the_blackboard.the_object['area'] = target + + # 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_param() + raise TypeError(f'unable to query logical pose due to no task has been submitted') + + def query_current_reference_frame(self): + if self.the_blackboard.exists('the_object'): + return self.the_blackboard.the_object['area'] + raise TypeError(f'unable to query the reference frame where the object is located') + + def query_target_reference_frame(self): + """ return the target area of the task + :raises TypeError: no task has been submitted + :return: the reference frame representing the target area + :rtype: str + """ + if self.the_blackboard.exists('task'): + 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_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') + + # return the ideal rotation of the end-effector for pushing from the source, which is derived from the source and the structure of the end-effector + # defined in the config file + def query_rotation_from_current(self): + if self.the_blackboard.exists('the_object'): + current = self.the_blackboard.the_object['area'] + rotation = self.the_scene.query_config(current + ".rotation") + return rotation + raise TypeError(f'unable to query the rotation due to no task has been submitted or the target area is wrong') + + # internal function for obtaining the time lapse since the current task was submitted + def _get_time_since_submit(self) -> float: + if self.the_blackboard.exists('task'): + return self.the_blackboard.task.get_time_since_submit() + # return -1 so that the timeout condition is never True + return -1 + + # return orientation constraint to fix the rotation of the end_effector during movement in the tank + def _create_intank_fix_rotate_constraint(self): + return moveit_tools.create_path_orientation_constraint( + self.arm_commander.END_EFFECTOR_LINK, self.arm_commander.pose_in_frame(), 0.05, 0.05, 0.05) + + # -------------------------------------- + # -- internal functions: conditions functions for behaviours for the building of the behaviour tree for this TaskManager + + def in_a_region(self, logical_region) -> bool: + current_pose = self.arm_commander.pose_of_robot_link() + return pose_tools.in_region((current_pose.pose.position.x, current_pose.pose.position.y,), self.the_scene.query_config(logical_region)) + + def over_an_object(self, object_name) -> bool: + the_link = self.the_scene.get_link_of_scene(object_name) + the_object_position_as_bbox = [the_link.xyz[0] - the_link.dimensions[0] / 2, the_link.xyz[1] - the_link.dimensions[1] / 2, + the_link.xyz[0] + the_link.dimensions[0] / 2, the_link.xyz[1] + the_link.dimensions[1] / 2] + current_pose = self.arm_commander.pose_of_robot_link() + return pose_tools.in_region((current_pose.pose.position.x, current_pose.pose.position.y,), the_object_position_as_bbox) + + def on_or_above_z(self, position) -> bool: + current_pose = self.arm_commander.pose_of_robot_link() + return current_pose.pose.position.z > self.the_scene.query_config(position)[2] - 0.05 + + def below_z(self, position) -> bool: + current_pose = self.arm_commander.pose_of_robot_link() + return current_pose.pose.position.z < self.the_scene.query_config(position)[2] - 0.05 + + def wrong_restart_position(self) -> bool: + target = self.query_target_reference_frame() + start_position = self.the_scene.query_config('area.positions.restart') + current_pose_in_target_frame = self.arm_commander.pose_in_frame_as_xyzrpy(reference_frame=target) + distance = math.dist(start_position[:2], current_pose_in_target_frame[:2]) + # logger.info(f'wrong_restart_position: {target} {start_position} {current_pose_in_target_frame} {distance}') + return distance > 0.02 + + def the_object_in_area(self, area) -> bool: + if self.the_blackboard.exists('the_object'): + if 'area' in self.the_blackboard.the_object: + current = self.the_blackboard.the_object['area'] + return current == area + logger.warning(f'the_object_in_area: the blackboard key "the_object" is not defined or it has no "area" key -> should not happen') + return False + + def the_object_over_an_object(self, object_name) -> bool: + the_target_object = self.the_scene.get_object_config(object_name) + the_target_position_as_bbox = [the_target_object.xyz[0] - the_target_object.dimensions[0] / 2, the_target_object.xyz[1] - the_target_object.dimensions[1] / 2, + the_target_object.xyz[0] + the_target_object.dimensions[0] / 2, the_target_object.xyz[1] + the_target_object.dimensions[1] / 2] + xyzrpy_of_the_object = self.arm_commander.pose_in_frame_as_xyzrpy('the_object') + return pose_tools.in_region((xyzrpy_of_the_object[0], xyzrpy_of_the_object[1],), the_target_position_as_bbox) + + def at_a_named_pose(self, named_pose) -> bool: + joint_values = self.arm_commander.current_joint_positons_as_list() + result = pose_tools.same_joint_values_with_tolerence(self.the_scene.query_config(named_pose), joint_values, 0.05) + # logger.info(f'at {named_pose} pose: {result}') + return result + + # ----------------------------------------------- + # --- internal functions: eternal guard condition + def guard_work_branch_condition(self) -> bool: + if self.the_blackboard.exists('task'): + if self.the_blackboard.task.state == TaskStates.CANCELLED: + return False + return True + return False + + # ------------------------------------------------- + # --- 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', [{'_not_fn': self.in_a_region, 'logical_region': 'regions.inner'}], + 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_push_block_task_branch(self) -> Composite: + # - the branch that executes the task ScanTask + move_area_branch = py_trees.decorators.Timeout( + duration=1000000, + name='scan_branch_timeout', + child=py_trees.composites.Sequence( + 'scan_branch', + memory=True, + children=[ + DoMoveNamedPose('move_home_first_if_inner', [{'_fn': self.in_a_region, 'logical_region': 'regions.inner'}], + arm_commander=self.arm_commander, scene=self.the_scene, named_pose='named_poses.home'), + DoMoveXYZ('move_to_area_edge', [{'_not_fn': self.the_object_in_area, 'area':'area_5'}], arm_commander=self.arm_commander, scene=self.the_scene, + target_xyz=['area.positions.start', 'area.positions.hover'], + reference_frame=self.query_current_reference_frame), + DoRotate('align_with_the_alley', [{'_not_fn': self.the_object_in_area, 'area':'area_5'}], arm_commander=self.arm_commander, scene=self.the_scene, + target_rpy=self.query_rotation_from_current, reference_frame=self.query_current_reference_frame), + DoMoveXYZ('move_down', [{'_not_fn': self.the_object_in_area, 'area':'area_5'}], arm_commander=self.arm_commander, scene=self.the_scene, + target_xyz=['area.positions.down'], cartesian=True, + reference_frame=self.query_current_reference_frame), + SimAttachObject('engage_object', True, arm_commander=self.arm_commander, object_name='the_object'), + DoMoveXYZ('push_object_to_area_5', [{'_not_fn': self.the_object_in_area, 'area':'area_5'}], arm_commander=self.arm_commander, scene=self.the_scene, + target_xyz=['area.positions.centre'], cartesian=True, + reference_frame=self.query_current_reference_frame), + SimDetachObject('detach_at_centre', True, arm_commander=self.arm_commander, object_name='the_object', post_fn=lambda: 'area_5'), + DoMoveXYZ('move_up', {'_fn': self.wrong_restart_position}, arm_commander=self.arm_commander, scene=self.the_scene, + target_xyz=['area.positions.hover'], + reference_frame=self.query_current_reference_frame), + DoMoveXYZRPY('move_align_for_restart', {'_fn': self.wrong_restart_position}, arm_commander=self.arm_commander, scene=self.the_scene, + target_xyz=['area.positions.restart', 'area.positions.hover'], target_rpy=self.query_rotation_from_target, + reference_frame=self.query_target_reference_frame), + DoMoveXYZ('move_down', {'_fn': self.on_or_above_z, 'position':'area.positions.hover'}, arm_commander=self.arm_commander, scene=self.the_scene, + target_xyz=['area.positions.down'], cartesian=True, + reference_frame=self.query_target_reference_frame), + SimAttachObject('touch_object', True, arm_commander=self.arm_commander, object_name='the_object'), + DoMoveXYZ('push_object_to_target', True, arm_commander=self.arm_commander, scene=self.the_scene, + target_xyz=['area.positions.end'], cartesian=True, + constraint_fn = self._create_intank_fix_rotate_constraint, + reference_frame=self.query_target_reference_frame), + SimDetachObject('detach_object', True, arm_commander=self.arm_commander, object_name='the_object', post_fn=self.post_fn_update_area), + DoMoveXYZ('move_up', True, arm_commander=self.arm_commander, scene=self.the_scene, + target_xyz=['area.positions.hover'], + reference_frame=self.query_target_reference_frame), + ], + ), + ) + return move_area_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 + + + + diff --git a/task_trees/scene_to_rviz.py b/task_trees/scene_to_rviz.py index 6d2a07f..e6f892e 100644 --- a/task_trees/scene_to_rviz.py +++ b/task_trees/scene_to_rviz.py @@ -16,17 +16,17 @@ from geometry_msgs.msg import Pose, PoseStamped, TransformStamped, Transform, Vector3, Quaternion from visualization_msgs.msg import Marker -from task_trees.task_scene import Scene, ObjectConfig -import tools.rviz_tools as rviz_tools -from tools.logging_tools import logger -from tools.rviz_tools import RvizVisualizer -from tools.pose_tools import pose_to_xyzq, list_to_xyzq, pose_stamped_to_transform_stamped, list_to_pose_stamped -from tools.rospkg_tools import PackageFile +from task_trees.task_scene import Scene, LinkConfig +import task_trees.tools.rviz_tools as rviz_tools +from task_trees.tools.logging_tools import logger +from task_trees.tools.rviz_tools import RvizVisualizer +from task_trees.tools.pose_tools import pose_to_xyzq, list_to_xyzq, pose_stamped_to_transform_stamped, list_to_pose_stamped +from task_trees.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, pub_period:float=0.1): + def __init__(self, the_scene:Scene, base_frame:str=None, publish_scenes_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: @@ -38,18 +38,18 @@ def __init__(self, the_scene:Scene, base_frame:str=None, publish_objects_transfo super(SceneToRViz, self).__init__() self.the_scene = the_scene self.base_frame = base_frame - self.is_transform_publisher = publish_objects_transform + self.is_transform_publisher = publish_scenes_transform self.is_world_publisher = False - self.publishable_object_names = [] + self.publishable_scene_names = [] # setup transform self.tf_broadcaster = tf2_ros.TransformBroadcaster() if self.base_frame is None: self.base_frame = 'world' self.is_world_publisher = True - logger.warning(f'SceneToRViz: no base_frame given, use default base frame "world"') + logger.warning(f'SceneToRViz: no base_frame given, define "world" as the default base_frame') # setup custom transforms - self.custom_transform_object_dict = defaultdict(lambda: None) + self.custom_transform_link_dict = defaultdict(lambda: None) # setup timer self.timer_transform = rospy.Timer(rospy.Duration(pub_period), self._cb_timer_transform) @@ -64,78 +64,64 @@ def _cb_timer_transform(self, event): self.tf_broadcaster.sendTransform(transform_stamped) self.is_world_publisher = False if self.is_transform_publisher: - self._pub_transform_all_objects() + self._pub_transform_specified_links() self._pub_custom_transform() # internal function: publish the transform of objects in the scene config subject to the list of publishable objects - def _pub_transform_all_objects(self): - for object_name in the_scene.list_object_names(): - if self.publishable_object_names is None or object_name in self.publishable_object_names: - the_object = the_scene.get_object_config(object_name) - xyzrpy = the_object.xyz + the_object.rpy - self._pub_transform_object(object_name, xyzrpy, the_object.frame) + def _pub_transform_specified_links(self): + for scene_name in self.the_scene.list_scene_names(): + if self.publishable_scene_names is None or scene_name in self.publishable_scene_names: + link_config = self.the_scene.get_link_of_scene(scene_name) + xyzrpy = link_config.xyz + link_config.rpy + self._pub_transform_link(scene_name, xyzrpy, link_config.parent_frame) # internal function: publish the transform of custom objects def _pub_custom_transform(self): - for the_object in self.custom_transform_object_dict.values(): - xyzrpy = the_object.xyz + the_object.rpy - self._pub_transform_object(the_object.name, xyzrpy, the_object.frame) + link_config:LinkConfig + for link_config in self.custom_transform_link_dict.values(): + xyzrpy = link_config.xyz + link_config.rpy + self._pub_transform_link(link_config.name, xyzrpy, link_config.parent_frame) # internal function: publish the transform of a specific named object - def _pub_transform_object(self, name, pose, frame=None): + def _pub_transform_link(self, name, pose, parent_frame=None): """ publish the transform of an object - :param name: name of the object + :param name: name of the link :type name: str - :param pose: the pose of the object + :param pose: the pose of the link :type pose: Pose, PoseStamped, list of 6 or 7 - :param frame: the frame against which the pose is defined, ignored if PoseStamped is provided, defaults to None - :type frame: str, optional + :param parent_frame: the parent frame of the pose, ignored if PoseStamped is provided, defaults to the base_frame + :type parent_frame: str, optional """ - frame = self.base_frame if frame is None else frame + parent_frame = self.base_frame if parent_frame is None else parent_frame if type(pose) in [list, tuple]: - pose_stamped = list_to_pose_stamped(pose, frame) + pose_stamped = list_to_pose_stamped(pose, parent_frame) elif type(pose) == Pose: pose_stamped = PoseStamped() - pose_stamped.header.frame_id = frame + pose_stamped.header.frame_id = parent_frame pose_stamped.header.stamp = rospy.Time.now() pose_stamped.pose = pose elif type(pose) == PoseStamped: - frame = pose.header.frame_id + parent_frame = pose.header.frame_id pose_stamped = pose else: rospy.logerr(f'{__class__.__name__}: parameter (pose) is not list of length 6 or 7 or a Pose object -> fix the parameter at behaviour construction') raise TypeError(f'A parameter is invalid') self.tf_broadcaster.sendTransform(pose_stamped_to_transform_stamped(pose_stamped, name)) - # try: - # # frame is ignored if pose is PoseStamped - # if type(pose) == Pose: - # xyzq = pose_to_xyzq(pose) - # elif type(pose) == PoseStamped: - # frame = pose.header.frame_id - # xyzq = pose_to_xyzq(pose.pose) - # else: - # xyzq = list_to_xyzq(pose) - # except Exception: - # logger.error(f'{__class__.__name__}: parameter (pose) is not list of length 6 or 7 or a Pose object -> fix the parameter at behaviour construction') - # raise - # frame = self.base_frame if frame is None else frame - # self.tf_pub.sendTransform(xyzq[:3], xyzq[3:], rospy.Time.now(), name, frame) - # -------------------------------- - def set_publish_objects_transform(self, object_names:list=None): - """ specify the names of the objects that are to be included in transform publish + def set_publish_links_transform(self, link_names:list=None): + """ specify the names of the links that are to be included in transform publish - :param object_names: a list of object names to be included, defaults to None - :type object_names: list, optional + :param link_names: a list of links names to be included, defaults to None + :type link_names: list, optional """ - if object_names is not None and type(object_names) not in [tuple, list]: + if link_names is not None and type(link_names) not in [tuple, list]: logger.error(f'SceneToRViz (set_publish_transform): parameter (object_names) must be a list of strings or None') raise AssertionError('Parameter (object_names) type is invalid') - self.publishable_object_names = object_names + self.publishable_scene_names = link_names - def display_bbox_regions(self, config_key:str, plane:str='xy', rgba=[1.0, 0.0, 1.0, 0.2]) -> None: + def display_bbox_regions(self, config_key:str, plane:str='xy', reference_frame=None, rgba=[1.0, 0.0, 1.0, 0.2]) -> None: """ display as a bbox 2d or 3d region specified by the key or set of keys under the config_key parameter along the plane :param config_key: the name of the config key in the scene configuration, which is either the key itself or a parent of keys @@ -149,8 +135,7 @@ def display_bbox_regions(self, config_key:str, plane:str='xy', rgba=[1.0, 0.0, 1 if value is None: logger.warning(f'SceneToRViz (register_regions_as_bbox): Non-existent key {config_key}') return - subscene = self.the_scene.get_subscene_of_config_name(config_key) - reference_frame = subscene if subscene is not None else self.base_frame + reference_frame = reference_frame if reference_frame is not None else self.base_frame if plane is None: plane = 'xy' # default to xy if type(value) in [list, tuple] and len(value) in [4, 6]: @@ -168,7 +153,7 @@ def display_bbox_regions(self, config_key:str, plane:str='xy', rgba=[1.0, 0.0, 1 continue logger.warning(f'SceneToRViz (display_bbox_regions): region ({key}) bbox should be a list of 4 or 6') - def display_rotations(self, config_key:str, arrow_length:float=1.0, rgba=[1.0, 0.0, 0.0, 0.8]) -> None: + def display_rotations(self, config_key:str, arrow_length:float=1.0, reference_frame=None, rgba=[1.0, 0.0, 0.0, 0.8]) -> None: """ display as an arrow the rotation specified by config_key :param config_key: the name of the config key in the scene configuration, which is either the key itself or a parent of keys @@ -181,9 +166,8 @@ def display_rotations(self, config_key:str, arrow_length:float=1.0, rgba=[1.0, 0 value = self.the_scene.query_config(config_key, None) if value is None: logger.warning(f'SceneToRViz (register_positions): Non-existent key {config_key}') - return - subscene = self.the_scene.get_subscene_of_config_name(config_key) - reference_frame = subscene if subscene is not None else self.base_frame + return + reference_frame = reference_frame if reference_frame is not None else self.base_frame if type(value) in [list, tuple] and len(value) == 3: value = {' ': value} if type(value) == dict: @@ -197,7 +181,7 @@ def display_rotations(self, config_key:str, arrow_length:float=1.0, rgba=[1.0, 0 logger.warning(f'SceneToRViz (display_rotations): rotation ({key}) should be a list of 3') - def display_positions(self, config_key:str, rgba=[1.0, 0.0, 0.0, 0.8]) -> None: + def display_positions(self, config_key:str, reference_frame=None, rgba=[1.0, 0.0, 0.0, 0.8]) -> None: """ display one or more positions or partial definitions, that a sphere represents full specification, a line represents a position with a null value, and a plane represents a position with two null values @@ -209,9 +193,8 @@ def display_positions(self, config_key:str, rgba=[1.0, 0.0, 0.0, 0.8]) -> None: value = self.the_scene.query_config(config_key, None) if value is None: logger.warning(f'SceneToRViz (display_positions): Non-existent key {config_key}') - return - subscene = self.the_scene.get_subscene_of_config_name(config_key) - reference_frame = subscene if subscene is not None else self.base_frame + return + reference_frame = reference_frame if reference_frame is not None else self.base_frame if type(value) == list and len(value) == 3: value = {' ': value} if type(value) == dict: @@ -249,42 +232,41 @@ def display_positions(self, config_key:str, rgba=[1.0, 0.0, 0.0, 0.8]) -> None: marker = rviz_tools.create_axisplane_marker(f'{config_key}.{key}', id, bbox, xyz[index], reference_frame, plane, rgba=rgba) self.add_persistent_marker(marker) - def display_object(self, config_key:str, rgba=[1.0, 0.0, 0.0, 0.8]) -> None: - """ display one object associated with the config key + def display_link(self, scene_name:str, reference_frame=None, rgba=[1.0, 0.0, 0.0, 0.8]) -> None: + """ display the link associated with the config key - :param config_key: the name of the config key in the scene configuration, which is either the key itself or a parent of keys - :type config_key: str + :param scene_name: the name of a scene + :type scene_name: str :param rgba: the colour of the objects defaults to [1.0, 0.0, 1.0, 0.2] :type rgba: list, optional """ - value = self.the_scene.query_config(config_key, None) + value = self.the_scene.query_config(scene_name, None) if value is None: - logger.warning(f'SceneToRViz (display_objects): Non-existent key {config_key}') + logger.warning(f'SceneToRViz (display_objects): Non-existent key {scene_name}') return - subscene = self.the_scene.get_subscene_of_config_name(config_key) - reference_frame = subscene if subscene is not None else self.base_frame if type(value) != dict: - logger.warning(f'SceneToRViz (display_objects): the key {config_key} is not associated with a single object in a dict structure') + logger.warning(f'SceneToRViz (display_objects): the key {scene_name} is not associated with a single object in a dict structure') return - the_object = self.the_scene._create_object_config(value) - if the_object.type == 'box': - xyzrpy = the_object.xyz + the_object.rpy - marker = rviz_tools.create_cube_marker_from_xyzrpy(f'object.{config_key}', 0, xyzrpy, reference_frame, the_object.dimensions, rgba=rgba) + reference_frame = reference_frame if reference_frame is not None else self.base_frame + the_link:LinkConfig = self.the_scene.get_link_of_scene(scene_name) + if the_link.type == 'box': + xyzrpy = the_link.xyz + the_link.rpy + marker = rviz_tools.create_cube_marker_from_xyzrpy(f'object.{scene_name}', 0, xyzrpy, reference_frame, the_link.dimensions, rgba=rgba) self.add_persistent_marker(marker) - 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) + elif the_link.type == 'sphere': + marker = rviz_tools.create_sphere_marker(f'object.{scene_name}', 0, the_link.xyz, reference_frame, the_link.dimensions, rgba=rgba) self.add_persistent_marker(marker) - elif the_object.type == 'object': + elif the_link.type == 'object': try: - model_file = PackageFile.resolve_to_file_or_http_uri(the_object.model_file) + model_file = PackageFile.resolve_to_file_or_http_uri(the_link.model_file) except Exception as ex: - logger.warning(f'SceneToRViz (display_objects): Invalid model_file for object ({the_object.model_file}): {ex}') + logger.warning(f'SceneToRViz (display_objects): Invalid model_file for object ({the_link.model_file}): {ex}') return - xyzrpy = the_object.xyz + the_object.rpy - marker = rviz_tools.create_mesh_marker(f'object.{config_key}', 0, model_file, xyzrpy, reference_frame, the_object.dimensions, rgba=rgba) + xyzrpy = the_link.xyz + the_link.rpy + marker = rviz_tools.create_mesh_marker(f'object.{scene_name}', 0, model_file, xyzrpy, reference_frame, the_link.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}') + logger.warning(f'SceneToRViz (display_objects): Invalid object type {the_link.type} of the key {scene_name}') # ------------------------------------------------------------------------ # Functions for adding custom transforms for visualization @@ -301,7 +283,7 @@ def add_custom_transform(self, name, xyz, rpy, frame) -> None: if name is None or xyz is None or rpy is None or frame is None: logger.error(f'SceneToRViz (add_custom_transform): No parameter can be None') raise AssertionError('A parameter is none') - self.custom_transform_object_dict[name] = ObjectConfig(name, None, None, None, xyz, rpy, frame) + self.custom_transform_link_dict[name] = LinkConfig(name, None, None, None, xyz, rpy, frame) def update_custom_transform_pose(self, name, xyz=None, rpy=None, frame=None) -> None: """ Update the pose of a custom transform @@ -311,7 +293,7 @@ def update_custom_transform_pose(self, name, xyz=None, rpy=None, frame=None) -> :param rpy: the updated rpy, defaults to None meaning unchanged :param frame: the updated frame, defaults to None meaning unchanged """ - the_object:ObjectConfig = self.custom_transform_object_dict[name] + the_object:LinkConfig = self.custom_transform_link_dict[name] if the_object is None: logger.warning(f'__name__ (update_custom_transform_pose): the custom transform name {name} is not found') return False @@ -326,20 +308,20 @@ def remove_custom_transform(self, name=None): :param name: the name of the transform to be removed, or None to remove all """ if name is None: - self.custom_transform_object_dict.clear() - elif name in self.custom_transform_object_dict: - del self.custom_transform_object_dict[name] + self.custom_transform_link_dict.clear() + elif name in self.custom_transform_link_dict: + del self.custom_transform_link_dict[name] # ----------------------------------------------------------- # test program if __name__ == '__main__': the_scene = Scene(os.path.join(os.path.dirname(__file__), '../demos/gridscan/task_scene.yaml')) scene_to_rviz = SceneToRViz(the_scene, 'world') - scene_to_rviz.set_publish_objects_transform(None) + scene_to_rviz.set_publish_links_transform(None) scene_to_rviz.display_bbox_regions('regions') scene_to_rviz.display_bbox_regions('tank.bbox', rgba=[0.2, 0.8, 0.4, 0.2]) scene_to_rviz.display_rotations('tank.rotations', arrow_length=0.5, rgba=[0.2, 0.0, 1.0, 0.8]) scene_to_rviz.display_positions('tank.positions', rgba=[1.0, 1.0, 0.0, 0.8]) # test display objects - scene_to_rviz.display_object('objects.tank', rgba=[1.0, 0.2, 0.2, 0.8]) + scene_to_rviz.display_link('objects.tank', rgba=[1.0, 0.2, 0.2, 0.8]) rospy.spin() \ No newline at end of file diff --git a/task_trees/task_scene.py b/task_trees/task_scene.py index a3fa2a6..097a6bf 100644 --- a/task_trees/task_scene.py +++ b/task_trees/task_scene.py @@ -11,12 +11,12 @@ import yaml, os, numbers from collections import namedtuple, defaultdict -from tools.logging_tools import logger +from task_trees.tools.logging_tools import logger # namedtuples for internal data structure for higher readability -ObjectConfig = namedtuple('ObjectConfig', ['name', 'type', 'model_file', 'dimensions', 'xyz', 'rpy', 'frame']) # representing an object -FrameConfig = namedtuple('FrameConfig', ['name', 'xyz', 'rpy', 'parent']) -SceneName = namedtuple('SceneName', ['subscene', 'path']) +SceneKeyParts = namedtuple('SceneKeyParts', ['scene', 'parts']) +LinkConfig = namedtuple('LinkConfig', ['name', 'type', 'model_file', 'dimensions', 'xyz', 'rpy', 'parent_frame']) # representing the scene +FrameConfig = namedtuple('FrameConfig', ['name', 'type', 'xyz', 'rpy', 'parent_frame']) class Scene(): """ The base model provides mapping between physical/joint-space pose and various named/logical poses. @@ -31,288 +31,227 @@ def __init__(self, scene_config_file:str = None): if scene_config_file is None: scene_config_file = os.path.join(os.path.dirname(__file__), '../demos/gridscan/task_scene.yaml') with open(scene_config_file, 'r') as f: - config = yaml.safe_load(f) - self.config = config - # if the default scene is defined in the file, define a variable for the default - self.scene_config = self.config['scene'] if 'scene' in self.config else None - # if subscenes are defined in the file, define another variable for that - self.subscene_config = self.config['subscenes'] if 'subscenes' in self.config else None - # preprocess the objects and frames - self.objects_map = self._process_objects() - self.frames_map = self._process_frames() + self.config_file_root = yaml.safe_load(f) + # print warning message + self._print_warning() + # valiate that there is a key 'scenes' + if 'scenes' not in self.config_file_root: + logger.error(f'The config file should contain a "scenes" key at the top level') + raise AssertionError(f'Invalid yaml config file: missing the key "scenes"') + self.scenes_branch = self.config_file_root['scenes'] + # valiate that there is a 'root' scene + if 'root' not in self.scenes_branch: + logger.error(f'The config file should contain a "root" scene defined under "scenes" key at the top level') + raise AssertionError(f'Invalid yaml config file: missing the "root" scene') + self.root_scene_config = self.scenes_branch['root'] + # model parameters + self.scenes_dict = defaultdict(lambda: None) + self.scenes_linkconfig = defaultdict(lambda: None) + self.frames_config = defaultdict(lambda: None) + # iterates all scenes in the config file + for key in self.scenes_branch.keys(): + self.scenes_dict[key] = self.scenes_branch[key] + link_config = self._extract_link_config(key, self.scenes_branch[key]) + if link_config is not None: + if link_config.type == 'frame': + self.frames_config[key] = link_config + else: + self.scenes_linkconfig[key] = link_config - # internal function: parse a dot-separated config name (path) into two parts, the subscene and a list of the path - def _parse_config_name(self, config_name:str) -> SceneName: - if config_name is None: - return None - parts = config_name.split('.') - # print(f'_parse_config_name: {config_name} {parts} {parts[0]} {self.subscene_config}') - if self.subscene_config is not None and len(parts) > 1 and (parts[0] in self.subscene_config.keys()): # one of the subscenes - return SceneName(parts[0], parts[1:]) + # internal function: parse a dot-separated config kay (path) into scene name and a list of the parts + def _parse_config_key(self, config_key:str) -> SceneKeyParts: + assert config_key is not None, 'Parameter (config_key) should not be None' + parts = config_key.split('.') + assert len(parts) >= 1, 'Parameter (config_key) is empty' + scene_name = parts[0] + if scene_name not in self.scenes_dict: + # the first name is not a known scene, assume that the root scene is inferred + scene_name = 'root' + return SceneKeyParts(scene_name, parts) else: - return SceneName(None, parts) + return SceneKeyParts(scene_name, parts[1:]) # ------------------------------------------- - # functions for extracting objects and frames + # internal functions for extracting objects and frames + def _extract_link_config(self, scene_name:str, scene_dict:dict) -> LinkConfig: + return self._create_link_config(scene_name, scene_dict.get('link', None)) + # internal function: extract and validate an object definition - def _create_object_config(self, obj:dict) -> ObjectConfig: - if obj is None: - return None - if 'name' not in obj: - logger.error(f'Scene (_process_an_object): the keys in the object config contains no "name"') + def _create_link_config(self, scene_name:str, link_dict:dict) -> LinkConfig: + if link_dict is None or scene_name is None: return None - if 'type' not in obj or 'xyz' not in obj or 'rpy' not in obj or 'dimensions' not in obj: - logger.error(f'Scene (_process_an_object): the keys in the object config ({obj["name"]}) contains no "type", "xyz", "rpy", "dimensions"') + # if 'name' not in link_dict: + # logger.error(f'Scene (_process_an_object): the keys in the link config contains no "name"') + # return None + if 'type' not in link_dict or 'xyz' not in link_dict or 'rpy' not in link_dict: + logger.error(f'Scene (_process_an_object): the keys in the scene config ({link_dict["name"]}) contains no "type", "xyz", "rpy"') return None - if obj['type'] not in ['box', 'sphere', 'object']: - logger.error(f'Scene (_process_an_object): the type of the object ({obj["name"]}) is not one of "sphere", "box", or "object"') - return None - model_file = obj.get('model_file', None) - if obj['type'] == 'object' and model_file is None: - logger.error(f'Scene (_process_an_object): the object ({obj["name"]}) expects a valid "model_file" key') + if link_dict['type'] not in ['box', 'sphere', 'object', 'frame']: + logger.error(f'Scene (_process_an_object): the type of the scene ({link_dict["name"]}) is not one of "sphere", "box", "frame", or "object"') + return None + if link_dict['type'] in ['box', 'sphere', 'object'] and 'dimensions' not in link_dict: + logger.error(f'Scene (_process_an_object): the keys in the box, sphere or object typed scene link ({link_dict["name"]}) contains no "dimensions"') + return None + model_file = link_dict.get('model_file', None) + if link_dict['type'] == 'object' and model_file is None: + logger.error(f'Scene (_process_an_object): the scene link ({link_dict["name"]}) expects a valid "model_file" key') return None - return ObjectConfig(obj['name'], obj['type'], model_file, obj['dimensions'], obj['xyz'], obj['rpy'], obj.get('frame', None)) - - def _extract_objects_dict(self, source_object_config:dict, the_map:dict, subscene:str=None) -> ObjectConfig: - # assume object_config is a dict - for name in source_object_config: - obj = source_object_config[name] - obj['name'] = name - if 'frame' not in obj: - obj['frame'] = subscene - object_config = self._create_object_config(obj) - if object_config: - if subscene is None: - the_map[name] = object_config - else: - the_map[f'{subscene}.{name}'] = object_config - - def _extract_objects_list(self, source_object_config:list, the_map:dict, subscene:str=None) -> ObjectConfig: - # assume object_config is a list - for obj in source_object_config: - name = obj['name'] - if 'frame' not in obj: - obj['frame'] = subscene - object_config = self._create_object_config(obj) - if object_config: - if subscene is None: - the_map[name] = object_config - else: - the_map[f'{subscene}.{name}'] = object_config + parent_name = link_dict.get('parent_frame', link_dict.get('frame', None)) # both parent_frame and frame are acceptable + if parent_name is not None and parent_name == 'root' and 'root' not in self.scenes_linkconfig: + parent_name = None + dimensions = link_dict.get('dimensions', None) + return LinkConfig(scene_name, link_dict['type'], model_file, dimensions, link_dict['xyz'], link_dict['rpy'], parent_name) def _print_warning(self) -> None: if not hasattr(self, 'warned'): self.warned = True - logger.warning(f'''Scene configuration file format changed. - Objects are defined using a dict structure instead of a list structure. - objects: - name: - type: ... - xyz: ... + logger.warning(f'''Scene configuration file format changed in task_trees 0.2.x. + The subscene concept is removed and scenes are equal in the yaml file + scenes: + scene_1_name: + link: ... + other_params: ... ... - The old format will not be acceptable in the future. Update the file is recommended.''') - - # internal function: returns a map of (name, ObjectConfig) of objects defined in the config file - def _process_objects(self) -> dict: - the_map = dict() - # process the main scene - if self.scene_config is not None and 'objects' in self.scene_config: - object_config = self.scene_config['objects'] - if type(object_config) in (tuple, list): - self._extract_objects_list(object_config, the_map) - self._print_warning() - else: - self._extract_objects_dict(object_config, the_map) - - # process each subscene - if self.subscene_config is not None: - for subscene in self.subscene_config.keys(): - if self.subscene_config[subscene] is None: - continue - if 'objects' in self.subscene_config[subscene]: - object_config = self.subscene_config[subscene]['objects'] - if type(object_config) in (tuple, list): - self._extract_objects_list(object_config, the_map, subscene) - self._print_warning() - else: - self._extract_objects_dict(object_config, the_map, subscene) - return the_map - - def _extract_frames_dict(self, source_frame_config:dict, the_map:dict, subscene:str=None) -> FrameConfig: - # assume object_config is a dict - for name in source_frame_config: - obj = source_frame_config[name] - obj['name'] = name - if 'frame' in obj: - obj['parent'] = obj['frame'] - if 'parent' not in obj: - obj['parent'] = subscene - if 'xyz' not in obj or 'rpy' not in obj: - logger.error(f'Scene (_extract_frames_dict): the keys in the object config ({obj["name"]}) contains no "xyz" or "rpy"') - return None - frame_config = FrameConfig(obj['name'], obj['xyz'], obj['rpy'], obj['parent']) - if frame_config: - if subscene is None: - the_map[name] = frame_config - else: - the_map[f'{subscene}.{name}'] = frame_config - - # internal function: returns a map of (name, FrameConfig) of frames defined in the config file - def _process_frames(self) -> dict: - the_map = dict() - # process the main scene - if self.scene_config is not None and 'frames' in self.scene_config: - frame_config = self.scene_config['frames'] - self._extract_frames_dict(frame_config, the_map) - # process each subscene - if self.subscene_config is not None: - for subscene in self.subscene_config.keys(): - if self.subscene_config[subscene] is None: - continue - if 'frames' in self.subscene_config[subscene]: - frame_config = self.subscene_config[subscene]['frames'] - self._extract_frames_dict(frame_config, the_map, subscene) - return the_map - + scene_2_name: + link: ... + The API is also changed. Please refer to the documentation''') + # ------------------------------------------- - # model functions + # config query functions - # returns the value of a named config given as a dot-separated path (e.g. table.objects.lamp.position) - def query_config(self, name:str, default=None): + # returns the value of a named config given as a dot-separated key (e.g. table.objects.lamp.position) + def query_config(self, key:str, default=None): """ returns the value of any named config given as a dot-separated path - :param name: the dot-separated path - :type name: str - :param default: the default value if nothing is found in the path, defaults to None + :param key: the dot-separated key, the root scene may have the "root" omitted + :type key: str + :param default: the default value if nothing is found in the key, defaults to None :type default: any :return: the value of the config :rtype: any """ - scene_name = self._parse_config_name(name) - if scene_name is None: + the_parsed_key = self._parse_config_key(key) + if the_parsed_key is None: return default - if scene_name.subscene is not None: - pointer = self.subscene_config[scene_name.subscene] - else: - pointer = self.scene_config - for item in scene_name.path: + pointer = self.scenes_dict[the_parsed_key.scene] + for item in the_parsed_key.parts: if item.isnumeric(): try: item = int(item) except: - logger.error(f'query_config: part of query key {name} contains an non-integer index {item}') - raise AssertionError(f'query_config: Non-existent config name "{name}", which contains an invalid index {item}') + logger.error(f'query_config: part of query key {the_parsed_key} contains an non-integer index {item}') + raise AssertionError(f'query_config: Non-existent config name "{the_parsed_key}", which contains an invalid index {item}') if item < 0 or item >= len(pointer): - logger.error(f'query_config: part of query key {name} contains an out-of-range integer index {item} ') - raise AssertionError(f'query_config: Non-existent config name "{name}", which contains an invalid index {item}') + logger.error(f'query_config: part of query key {the_parsed_key} contains an out-of-range integer index {item} ') + raise AssertionError(f'query_config: Non-existent config name "{the_parsed_key}", which contains an invalid index {item}') if type(pointer) not in (tuple, list): - logger.error(f'query_config: part of query key {name} contains an invalid index {item}, expecting a string key') - raise AssertionError(f'query_config: Non-existent config name "{name}", which contains an invalid index {item}') + logger.error(f'query_config: part of query key {the_parsed_key} contains an invalid index {item}, expecting a string key') + raise AssertionError(f'query_config: Non-existent config name "{the_parsed_key}", which contains an invalid index {item}') pointer = pointer[item] else: if type(pointer) != dict: - logger.error(f'query_config: part of query key {name} contains an invalid index {item}, expecting an integer') - raise AssertionError(f'query_config: Non-existent config name "{name}", which contains an invalid index {item}') + logger.error(f'query_config: part of query key {the_parsed_key} contains an invalid index {item}, expecting an integer') + raise AssertionError(f'query_config: Non-existent config name "{the_parsed_key}", which contains an invalid index {item}') if item not in pointer: - logger.error(f'query_config: part of query key {name} contains a non-existent index {item}') - raise AssertionError(f'query_config: Non-existent config name "{name}", which contains an invalid index {item}') + logger.error(f'query_config: part of query key {the_parsed_key} contains a non-existent index {item}') + raise AssertionError(f'query_config: Non-existent config name "{the_parsed_key}", which contains an invalid index {item}') pointer = pointer[item] - return pointer - - # alias for backward compatibility query_config - def query_position_as_xyz(self, name, default=None): - return self.query_config(name, default) - - def query_rotation_as_rpy(self, name, default=None): - return self.query_config(name, default) - - # returns the subscene name if the config is a subscene config - def get_subscene_of_config_name(self, name) -> str: - """ returns the subscene name if the config is one belonging to a subscene - :return: the name of a subscene or None - :rtype: str - """ - scene_name = self._parse_config_name(name) - if scene_name is None: - return None - return scene_name.subscene - - # returns the root scene config as a dict - def get_scene_config(self) -> dict: - """ returns the main scene config in the config file as a dict - :return: the content in the config file - :rtype: dict - """ - return self.scene_config - - # returns a subscene config as a dict - def get_subscene_config(self, subscene_name) -> dict: - """ returns the config of a subscene in the config file as a dict - :return: the content in the config file about the subscene or None - :rtype: dict - """ - if subscene_name not in self.subscene_config: - return None - return self.subscene_config[subscene_name] + return pointer # returns True of the given config name exists - def exists_config(self, name:str) -> bool: + def exists_config(self, key:str) -> bool: """ returns True of the given config name exists - :param name: The config name - :type name: str + :param key: The config name + :type key: str :return: True if the given config name exists :rtype: bool """ - return self.query_config(name) != None + return self.query_config(key) != None - # returns a list of keys specified under the config name - def keys_of_config(self, name:str) -> list: + # returns a list of keys specified under the config key if it is a dictionary or an empty list + def key_list_under_config_key(self, key:str) -> list: """ returns a list of keys specified under the config name or an empty list - :param name: The config name - :type name: str + :param key: The config name + :type key: str :return: a list of keys specified under the config name :rtype: list """ - pointer = self.query_config(name) + pointer = self.query_config(key) if pointer is None or type(pointer) not in [dict, map]: return [] return list(pointer.keys()) - # returns the length of the list under the config name or an empty list - def len_of_list_config(self, name:str) -> int: + # returns the length of the list if the config key is a list or an empty list + def len_of_key_list_under_config_key(self, key:str) -> int: """ returns the length of the list under the config name - :param name: The config name - :type name: str + :param key: The config name + :type key: str :return: the length of the list the config name :rtype: int """ - pointer = self.query_config(name) + pointer = self.query_config(key) if pointer is None or type(pointer) not in [tuple, list]: return 0 return len(pointer) - # list the object names - def list_object_names(self) -> list: + # list the scene names + def list_scene_names(self) -> list: """ return the list of objects defined in the scene configuration :return: a list of object names as strings :rtype: list """ - if self.objects_map is None: - return [] - return self.objects_map.keys() + return list(self.scenes_dict.keys()) + + # return scene config given the scene name + def get_scene_config_as_dict(self, scene_name: str) -> dict: + """ return the scene configuration as a dict, or None if the scene not exists + :param scene_name: name of the object + :type scene_name: str + :return: the configuration of the scene as a dict or None if not exists + :rtype: dict + """ + return self.scenes_dict.get(scene_name, None) # return config given the object name - def get_object_config(self, object_name: str) -> ObjectConfig: + def get_link_of_scene(self, scene_name: str) -> LinkConfig: """ return the object configuration as ObjectConfig tuple, or None if the name not exists - :param object_name: name of the object - :type object_name: str - :return: the configuration as a ObjectConfig or None if not exists - :rtype: ObjectConfig + :param scene_name: name of the scene + :type scene_name: str + :return: the LinkConfig of the scene or None if not exists + :rtype: LinkConfig """ - return self.objects_map.get(object_name) + return self.scenes_linkconfig.get(scene_name, None) + + # list the frame names + def list_frame_names(self) -> list: + """ return the list of frame type scene defined in the scene configuration + :return: a list of frame names as strings + :rtype: list + """ + return list(self.frames_config.keys()) + # return scene config given the scene name + def get_frame_config_as_dict(self, frame_name: str) -> dict: + """ return the frame configuration as a dict, or None if the scene not exists + :param frame_name: name of the frame + :type frame_name: str + :return: the configuration of the frame type scene or None if not exists + :rtype: dict + """ + return self.scenes_dict.get(frame_name, None) + + # return config given the object name + def get_link_of_frame(self, frame_name: str) -> LinkConfig: + """ return the object configuration as ObjectConfig tuple, or None if the name not exists + :param frame_name: name of the frame + :type frame_name: str + :return: the LinkConfig of the frame or None if not exists + :rtype: LinkConfig + """ + return self.frames_config.get(frame_name, None) + # return the named poses as a dict - def get_named_poses_as_dict(self) -> dict: + def get_named_poses_of_root_as_dict(self) -> dict: """ return the named poses defined in the scene configuration as a dict :return: dict containing key value pairs of named poses @@ -324,25 +263,7 @@ def get_named_poses_as_dict(self) -> dict: result[pose_name] = self.query_config(pose_name) return result - # list the frame names - def list_frame_names(self) -> list: - """ return the list of frames defined in the scene configuration - :return: a list of frames names as strings - :rtype: list - """ - if self.frames_map is None: - return [] - return self.frames_map.keys() - # return config given the object name - def get_frame_config(self, frame_name: str) -> FrameConfig: - """ return the object configuration as FrameConfig tuple, or None if the name not exists - :param frame_name: name of the object - :type frame_name: str - :return: the configuration as a FrameConfig or None if not exists - :rtype: FrameConfig - """ - return self.frames_map.get(frame_name) # ----------------------------------------------------------- # test program @@ -364,16 +285,4 @@ def get_frame_config(self, frame_name: str) -> FrameConfig: for rotation in rotations: print(f'rotation {rotation}: {the_scene.query_config(rotation)}') - objects = the_scene.list_object_names() - for object_name in objects: - print(f'object {object_name}: {the_scene.get_object_config(object_name)}') - - the_object = the_scene.query_config('objects.tank') - print(the_object) - - print(f'tank tile step_sizes: {the_scene.query_config("tank.tile")}') - - keys = ['grid.0.rotation.alpha', 'grid.1.rotation.alpha', 'grid.2.rotation.alpha'] - for key in keys: - print(f'key {key}', the_scene.query_config(key)) \ No newline at end of file diff --git a/task_trees/task_scene_old.py b/task_trees/task_scene_old.py new file mode 100644 index 0000000..aeb5fac --- /dev/null +++ b/task_trees/task_scene_old.py @@ -0,0 +1,379 @@ +# 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' + +import yaml, os, numbers +from collections import namedtuple, defaultdict +from task_trees.tools.logging_tools import logger + +# namedtuples for internal data structure for higher readability +ObjectConfig = namedtuple('ObjectConfig', ['name', 'type', 'model_file', 'dimensions', 'xyz', 'rpy', 'frame']) # representing an object +FrameConfig = namedtuple('FrameConfig', ['name', 'xyz', 'rpy', 'parent']) +SceneName = namedtuple('SceneName', ['subscene', 'path']) + +class Scene(): + """ The base model provides mapping between physical/joint-space pose and various named/logical poses. + """ + def __init__(self, scene_config_file:str = None): + """the constructor + :param scene_config_file: the path to the task scene configuration file, defaults to None, which then the ros parameter + 'scene_config_file' will be used as the path if exists, and the task_scene.yaml file in the config folder is the last resort + :type scene_config_file: str, optional + """ + # load data from the config yaml file + if scene_config_file is None: + scene_config_file = os.path.join(os.path.dirname(__file__), '../demos/gridscan/task_scene.yaml') + with open(scene_config_file, 'r') as f: + config = yaml.safe_load(f) + self.config = config + # if the default scene is defined in the file, define a variable for the default + self.scene_config = self.config['scene'] if 'scene' in self.config else None + # if subscenes are defined in the file, define another variable for that + self.subscene_config = self.config['subscenes'] if 'subscenes' in self.config else None + # preprocess the objects and frames + self.objects_map = self._process_objects() + self.frames_map = self._process_frames() + + # internal function: parse a dot-separated config name (path) into two parts, the subscene and a list of the path + def _parse_config_name(self, config_name:str) -> SceneName: + if config_name is None: + return None + parts = config_name.split('.') + # print(f'_parse_config_name: {config_name} {parts} {parts[0]} {self.subscene_config}') + if self.subscene_config is not None and len(parts) > 1 and (parts[0] in self.subscene_config.keys()): # one of the subscenes + return SceneName(parts[0], parts[1:]) + else: + return SceneName(None, parts) + + # ------------------------------------------- + # functions for extracting objects and frames + + # internal function: extract and validate an object definition + def _create_object_config(self, obj:dict) -> ObjectConfig: + if obj is None: + return None + if 'name' not in obj: + logger.error(f'Scene (_process_an_object): the keys in the object config contains no "name"') + return None + if 'type' not in obj or 'xyz' not in obj or 'rpy' not in obj or 'dimensions' not in obj: + logger.error(f'Scene (_process_an_object): the keys in the object config ({obj["name"]}) contains no "type", "xyz", "rpy", "dimensions"') + return None + if obj['type'] not in ['box', 'sphere', 'object']: + logger.error(f'Scene (_process_an_object): the type of the object ({obj["name"]}) is not one of "sphere", "box", or "object"') + return None + model_file = obj.get('model_file', None) + if obj['type'] == 'object' and model_file is None: + logger.error(f'Scene (_process_an_object): the object ({obj["name"]}) expects a valid "model_file" key') + return None + return ObjectConfig(obj['name'], obj['type'], model_file, obj['dimensions'], obj['xyz'], obj['rpy'], obj.get('frame', None)) + + def _extract_objects_dict(self, source_object_config:dict, the_map:dict, subscene:str=None) -> ObjectConfig: + # assume object_config is a dict + for name in source_object_config: + obj = source_object_config[name] + obj['name'] = name + if 'frame' not in obj: + obj['frame'] = subscene + object_config = self._create_object_config(obj) + if object_config: + if subscene is None: + the_map[name] = object_config + else: + the_map[f'{subscene}.{name}'] = object_config + + def _extract_objects_list(self, source_object_config:list, the_map:dict, subscene:str=None) -> ObjectConfig: + # assume object_config is a list + for obj in source_object_config: + name = obj['name'] + if 'frame' not in obj: + obj['frame'] = subscene + object_config = self._create_object_config(obj) + if object_config: + if subscene is None: + the_map[name] = object_config + else: + the_map[f'{subscene}.{name}'] = object_config + + def _print_warning(self) -> None: + if not hasattr(self, 'warned'): + self.warned = True + logger.warning(f'''Scene configuration file format changed. + Objects are defined using a dict structure instead of a list structure. + objects: + name: + type: ... + xyz: ... + ... + The old format will not be acceptable in the future. Update the file is recommended.''') + + # internal function: returns a map of (name, ObjectConfig) of objects defined in the config file + def _process_objects(self) -> dict: + the_map = dict() + # process the main scene + if self.scene_config is not None and 'objects' in self.scene_config: + object_config = self.scene_config['objects'] + if type(object_config) in (tuple, list): + self._extract_objects_list(object_config, the_map) + self._print_warning() + else: + self._extract_objects_dict(object_config, the_map) + + # process each subscene + if self.subscene_config is not None: + for subscene in self.subscene_config.keys(): + if self.subscene_config[subscene] is None: + continue + if 'objects' in self.subscene_config[subscene]: + object_config = self.subscene_config[subscene]['objects'] + if type(object_config) in (tuple, list): + self._extract_objects_list(object_config, the_map, subscene) + self._print_warning() + else: + self._extract_objects_dict(object_config, the_map, subscene) + return the_map + + def _extract_frames_dict(self, source_frame_config:dict, the_map:dict, subscene:str=None) -> FrameConfig: + # assume object_config is a dict + for name in source_frame_config: + obj = source_frame_config[name] + obj['name'] = name + if 'frame' in obj: + obj['parent'] = obj['frame'] + if 'parent' not in obj: + obj['parent'] = subscene + if 'xyz' not in obj or 'rpy' not in obj: + logger.error(f'Scene (_extract_frames_dict): the keys in the object config ({obj["name"]}) contains no "xyz" or "rpy"') + return None + frame_config = FrameConfig(obj['name'], obj['xyz'], obj['rpy'], obj['parent']) + if frame_config: + if subscene is None: + the_map[name] = frame_config + else: + the_map[f'{subscene}.{name}'] = frame_config + + # internal function: returns a map of (name, FrameConfig) of frames defined in the config file + def _process_frames(self) -> dict: + the_map = dict() + # process the main scene + if self.scene_config is not None and 'frames' in self.scene_config: + frame_config = self.scene_config['frames'] + self._extract_frames_dict(frame_config, the_map) + # process each subscene + if self.subscene_config is not None: + for subscene in self.subscene_config.keys(): + if self.subscene_config[subscene] is None: + continue + if 'frames' in self.subscene_config[subscene]: + frame_config = self.subscene_config[subscene]['frames'] + self._extract_frames_dict(frame_config, the_map, subscene) + return the_map + + # ------------------------------------------- + # model functions + + # returns the value of a named config given as a dot-separated path (e.g. table.objects.lamp.position) + def query_config(self, name:str, default=None): + """ returns the value of any named config given as a dot-separated path + :param name: the dot-separated path + :type name: str + :param default: the default value if nothing is found in the path, defaults to None + :type default: any + :return: the value of the config + :rtype: any + """ + scene_name = self._parse_config_name(name) + if scene_name is None: + return default + if scene_name.subscene is not None: + pointer = self.subscene_config[scene_name.subscene] + else: + pointer = self.scene_config + for item in scene_name.path: + if item.isnumeric(): + try: + item = int(item) + except: + logger.error(f'query_config: part of query key {name} contains an non-integer index {item}') + raise AssertionError(f'query_config: Non-existent config name "{name}", which contains an invalid index {item}') + if item < 0 or item >= len(pointer): + logger.error(f'query_config: part of query key {name} contains an out-of-range integer index {item} ') + raise AssertionError(f'query_config: Non-existent config name "{name}", which contains an invalid index {item}') + if type(pointer) not in (tuple, list): + logger.error(f'query_config: part of query key {name} contains an invalid index {item}, expecting a string key') + raise AssertionError(f'query_config: Non-existent config name "{name}", which contains an invalid index {item}') + pointer = pointer[item] + else: + if type(pointer) != dict: + logger.error(f'query_config: part of query key {name} contains an invalid index {item}, expecting an integer') + raise AssertionError(f'query_config: Non-existent config name "{name}", which contains an invalid index {item}') + if item not in pointer: + logger.error(f'query_config: part of query key {name} contains a non-existent index {item}') + raise AssertionError(f'query_config: Non-existent config name "{name}", which contains an invalid index {item}') + pointer = pointer[item] + return pointer + + # alias for backward compatibility query_config + def query_position_as_xyz(self, name, default=None): + return self.query_config(name, default) + + def query_rotation_as_rpy(self, name, default=None): + return self.query_config(name, default) + + # returns the subscene name if the config is a subscene config + def get_subscene_of_config_name(self, name) -> str: + """ returns the subscene name if the config is one belonging to a subscene + :return: the name of a subscene or None + :rtype: str + """ + scene_name = self._parse_config_name(name) + if scene_name is None: + return None + return scene_name.subscene + + # returns the root scene config as a dict + def get_scene_config(self) -> dict: + """ returns the main scene config in the config file as a dict + :return: the content in the config file + :rtype: dict + """ + return self.scene_config + + # returns a subscene config as a dict + def get_subscene_config(self, subscene_name) -> dict: + """ returns the config of a subscene in the config file as a dict + :return: the content in the config file about the subscene or None + :rtype: dict + """ + if subscene_name not in self.subscene_config: + return None + return self.subscene_config[subscene_name] + + # returns True of the given config name exists + def exists_config(self, name:str) -> bool: + """ returns True of the given config name exists + :param name: The config name + :type name: str + :return: True if the given config name exists + :rtype: bool + """ + return self.query_config(name) != None + + # returns a list of keys specified under the config name + def keys_of_config(self, name:str) -> list: + """ returns a list of keys specified under the config name or an empty list + :param name: The config name + :type name: str + :return: a list of keys specified under the config name + :rtype: list + """ + pointer = self.query_config(name) + if pointer is None or type(pointer) not in [dict, map]: + return [] + return list(pointer.keys()) + + # returns the length of the list under the config name or an empty list + def len_of_list_config(self, name:str) -> int: + """ returns the length of the list under the config name + :param name: The config name + :type name: str + :return: the length of the list the config name + :rtype: int + """ + pointer = self.query_config(name) + if pointer is None or type(pointer) not in [tuple, list]: + return 0 + return len(pointer) + + # list the object names + def list_object_names(self) -> list: + """ return the list of objects defined in the scene configuration + :return: a list of object names as strings + :rtype: list + """ + if self.objects_map is None: + return [] + return self.objects_map.keys() + + # return config given the object name + def get_object_config(self, object_name: str) -> ObjectConfig: + """ return the object configuration as ObjectConfig tuple, or None if the name not exists + :param object_name: name of the object + :type object_name: str + :return: the configuration as a ObjectConfig or None if not exists + :rtype: ObjectConfig + """ + return self.objects_map.get(object_name) + + # return the named poses as a dict + def get_named_poses_as_dict(self) -> dict: + """ return the named poses defined in the scene configuration as a dict + + :return: dict containing key value pairs of named poses + """ + result = dict() + named_poses = self.query_config('named_poses') + for pose_name in named_poses: + pose_name = 'named_poses.' + pose_name + result[pose_name] = self.query_config(pose_name) + return result + + # list the frame names + def list_frame_names(self) -> list: + """ return the list of frames defined in the scene configuration + :return: a list of frames names as strings + :rtype: list + """ + if self.frames_map is None: + return [] + return self.frames_map.keys() + + # return config given the object name + def get_frame_config(self, frame_name: str) -> FrameConfig: + """ return the object configuration as FrameConfig tuple, or None if the name not exists + :param frame_name: name of the object + :type frame_name: str + :return: the configuration as a FrameConfig or None if not exists + :rtype: FrameConfig + """ + return self.frames_map.get(frame_name) + +# ----------------------------------------------------------- +# test program +if __name__ == '__main__': + the_scene = Scene() + named_poses = ['named_poses.stow', 'named_poses.home'] + for named_pose in named_poses: + print(f'named_pose {named_pose}: {the_scene.query_config(named_pose)}') + + regions = ['regions.workspace', 'regions.inner', 'regions.work', 'regions.work3d'] + for region in regions: + print(f'region {region}: {the_scene.query_config(region)}') + + positions = ['tank.positions.default', 'tank.positions.hover', 'tank.positions.submerged'] + for position in positions: + print(f'position {position}: {the_scene.query_config(position)}') + + rotations = ['tank.rotations.alpha', 'tank.rotations.beta', 'tank.rotations.delta', 'tank.rotations.gamma'] + for rotation in rotations: + print(f'rotation {rotation}: {the_scene.query_config(rotation)}') + + objects = the_scene.list_object_names() + for object_name in objects: + print(f'object {object_name}: {the_scene.get_object_config(object_name)}') + + the_object = the_scene.query_config('objects.tank') + print(the_object) + + print(f'tank tile step_sizes: {the_scene.query_config("tank.tile")}') + + keys = ['grid.0.rotation.alpha', 'grid.1.rotation.alpha', 'grid.2.rotation.alpha'] + for key in keys: + print(f'key {key}', the_scene.query_config(key)) + \ No newline at end of file diff --git a/task_trees/task_trees_manager.py b/task_trees/task_trees_manager.py index 09957ca..c94b60b 100644 --- a/task_trees/task_trees_manager.py +++ b/task_trees/task_trees_manager.py @@ -17,21 +17,19 @@ from task_trees.behaviours_base import * from task_trees.states import TaskStates, COMPLETION_STATES -from tools.logging_tools import logger +from task_trees.tools.logging_tools import logger # -- The abstract class for implementing project specific Task for the TaskTreesManager class BasicTask(): """ It defines the essential parameters for the lifecycle management by the TaskTreesManager :meta private: """ - def __init__(self, goal_as_logical_pose=None): + def __init__(self, param=None): self.task_lock = threading.Lock() # task parameters self.name = self.__class__ # the class name for identification of the task - self.goal_as_logical_pose = goal_as_logical_pose # the main goal of the task - assume to be a logical pose - self.xyz = None # the mapped physical pose in xyz format - self.rpy = None # the mapped physical pose in euler angles in rpy order + self.param = param # arbitrary param of the task # task state self.state = TaskStates.STANDBY # the init state self.result = '' # the task result, which will be assigned by the TaskTreesManager at completion @@ -86,27 +84,13 @@ def cancel(self, wait=True) -> bool: logger.error('This task has not been submitted -> check program logic') return False - def get_goal_as_logical_pose(self): - """ returns the main goal of the task, which is an logical pose + def get_param(self): + """ returns the param of the task - :return: the goal as a logical pose + :return: the param of the task :rtype: arbitrary, dependent on the subclass definition """ - return self.goal_as_logical_pose - - def get_xyz(self) -> list: - """ return the position of the target pose in xyz - :return: the position of the target pose in xyz - :rtype: list - """ - return self.xyz - - def get_rpy(self) -> list: - """ return the orientation part of the target pose - :return: the orientation part of the target pose - :rtype: list - """ - return self.rpy + return self.param def wait_for_completion(self, until_states:list=None): """ a blocking function that returns when the task has entered into one of the :param:until_states @@ -336,37 +320,40 @@ def _define_named_poses(self, the_scene:Scene): :param the_scene: the scene configuration object of the Scene class """ - named_poses = the_scene.get_named_poses_as_dict() + named_poses = the_scene.get_named_poses_of_root_as_dict() for name in named_poses: self.arm_commander.add_named_pose(name, named_poses[name]) - def _define_objects(self, the_scene:Scene, object_type=None, include_list:list=None, ignore_list:list=None): + def _define_links(self, the_scene:Scene, link_type=None, include_list:list=None, ignore_list:list=None): """ Define the collision objects in the arm commander as specified in the scene configuration file :param the_scene: the scene configuration object of the Scene class :param include_list: the only frames to be included and subject to the ignore_list, defaults to None - :param object_type: the object type to be included, defaults to None + :param link_type: the link_type to be included, defaults to None """ - if object_type is not None and type(object_type) is not list: - object_type = [object_type] + if link_type is not None and type(link_type) is not list: + link_type = [link_type] if ignore_list is None: ignore_list = [] - for object_name in the_scene.list_object_names(): - the_object = the_scene.get_object_config(object_name) - if object_type is not None and the_object.type not in object_type: + for scene_name in the_scene.list_scene_names(): + the_link = the_scene.get_link_of_scene(scene_name) + if the_link is None: + continue + + if link_type is not None and the_link.type not in link_type: continue - if include_list is not None and object_name not in include_list: + if include_list is not None and scene_name not in include_list: continue - if object_name in ignore_list: + if scene_name in ignore_list: continue - 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) - elif the_object.type == 'sphere': - self.arm_commander.add_sphere_to_scene(object_name, the_object.dimensions, the_object.xyz, the_object.frame) - elif the_object.type == 'object': - self.arm_commander.add_object_to_scene(object_name, the_object.model_file, the_object.dimensions, the_object.xyz, the_object.rpy, the_object.frame) + if the_link.type == 'box': + self.arm_commander.add_box_to_scene(scene_name, the_link.dimensions, the_link.xyz, the_link.rpy, the_link.parent_frame) + elif the_link.type == 'sphere': + self.arm_commander.add_sphere_to_scene(scene_name, the_link.dimensions, the_link.xyz, the_link.parent_frame) + elif the_link.type == 'object': + self.arm_commander.add_object_to_scene(scene_name, the_link.model_file, the_link.dimensions, the_link.xyz, the_link.rpy, the_link.parent_frame) else: - logger.warning(f'TaskTreesManager (_define_objects): unrecognize object type "{the_object.type}"') + logger.warning(f'TaskTreesManager (_define_objects): unrecognize object type "{the_link.type}"') def _define_custom_frames(self, the_scene:Scene, include_list:list=None, ignore_list:list=None): """ Define the custom frames in the arm commander as specified in the scene configuration file @@ -382,8 +369,8 @@ def _define_custom_frames(self, the_scene:Scene, include_list:list=None, ignore_ continue if frame_name in ignore_list: continue - the_frame = the_scene.get_frame_config(frame_name) - self.arm_commander.add_custom_transform(frame_name, the_frame.xyz, the_frame.rpy, the_frame.parent) + the_frame_link = the_scene.get_link_of_frame(frame_name) + self.arm_commander.add_custom_transform(frame_name, the_frame_link.xyz, the_frame_link.rpy, the_frame_link.parent_frame) def _set_initialize_branch(self, branch:Composite): """ Set or replace a branch to be the initialization branch of the behaviour tree of this manager diff --git a/tools/__init__.py b/task_trees/tools/__init__.py similarity index 100% rename from tools/__init__.py rename to task_trees/tools/__init__.py diff --git a/tools/logging_tools.py b/task_trees/tools/logging_tools.py similarity index 100% rename from tools/logging_tools.py rename to task_trees/tools/logging_tools.py diff --git a/tools/pose_tools.py b/task_trees/tools/pose_tools.py similarity index 100% rename from tools/pose_tools.py rename to task_trees/tools/pose_tools.py diff --git a/tools/rospkg_tools.py b/task_trees/tools/rospkg_tools.py similarity index 100% rename from tools/rospkg_tools.py rename to task_trees/tools/rospkg_tools.py diff --git a/tools/rviz_tools.py b/task_trees/tools/rviz_tools.py similarity index 60% rename from tools/rviz_tools.py rename to task_trees/tools/rviz_tools.py index 3633ebd..5a5080b 100644 --- a/tools/rviz_tools.py +++ b/task_trees/tools/rviz_tools.py @@ -20,15 +20,18 @@ from collections import defaultdict import cv2 import numpy as np -import rospy, tf +import rospy, tf, tf2_ros +from std_msgs.msg import Header from tf2_msgs.msg import TFMessage from std_msgs.msg import ColorRGBA, Header -from geometry_msgs.msg import Pose, PoseStamped, Twist, TwistStamped, Vector3, Point +from geometry_msgs.msg import Pose, PoseStamped, Twist, TwistStamped, Vector3, Point, Quaternion from sensor_msgs.msg import PointCloud2, PointField from sensor_msgs import point_cloud2 -from visualization_msgs.msg import Marker -from tools.pose_tools import list_to_pose -from tools.logging_tools import logger +from visualization_msgs.msg import Marker, MarkerArray +from task_trees.tools.pose_tools import list_to_pose, pose_to_xyzq +from task_trees.tools.rospkg_tools import PackageFile +from task_trees.tools.logging_tools import logger +import task_trees.tools.pose_tools as pose_tools class RGBAColors(int, Enum): """ Define common use colours for visualization @@ -42,6 +45,12 @@ def __new__(cls, value, rgba='...'): obj._value_ = value obj.rgba = rgba return obj + @staticmethod + def validate_rgba(rgba): + rgba = RGBAColors.RED.rgba if rgba is None else rgba + if type(rgba) in (list, tuple) and len(rgba) == 3: + rgba.append(1) + return rgba def _create_marker(name:str, id:int, reference_frame:str=None, lifetime=rospy.Duration(0.0)) -> Marker: """ Create a Marker object @@ -59,6 +68,9 @@ def _create_marker(name:str, id:int, reference_frame:str=None, lifetime=rospy.Du if name is not None: the_marker.ns = f'{name}' the_marker.id = id + lifetime = rospy.Duration(0.0) if lifetime is None else lifetime + if type(lifetime) in (float, int): + lifetime = rospy.Duration(lifetime) the_marker.lifetime = lifetime return the_marker @@ -122,7 +134,7 @@ def create_axisplane_marker(name:str, id:int, bbox2d:list, offset:float, referen else: logger.warning(f'create_2dregion_marker: invalid plane parameter {axes}') return None - rgba = RGBAColors.RED.rgba if rgba is None else rgba + rgba = RGBAColors.validate_rgba(rgba) the_marker.color = ColorRGBA(*rgba) return the_marker @@ -143,7 +155,7 @@ def create_cube_marker_from_bbox(name:str, id:int, bbox3d:list, reference_frame: xyz = [(bbox3d[0] + bbox3d[3]) / 2, (bbox3d[1] + bbox3d[4]) / 2, (bbox3d[2] + bbox3d[5]) / 2] the_marker.pose = list_to_pose(xyz + rpy) the_marker.scale = Vector3(bbox3d[3] - bbox3d[0], bbox3d[4] - bbox3d[1], bbox3d[5] - bbox3d[2]) - rgba = RGBAColors.RED.rgba if rgba is None else rgba + rgba = RGBAColors.validate_rgba(rgba) the_marker.color = ColorRGBA(*rgba) return the_marker @@ -164,7 +176,7 @@ def create_cube_marker_from_xyzrpy(name:str, id:int, xyzrpy:list, reference_fram if isinstance(dimensions, numbers.Number): dimensions = [dimensions, dimensions, dimensions] the_marker.scale = Vector3(*dimensions) - rgba = RGBAColors.RED.rgba if rgba is None else rgba + rgba = RGBAColors.validate_rgba(rgba) the_marker.color = ColorRGBA(*rgba) return the_marker @@ -184,9 +196,9 @@ def create_arrow_marker(name:str, id:int, xyzrpy:list, reference_frame:str, dime the_marker.type = Marker().ARROW the_marker.pose = list_to_pose(xyzrpy) if isinstance(dimensions, numbers.Number): - dimensions = [dimensions, dimensions/10, dimensions/10] + dimensions = [dimensions, dimensions/10, dimensions/25] the_marker.scale = Vector3(*dimensions) - rgba = RGBAColors.RED.rgba if rgba is None else rgba + rgba = RGBAColors.validate_rgba(rgba) the_marker.color = ColorRGBA(*rgba) return the_marker @@ -208,7 +220,7 @@ def create_line_marker(name:str, id:int, xyz1:list, xyz2:list, reference_frame:s the_marker.pose = list_to_pose([0, 0, 0, 0, 0, 0]) the_marker.points[:] = [Point(*xyz1), Point(*xyz2)] the_marker.scale.x = line_width - rgba = RGBAColors.RED.rgba if rgba is None else rgba + rgba = RGBAColors.validate_rgba(rgba) the_marker.color = ColorRGBA(*rgba) return the_marker @@ -269,7 +281,7 @@ def create_sphere_marker(name:str, id:int, xyz:list, reference_frame:str, dimens if isinstance(dimensions, numbers.Number): dimensions = [dimensions, dimensions, dimensions] the_marker.scale = Vector3(*dimensions) - rgba = RGBAColors.RED.rgba if rgba is None else rgba + rgba = RGBAColors.validate_rgba(rgba) the_marker.color = ColorRGBA(*rgba) return the_marker @@ -292,7 +304,7 @@ def create_cylinder_marker(name:str, id:int, xyzrpy:list, reference_frame:str, d logger.warning(f'create_cylinder_marker: dimensions should be a list of 3 numbers (radius, radius, height)') return None the_marker.scale = Vector3(*dimensions) - rgba = RGBAColors.RED.rgba if rgba is None else rgba + rgba = RGBAColors.validate_rgba(rgba) the_marker.color = ColorRGBA(*rgba) return the_marker @@ -315,17 +327,17 @@ def create_text_marker(name:str, id:int, text:str, xyzrpy:list, reference_frame: if isinstance(dimensions, numbers.Number): dimensions = [dimensions, dimensions, dimensions] the_marker.scale = Vector3(*dimensions) - rgba = RGBAColors.RED.rgba if rgba is None else rgba + rgba = RGBAColors.validate_rgba(rgba) the_marker.color = ColorRGBA(*rgba) the_marker.text = text return the_marker -def create_mesh_marker(name:str, id:int, filename:str, xyzrpy:list, reference_frame:str, dimensions:list=0.5, rgba:list=None, lifetime=rospy.Duration(0.0)) -> Marker: +def create_mesh_marker(name:str, id:int, file_uri:str, xyzrpy:list, reference_frame:str, dimensions:list=0.5, rgba:list=None, lifetime=rospy.Duration(0.0)) -> Marker: """ Creates a marker for displaying a mesh object :param name: the name space of the marker :param id: the id of the marker - :param filename: the file containing a binary STL or DAE file + :param file_uri: the full path to the file containing a binary STL or DAE file or using protocols such as file://, package://, or http:// :param xyzrpy: the pose of the text as a list of 6 :param reference_frame: the reference frame, defaults to None :param dimensions: the scale factor of the mesh object, defaults to [1, 1, 1] @@ -340,21 +352,26 @@ def create_mesh_marker(name:str, id:int, filename:str, xyzrpy:list, reference_fr logger.warning(f'create_mesh_marker: dimensions should be a list of 3 numbers (radius, radius, height)') return None the_marker.scale = Vector3(*dimensions) - rgba = RGBAColors.RED.rgba if rgba is None else rgba + rgba = RGBAColors.validate_rgba(rgba) the_marker.color = ColorRGBA(*rgba) - the_marker.mesh_resource = filename + try: + file_uri = PackageFile.resolve_to_file_or_http_uri(file_uri) + except Exception as ex: + logger.warning(f'create_mesh_marker: Invalid model_file for object ({file_uri}): {ex}') + return + the_marker.mesh_resource = file_uri the_marker.mesh_use_embedded_materials = True return the_marker -def create_pointcloud_from_image(image_bgr, xyz:list=(0, 0, 0), pixel_physical_size:float=0.005, reference_frame=None, opacity=255, depth_array=None) -> PointCloud2: +def create_pointcloud_from_image(image_bgr:np.ndarray, xyz:list=(0, 0, 0), pixel_physical_size:float=0.005, reference_frame=None, opacity=255, depth_array:np.ndarray=None) -> PointCloud2: """ Create a PointCloud2 for displaying a OpenCV image (color or greyscale) - :param image_bgr: the image to be displayed, type numpy array + :param image_bgr: the image to be displayed, type numpy ndarray :param xyz: the position of the bottom left hand corner of the image, defaults to (0, 0, 0) :param pixel_physical_size: the length of each pixel in x, y, defaults to [0.005, 0.005], and optionally the third value in the list for z scaling factor :param reference_frame: the reference frame, defaults to None :param opacity: the opacity of the displayed image, defaults to 255 - :param depth_array: optionally a numpy array of exact the same shape as the image indicating the depth, defaults to None + :param depth_array: optionally a numpy ndarray of exact the same shape as the image indicating the depth, defaults to None :return: the PointCloud2 object """ if image_bgr is None: @@ -419,7 +436,7 @@ class RvizVisualizer(): """ A publisher of markers, which handles persistent markers, which is published repeatedly and temporary markers, which are published once. """ - def __init__(self, **kwargs): + def __init__(self, **config_dict): """ The constructur :param pub_period_marker: the default period of publishing marker, defaults to 1.0 second @@ -427,101 +444,240 @@ def __init__(self, **kwargs): :param topic_marker: the topic used to publish markers, defaults to visualization_marker :param topic_cloud: the topic used to publish point cloud, defaults to visualization_cloud """ - self.lock = threading.Lock() - # the constant defining the timer period, the actual rate of publishing depends on the individual markers - TIMER_PERIOD = 0.1 + self.lock = threading.RLock() + # constant + # the time delay publishing the temp marker necessary for RViz to subscribe and read + self.TEMP_MAKRER_PUB_DELAY = config_dict.get('pub_temp_marker_delay', 0.2) # set default values for keyword argument - self.default_pub_period_marker = kwargs.get('pub_period_marker', 1.0) - topic_marker = kwargs.get('topic_marker', '/visualization_marker') - self.default_pub_period_cloud = kwargs.get('pub_period_cloud', 1.0) - topic_cloud = kwargs.get('topic_cloud', '/visualization_cloud') + self.default_pub_period_marker = config_dict.get('pub_period_marker', 1.0) + topic_marker = config_dict.get('topic_marker', '/visualization_marker') + topic_marker_array = config_dict.get('topic_marker_array', '/visualization_marker_array') + + self.default_pub_period_cloud = config_dict.get('pub_period_cloud', 1.0) + topic_cloud = config_dict.get('topic_cloud', '/visualization_cloud') + self.default_pub_period_tf = config_dict.get('pub_period_tf', 0.1) + logger.info(f'RvizVisualizer: publishing markers to "{topic_marker}" (max rate {1 / self.default_pub_period_marker:.2f} Hz)') + logger.info(f'RvizVisualizer: publishing marker arrays to "{topic_marker_array}" (max rate {1 / self.default_pub_period_marker:.2f} Hz)') + logger.info(f'RvizVisualizer: publishing pointclouds to "{topic_cloud}" (max rate {1 / self.default_pub_period_cloud:.2f} Hz)') # the storage for markers - self.marker_dict = defaultdict(lambda: None) + self.markers_dict = defaultdict(lambda: None) # (marker_namespace, marker_id) -> dict + self.marker_arrays_list = [] self.temp_marker_list = [] - self.pointcloud_dict = defaultdict(lambda: None) + self.pointclouds_dict = defaultdict(lambda: None) # pointcloud_name -> dict + # setup tf publish + self.tf_broadcaster = tf2_ros.TransformBroadcaster() + self.tfs_dict = defaultdict(lambda: None) # frame_name -> dict # setup marker publisher self.marker_pub = rospy.Publisher(topic_marker, Marker, queue_size = 100) - self.cloud_pub = rospy.Publisher(topic_cloud, PointCloud2, queue_size = 2) + self.marker_array_pub = rospy.Publisher(topic_marker_array, MarkerArray, queue_size = 100) + self.cloud_pub = rospy.Publisher(topic_cloud, PointCloud2, queue_size = 20) # setup timer - self.timer_marker_viz = rospy.Timer(rospy.Duration(TIMER_PERIOD), self._cb_timer_marker_viz) - self.timer_cloud_viz = rospy.Timer(rospy.Duration(TIMER_PERIOD), self._cb_timer_cloud_viz) + self.timer_marker_viz = rospy.Timer(rospy.Duration(self.default_pub_period_marker), self._cb_timer_marker_viz) + self.timer_cloud_viz = rospy.Timer(rospy.Duration(self.default_pub_period_cloud), self._cb_timer_cloud_viz) + self.timer_tf = rospy.Timer(rospy.Duration(self.default_pub_period_tf), self._cb_timer_tf) def _cb_timer_marker_viz(self, event): """ internal callback function :meta private: """ with self.lock: - # publish the persistent markers of which the pub_period has lapsed - for key in self.marker_dict.keys(): - marker_dict = self.marker_dict.get(key) + # publish the persistent markers of which the pub_period has lapsed + current_time = time.time() + for key in self.markers_dict.keys(): + marker_dict = self.markers_dict.get(key) marker = marker_dict['marker'] - current_time = time.time() - if marker_dict['last_time'] is None or current_time - marker_dict['last_time'] >= marker_dict['pub_period']: + if marker_dict['next_time'] is None or current_time >= marker_dict['next_time']: self.marker_pub.publish(marker) - marker_dict['last_time'] = current_time + marker_dict['next_time'] = current_time + marker_dict['pub_period'] + # publish the persistent marker arrays + for index, marker_array_dict in enumerate(self.marker_arrays_list): + marker_array = marker_array_dict['marker_array'] + if marker_array_dict['next_time'] is None or current_time >= marker_array_dict['next_time']: + self.marker_array_pub.publish(marker_array) + marker_array_dict['next_time'] = current_time + marker_array_dict['pub_period'] # publish the temporary markers - for marker in self.temp_marker_list: - self.marker_pub.publish(marker) - self.temp_marker_list.clear() + for marker_dict in list(self.temp_marker_list): + marker = marker_dict['marker'] + if current_time > marker_dict['pub_time']: + self.marker_pub.publish(marker) + self.temp_marker_list.remove(marker_dict) def _cb_timer_cloud_viz(self, event): """ internal callback function :meta private: """ with self.lock: - for pointcloud_dict in self.pointcloud_dict.values(): + for pointcloud_dict in self.pointclouds_dict.values(): pointcloud = pointcloud_dict['pointcloud'] current_time = time.time() - if pointcloud_dict['last_time'] is None or current_time - pointcloud_dict['last_time'] >= pointcloud_dict['pub_period']: + if pointcloud_dict['next_time'] is None or current_time >= pointcloud_dict['next_time']: pointcloud.header.stamp = rospy.Time.now() self.cloud_pub.publish(pointcloud) - pointcloud_dict['last_time'] = current_time + pointcloud_dict['next_time'] = current_time + pointcloud_dict['pub_period'] - def add_persistent_marker(self, marker:Marker, pub_period:float=None) -> Marker: + def _cb_timer_tf(self, event): + """ internal callback function + :meta private: + """ + with self.lock: + for custom_tf in self.tfs_dict.values(): + name, parent_frame, pose = custom_tf['frame'], custom_tf['parent_frame'], custom_tf['pose'] + # xyzq = pose_to_xyzq(pose) + # self.tf_pub.sendTransform(xyzq[:3], xyzq[3:], rospy.Time.now(), name, parent_frame) + self._pub_transform(name, pose, parent_frame) + + # internal function: publish the transform of a specific named object + def _pub_transform(self, name:str, pose, frame=None): + """ publish the transform of an object + + :param name: name of the object + :type name: str + :param pose: the pose of the object + :type pose: Pose, PoseStamped, list of 6 or 7 + :param frame: the frame against which the pose is defined, ignored if PoseStamped is provided, defaults to None + :type frame: str, optional + """ + frame = self.base_frame if frame is None else frame + if type(pose) in [list, tuple]: + pose_stamped = pose_tools.list_to_pose_stamped(pose, frame) + elif type(pose) == Pose: + pose_stamped = PoseStamped() + pose_stamped.header.frame_id = frame + pose_stamped.header.stamp = rospy.Time.now() + pose_stamped.pose = pose + elif type(pose) == PoseStamped: + frame = pose.header.frame_id + pose_stamped = pose + else: + rospy.logerr(f'{__class__.__name__}: parameter (pose) is not list of length 6 or 7 or a Pose object -> fix the parameter at behaviour construction') + raise TypeError(f'A parameter is invalid') + self.tf_broadcaster.sendTransform(pose_tools.pose_stamped_to_transform_stamped(pose_stamped, name)) + + def add_persistent_marker(self, marker:Marker, pub_period:float=None, pub_tf:bool=False) -> Marker: """ Add a persistent marker :param marker: A marker to be persistently published :param pub_period: The rate of publishing, which cannot be smaller than 0.1 seconds + :param pub_tf: if True, the pose of the marker is published as a tf frame :return: The mrker """ + assert marker is not None, 'RvizVisualizer (add_persistent_marker): Parameter (marker) cannot be None' with self.lock: pub_period = self.default_pub_period_marker if pub_period is None else pub_period pub_period = 0.1 if pub_period < 0.1 else pub_period - self.marker_dict[marker.ns, marker.id] = {'marker': marker, 'pub_period': pub_period, 'last_time': None} + self.markers_dict[marker.ns, marker.id] = {'marker': marker, 'pub_period': pub_period, 'next_time': None, 'pub_tf': pub_tf} + if pub_tf: + # self.add_custom_tf(f'{marker.ns}.{marker.id}', marker.header.frame_id, marker.pose) + self.add_custom_tf(f'{marker.ns}.{marker.id}', marker.header.frame_id, marker.pose) return marker def pub_temporary_marker(self, marker:Marker) -> Marker: - """ Add a temporary marker + """ Add a temporary marker, which is to be published only once :param marker: A marker to be published only once - :return: The mrker + :return: The marker + """ + assert marker is not None, 'RvizVisualizer (pub_temporary_marker): Parameter (marker) cannot be None' + with self.lock: + self.temp_marker_list.append({'marker': marker, 'pub_time': time.time() + self.TEMP_MAKRER_PUB_DELAY}) + return marker + + def add_persistent_marker_array(self, marker_array:MarkerArray, pub_period:float=None) -> Marker: + """ Add a persistent marker array + + :param marker_array: A marker array to be persistently published + :param pub_period: The rate of publishing, which cannot be smaller than 0.1 seconds + :param pub_tf: if True, the pose of the marker is published as a tf frame + :return: The index of the marker array """ + assert marker_array is not None, 'RvizVisualizer (add_persistent_marker_array): Parameter (marker_array) cannot be None' with self.lock: - self.temp_marker_list.append(marker) - return marker + pub_period = self.default_pub_period_marker if pub_period is None else pub_period + pub_period = 0.1 if pub_period < 0.1 else pub_period + self.marker_arrays_list.append({'marker_array': marker_array, 'pub_period': pub_period, 'next_time': None}) + return len(self.marker_arrays_list) - 1 + + def add_custom_tf(self, name:str, parent_frame:str, pose:Pose) -> None: + """ Add a custom transform to the rviz visualizer, which is broadcast regularly + + :param name: the name of the transform + :param xyz: the xyz pose + :param rpy: the rpy pose + :param frame: the reference frame + """ + if name is None or parent_frame is None or pose is None: + raise AssertionError(f'RvizVisualizer (add_custom_tf): No parameter can be None') + self.tfs_dict[name] = {'pose': pose, 'frame':name, 'parent_frame': parent_frame} - def delete_all_persistent_markers(self, frame) -> None: + def delete_all_persistent_markers(self) -> None: """ Remove all persistent markers from RViz and this object - :param frame: The reference frame """ with self.lock: - self.marker_dict.clear() - self.temp_marker_list.append(create_delete_all_marker(frame)) + # remove the tf associated with markers with pub_tf True + for marker_dict in self.markers_dict.values(): + if marker_dict['pub_tf']: + marker:Marker = marker_dict['marker'] + self.temp_marker_list.append({'marker': create_delete_marker(marker.ns, marker.id, marker.header.frame_id), + 'pub_time': time.time() + self.TEMP_MAKRER_PUB_DELAY}) + tf_frame = f'{marker.ns}.{marker.id}' + if tf_frame in self.tfs_dict: + del self.tfs_dict[tf_frame] + self.markers_dict.clear() + - def delete_persistent_markers(self, name, id, frame) -> None: + def delete_persistent_marker(self, name:str, id) -> None: """ Remove a persistent marker from RViz and this object :param name: the name space of the marker :param id: the id of the marker :param frame: the reference frame """ + assert name is not None and id is not None, \ + 'RvizVisualizer (delete_persistent_markers): Parameter (any) cannot be None' with self.lock: - if (name, id) in self.marker_dict: - del self.marker_dict[name, id] - self.temp_marker_list.append(create_delete_marker(name, id, frame)) + if (name, id) in self.markers_dict: + marker:Marker = self.markers_dict[name, id]['marker'] + del self.markers_dict[name, id] + self.temp_marker_list.append({'marker': create_delete_marker(name, id, marker.header.frame_id), + 'pub_time': time.time() + self.TEMP_MAKRER_PUB_DELAY}) + tf_frame = f'{name}.{id}' + if tf_frame in self.tfs_dict: + del self.tfs_dict[tf_frame] + + def delete_all_persistent_marker_arrays(self) -> None: + """ Remove all persistent marker arrays from RViz and this object - def add_pointcloud(self, name, pointcloud:PointCloud2, pub_period:float=None) -> PointCloud2: + """ + with self.lock: + num_marker_array = len(self.marker_arrays_list) + for _ in range(num_marker_array): + self.delete_persistent_marker_array(index=0) + + def delete_persistent_marker_array(self, index:int) -> None: + """ Remove a persistent marker from RViz and this object + + :param name: the name space of the marker + :param id: the id of the marker + :param frame: the reference frame + """ + assert index is not None, \ + 'RvizVisualizer (delete_persistent_marker_array): Parameter (any) cannot be None' + with self.lock: + if 0 <= index < len(self.marker_arrays_list): + marker_array:MarkerArray = self.marker_arrays_list[index]['marker_array'] + self.marker_arrays_list.pop(index) + marker:Marker = None + for marker in marker_array.markers: + self.temp_marker_list.append({'marker': create_delete_marker(marker.ns, marker.id, marker.header.frame_id), + 'pub_time': time.time() + self.TEMP_MAKRER_PUB_DELAY}) + return marker_array + return None + + def clear_all_markers_in_rviz(self, frame:str): + self.temp_marker_list.append({'marker': create_delete_all_marker(frame), 'pub_time': time.time() + self.TEMP_MAKRER_PUB_DELAY}) + + def add_pointcloud(self, name:str, pointcloud:PointCloud2, pub_period:float=None) -> PointCloud2: """ Add a PointCloud2 object for regular publishing :param name: the name of the pointcloud of type str @@ -530,43 +686,80 @@ def add_pointcloud(self, name, pointcloud:PointCloud2, pub_period:float=None) -> :return: the point cloud input parameter """ with self.lock: - pub_period = self.default_pub_period_marker if pub_period is None else pub_period + pub_period = self.default_pub_period_cloud if pub_period is None else pub_period pub_period = 0.1 if pub_period < 0.1 else pub_period - self.pointcloud_dict[name] = {'pointcloud': pointcloud, 'pub_period': pub_period, 'last_time': None} + self.pointclouds_dict[name] = {'pointcloud': pointcloud, 'pub_period': pub_period, 'next_time': None} return pointcloud - def delete_pointcloud(self, name) -> PointCloud2: + def delete_pointcloud(self, name:str) -> PointCloud2: """ Remove a pointcloud from regular publishing :param name: the name of the pointcloud of type str :return: the PointCloud2 object removed """ with self.lock: - if name in self.pointcloud_dict: - pointcloud = self.pointcloud_dict[name] - del self.pointcloud_dict[name] + if name in self.pointclouds_dict: + pointcloud = self.pointclouds_dict[name] + del self.pointclouds_dict[name] return pointcloud return None +# -------------------------------------------- +# -- Animation classes +class PoseAnimator(): + def __init__(self, pose): + self.pose:Pose = pose + # -- the test program if __name__ == '__main__': rospy.loginfo(f'running test_rv_node') rospy.init_node('test_rv_node', anonymous=False) # test the mark visualization rv = RvizVisualizer() - text_marker = rv.add_persistent_marker(create_text_marker('text', 1, 'Hello', [0, 0, 0, 0.2, 0, 0], 'world', 0.3)) + the_pose:Pose = Pose() + the_pose.position = Point(0, 0, 0) + the_pose.orientation = Quaternion(0, 0, 0, 1) + # create world frame + rv.add_custom_tf('world', 'map', the_pose) + + text_marker_1 = rv.add_persistent_marker(create_text_marker(name='text', id=1, text='Hello', xyzrpy=[0, 0, 0, 0.2, 0, 0], reference_frame='world', dimensions=0.3), pub_tf=True) + text_marker_2 = rv.add_persistent_marker(create_text_marker(name='text', id=2, text='World', xyzrpy=[0, 1, 0, 0.2, 0, 0], reference_frame='world', dimensions=0.3), pub_tf=True) rv.add_persistent_marker(create_line_marker('line', 1, [1, 0, 0], [0, 0, 1], 'world', 0.01, rgba=[0.0, 1.0, 1.0, 1.0]), pub_period=0.1) rv.add_persistent_marker(create_sphere_marker('sphere', 1, [1, 1, 1], 'world', 0.05, rgba=[0.5, 1.0, 1.0, 1.0])) rv.pub_temporary_marker(create_arrow_marker('arrow', 1, [1.0, 0.0, 0.0, 1.0, 0.0, 0.0], 'world', lifetime=rospy.Duration(3.0))) + # delete the line marker rospy.sleep(rospy.Duration(2.0)) - # rv.delete_all_persistent_markers('world') - # for i in range(100): - # pose = text_marker.pose - # pose.position.x += random.uniform(-0.5, 0.5) - # rospy.sleep(rospy.Duration(0.2)) - # rv.delete_persistent_markers('text', 1, 'world') - # test the image visualization - image_bgr = cv2.imread(os.path.join(os.path.dirname(__file__), '../demos/rviz_display/bird.png')) - pc2_message = create_pointcloud_from_image(image_bgr, (1, 0.5, 1), pixel_physical_size=[0.002, 0.002, -1], reference_frame='world') + rv.delete_persistent_marker('line', 1) + # delete all markers + rospy.sleep(rospy.Duration(2.0)) + rv.delete_all_persistent_markers() + # display stl mesh file + teapot_mesh = os.path.join(os.path.dirname(__file__), '../../demos/rviz_display/utah_teapot.stl') + teapot_mesh = 'file://' + teapot_mesh + rv.add_persistent_marker(create_mesh_marker('teapot', 1, teapot_mesh, [-1.0, -1.0, 0.0, 0, 0, 0], 'world', [0.05, 0.05, 0.05], rgba=[0.5, 1.0, 1.0, 1.0])) + # display image as pointcloud + image_bgr = cv2.imread(os.path.join(os.path.dirname(__file__), '../../demos/rviz_display/bird.png')) + pc2_message = create_pointcloud_from_image(image_bgr, (0, 0.5, 0), pixel_physical_size=[0.002, 0.002, -1], reference_frame='world') rv.add_pointcloud('the_image', pc2_message) + # add the text marker + rospy.sleep(rospy.Duration(2.0)) + text_marker_1 = rv.add_persistent_marker(create_text_marker('text', 1, 'Hello', [0, 0, 0, 0.2, 0, 0], 'world', 0.3), pub_period=0.1, pub_tf=True) + for i in range(100): + pose = text_marker_1.pose + pose.position.x += random.uniform(-0.5, 0.5) + rospy.sleep(rospy.Duration(0.2)) + # delete the text marker again + rv.delete_persistent_marker('text', 1) + # create marker array + marker_array = MarkerArray() + for x in range(4): + for y in range(4): + xyzrpy=[x * 0.4, y * 0.4, 1.0, 0, 0, 0] + tile = create_cube_marker_from_xyzrpy('tile', x + y * 4, xyzrpy, reference_frame='world', + dimensions=[0.3, 0.3, 0.05], rgba=[0.0, 0.2, 1.0, 0.5]) + marker_array.markers.append(tile) + rv.add_persistent_marker_array(marker_array) + rospy.sleep(rospy.Duration(5.0)) + rv.delete_all_persistent_marker_arrays() + rospy.spin() \ No newline at end of file