revise tf broadcast to tf2_ros in pose queries functions, pose transf…

…orm functions, object transform boardcast functions in commander_moveit.py and add functions to pose_tools as a consequence, remove the use of GeneralCommanderFactory in all the example programs (the use of GeneralCommanderFactory is not recommended because the use case is not practical, added launch files for two different robots

README.md

@@ -24,12 +24,12 @@ Use the [Documentation Entry Point](http://REF-RAS.github.io/arm_commander) to b
 The arm commander presents a programming interface dedicated to robot arm manipulation and encapsulates useful but tedious programming components such as ROS and arm movement planning.  The arm commander can support the development of applications using or not using ROS. The following example uses the arm commander API to move the end-effector of the robot arm `panda_arm` to the position (0.6, 0.0, 0.4), and then move it to another position (0.4, 0.2, 0.4)
 
 ```
-from arm_commander.commander_moveit import GeneralCommander, GeneralCommanderFactory
+from arm_commander.commander_moveit import GeneralCommander
 
 class ArmCommanderMoveExample():
     def __init__(self):
         # create the General Commander and wait for it being ready to service move commands
-        arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
+        arm_commander: GeneralCommander = GeneralCommander('panda_arm')
         arm_commander.spin(spin_in_thread=True)
         arm_commander.wait_for_ready_to_move()
         # send two move commands one after another

arm_commander/tools/pose_tools.py

@@ -10,9 +10,9 @@
 __status__ = 'Development'
 
 import math
-import rospy, tf
+import rospy
 import tf.transformations 
-from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
+from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, TransformStamped, Transform, Vector3, PointStamped
 
 def list_to_pose(pose) -> Pose:
     """ Convert a pose in xyzrpy or xyzq format to Pose
@@ -90,6 +90,30 @@ def list_to_xyzrpy(pose) -> list:
     else:
         return pose[:3] + list(tf.transformations.euler_from_quaternion(pose[3:]))
 
+def transform_to_pose_stamped(transform:TransformStamped) -> PoseStamped:
+    """ Convert a TransformStamped object into PoseStamped object
+
+    """
+    point = Point(transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z)
+    quaternion = Quaternion(transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w)
+    return PoseStamped(transform.header, Pose(point, quaternion))
+
+def pose_stamped_to_transform_stamped(pose_stamped:PoseStamped, reference_frame:str) -> TransformStamped:
+    """ Convert a PoseStamped object into TransformStamped object
+
+    """
+    translation = Vector3(pose_stamped.pose.position.x, pose_stamped.pose.position.y, pose_stamped.pose.position.z)
+    transform = Transform(translation, pose_stamped.pose.orientation)
+    transform_stamped = TransformStamped(pose_stamped.header, reference_frame, transform)
+    return transform_stamped   
+
+def pose_stamped_to_point_stamped(pose_stamped:PoseStamped) -> PointStamped:
+    """ Convert a PoseStamped object into PointStamped object
+
+    """    
+    point_stamped = PointStamped(pose_stamped.header, pose_stamped.pose.position)
+    return point_stamped
+
 def same_rpy_with_tolerence(rpy_1:list, rpy_2:list, tolerance:float=0.01) -> bool:
     """ test if two euler's angles are the same
 

docs/source/API_SUMMARY.md

@@ -13,16 +13,6 @@ This page provides a summary of the API. Refer to the [Full API Reference](arm_c
 
 The **Arm Commander** is a Python object of the class `GeneralCommander` that represents the commander of a particular __arm__ or manipulation device. An application can use more than one `GeneralCommander` object if a robot arm platform comprising multiple manipulators.
 
-### Factory for Creating Arm Commander
-
-The function `get_object` of the class `GeneralCommanderFactory` returns the __singleton__ `GeneralCommander` object for a particular manipulator name. The current version is coupled with Moveit 1 and the moveit group name is to be provided to the factory. 
-
-#### GeneralCommanderFactory
-
-| Function | Parameters | Remarks |
-| -------- | ---------- | ------- |
-| get_object | The manipulator group name and optionally the name of the world reference frame | |
-
 ### System Parameters
 
 | Functions | Parameters | Remarks |
@@ -61,15 +51,14 @@ The function `get_object` of the class `GeneralCommanderFactory` returns the __s
 
 | Functions | Parameters | Remarks |
 | -------- | ---------- | ------- |
-| get_latest_moveit_feedback |  | Returns the latest `MoveGroupActionFeedback` received |
 | get_error_code |  | Returns the latest error code received |
 
 
 ### 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:GeneralCommander = GeneralCommander('panda_arm')
 arm_commander.spin(spin_in_thread=True)
 arm_commander.wait_for_ready_to_move()
 ```

docs/source/TUTORIAL_PART1.md

@@ -20,7 +20,7 @@ Example programs are provided for illustrating how to use the **general arm comm
 ## Programming Essentials
 
 The essential Python modules are found in the following three files.
-- `commander_moveit.py`: defines main classes including the `GeneralCommander` and the factory class `GeneralCommanderFactory`.
+- `commander_moveit.py`: defines main classes including the `GeneralCommander`.
 - `states.py`: defines the key `Enum` classes for state management.
 - `moveit_tools.py`: defines utility and helper functions.
 
@@ -29,7 +29,7 @@ The essential Python modules are found in the following three files.
 To use the Python arm_commander interface, import the modules in the concerned Python program file as below.
 
 ```python
-from arm_commander.commander_moveit import GeneralCommander, GeneralCommanderFactory
+from arm_commander.commander_moveit import GeneralCommander
 from arm_commander.states import ControllerState, GeneralCommanderStates
 import arm_commander.moveit_tools as moveit_tools
 ```
@@ -43,7 +43,7 @@ The program file `move/simple_move_1.py` illustrates the essentials of programmi
 The string parameter `panda_arm` specifies the __move_group__ as defined in the robot arm configuration (`panda_moveit_config`).
 
 ```python
-arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
+arm_commander:GeneralCommander = GeneralCommander('panda_arm')
 arm_commander.spin(spin_in_thread=True)
 arm_commander.wait_for_ready_to_move()
 ```
@@ -62,7 +62,7 @@ Alternatively, the client can create a new thread and use it to execute the spin
 class ArmCommanderMoveExample():
     def __init__(self):
         ...
-        arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
+        arm_commander:GeneralCommander = GeneralCommander('panda_arm')
         self.arm_commander = arm_commander
         self.the_thread = threading.Thread(target=self.spin_commander, daemon=True)
         self.the_thread.start()
@@ -81,7 +81,7 @@ class ArmCommanderMoveExample():
     def __init__(self):
         ...
         signal.signal(signal.SIGINT, self.stop)
-        arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
+        arm_commander: GeneralCommander = GeneralCommander('panda_arm')
         self.arm_commander = arm_commander
         ...
 

examples/collision/avoid_object.py

@@ -13,8 +13,8 @@
 __email__ = 'robotics.ref@qut.edu.au'
 __status__ = 'Development'
 
-import sys, signal
-from arm_commander.commander_moveit import GeneralCommander, GeneralCommanderFactory
+import sys, signal, time
+from arm_commander.commander_moveit import GeneralCommander
 import arm_commander.tools.moveit_tools as moveit_tools
 
 class ArmCommanderCollisionAvoidExample():
@@ -26,7 +26,7 @@ def __init__(self):
         # rospy.init_node('moveit_general_commander_node', anonymous=False)
         signal.signal(signal.SIGINT, self.stop)
         # create the General Commander and wait for it being ready to service move commands
-        arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
+        arm_commander:GeneralCommander = GeneralCommander('panda_arm')
         arm_commander.spin(spin_in_thread=True)
         arm_commander.reset_world()  # to remove any workspace or object previously defined
         arm_commander.wait_for_ready_to_move()
@@ -48,7 +48,9 @@ def __init__(self):
         # move to the top of the object
         xyzrpy = [0.4, 0.0, 0.6, 3.14, 0.0, 0.6]
         arm_commander.move_to_pose(xyzrpy, wait=True)
-        arm_commander.reset_state()         
+        arm_commander.reset_state()    
+
+        time.sleep(5)     
 
     def stop(self, *args, **kwargs):
         self.arm_commander.abort_move()

examples/collision/avoid_object_fix_region.py

@@ -13,8 +13,8 @@
 __email__ = 'robotics.ref@qut.edu.au'
 __status__ = 'Development'
 
-import sys, signal
-from arm_commander.commander_moveit import GeneralCommander, GeneralCommanderFactory
+import sys, signal, time
+from arm_commander.commander_moveit import GeneralCommander
 import arm_commander.tools.moveit_tools as moveit_tools
 import arm_commander.tools.pose_tools as pose_tools
 
@@ -27,7 +27,7 @@ def __init__(self):
         # rospy.init_node('moveit_general_commander_node', anonymous=False)
         signal.signal(signal.SIGINT, self.stop)
         # create the General Commander and wait for it being ready to service move commands
-        arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
+        arm_commander:GeneralCommander = GeneralCommander('panda_arm')
         arm_commander.spin(spin_in_thread=True)
         arm_commander.reset_world()  # to remove any workspace or object previously defined
         arm_commander.wait_for_ready_to_move()
@@ -65,6 +65,9 @@ def __init__(self):
         arm_commander.reset_state()    
 
         arm_commander.clear_path_constraints()     
+
+        time.sleep(5)
+
 
     def stop(self, *args, **kwargs):
         self.arm_commander.abort_move()

examples/collision/avoid_object_fix_rotation.py

@@ -13,8 +13,8 @@
 __email__ = 'robotics.ref@qut.edu.au'
 __status__ = 'Development'
 
-import sys, signal
-from arm_commander.commander_moveit import GeneralCommander, GeneralCommanderFactory
+import sys, signal, time
+from arm_commander.commander_moveit import GeneralCommander
 import arm_commander.tools.moveit_tools as moveit_tools
 
 class ArmCommanderCollisionAvoidExample():
@@ -27,7 +27,7 @@ def __init__(self):
         # rospy.init_node('moveit_general_commander_node', anonymous=False)
         signal.signal(signal.SIGINT, self.stop)
         # create the General Commander and wait for it being ready to service move commands
-        arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
+        arm_commander:GeneralCommander = GeneralCommander('panda_arm')
         arm_commander.spin(spin_in_thread=True)
         arm_commander.reset_world()  # to remove any workspace or object previously defined
         arm_commander.wait_for_ready_to_move()
@@ -61,6 +61,8 @@ def __init__(self):
 
         arm_commander.clear_path_constraints()     
 
+        time.sleep(5)
+
     def stop(self, *args, **kwargs):
         self.arm_commander.abort_move()
         self.arm_commander.clear_path_constraints() 

examples/collision/workspace.py

@@ -14,7 +14,7 @@
 __status__ = 'Development'
 
 import sys, signal
-from arm_commander.commander_moveit import GeneralCommander, GeneralCommanderFactory
+from arm_commander.commander_moveit import GeneralCommander
 
 class ArmCommanderWorkspaceExample():
     """ This example demonstrates the following:
@@ -25,7 +25,7 @@ def __init__(self):
         # rospy.init_node('moveit_general_commander_node', anonymous=False)
         signal.signal(signal.SIGINT, self.stop)
         # create the General Commander and wait for it being ready to service move commands
-        arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
+        arm_commander:GeneralCommander = GeneralCommander('panda_arm')
         arm_commander.spin(spin_in_thread=True)
         arm_commander.reset_world()  # to remove any workspace or object previously defined
         arm_commander.wait_for_ready_to_move()

examples/commander_demo.py

@@ -19,7 +19,7 @@
 from arm_commander.states import ControllerState, GeneralCommanderStates
 import arm_commander.tools.moveit_tools as moveit_tools
 import arm_commander.tools.pose_tools as pose_tools
-from arm_commander.commander_moveit import GeneralCommander, GeneralCommanderFactory, logger
+from arm_commander.commander_moveit import GeneralCommander, logger
 
 class GeneralCommanderDemo():
     """ A demo program for the :class:'GeneralCommander', which illustrates the movement commands offered by 
@@ -41,8 +41,8 @@ def __init__(self, config_file='panda_demo.yaml'):
             logger.error(f'Error in loading demo config file {config_file}')
             raise
         # TODO: config input to factory for group name and base
-        self.arm_commander: GeneralCommander = GeneralCommanderFactory.get_object(self.demo_config['moveit_group_name'])
-        # self.arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('xarm7', 'link_base')
+        self.arm_commander:GeneralCommander = GeneralCommander(self.demo_config['moveit_group_name'])
+        # self.arm_commander:GeneralCommander = GeneralCommander('xarm7', 'link_base')
         # self.arm_servo: CommanderServo = CommanderServo(moveit_group_name='xarm7', world_link='link_base')
         self.arm_commander.spin(spin_in_thread=True)
 
@@ -295,7 +295,6 @@ def test_2(self):
                     break
                 time.sleep(0.1)
             logger.info(f'the commander has completed with the result: {the_state} {the_state.message}')
-            # logger.info(f'Final result: {arm_commander.get_latest_moveit_feedback()}')
             arm_commander.reset_state() 
             logger.info(f'=== Test 2: Finished')
 

examples/framemove/frame_move_1.py

@@ -14,7 +14,7 @@
 __status__ = 'Development'
 
 import sys, signal
-from arm_commander.commander_moveit import GeneralCommander, GeneralCommanderFactory
+from arm_commander.commander_moveit import GeneralCommander
 
 class ArmCommanderMoveExample():
     """ This example demonstrates the following:
@@ -27,7 +27,7 @@ def __init__(self):
         signal.signal(signal.SIGINT, self.stop)
 
         # create the General Commander and wait for it being ready to service move commands
-        arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
+        arm_commander:GeneralCommander = GeneralCommander('panda_arm')
         arm_commander.spin(spin_in_thread=True)
         arm_commander.reset_world()
         arm_commander.wait_for_ready_to_move()

examples/framemove/frame_move_2.py

@@ -14,7 +14,7 @@
 __status__ = 'Development'
 
 import sys, signal
-from arm_commander.commander_moveit import GeneralCommander, GeneralCommanderFactory
+from arm_commander.commander_moveit import GeneralCommander
 
 class ArmCommanderMoveExample():
     """ This example demonstrates the following:
@@ -26,7 +26,7 @@ def __init__(self):
         signal.signal(signal.SIGINT, self.stop)
 
         # create the General Commander and wait for it being ready to service move commands
-        arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
+        arm_commander:GeneralCommander = GeneralCommander('panda_arm')
         arm_commander.spin(spin_in_thread=True)
         arm_commander.reset_world()
         arm_commander.wait_for_ready_to_move()

examples/move/display_info.py

@@ -16,15 +16,15 @@
 
 import sys, copy, threading, time, signal
 
-from arm_commander.commander_moveit import GeneralCommander, GeneralCommanderFactory, logger
+from arm_commander.commander_moveit import GeneralCommander, logger
 
 class ArmCommanderDisplayInfo():
 
     def __init__(self):
         # rospy.init_node('moveit_general_commander_node', anonymous=False)
         signal.signal(signal.SIGINT, self.stop)
         logger.info(f'Creating the General Commander')
-        self.arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
+        arm_commander:GeneralCommander = GeneralCommander('panda_arm')
         arm_commander = self.arm_commander
 
         self.the_thread = threading.Thread(target=self.arm_commander.spin, daemon=True)

examples/move/pose_move.py

@@ -14,15 +14,15 @@
 __status__ = 'Development'
 
 import sys, threading, signal
-from arm_commander.commander_moveit import GeneralCommander, GeneralCommanderFactory
+from arm_commander.commander_moveit import GeneralCommander
 import arm_commander.tools.pose_tools as pose_tools
 
 class ArmCommanderMoveExample():
     def __init__(self):
         # rospy.init_node('moveit_general_commander_node', anonymous=False)
         signal.signal(signal.SIGINT, self.stop)
         # create the General Commander and wait for it being ready to service move commands
-        arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
+        arm_commander:GeneralCommander = GeneralCommander('panda_arm')
         arm_commander.spin(spin_in_thread=True)
         arm_commander.wait_for_ready_to_move()
         self.arm_commander = arm_commander

examples/move/reset_robot.py

@@ -14,7 +14,7 @@
 __status__ = 'Development'
 
 import sys, signal
-from arm_commander.commander_moveit import GeneralCommander, GeneralCommanderFactory, logger
+from arm_commander.commander_moveit import GeneralCommander, logger
 
 class ResetRobotExample():
     """ This example demonstrates moving the robot arm back to a proper state
@@ -24,14 +24,14 @@ def __init__(self):
         # rospy.init_node('moveit_general_commander_node', anonymous=False)
         signal.signal(signal.SIGINT, self.stop)
         # create the General Commander and wait for it being ready to service move commands
-        arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
+        arm_commander:GeneralCommander = GeneralCommander('panda_arm')
         arm_commander.spin(spin_in_thread=True)
         arm_commander.wait_for_ready_to_move()
         self.arm_commander = arm_commander
         arm_commander.reset_world()
         # send a move command to move to a joint pose
         arm_commander.move_to_joint_pose([0.00, -1.243, 0.00, -2.949, 0.00, 1.704, 0.785], wait=True)
-        logger.loginfo(f'The robot arm has returned to the stow pose')
+        logger.info(f'The robot arm has returned to the stow pose')
         arm_commander.reset_state()
 
     def stop(self, *args, **kwargs):

examples/move/simple_move_1.py

@@ -14,7 +14,7 @@
 __status__ = 'Development'
 
 import sys, signal
-from arm_commander.commander_moveit import GeneralCommander, GeneralCommanderFactory, logger
+from arm_commander.commander_moveit import GeneralCommander, logger
 from arm_commander.states import GeneralCommanderStates
 
 class ArmCommanderMoveExample():
@@ -30,7 +30,7 @@ def __init__(self):
         # rospy.init_node('moveit_general_commander_node', anonymous=False)
         signal.signal(signal.SIGINT, self.stop)
         # create the General Commander and wait for it being ready to service move commands
-        arm_commander: GeneralCommander = GeneralCommanderFactory.get_object('panda_arm')
+        arm_commander:GeneralCommander = GeneralCommander('panda_arm')
         arm_commander.spin(spin_in_thread=True)
         arm_commander.wait_for_ready_to_move()
         self.arm_commander = arm_commander