diff --git a/docs/stylesheets/extra.css b/docs/stylesheets/extra.css
index 61948f5a2..1b7d703f0 100644
--- a/docs/stylesheets/extra.css
+++ b/docs/stylesheets/extra.css
@@ -11,12 +11,12 @@
/* Ensure navigation tabs are above splash content */
.md-tabs {
position: relative;
- z-index: 10;
+ z-index: 3;
}
-.md-version {
+.md-version * {
position: relative;
- z-index: 25;
+ z-index: 5;
}
/* Splash container and background */
diff --git a/robot/docker/robot.env b/robot/docker/robot.env
index fe9021d34..807fc0053 100644
--- a/robot/docker/robot.env
+++ b/robot/docker/robot.env
@@ -1,2 +1,2 @@
# These become environment variables in the robot container
-USE_MACVO="false"
\ No newline at end of file
+USE_MACVO="true"
\ No newline at end of file
diff --git a/robot/ros_ws/src/autonomy/2_perception/macvo/launch/macvo.launch.xml b/robot/ros_ws/src/autonomy/2_perception/macvo/launch/macvo.launch.xml
index 287b11934..332d87e5a 100644
--- a/robot/ros_ws/src/autonomy/2_perception/macvo/launch/macvo.launch.xml
+++ b/robot/ros_ws/src/autonomy/2_perception/macvo/launch/macvo.launch.xml
@@ -1,6 +1,6 @@
-
-
+
+
\ No newline at end of file
diff --git a/robot/ros_ws/src/autonomy/2_perception/macvo/macvo/macvo.py b/robot/ros_ws/src/autonomy/2_perception/macvo/macvo/macvo.py
index 790c91466..ca4ef895d 100644
--- a/robot/ros_ws/src/autonomy/2_perception/macvo/macvo/macvo.py
+++ b/robot/ros_ws/src/autonomy/2_perception/macvo/macvo/macvo.py
@@ -7,6 +7,8 @@
from rclpy.node import Node
from sensor_msgs.msg import Image, PointCloud
from geometry_msgs.msg import PoseStamped
+from scipy.spatial.transform import Rotation
+from geometry_msgs.msg import Transform, Pose, Quaternion
from message_filters import ApproximateTimeSynchronizer, Subscriber
from pathlib import Path
@@ -38,10 +40,8 @@ def __init__(self):
self.bridge = None
self.time = None
self.prev_time = None
- self.frame = None
self.camera_info = None
self.baseline = None
- self.prev_frame = None
self.odometry = None
# Declare subscriptions and publishers ----------------
@@ -111,8 +111,16 @@ def __init__(self):
self.scale_u = float(self.camera_info.width / u_dim)
self.scale_v = float(self.camera_info.height / v_dim)
- self.rot_correction_matrix = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]])
- self.rot_correction_matrix = np.eye(3)
+ # 180 degree rotation around x-axis
+ x_rot = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]])
+ # 90 degree rotation around z-axis
+ z_rot = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]])
+
+ # self.rot_correction_matrix = np.dot(x_rot, z_rot)
+ # self.rot_correction_matrix = np.dot(z_rot, x_rot)
+ self.correction_matrix = np.eye(4)
+ # self.correction_matrix[:3, :3] =
+ self.correction_matrix[:3, :3] = np.dot(x_rot, z_rot)
# self.get_logger().info(f"scale u: {self.scale_u}, scale v: {self.scale_v}, u_dim: {u_dim}, v_dim: {v_dim}, width: {self.camera_info.width}, height: {self.camera_info.height}")
@@ -144,8 +152,10 @@ def publish_latest_pose(self, system: MACVO):
out_msg = to_stamped_pose(pose, frame, time)
# Correction for the camera coordinate frame
- out_msg.pose.position.x, out_msg.pose.position.y, out_msg.pose.position.z = np.dot(self.rot_correction_matrix, np.array([out_msg.pose.position.x, out_msg.pose.position.y, out_msg.pose.position.z]))
-
+ transform = self.pose_to_homogeneous(out_msg.pose)
+ transform = np.dot(self.correction_matrix, transform)
+ out_msg.pose = self.homogeneous_to_pose(transform)
+
self.pose_pipe.publish(out_msg)
def publish_latest_points(self, system: MACVO):
@@ -169,7 +179,7 @@ def publish_latest_points(self, system: MACVO):
# Correction for the camera coordinate frame
for pt in out_msg.points:
- pt.x, pt.y, pt.z = np.dot(self.rot_correction_matrix, np.array([pt.x, pt.y, pt.z]))
+ pt.x, pt.y, pt.z, _ = np.dot(self.correction_matrix, np.array([pt.x, pt.y, pt.z, 1]))
self.point_pipe.publish(out_msg)
@@ -216,10 +226,7 @@ def receive_frame(self, msg_L: Image, msg_R: Image) -> None:
self.get_logger().error("MACVO Node not initialized yet, skipping frame")
return
- self.prev_frame, self.prev_time = self.frame, self.time
-
- self.frame = msg_L.header.frame_id
-
+ self.prev_time = self.time
self.time = msg_L.header.stamp
imageL = self.bridge.imgmsg_to_cv2(msg_L, desired_encoding="passthrough")
imageR = self.bridge.imgmsg_to_cv2(msg_R, desired_encoding="passthrough")
@@ -248,6 +255,61 @@ def receive_frame(self, msg_L: Image, msg_R: Image) -> None:
end_time = self.get_clock().now()
# self.get_logger().info(f"Frame {self.frame_idx} processed in {end_time - start_time}")
self.frame_idx += 1
+
+ def pose_to_homogeneous(self, pose):
+ """
+ Converts a Pose message into a 4x4 homogeneous transformation matrix.
+
+ Args:
+ Pose (Pose): The ROS Pose message to convert.
+
+ Returns:
+ np.ndarray: A 4x4 homogeneous transformation matrix.
+ """
+ assert isinstance(pose, Pose) , "Input must be a Pose message, got %s" % type(Pose)
+ # Extract translation components
+ translation = pose.position
+ translation_vector = np.array([translation.x, translation.y, translation.z])
+
+ # Extract rotation components
+ rotation = pose.orientation
+ # quaternion = np.array([rotation.x, rotation.y, rotation.z, rotation.w]) * np.array([1, -1, -1, 1])
+ quaternion = np.array([rotation.x, rotation.y, rotation.z, rotation.w])
+ rot_matrix = Rotation.from_quat(quaternion).as_matrix()
+ # assert False, f"rot_vector: {Rotation.from_quat(quaternion).as_rotvec()}"
+
+ # Combine into a homogeneous transformation matrix
+ homogeneous_matrix = np.eye(4)
+ homogeneous_matrix[:3, :3] = rot_matrix[:3, :3]
+ homogeneous_matrix[:3, 3] = translation_vector
+ assert homogeneous_matrix.shape == (4, 4), "Output must be a 4x4 numpy array, got %s" % homogeneous_matrix.shape
+ return homogeneous_matrix
+
+ def homogeneous_to_pose(self, homogeneous_matrix):
+ """
+ Converts a 4x4 homogeneous transformation matrix into a Pose message.
+
+ Args:
+ homogeneous_matrix (np.ndarray): The 4x4 homogeneous transformation matrix to convert.
+
+ Returns:
+ Pose: A ROS Pose message.
+ """
+ assert isinstance(homogeneous_matrix, np.ndarray) and homogeneous_matrix.shape == (4, 4), "Input must be a 4x4 numpy array, got %s" % type(homogeneous_matrix)
+ # Extract translation components
+ translation_vector = homogeneous_matrix[:3, 3]
+ translation = translation_vector
+
+ # Extract rotation components
+ rot_matrix = homogeneous_matrix[:3, :3]
+ quaternion = Rotation.from_matrix(rot_matrix).as_quat()
+ rotation = quaternion
+
+ # Combine into a Pose message
+ pose = Pose()
+ pose.position.x, pose.position.y, pose.position.z = translation
+ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w = rotation
+ return pose
def main():
rclpy.init()
diff --git a/robot/ros_ws/src/autonomy/2_perception/perception_bringup/launch/perception.launch.xml b/robot/ros_ws/src/autonomy/2_perception/perception_bringup/launch/perception.launch.xml
index e53991382..a2b28dcf6 100644
--- a/robot/ros_ws/src/autonomy/2_perception/perception_bringup/launch/perception.launch.xml
+++ b/robot/ros_ws/src/autonomy/2_perception/perception_bringup/launch/perception.launch.xml
@@ -4,7 +4,7 @@
-
+
Value: Orbit (rviz)
- Yaw: 0.3381178677082062
+ Yaw: 2.166358232498169
Saved: ~
Window Geometry:
Displays:
- collapsed: false
+ collapsed: true
Front Left Depth:
collapsed: false
Front Left RGB:
collapsed: false
- Height: 1376
- Hide Left Dock: false
+ Height: 2096
+ Hide Left Dock: true
Hide Right Dock: false
- QMainWindow State: 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
+ QMainWindow State: 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
Selection:
collapsed: false
Time:
@@ -547,6 +546,6 @@ Window Geometry:
collapsed: false
Visual Odometry Features:
collapsed: false
- Width: 2490
- X: 1990
+ Width: 3770
+ X: 70
Y: 27
diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/Makefile b/simulation/isaac-sim/sitl_integration/drag_and_drop/Makefile
index bcb26a80b..0f4a56842 100644
--- a/simulation/isaac-sim/sitl_integration/drag_and_drop/Makefile
+++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/Makefile
@@ -1,2 +1,2 @@
all:
- g++ inject.cpp -fPIC -shared -o inject.so
+ g++ inject.cpp -ggdb -fPIC -shared -o inject.so
diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/inject.so b/simulation/isaac-sim/sitl_integration/drag_and_drop/inject.so
index 7bd0d8dbe..824d95f62 100755
Binary files a/simulation/isaac-sim/sitl_integration/drag_and_drop/inject.so and b/simulation/isaac-sim/sitl_integration/drag_and_drop/inject.so differ