rewrite and varlidate the pose tools for pose conversion and comparis…

…on, consequential change to the task trees so that it uses its own tools instead of the moveit tools to achieve better decoupling, added pointcloud for displaying image, added custom transform function to scene_to_rviz.

Dockerfile

@@ -73,6 +73,9 @@ RUN source /opt/ros/${ROS_DISTRO}/setup.bash && \
     cd ${ARM_WS} && \
     catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3
 
+# install tk
+sudo apt-get install -y python3-tk
+
 # set locales
 RUN sudo locale-gen en_US.UTF-8  
 RUN sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8

demos/gridscan/behaviours_advanced.py

@@ -10,14 +10,12 @@
 __status__ = 'Development'
 
 import operator, yaml, os, math, random, copy, sys, signal, threading
-from collections import namedtuple, defaultdict
 import rospy
 from std_msgs.msg import Float32
 from py_trees.common import Status
 # robot control module
-from task_trees.behaviours_base import ConditionalBehaviour
 from task_scene_gridscan import GridScanScene
-from task_trees.tools import logger
+from tools.logging_tools import logger
 from demos.gridscan.behaviours_gridscan import DoMoveTankGrid
 
 # -----------------------------------------------------------------------------------------------------

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 task_trees.tools import logger
+from tools.logging_tools import logger
 
 # ----------------------------------------------------------------------
 # Custom Behaviour Classes

demos/gridscan/task_trees_manager_gridscan.py

@@ -17,16 +17,17 @@
 from py_trees.composites import Sequence, Parallel, Composite, Selector
 from moveit_msgs.msg import Constraints
 # robot control module
-import arm_commander.moveit_tools as moveit_tools
+import arm_commander.tools.moveit_tools as moveit_tools
 from task_scene_gridscan import GridScanScene
 from task_trees.behaviours_move import DoMoveNamedPose, DoMoveXYZ, DoRotate, DoMoveXYZRPY
 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 task_trees.tools import logger
 from demos.gridscan.behaviours_gridscan import SimCalibrate, DoMoveTankGrid
 from demos.gridscan.behaviours_advanced import DoMoveTankGridVisualCDROS
-from task_trees.scene_to_rviz import SceneToRViz
+
+from tools.logging_tools import logger
+import tools.pose_tools as pose_tools
 # -------------------------------------
 # Tasks specialized for this application 
 
@@ -162,7 +163,7 @@ def task_is_timeout(self, duration=30):
 
     def in_a_region(self, region) -> bool:
         current_pose = self.arm_commander.pose_of_robot_link()
