v0.2.2 Push (#3)

* pytest + formatting

* Fixed CI

* Added AC wrapper

* change warning level if timeout

* Added polygon

* version 0.2.0 push

* correct versioning

* added ROS methods

* fixed dependencies problem

* Added word_to_num converter

* Version bump to 0.2.2

.github/workflows/pythonapp.yml

@@ -18,6 +18,10 @@ jobs:
           python-version: [3.6, 3.7, 3.8]
 
     steps:
+    - name: Setup ROS
+      uses: ros-tooling/[email protected]
+      with: 
+        required-ros-distributions: melodic
     - uses: actions/checkout@v2
     - name: Set up Python ${{ matrix.python-version }}
       uses: actions/setup-python@v1
@@ -27,6 +31,7 @@ jobs:
       run: |
         python -m pip install --upgrade pip
         pip install .
+        pip install empy pyyaml rospkg
 #     - name: Lint with flake8
 #       run: |
 #         pip install flake8
@@ -36,5 +41,6 @@ jobs:
 #         flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics
     - name: Test with pytest
       run: |
+        source /opt/ros/melodic/setup.bash
         pip install pytest
         python -m pytest

CHANGELOG.md

@@ -1,6 +1,16 @@
 # Changelog
 
 
+## [0.2.2] - 2020-08-03
+- **[Added]** *[ros]* Implemented same transformation function (`do_transform_point`,`do_transform_pose`) interface from tf2_geometry_msgs to bypass python3/PyKDL requirements
+- **[Added]** *[math]* Conversion from transformation matrix to array.
+- **[Added]** Test for ros methods
+- **[Added]** *[tools]* A function `convert_word_list_to_num_str` that converts a list of literal numbers ('four two') to number string ('42').
+
+#### Changed
+- **[Changed]** Switch to manually declaring the exposed methods in `__init__.py`.
+- **[Changed]** Added ROS dependencies to github actions to allow ROS testing
+
 ## [0.2.1] - 2020-06-04
 #### Changed
 - Update README 

alloy/math/vector_3D_op.py

@@ -10,7 +10,7 @@
 
 __all__ = ['skew_symmetric_matrix', 'rotation_matrix_from_axis_angle', 'axis_angles_from_rotation_matrix',
            'transformation_matrix_from_array', 'inverse_transformation_matrix', 'ypr_from_quaterion',
-           'rot2D_from_quaternion'
+           'rot2D_from_quaternion','transformation_matrix_to_array'
            ]
 
 
@@ -75,6 +75,16 @@ def transformation_matrix_from_array(arr):
     trans[0:3, 0:3] = quaternion.rotation_matrix
     return trans
 
+def transformation_matrix_to_array(mat):
+    arr = np.zeros((7,))
+    quaternion = Quaternion(matrix=mat[0:3,0:3])
+    arr[0:3] = mat[0:3,3]
+    arr[3] = quaternion.w
+    arr[4] = quaternion.x
+    arr[5] = quaternion.y
+    arr[6] = quaternion.z
+    return arr
+
 
 def ypr_from_quaterion(arr):
     """Calculate the yaw,pitch,roll from a numpy quaternion [w,x,y,z]
@@ -112,7 +122,6 @@ def inverse_transformation_matrix(m: np.array) -> np.array:
     inv_m[0:3, 0:3] = inv_rot
     return inv_m
 
-
 def main():
     # skew symmetric test
     # print(skew_symmetric_matrix(np.array([1,2,3])))

alloy/ros/__init__.py

@@ -5,13 +5,33 @@
 #test if ros exist
 try:
     import rospy
-    #import everything else if possible
-    from .basic import *
-    from .ros_conversions import *
 
-    __all__ = []
-    __all__ += basic.__all__
-    __all__ += ros_conversions.__all__
+    # specify what kind of function to import
+    from .basic import (
+        create_ros_header,
+        resolve_res_path,
+        create_res_dir,
+        get_res_path,
+        ac_wait_for_server_wrapper
+    )
+    from .ros_conversions import (
+        numpy_to_wrench, 
+        wrench_to_numpy, 
+        twist_to_numpy, 
+        numpy_to_twist,
+        pose_to_numpy, 
+        dict_to_pose, 
+        transform_to_numpy,
+        transform_to_pose,
+        point_to_numpy,
+        numpy_to_point,
+        numpy_to_pose
+    )
+
+    from .transform import (
+        do_transform_point,
+        do_transform_pose
+    )
 
 except ImportError:
     warnings.warn(RuntimeWarning('Unable to find ROS specific libraries, you maybe trying to reference this sub-module on a none ROS machine'))

alloy/ros/ros_conversions.py

@@ -11,13 +11,14 @@
 from geometry_msgs.msg import(
     Wrench,
     Twist,
-    Pose
+    Pose,
+    Point
 )
 
 __all__ = [
     'numpy_to_wrench', 'wrench_to_numpy', 'twist_to_numpy', 'numpy_to_twist',
     'pose_to_numpy', 'dict_to_pose', 'transform_to_numpy','transform_to_pose',
-    'point_to_numpy'
+    'point_to_numpy','numpy_to_point','numpy_to_pose'
 ]
 
 
@@ -113,6 +114,17 @@ def pose_to_numpy(pose):
     arr[6] = pose.orientation.z
     return arr
 
+def numpy_to_pose(pose_np):
+    pose = Pose()
+    pose.position.x = pose_np[0]
+    pose.position.y = pose_np[1]
+    pose.position.z = pose_np[2]
+    pose.orientation.w = pose_np[3]
+    pose.orientation.x = pose_np[4]
+    pose.orientation.y = pose_np[5]
+    pose.orientation.z = pose_np[6]
+    return pose
+
 def point_to_numpy(point) -> np.array:
     """Convert a Geometry_msgs/Point to a (3,) numpy array
 
@@ -132,6 +144,12 @@ def point_to_numpy(point) -> np.array:
     arr[2] = point.z
     return arr
 
+def numpy_to_point(point_np):
+    p = Point()
+    p.x = point_np[0]
+    p.y = point_np[1]
+    p.z = point_np[2]
+    return p
 
 def transform_to_numpy(transform):
     """Convert geometry_msgs/Transform to a Numpy array with 

alloy/ros/transform.py

@@ -0,0 +1,59 @@
+import numpy as np
+import alloy.math
+import alloy.ros
+
+from geometry_msgs.msg import(
+    Pose,
+    Point,
+    Transform
+)
+
+__all__ = [
+    'do_transform_point'
+]
+
+def do_transform_point(transform: Transform, point: Point) -> Point:
+    """An implementation of the tf2_geometry_msgs interface to get around PyKDL.
+    Transform a point with the given transform message.
+
+    Parameters
+    ----------
+    transform : geometry_msgs/Transform
+        Message that describes the transform
+    point : geometry_msgs/Point
+        The point to be transformed
+
+    Returns
+    -------
+    geometry_msgs/Point
+        Transformed point
+    """
+
+    transform_mat = alloy.math.transformation_matrix_from_array(alloy.ros.transform_to_numpy(transform))
+    point_np = alloy.ros.point_to_numpy(point)
+    point_np = np.append(point_np, 1).reshape((4,1))
+    trans_point = np.matmul(transform_mat, point_np)
+    return alloy.ros.numpy_to_point(trans_point[0:3,0])
+
+def do_transform_pose(transform: Transform, pose: Pose) -> Pose:
+    """An implementation of the tf2_geometry_msgs interface to get around PyKDL.
+    Transform a pose with the given transform message.
+
+    Parameters
+    ----------
+    transform : geometry_msgs/Transform
+        Message that describes the transform
+    pose : geometry_msgs/Pose
+        The pose to be transformed
+
+    Returns
+    -------
+    geometry_msgs/Pose
+        Transformed pose.
+    """
+    transform_mat = alloy.math.transformation_matrix_from_array(alloy.ros.transform_to_numpy(transform))
+    pose_mat = alloy.math.transformation_matrix_from_array(alloy.ros.pose_to_numpy(pose))
+    #combine two transformation matrix
+    trans_pose_mat = np.matmul(transform_mat, pose_mat)
+
+    return alloy.ros.numpy_to_pose(alloy.math.transformation_matrix_to_array(trans_pose_mat))

alloy/tools/__init__.py

@@ -0,0 +1 @@
+from .nlp import convert_word_list_to_num_str

alloy/tools/nlp.py

@@ -0,0 +1,120 @@
+import enum
+import typing
+
+en_num_list = ['zero', 'one', 'two', 'three',
+               'four', 'five', 'six', 'seven', 'eight', 'nine']
+
+en_teen_list = ['eleven', 'twelve', 'thirteen', 'fourteen',
+                'fifteen', 'sixteen', 'seventeen', 'eighteen', 'nineteen']
+
+en_ten_list = ['ten', 'twenty', 'thirty', 'forty',
+               'fifty', 'sixty', 'seventy', 'eighty', 'ninety']
+
+
+@enum.unique
+class Position(enum.Enum):
+    unknown = -1
+    ones = 0
+    tenths = 1
+    hundreds = 2
+    thousands = 3
+    millions = 4
+
+
+def convert_word_list_to_num_str(local: typing.List[str]) -> str:
+    """Converts literal word list ("four two") into number string ("42").
+    It tries to seperate numbers like "four two two one" into "4221". It
+    works up to 999,999.
+
+    Parameters
+    ----------
+    local : Typing.list[str]
+        List of number words
+
+    Returns
+    -------
+    str
+        return word list
+
+    Raises
+    ------
+    ValueError
+        If the word list is not formed correctly
+    """
+    val_group = []
+    current_val = 0
+    buffer_val = -1
+    last_pos = Position.unknown
+
+    for n in local:
+        # if its in the ones group
+        if n in en_num_list:
+            # previous unknown
+            if last_pos == Position.unknown:
+                buffer_val = en_num_list.index(n)
+            elif last_pos == Position.ones:
+                # this means the previous stack/buffer is a different number group
+                if buffer_val != -1:
+                    current_val += buffer_val
+                val_group.append(current_val)
+                current_val = 0
+                buffer_val = en_num_list.index(n)
+            elif last_pos == Position.thousands:
+                current_val = buffer_val
+                buffer_val = en_num_list.index(n)
+            else:
+                buffer_val += en_num_list.index(n)
+            last_pos = Position.ones
+
+        elif n in en_teen_list:
+            if last_pos == Position.unknown:
+                buffer_val = en_teen_list.index(n) + 11
+            elif last_pos == Position.tenths:
+                # this mean the previous stack/buffer is a different number group
+                if buffer_val != -1:
+                    current_val += buffer_val
+                val_group.append(current_val)
+                current_val = 0
+                buffer_val = en_teen_list.index(n) + 11
+            else:
+                buffer_val += en_teen_list.index(n) + 11
+
+            last_pos = Position.tenths
+
+        elif n in en_ten_list:
+            # previous unknown
+            if last_pos == Position.unknown:
+                buffer_val = (en_ten_list.index(n) + 1) * 10
+            elif last_pos.value <= Position.tenths.value:
+                # this means the previous stack/buffer is a different number group
+                if buffer_val != -1:
+                    current_val += buffer_val
+                val_group.append(current_val)
+                current_val = 0
+                buffer_val = (en_ten_list.index(n) + 1) * 10
+            else:
+                buffer_val += (en_ten_list.index(n) + 1) * 10
+
+            last_pos = Position.tenths
+
+        elif n == 'hundred':
+            if last_pos == Position.unknown:
+                raise ValueError("hundred without prefix")
+
+            buffer_val = buffer_val * 100
+            last_pos = Position.hundreds
+
+        elif n == 'thousand':
+            if last_pos == Position.unknown:
+                raise ValueError("thousands without prefix")
+
+            buffer_val = buffer_val * 1000
+            last_pos = Position.thousands
+
+    if buffer_val != -1:
+        current_val += buffer_val
+        val_group.append(current_val)
+    elif current_val != 0:
+        val_group.append(current_val)
+
+    return "".join([str(n) for n in val_group])

setup.py

@@ -4,7 +4,7 @@
     long_description = fh.read()
 
 setuptools.setup(name='alloylib',
-      version='0.2.1',
+      version='0.2.2',
       description='Lab Library for Commonly used Modules/Functions',
       long_description=long_description,
       long_description_content_type="text/markdown",