fixed the asset file loading issue add_object_to_scene in the arm_commander, both are accepting the same formats including absolute file path, relative package file path, and file, package, http uri. Removed the pycache file under arm_commander/tools from git cache. Added custom transform function. Updated the documentation accordingly.

Author: andrewluiqut

arm_commander/commander_moveit.py

@@ -14,6 +14,7 @@
 __status__ = 'Development'
 
 import sys, copy, threading, time, signal, math, traceback, timeit, numbers, logging
+from collections import defaultdict
 import rospy, tf
 from tf2_msgs.msg import TFMessage
 import moveit_commander
@@ -26,8 +27,10 @@
 from controller_manager_msgs.srv import SwitchController
 
 from arm_commander.tools.moveit_tools import MOVEIT_ERROR_CODE_MAP, GOAL_STATUS_MAP
+from arm_commander.tools.rospkg_tools import PackageFile
 from arm_commander.states import GeneralCommanderStates, ControllerState
 
+
 class GeneralCommander():
     """
     GeneralCommander: an interface for robot commander, with this class specifically interfacing
@@ -93,6 +96,8 @@ def __init__(self, moveit_group_name:str, world_link:str=None) -> None:
         self.wall_name_list = []
         # record the collision objects, including the walls, added to the commander and their color
         self.object_name_color_list = {}
+        # setup custom transforms
+        self.custom_transform_object_dict = defaultdict(lambda: None)
         # publisher for the planning scene
         self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
         self.tf_pub = tf.TransformBroadcaster()
@@ -124,14 +129,20 @@ def _pub_objects_color(self):
             color = self._set_object_color(object_name, *rgba)
             p.object_colors.append(color)
         self.scene_pub.publish(p)
-                
+
     # publish the transform of the collision objects
     def _pub_transform_all_objects(self):
+        # publish the collision objects 
         objects = self.scene.get_objects()
         for object_name in objects.keys():
             pose_of_object = objects[object_name].pose
             frame = objects[object_name].header.frame_id
             self._pub_transform_object(object_name, pose_of_object, frame)
+        # publish the custom transform objects
+        for name in self.custom_transform_object_dict:
+            the_transform = self.custom_transform_object_dict[name]
+            xyzrpy = the_transform['xyzrpy']
+            self._pub_transform_object(name, xyzrpy, the_transform['frame'])
 
     # publish the transform of a specific named object
     def _pub_transform_object(self, name, pose, frame=None) -> None:
@@ -1084,6 +1095,11 @@ def add_object_to_scene(self, object_name:str, model_file:str, object_scale:list
         reference_frame = self.WORLD_REFERENCE_LINK if reference_frame is None else reference_frame
         self.scene.remove_world_object(object_name)
         object_pose = conversions.list_to_pose_stamped(xyz + rpy, reference_frame)
+        try:
+            model_file = PackageFile.resolve_to_path(model_file)
+        except Exception as ex:
+            logger.warning(f'SceneToRViz (display_objects): Invalid model_file for object ({model_file}): {ex}')
+            return        
         self.scene.add_mesh(
                 object_name, object_pose,
                 model_file, object_scale
@@ -1297,6 +1313,49 @@ def _set_object_color(self, name, r, g, b, a = 0.9) -> ObjectColor:
         color.color.a = a
         return color
 
+    # --------------------------------
+    # functions for adding custom transforms
+    def add_custom_transform(self, name, xyz, rpy, parent_frame=None) -> 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 parent_frame: the reference frame
+        """
+        if name is None or xyz is None or rpy 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] = {'xyzrpy': xyz + rpy, 'frame': parent_frame}
+
+    def update_custom_transform_pose(self, name, xyz=None, rpy=None, parent_frame=None) -> None:
+        """ Update the pose of a custom transform
+
+        :param name: the name of the transform to be updated
+        :param xyz: the updated xyz, defaults to None meaning unchanged
+        :param rpy: the updated rpy, defaults to None meaning unchanged
+        :param frame: the updated frame, defaults to None meaning unchanged
+        """
+        transform_object = self.custom_transform_object_dict[name]
+        if transform_object is None:
+            logger.warning(f'__name__ (update_custom_transform_pose): the custom transform name {name} is not found')
+            return False
+        xyz = xyz if xyz is not None else transform_object['xyzrpy'][:3]
+        rpy = rpy if rpy is not None else transform_object['xyzrpy'][3:]
+        parent_frame = parent_frame if xyz is not None else transform_object['frame']
+        self.add_custom_transform(name, xyz, rpy, parent_frame)
+
+    def remove_custom_transform(self, name=None):
+        """ Clear a custom transform or all if name is 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]
+    
+
     # --------------------------------
     # functions for setting path constraints
 

arm_commander/tools/__pycache__/__init__.cpython-38.pyc

@@ -0,0 +1,108 @@
+# 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 math, os
+import rospy, tf, rospkg
+import tf.transformations 
+from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
+
+class PackageFile():
+    """ The class provides tools for dealing with the uri for ros resource retriever
+    """
+    
+    def resolve_to_file_uri(uri_or_path:str) -> str:
+        """ Converts a uri or a file path into a uri starting with file:// 
+
+        :param uri_or_path: a file, package or http uri or a full or package relative file path
+        :type uri_or_path: str
+        :return: the file prototcol uri of type str
+        """
+        if uri_or_path is None:
+            raise Exception(f'Parameter (uri_or_path) is None')
+        try:
+            resolved_path = PackageFile._parse(uri_or_path)
+            return 'file://' + resolved_path
+        except:
+            raise
+        
+    def resolve_to_file_or_http_uri(uri_or_path:str) -> str:
+        """ Converts a uri or a file path into a uri starting with file:// or http://
+
+        :param uri_or_path: a file, package or http uri or a full or package relative file path
+        :type uri_or_path: str
+        :return: the file prototcol uri
+        :rtype: str
+        """
+        if uri_or_path.startswith(('http://', 'https://')):
+            return uri_or_path
+        return PackageFile.resolve_to_file_uri(uri_or_path)
+
+    def resolve_to_path(uri_or_path:str) -> str:
+        """Converts a uri or a file path into a full local file path
+        
+        :param uri_or_path: a file, package or http uri or a full or package relative file path
+        :type uri_or_path: str
+        :return: the absolute file path
+        :rtype: str
+        """
+        if uri_or_path is None:
+            raise Exception(f'Parameter (uri_or_path) is None')
+        try:
+            resolved_path = PackageFile._parse(uri_or_path)
+            return resolved_path
+        except:
+            raise
+    
+    # -----------------------------------
+    # internal function: searching if the relative path leads to an existing file
+    def _search_rospkgs_for_file(path):
+        ros_package = rospkg.RosPack()
+        package_path_list = ros_package.get_ros_paths()      
+        for package_path in package_path_list:
+            candidate_path = os.path.join(package_path, path)
+            if os.path.isfile(candidate_path):
+                return candidate_path
+        return None
+    
+    # internal function: parse the input uri or file path
+    def _parse(uri_or_path):
+        if uri_or_path.startswith('file://localhost'):
+            path = uri_or_path[16:]  
+        elif uri_or_path.startswith('file://'):
+            path = uri_or_path[7:]
+        elif uri_or_path.startswith('package://'):
+            path = uri_or_path[10:]
+        elif uri_or_path.startswith(('http://', 'https://')):
+            raise Exception(f'Unsupported protocol: http or https')
+        elif uri_or_path.find('://') > 0:
+            raise Exception(f'Unsupported protocol in uri: {uri_or_path}')
+        else:
+            path = uri_or_path
+        if len(path) == 0:
+            raise Exception(f'Malformed uri or path: empty path')
+        if path[0] != '/': # path relative to package
+            path = PackageFile._search_rospkgs_for_file(path)
+            if path is None:
+                raise Exception('No file is found by prepending ros package directories to the partial path')
+        return path
+
+if __name__ == '__main__':
+    rospy.init_node('test_node')
+    # test conversion functions
+    tests = ['/home/qcr/arm_commander_ws/src/task_trees/demos/rviz_display/utah_teapot.stl',
+            'task_trees/demos/rviz_display/utah_teapot.stl',
+            'file:///home/qcr/arm_commander_ws/src/task_trees/demos/rviz_display/utah_teapot.stl',
+            'file://localhost/home/qcr/arm_commander_ws/src/task_trees/demos/rviz_display/utah_teapot.stl',
+            'package://task_trees/demos/rviz_display/utah_teapot.stl',
+            ]
+    for test in tests:
+        print(f'resolve_to_fileuri: {test}\n -> {PackageFile.resolve_to_file_uri(test)}')
+        print(f'resolve_to_path: {test}\n -> {PackageFile.resolve_to_path(test)}')              

arm_commander/tools/__pycache__/moveit_tools.cpython-38.pyc

@@ -68,7 +68,7 @@ The function `get_object` of the class `GeneralCommanderFactory` returns the __s
 ### Example: Essential Startup Sequence in Applications
 
 The following shows the essential `GeneralCommander` startup sequence in applications.
-```
+```python
 arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
 arm_commander.spin(spin_in_thread=True)
 arm_commander.wait_for_ready_to_move()
@@ -126,7 +126,7 @@ The following functions accepts multiple positions or poses in one move command.
 ### Example: Issuing an Asynchronous Move Command
 
 The following shows an example of issuing an asynchronous move command
-```
+```python
 arm_commander.move_to_position(x = -0.6, y = 0.2, wait=False)        
 while True:
     the_state = arm_commander.get_commander_state()
@@ -138,7 +138,7 @@ arm_commander.reset_state()
 ### Example: Issuing an Multi-Waypoints Move Command
 
 The following shows an example of issuing a move command comprising multiple waypoints.
-```
+```python
 xyzrpy_list = [(0.6, 0.0, 0.4, 3.14, 0, 0), 
             (0.6, 0.2, 0.5, 3.14, 0, 0), 
             (0.6, 0.2, 0.6, 3.14, 0, 1.58)]
@@ -173,6 +173,23 @@ elif the_state in [GeneralCommanderStates.ABORTED, GeneralCommanderStates.ERROR]
 | attach_object_to_end_effector | The object name | |
 | detach_all_from_end_effector | The object name | |
 
+The object mesh file accepts absolute file path or package relative file path.
+
+#### Example: Adding a collision object
+
+The following shows an example of issuing an asynchronous move command
+```python
+arm_commander.add_object_to_scene('the_teapot', '/path_to_file/utah_teapot.stl', [0.5, 1.2, 0.3], [0, 0, 3.14])
+```
+
+### Custom Transform in the Scene
+
+| Functions | Parameters | Remarks |
+| -------- | ---------- | ------- |
+| add_custom_transform | The name, xyz, rpy and the parent_frame of the custom transform | | 
+| update_custom_transform_pose | The updated xyz, rpy or the parent frame of the named custom transform | the xyz, rpy, and the parent frame are optional|
+| remove_custom_transform| The name of the custom transform | |
+
 ### Path Constraints
 
 | Functions | Parameters | Remarks |