Merge branch 's2025_teamA' into A4_planning

Author: udymd

.gitignore

@@ -168,4 +168,8 @@ cython_debug/
 
 .vscode/
 setup/zed_sdk.run
-cuda/
+
+cuda/
+#Ignore ROS bags
+*.bag
+

GEMstack/mathutils/dubins.py

@@ -44,7 +44,7 @@ def derivative(self,x,u):
         right = [-fwd[1],fwd[0]]
         phi = u[1]
         d = u[0]
-        return np.array([fwd[0]*d,fwd[1]*d,phi])
+        return np.array([fwd[0]*d,fwd[1]*d,phi*d])
 
 
 class DubinsCarIntegrator(ControlSpace):

GEMstack/mathutils/transforms.py

@@ -36,6 +36,14 @@ def vector_dist(v1, v2) -> float:
     """Euclidean distance between two vectors"""
     return vo.distance(v1,v2)
 
+def vector_dot(v1, v2) -> float:
+    """Dot product between two vectors"""
+    return vo.dot(v1,v2)
+
+def vector_cross(v1, v2) -> float:
+    """Cross product between two 2D vectors"""
+    return vo.cross(v1,v2)
+
 def vector2_angle(v1, v2 = None) -> float:
     """find the ccw angle bewtween two 2d vectors"""
     if v2 is None:
