Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add transforms3d dep #53

Closed
wants to merge 2 commits into from
Closed

Add transforms3d dep #53

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6724044
Add transforms3d dep
Flova Feb 10, 2023
cb6a54e
Use filterwarnings to suppress the deprecation warning
Flova Feb 11, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
39 changes: 10 additions & 29 deletions ipm_library/ipm_library/utils.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
# limitations under the License.

from typing import Optional, Tuple
from warnings import filterwarnings

from builtin_interfaces.msg import Time
from geometry_msgs.msg import Transform
Expand All @@ -23,6 +24,14 @@
from tf2_geometry_msgs import PointStamped
import tf2_ros

# Ignore deprication warning issue described in #53
filterwarnings(
action='ignore',
category=DeprecationWarning,
message='`np.float` is a deprecated alias for the builtin `float`.')

from transforms3d.quaternions import quat2mat


def plane_general_to_point_normal(plane: Plane) -> Tuple[np.ndarray, np.ndarray]:
"""
Expand Down Expand Up @@ -176,7 +185,7 @@ def transform_points(point_cloud: np.ndarray, transform: Transform) -> np.ndarra
transform.translation.y,
transform.translation.z
])
transform_rotation_matrix = _get_mat_from_quat(
transform_rotation_matrix = quat2mat(
np.array([
transform.rotation.w,
transform.rotation.x,
Expand All @@ -193,34 +202,6 @@ def transform_points(point_cloud: np.ndarray, transform: Transform) -> np.ndarra
point_cloud) + transform_translation


def _get_mat_from_quat(quaternion: np.ndarray) -> np.ndarray:
"""
Convert a quaternion to a rotation matrix.

This method is currently needed because transforms3d is not released as a `.dep` and
would require user interaction to set up.
For reference see: https://github.com/matthew-brett/transforms3d/blob/
f185e866ecccb66c545559bc9f2e19cb5025e0ab/transforms3d/quaternions.py#L101

:param quaternion: A numpy array containing the w, x, y, and z components of the quaternion
:returns: The rotation matrix
"""
Nq = np.sum(np.square(quaternion))
if Nq < np.finfo(np.float64).eps:
return np.eye(3)

XYZ = quaternion[1:] * 2.0 / Nq
wXYZ = XYZ * quaternion[0]
xXYZ = XYZ * quaternion[1]
yYZ = XYZ[1:] * quaternion[2]
zZ = XYZ[2] * quaternion[3]

return np.array(
[[1.0-(yYZ[0]+zZ), xXYZ[1]-wXYZ[2], xXYZ[2]+wXYZ[1]],
[xXYZ[1]+wXYZ[2], 1.0-(xXYZ[0]+zZ), yYZ[1]-wXYZ[0]],
[xXYZ[2]-wXYZ[1], yYZ[1]+wXYZ[0], 1.0-(xXYZ[0]+yYZ[0])]])


def create_horizontal_plane(
height_offset: float = 0.0) -> Plane:
"""Create a plane message for a given frame at a given time, with a given height offset."""
Expand Down
3 changes: 2 additions & 1 deletion ipm_library/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,12 @@
<depend>geometry_msgs</depend>
<depend>ipm_interfaces</depend>
<depend>python3-numpy</depend>
<depend>python3-transforms3d</depend>
<depend>sensor_msgs</depend>
<depend>shape_msgs</depend>
<depend>std_msgs</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2</depend>
<depend>vision_msgs</depend>

<test_depend>ament_copyright</test_depend>
Expand Down