fix cmakelists and add tf_transformations

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")

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()

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
             # ==========

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,

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