From 33d94d61d45209d0b7590a10c844021b3ffbfc8c Mon Sep 17 00:00:00 2001 From: Ali Ryckman <51866496+alisonryckman@users.noreply.github.com> Date: Sun, 29 Sep 2024 13:32:52 -0400 Subject: [PATCH] fix cmakelists and add tf_transformations (#12) * fix cmakelists and add tf_transformations * oops --- CMakeLists.txt | 3 +- cmake/macros.cmake | 6 ++ .../autonomy/launch/starter_project.launch.py | 9 -- starter_project/autonomy/src/util/SO3.py | 2 +- starter_project/autonomy/src/util/tf_utils.py | 97 ++++++++++++++++++- 5 files changed, 102 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c0ad9825..8c9e02fe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,10 +16,11 @@ option(MROVER_WARNINGS_AS_ERRORS "Treat warnings as errors" ${MROVER_CI}) if (APPLE) # Ensures that homebrew packages are never used over miniforge packages. + # TODO (ali): Pretty sure this doesn't do anything on macs set(CMAKE_IGNORE_PATH /opt/homebrew) # Find mamba python find_package(PythonLibs REQUIRED) - link_libraries(${PYTHON_LIBRARIES}) + # link_libraries(${PYTHON_LIBRARIES}) endif () if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/cmake/macros.cmake b/cmake/macros.cmake index 8328a2bf..22ff653b 100644 --- a/cmake/macros.cmake +++ b/cmake/macros.cmake @@ -22,6 +22,9 @@ macro(mrover_add_library name sources includes) add_library(${name} ${ARGV3} ${LIBRARY_SOURCES}) mrover_target(${name}) target_include_directories(${name} PUBLIC ${includes}) + if (APPLE) + target_link_libraries(${name} /opt/homebrew/Caskroom/miniforge/base/envs/ros2_env/lib/libpython3.11.dylib) + endif () endmacro() macro(mrover_add_component name sources includes) @@ -44,4 +47,7 @@ macro(mrover_add_node name sources) TARGETS ${name} DESTINATION lib/${PROJECT_NAME} ) + if (APPLE) + target_link_libraries(${name} /opt/homebrew/Caskroom/miniforge/base/envs/ros2_env/lib/libpython3.11.dylib) + endif () endmacro() diff --git a/starter_project/autonomy/launch/starter_project.launch.py b/starter_project/autonomy/launch/starter_project.launch.py index f8b24620..06130546 100644 --- a/starter_project/autonomy/launch/starter_project.launch.py +++ b/starter_project/autonomy/launch/starter_project.launch.py @@ -23,18 +23,9 @@ def generate_launch_description(): localization_node = Node(package="mrover", executable="localization.py", name="localization") - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - arguments=["-d", [os.path.join(get_package_share_directory("mrover"), "config/rviz", "auton_sim.rviz")]], - condition=LaunchConfigurationEquals("rviz", "true"), - ) - return LaunchDescription( [ launch_include_sim, - rviz_node, # ========== # Perception # ========== diff --git a/starter_project/autonomy/src/util/SO3.py b/starter_project/autonomy/src/util/SO3.py index abc4f5a8..6ed934d7 100644 --- a/starter_project/autonomy/src/util/SO3.py +++ b/starter_project/autonomy/src/util/SO3.py @@ -3,7 +3,7 @@ from dataclasses import dataclass, field import numpy as np -from tf_transformations import ( +from .tf_utils import ( quaternion_inverse, quaternion_matrix, quaternion_from_matrix, diff --git a/starter_project/autonomy/src/util/tf_utils.py b/starter_project/autonomy/src/util/tf_utils.py index ee0037b6..51be211c 100644 --- a/starter_project/autonomy/src/util/tf_utils.py +++ b/starter_project/autonomy/src/util/tf_utils.py @@ -1,10 +1,15 @@ import numpy as np -import rospy +import rclpy +import transforms3d from geometry_msgs.msg import Point, TransformStamped, Vector3 -from sensor_msgs.msg import NavSatFix, geometry_msgs +from sensor_msgs.msg import NavSatFix EARTH_RADIUS = 6371000 +TRANSLATION_IDENTITY = [0.0, 0.0, 0.0] +ROTATION_IDENTITY = np.identity(3, dtype=np.float64) +ZOOM_IDENTITY = [1.0, 1.0, 1.0] +SHEAR_IDENTITY = TRANSLATION_IDENTITY def gps_to_world(gps_coord: NavSatFix, ref_coord: NavSatFix, name: str, parent: str = "world") -> TransformStamped: @@ -15,8 +20,8 @@ def gps_to_world(gps_coord: NavSatFix, ref_coord: NavSatFix, name: str, parent: :param name: The name of the returned transform frame :param parent: The name of the reference world frame """ - t = geometry_msgs.msg.TransformStamped() - t.header.stamp = rospy.Time.now() + t = TransformStamped() + t.header.stamp = rclpy.time.Time() t.header.frame_id = parent t.child_frame_id = name longitude_delta = gps_coord.longitude - ref_coord.longitude @@ -33,3 +38,87 @@ def vector3_to_point(vec3: Vector3) -> Point: def point_to_vector3(pt: Point) -> Vector3: return Vector3(x=pt.x, y=pt.y, z=pt.z) + +# taken from tf_transformations package src + +def _reorder_output_quaternion(quaternion): + """Reorder quaternion to have w term last.""" + w, x, y, z = quaternion + return x, y, z, w + +def quaternion_conjugate(quaternion): + """ + Return conjugate of quaternion. + + >>> q0 = random_quaternion() + >>> q1 = quaternion_conjugate(q0) + >>> q1[3] == q0[3] and all(q1[:3] == -q0[:3]) + True + + """ + return np.array((-quaternion[0], -quaternion[1], + -quaternion[2], quaternion[3]), dtype=np.float64) + + +def quaternion_inverse(quaternion): + """ + Return inverse of quaternion. + + >>> q0 = random_quaternion() + >>> q1 = quaternion_inverse(q0) + >>> numpy.allclose(quaternion_multiply(q0, q1), [0, 0, 0, 1]) + True + + """ + return quaternion_conjugate(quaternion) / np.dot(quaternion, quaternion) + +def _reorder_input_quaternion(quaternion): + """Reorder quaternion to have w term first.""" + x, y, z, w = quaternion + return w, x, y, z + +def quaternion_matrix(quaternion): + """ + Return 4x4 homogeneous rotation matrix from quaternion. + + >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947]) + >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0))) + True + + """ + rotation_matrix = transforms3d.quaternions.quat2mat( + _reorder_input_quaternion(quaternion) + ) + return transforms3d.affines.compose(TRANSLATION_IDENTITY, + rotation_matrix, + ZOOM_IDENTITY) + +def quaternion_from_matrix(matrix): + """ + Return quaternion from rotation matrix. + + >>> R = rotation_matrix(0.123, (1, 2, 3)) + >>> q = quaternion_from_matrix(R) + >>> numpy.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095]) + True + + """ + rotation_matrix = transforms3d.affines.decompose(matrix)[1] + return _reorder_output_quaternion( + transforms3d.quaternions.mat2quat(rotation_matrix) + ) + +def quaternion_multiply(quaternion1, quaternion0): + """ + Return multiplication of two quaternions. + + >>> q = quaternion_multiply([1, -2, 3, 4], [-5, 6, 7, 8]) + >>> numpy.allclose(q, [-44, -14, 48, 28]) + True + + """ + x0, y0, z0, w0 = quaternion0 + x1, y1, z1, w1 = quaternion1 + w2, x2, y2, z2 = transforms3d.quaternions.qmult([w1, x1, y1, z1], + [w0, x0, y0, z0]) + return x2, y2, z2, w2 \ No newline at end of file