A1 control

.gitignore

@@ -165,3 +165,7 @@ cython_debug/
 #  and can be added to the global gitignore or merged into this file.  For a more nuclear
 #  option (not recommended) you can uncomment the following to ignore the entire idea folder.
 #.idea/
+
+.vscode/
+setup/zed_sdk.run
+cuda/

GEMstack/knowledge/defaults/computation_graph.yaml

@@ -52,3 +52,6 @@ components:
   - trajectory_tracking:
       inputs: [vehicle, trajectory]
       outputs: 
+  - signaling:
+      inputs: [intent] 
+      outputs: 

GEMstack/knowledge/defaults/current.yaml

@@ -13,12 +13,12 @@ control:
         brake_speed : 2.0
     pure_pursuit:
         lookahead: 2.0
-        lookahead_scale: 3.0
-        crosstrack_gain: 1.0
+        lookahead_scale: 2.0
+        crosstrack_gain: 0.5
         desired_speed: trajectory
     longitudinal_control:
         pid_p: 1.0
-        pid_i: 0.1
+        pid_i: 0.05
         pid_d: 0.0
 
 #configure the simulator, if using

GEMstack/knowledge/vehicle/gem_e2_dynamics.yaml

@@ -14,7 +14,7 @@ max_accelerator_power_reverse: 10000.0 #Watts.  Power (backwards) in reverse gea
 
 acceleration_model : kris_v1
 accelerator_active_range : [0.32, 1.0]   #range of accelerator pedal where output acceleration is not flat
-brake_active_range : [0,1]             #range of brake pedal where output deceleration is not flat
+brake_active_range : [0.25,1]             #range of brake pedal where output deceleration is not flat
 
 internal_dry_deceleration: 0.2      #m/s^2: deceleration due to internal dry friction (non-speed dependent)
 internal_viscous_deceleration: 0.05  #1/s: scales the current velocity to get deceleration due to internal viscous friction (speed dependent)

GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml

@@ -1,20 +1,20 @@
-mass: 300.0  #kg
+mass: 700.0  #kg
 gravity: 9.81 #m/s^2
-longitudinal_friction : 1.0  # unitless
-lateral_friction : 1.0       # unitless
-max_brake_deceleration: 8.0  #m/s^2.  Deceleration at max brake pedal
-max_accelerator_acceleration: #m/s^2. Acceleration at max accelerator pedal, by gear
+longitudinal_friction : 1.0  # unitless TODO find make/model of tires 
+lateral_friction : 1.0       # unitless (I will bet these things do not have 1 coefficent of fricition)
+max_brake_deceleration: 8.0  #m/s^2.  Deceleration at max brake pedal TODO (un cap this cap)
+max_accelerator_acceleration: #m/s^2. Acceleration at max accelerator pedal, by gear (un cap this cap)
   - 0.0
   - 5.0
 max_accelerator_acceleration_reverse: 2.5 #m/s^2. Acceleration (backwards) in reverse gear
-max_accelerator_power:  #Watts.  Power at max accelerator pedal, by gear
+max_accelerator_power:  #Watts.  Power at max accelerator pedal, by gear (updated from website)
   - 0.0
-  - 10000.0
-max_accelerator_power_reverse: 10000.0 #Watts.  Power (backwards) in reverse gear
+  - 5000.0
+max_accelerator_power_reverse: 5000.0 #Watts.  Power (backwards) in reverse gear
 
-acceleration_model : kris_v1
+acceleration_model : hang_v1
 accelerator_active_range : [0.2, 1.0]   #range of accelerator pedal where output acceleration is not flat
-brake_active_range : [0,1]             #range of brake pedal where output deceleration is not flat
+brake_active_range : [0.5,1]             #range of brake pedal where output deceleration is not flat
 
 internal_dry_deceleration: 0.2      #m/s^2: deceleration due to internal dry friction (non-speed dependent)
 internal_viscous_deceleration: 0.05  #1/s: scales the current velocity to get deceleration due to internal viscous friction (speed dependent)

GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml

@@ -2,5 +2,4 @@ ros_topics:
   top_lidar: /ouster/points
   front_camera: /oak/rgb/image_raw
   front_depth: /oak/stereo/image_raw
-  gnss: /septentrio_gnss/insnavgeod
-
+  gnss: /novatel/inspva

GEMstack/offboard/calibration/camera_info.py

@@ -0,0 +1,41 @@
+# ROS Headers
+import rospy
+from sensor_msgs.msg import Image,PointCloud2, CameraInfo
+import sensor_msgs.point_cloud2 as pc2
+import ctypes
+import struct
+import pickle
+import image_geometry
+
+import numpy as np
+import os
+import time
+
+camera_image = None
+
+def camera_callback(info : CameraInfo):
+    global camera_image
+    camera_image = info
+
+def get_intrinsics():
+    model = image_geometry.PinholeCameraModel()
+    model.fromCameraInfo(camera_image)
+    print(model.intrinsicMatrix())
+
+def main(folder='data',start_index=1):
+    rospy.init_node("capture_cam_info",disable_signals=True)
+    caminfo_sub = rospy.Subscriber("/oak/rgb/camera_info", CameraInfo, camera_callback)
+    while True:
+        if camera_image:
+            time.sleep(1)
+            get_intrinsics()
+
+if __name__ == '__main__':
+    import sys
+    folder = 'data'
+    start_index = 1
+    if len(sys.argv) >= 2:
+        folder = sys.argv[1]
+    if len(sys.argv) >= 3:
+        start_index = int(sys.argv[2])
+    main(folder,start_index)

GEMstack/offboard/calibration/capture_ouster_oak.py

@@ -0,0 +1,102 @@
+# ROS Headers
+import rospy
+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 time
+
+lidar_points = None
+camera_image = None
+depth = None
+depth_image = None
+bridge = CvBridge()
+
+def lidar_callback(lidar : PointCloud2):
+    global lidar_points
+    lidar_points = lidar
+
+def camera_callback(img : Image):
+    global camera_image
+    camera_image = img
+
+def depth_callback(img : Image):
+    global depth_image
+    depth_image = img
+
+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)
+        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
+            b = (pack & 0x000000FF)
+            #r,g,b values in the 0-255 range
+            xyzrgb[i,3:] = (r,g,b)
+        return xyzrgb
+    else:
+        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)
+
+    pc = pc2_to_numpy(lidar_points,want_rgb=False) # convert lidar feed to numpy
+    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))
+    dimage = dimage.astype(np.uint16)
+    cv2.imwrite(depth_fn,dimage)
+
+def main(folder='data',start_index=1):
+    rospy.init_node("capture_ouster_oak",disable_signals=True)
+    lidar_sub = rospy.Subscriber("/ouster/points", PointCloud2, lidar_callback)
+    camera_sub = rospy.Subscriber("/oak/rgb/image_raw", Image, camera_callback)
+    depth_sub = rospy.Subscriber("/oak/stereo/image_raw", Image, depth_callback)
+    index = 0
+    print(" Storing lidar point clouds as npz")
+    print(" Storing color images as png")
+    print(" Storing depth images as tif")
+    print(" Ctrl+C to quit")
+    while True:
+        if camera_image and depth_image:
+            cv2.imshow("result",bridge.imgmsg_to_cv2(camera_image))
+            time.sleep(.5)
+            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))]
+            save_scan(*files)
+            index += 1
+
+if __name__ == '__main__':
+    import sys
+    folder = 'data'
+    start_index = 1
+    if len(sys.argv) >= 2:
+        folder = sys.argv[1]
+    if len(sys.argv) >= 3:
+        start_index = int(sys.argv[2])
+    main(folder,start_index)