-        return moveit_tools.in_region(current_pose.pose.position.x, current_pose.pose.position.y, 
+        return pose_tools.in_region(current_pose.pose.position.x, current_pose.pose.position.y, 
                                     self.the_scene.query_config(region))  
 
     def on_or_above_z(self, position) -> bool:
@@ -177,7 +178,7 @@ def wrong_orientation(self) -> bool:
         target_xyzrpy = self._get_task_target_xyzrpy()
         current_xyzrpy = self.arm_commander.pose_in_frame_as_xyzrpy(reference_frame='tank')
         # logger.info(f'same orientation: {target_xyzrpy[3:]} {current_xyzrpy[3:]}')
-        return not moveit_tools.same_rpy_with_tolerence(target_xyzrpy[3:], current_xyzrpy[3:], 0.1)
+        return not pose_tools.same_rpy_with_tolerence(target_xyzrpy[3:], current_xyzrpy[3:], 0.1)
 
     def wrong_xy_at_tank(self) -> bool:
         target_xyzrpy = self._get_task_target_xyzrpy()
@@ -197,7 +198,7 @@ def at_angle(self, rotation_pose) -> bool:
 
     def at_a_named_pose(self, named_pose) -> bool:
         joint_values = self.arm_commander.current_joint_positons_as_list()
-        result = moveit_tools.same_joint_values_with_tolerence(self.the_scene.query_config(named_pose), joint_values, 0.05)
+        result = pose_tools.same_joint_values_with_tolerence(self.the_scene.query_config(named_pose), joint_values, 0.05)
         # logger.info(f'at_home_pose: {result}')
         return result    
 

demos/pickndrop/behaviours_pnd.py

@@ -11,9 +11,9 @@
 
 from py_trees.common import Status
 # robot control module
-import arm_commander.moveit_tools as moveit_tools
+import arm_commander.tools.moveit_tools as moveit_tools
 from task_trees.behaviours_base import ConditionalBehaviour, ConditionalCommanderBehaviour
-from task_trees.tools import logger
+from tools.logging_tools import logger
 from scan_model import ScanRegionModel
 
 # ----------------------------------------------------------------------

demos/pickndrop/demo.py

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

demos/pickndrop/task_trees_manager_pnd.py

@@ -16,14 +16,13 @@
 from py_trees.composites import Sequence, Parallel, Composite, Selector
 # robot control module
 from arm_commander.commander_moveit import GeneralCommander, logger
-import arm_commander.moveit_tools as moveit_tools
 
 from task_trees.behaviours_base import SimAttachObject, SimDetachObject
 from task_trees.behaviours_move import DoMoveNamedPose, DoMoveXYZ
 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
 from scan_model import SingleLineScanModel, SteppingScanModel, FourCornersScanModel
 
@@ -210,19 +209,19 @@ def task_is_timeout(self, duration=30):
 
     def in_a_region(self, logical_region) -> bool:
         current_pose = self.arm_commander.pose_of_robot_link()
-        return moveit_tools.in_region(current_pose.pose.position.x, current_pose.pose.position.y, 
+        return pose_tools.in_region(current_pose.pose.position.x, current_pose.pose.position.y, 
                                     self.the_scene.query_config(logical_region)) 
 
     def over_the_table(self) -> bool:
         the_table = self.the_scene.get_object_config('the_table')
         the_table_position_as_bbox = [the_table.xyz[0] - the_table.dimensions[0] / 2, the_table.xyz[1] - the_table.dimensions[1] / 2,
                                     the_table.xyz[0] + the_table.dimensions[0] / 2, the_table.xyz[1] + the_table.dimensions[1] / 2]
         current_pose = self.arm_commander.pose_of_robot_link()
-        return moveit_tools.in_region(current_pose.pose.position.x, current_pose.pose.position.y, the_table_position_as_bbox)         
+        return pose_tools.in_region(current_pose.pose.position.x, current_pose.pose.position.y, the_table_position_as_bbox)         
 
     def at_a_named_pose(self, named_pose) -> bool:
         joint_values = self.arm_commander.current_joint_positons_as_list()
-        result = moveit_tools.same_joint_values_with_tolerence(self.the_scene.query_config(named_pose), joint_values, 0.05)
+        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    
 

demos/pickndrop_estop/task_trees_manager_pnd.py

@@ -18,7 +18,7 @@
 
 # robot control module
 from arm_commander.commander_moveit import GeneralCommander, logger
-import arm_commander.moveit_tools as moveit_tools
+import tools.pose_tools as pose_tools
 
 from task_trees.behaviours_base import SimAttachObject, SimDetachObject
 from task_trees.behaviours_move import DoMoveNamedPose, DoMoveXYZ
@@ -205,19 +205,19 @@ def task_is_timeout(self, duration=30):
 
     def in_a_region(self, logical_region) -> bool:
         current_pose = self.arm_commander.pose_of_robot_link()
-        return moveit_tools.in_region(current_pose.pose.position.x, current_pose.pose.position.y, 
+        return pose_tools.in_region(current_pose.pose.position.x, current_pose.pose.position.y, 
                                     self.the_scene.query_config(logical_region)) 
 
     def over_the_table(self) -> bool:
         the_table = self.the_scene.get_object_config('the_table')
         the_table_position_as_bbox = [the_table.xyz[0] - the_table.dimensions[0] / 2, the_table.xyz[1] - the_table.dimensions[1] / 2,
                                     the_table.xyz[0] + the_table.dimensions[0] / 2, the_table.xyz[1] + the_table.dimensions[1] / 2]
         current_pose = self.arm_commander.pose_of_robot_link()
-        return moveit_tools.in_region(current_pose.pose.position.x, current_pose.pose.position.y, the_table_position_as_bbox)         
+        return pose_tools.in_region(current_pose.pose.position.x, current_pose.pose.position.y, the_table_position_as_bbox)         
 
     def at_a_named_pose(self, named_pose) -> bool:
         joint_values = self.arm_commander.current_joint_positons_as_list()
-        result = moveit_tools.same_joint_values_with_tolerence(self.the_scene.query_config(named_pose), joint_values, 0.05)
+        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    
 

demos/pushblock/task_trees_manager_pushblock.py

@@ -17,14 +17,15 @@
 
 # robot control module
 from arm_commander.commander_moveit import GeneralCommander, logger
-import arm_commander.moveit_tools as moveit_tools
+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 the Pick-n-Drop 
 
@@ -83,7 +84,7 @@ def __init__(self, arm_commander:GeneralCommander, spin_period_ms:int=10):
         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, object_area1.frame)
+        self.scene_to_rviz.add_custom_transform('area', object_area1.xyz, object_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)
@@ -120,9 +121,9 @@ def generate_the_object(self):
         self.the_object['area'] = 'area_1'
         # transform the pose
         xyzrpy = the_block_config['xyz'] + the_block_config['rpy']
-        the_pose = moveit_tools.xyzrpy_to_pose_stamped(xyzrpy, self.the_object['area'])
+        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 = moveit_tools.pose_to_xyzrpy(the_pose.pose)
+        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:])
 
     # -----------------------------------------