@@ -123,3 +131,16 @@ def xy_to_lat_lon(x_east : float, y_north : float, lat_reference : float, lon_re
     # convert GNSS waypoints into local fixed frame reprented in x and y
     lat, lon = axy.xy2ll(x_east, y_north, lat_reference, lon_reference)
     return lat, lon
+
+def quaternion_to_euler(x : float, y : float, z : float, w : float):
+    t0 = +2.0 * (w * x + y * z)
+    t1 = +1.0 - 2.0 * (x * x + y * y)
+    roll = np.arctan2(t0, t1)
+    t2 = +2.0 * (w * y - z * x)
+    t2 = +1.0 if t2 > +1.0 else t2
+    t2 = -1.0 if t2 < -1.0 else t2
+    pitch = np.arcsin(t2)
+    t3 = +2.0 * (w * z + x * y)
+    t4 = +1.0 - 2.0 * (y * y + z * z)
+    yaw = np.arctan2(t3, t4)
+    return [roll, pitch, yaw]

GEMstack/offboard/calibration/capture_lidar_zed.py

@@ -1,99 +1,107 @@
-# ROS Headers
+#!/usr/bin/env python3
+"""
+Capture ROS messages and save each run in its own time-stamped folder.
+"""
+
 import rospy
-from sensor_msgs.msg import Image,PointCloud2
+from sensor_msgs.msg import Image, PointCloud2
 import sensor_msgs.point_cloud2 as pc2
 import ctypes
 import struct
-
-# OpenCV and cv2 bridge
 import cv2
 from cv_bridge import CvBridge
 import numpy as np
 import os
+import datetime
 
 lidar_points = None
 camera_image = None
 depth_image = None
 bridge = CvBridge()
 
-def lidar_callback(lidar : PointCloud2):
+def lidar_callback(lidar: PointCloud2):
     global lidar_points
     lidar_points = lidar
 
-def camera_callback(img : Image):
+def camera_callback(img: Image):
     global camera_image
     camera_image = img
 
-def depth_callback(img : Image):
+def depth_callback(img: Image):
     global depth_image
     depth_image = img
 
-def pc2_to_numpy(pc2_msg, want_rgb = False):
+def pc2_to_numpy(pc2_msg, want_rgb=False):
     gen = pc2.read_points(pc2_msg, skip_nans=True)
     if want_rgb:
-        xyzpack = np.array(list(gen),dtype=np.float32)
+        xyzpack = np.array(list(gen), dtype=np.float32)
         if xyzpack.shape[1] != 4:
             raise ValueError("PointCloud2 does not have points")
-        xyzrgb = np.empty((xyzpack.shape[0],6))
-        xyzrgb[:,:3] = xyzpack[:,:3]
-        for i,x in enumerate(xyzpack):
-            rgb = x[3] 
-            # cast float32 to int so that bitwise operations are possible
-            s = struct.pack('>f' ,rgb)
-            i = struct.unpack('>l',s)[0]
-            # you can get back the float value by the inverse operations
-            pack = ctypes.c_uint32(i).value
-            r = (pack & 0x00FF0000)>> 16
-            g = (pack & 0x0000FF00)>> 8
+        xyzrgb = np.empty((xyzpack.shape[0], 6))
+        xyzrgb[:, :3] = xyzpack[:, :3]
+        for i, x in enumerate(xyzpack):
+            rgb = x[3]
+            s = struct.pack('>f', rgb)
+            i_val = struct.unpack('>l', s)[0]
+            pack = ctypes.c_uint32(i_val).value
+            r = (pack & 0x00FF0000) >> 16
+            g = (pack & 0x0000FF00) >> 8
             b = (pack & 0x000000FF)
-            #r,g,b values in the 0-255 range
-            xyzrgb[i,3:] = (r,g,b)
+            xyzrgb[i, 3:] = (r, g, b)
         return xyzrgb
     else:
-        return np.array(list(gen),dtype=np.float32)[:,:3]
+        return np.array(list(gen), dtype=np.float32)[:, :3]
 
-def save_scan(lidar_fn,color_fn,depth_fn):
-    print("Saving scan to",lidar_fn,color_fn,depth_fn)
-    pc = pc2_to_numpy(lidar_points,want_rgb=False)
-    np.savez(lidar_fn,pc)
-    cv2.imwrite(color_fn,bridge.imgmsg_to_cv2(camera_image))
+def save_scan(lidar_fn, color_fn, depth_fn):
+    print("Saving scan to", lidar_fn, color_fn, depth_fn)
+    pc = pc2_to_numpy(lidar_points, want_rgb=False)
+    np.savez(lidar_fn, pc)
+    cv2.imwrite(color_fn, bridge.imgmsg_to_cv2(camera_image))
     dimage = bridge.imgmsg_to_cv2(depth_image)
     dimage_non_nan = dimage[np.isfinite(dimage)]
-    print("Depth range",np.min(dimage_non_nan),np.max(dimage_non_nan))
-    dimage = np.nan_to_num(dimage,nan=0,posinf=0,neginf=0)
-    dimage = (dimage/4000*0xffff)
-    print("Depth pixel range",np.min(dimage),np.max(dimage))
+    print("Depth range", np.min(dimage_non_nan), np.max(dimage_non_nan))
+    dimage = np.nan_to_num(dimage, nan=0, posinf=0, neginf=0)
+    dimage = (dimage / 4000 * 0xffff)
+    print("Depth pixel range", np.min(dimage), np.max(dimage))
     dimage = dimage.astype(np.uint16)
-    cv2.imwrite(depth_fn,dimage)
+    cv2.imwrite(depth_fn, dimage)
+
+def main(base_folder='data', start_index=1):
+    # unique folder for this capture run based on the current date/time.
+    run_timestamp = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
+    run_folder = os.path.join(base_folder, run_timestamp)
+    os.makedirs(run_folder, exist_ok=True)
+    print("Capture run folder:", run_folder)
+
+    rospy.init_node("capture_lidar_zed", disable_signals=True)
+    rospy.Subscriber("/lidar1/velodyne_points", PointCloud2, lidar_callback)
+    rospy.Subscriber("/zed2/zed_node/rgb/image_rect_color", Image, camera_callback)
+    rospy.Subscriber("/zed2/zed_node/depth/depth_registered", Image, depth_callback)
 
-def main(folder='data',start_index=1):
-    rospy.init_node("capture_lidar_zed",disable_signals=True)
-    lidar_sub = rospy.Subscriber("/lidar1/velodyne_points", PointCloud2, lidar_callback)
-    camera_sub = rospy.Subscriber("/zed2/zed_node/rgb/image_rect_color", Image, camera_callback)
-    depth_sub = rospy.Subscriber("/zed2/zed_node/depth/depth_registered", Image, depth_callback)
     index = start_index
     print("Press any key to:")
-    print("  store lidar point clouds as npz")
-    print("  store color images as png")
-    print("  store depth images (m scaled by 0xffff/4000) as 16-bit tif")
+    print("  - store lidar point clouds as npz")
+    print("  - store color images as png")
+    print("  - store depth images (m scaled by 0xffff/4000) as 16-bit tif")
     print("Press Escape or Ctrl+C to quit")
-    while True:
-        if camera_image:
-            cv2.imshow("result",bridge.imgmsg_to_cv2(camera_image))
+
+    while not rospy.is_shutdown():
+        if camera_image is not None:
+            cv2.imshow("result", bridge.imgmsg_to_cv2(camera_image))
             key = cv2.waitKey(30)
             if key == -1:
-                #nothing
-                pass
+                pass  # No key pressed.
             elif key == 27:
-                #escape
-                break
+                break  # Escape key pressed.
             else:
                 if lidar_points is None or camera_image is None or depth_image is None:
                     print("Missing some messages?")
                 else:
-                    files = [os.path.join(folder,'lidar{}.npz'.format(index)),
-                        os.path.join(folder,'color{}.png'.format(index)),
-                        os.path.join(folder,'depth{}.tif'.format(index))]
+                    files = [
+                        os.path.join(run_folder, 'lidar{}.npz'.format(index)),
+                        os.path.join(run_folder, 'color{}.png'.format(index)),
+                        os.path.join(run_folder, 'depth{}.tif'.format(index))
+                    ]
                     save_scan(*files)
                     index += 1
 
@@ -105,4 +113,4 @@ def main(folder='data',start_index=1):
         folder = sys.argv[1]
     if len(sys.argv) >= 3:
         start_index = int(sys.argv[2])
-    main(folder,start_index)
+    main(folder, start_index)