@@ -193,14 +194,14 @@ def _create_intank_fix_rotate_constraint(self):
 
     def in_a_region(self, logical_region) -> bool:
         current_pose = self.arm_commander.pose_of_robot_link()
-        return moveit_tools.in_region(current_pose.pose.position.x, current_pose.pose.position.y, self.the_scene.query_config(logical_region))     
+        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]
         current_pose = self.arm_commander.pose_of_robot_link()
-        return moveit_tools.in_region(current_pose.pose.position.x, current_pose.pose.position.y, the_object_position_as_bbox)   
+        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()
@@ -231,11 +232,11 @@ def the_object_over_an_object(self, object_name) -> bool:
         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 moveit_tools.in_region(xyzrpy_of_the_object[0], xyzrpy_of_the_object[1], the_target_position_as_bbox)           
+        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 = moveit_tools.same_joint_values_with_tolerence(self.the_scene.query_config(named_pose), joint_values, 0.05)
+        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    
 

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 task_trees.tools import logger
+from tools.logging_tools import logger
 
 # -- Test cases specialized for the Push Block Demo
 class PushBlockROSClientDemo():

demos/task_moves/task_move_4_gui.py

@@ -15,7 +15,7 @@
 from graphics import *
 import rospy
 from std_msgs.msg import Int8
-from task_trees.tools import logger
+from tools.logging_tools import logger
 
 # Require the python Tkinter module
 # sudo apt install python3-tk

docs/source/TUT_MOVE_PYTREES.md

@@ -4,7 +4,7 @@ This tutorials illustrates examples of using the move behaviour classes for buil
 
 This demo requires the Panda robot model.
 
-[Source Code](../../demos/pytrees_moves/simple_move_1.py)
+[Source Code](https://github.com/REF-RAS/task_trees/tree/main/demos/pytrees_moves)
 
 ## Running the Demo Program
 

docs/source/TUT_MOVE_SCENE_PYTREES.md

@@ -4,7 +4,7 @@ This tutorials illustrates how to use scene configuration to enrich the target p
 
 This demo requires the Panda robot model.
 
-[Source Code](../../demos/pytrees_moves_scene/move_scene_1.py)
+[Source Code](https://github.com/REF-RAS/task_trees/tree/main/demos/pytrees_moves_scene)
 
 ## Running the Demo Program
 

docs/source/TUT_MOVE_SCENE_TASKTREES.md

@@ -4,7 +4,7 @@ This tutorials illustrates how to use scene configuration to enrich the target p
 
 This demo requires the Panda robot model.
 
-[Source Code](../../demos/simple_moves_scene/move_scene_1.py)
+[Source Code](https://github.com/REF-RAS/task_trees/tree/main/demos/simple_moves_scene)
 
 ## Running the Demo Program
 

docs/source/TUT_MOVE_TASKTREES.md

@@ -6,7 +6,7 @@ The task trees framework reduces the complexity in building behaviour trees by p
 
 This demo requires the Panda robot model.
 
-[Source Code](../../demos/simple_moves/simple_move_1.py)
+[Source Code](https://github.com/REF-RAS/task_trees/tree/main/demos/simple_moves)
 
 ## Running the Demo Program
 

docs/source/TUT_TASK_MOVE_TASKTREES.md

@@ -4,7 +4,7 @@ This tutorials illustrates how to define tasks for the task tree manager, and to
 
 This demo requires the Panda robot model.
 
-[Source Code](../../demos/task_moves/task_move_1.py)
+[Source Code](https://github.com/REF-RAS/task_trees/tree/main/demos/task_moves)
 
 ## Running the Demo Program
 

docs/source/index.rst

@@ -29,6 +29,7 @@ The Task Trees Framework
    TUT_TASK_MOVE_TASKTREES.md 
    AKNOWLEDGEMENT.md
    task_trees_modules.rst
+   tools_modules.rst
 
 Indices and tables
 ===================

docs/source/task_trees_modules.rst

@@ -1,12 +1,10 @@
 Task Trees API Reference
 ===============================================
 
-The API reference is currently incomplete.
+task\_trees.task\_trees\_manager module
+-----------------------------------------
 
-task\_trees.behaviours\_base module
----------------------------------------
-
-.. automodule:: task_trees.behaviours_base
+.. automodule:: task_trees.task_trees_manager
     :members:
     :undoc-members:
     :show-inheritance:
@@ -27,10 +25,26 @@ task\_trees.behaviours\_move\_sense module
     :undoc-members:
     :show-inheritance:
 
+task\_trees.behaviours\_base module
+---------------------------------------
+
+.. automodule:: task_trees.behaviours_base
+    :members:
+    :undoc-members:
+    :show-inheritance:
+
 task\_trees.task\_scene module
 ---------------------------------------
 
 .. automodule:: task_trees.task_scene
     :members:
     :undoc-members:
     :show-inheritance:
+
+task\_trees.scene\_to\_rviz module
+---------------------------------------
+
+.. automodule:: task_trees.scene_to_rviz
+    :members:
+    :undoc-members:
+    :show-inheritance:

docs/source/tools_modules.rst

@@ -0,0 +1,27 @@
+Supporting Tools API Reference
+===============================================
+
+tools.pose\_tools module
+---------------------------------------
+
+.. automodule:: tools.pose_tools
+    :members:
+    :undoc-members:
+    :show-inheritance:
+
+tools.rviz\_tools module
+---------------------------------------
+
+.. automodule:: tools.rviz_tools
+    :members:
+    :undoc-members:
+    :show-inheritance:
+
+tools.logging\_tools module
+--------------------------------------------
+
+.. automodule:: tools.logging_tools
+    :members:
+    :undoc-members:
+    :show-inheritance:
+

setup.py

@@ -7,7 +7,7 @@
 
 # fetch values from package.xml
 setup_args = generate_distutils_setup(
-    packages=['task_trees', 'demos'],
+    packages=['task_trees', 'demos', 'tools'],
     package_dir={'': '.'},
     python_requires='>=3.9',
     install_requires=req_packages