diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 000000000..043038f0b Binary files /dev/null and b/.DS_Store differ diff --git a/.gitignore b/.gitignore index 0c36d682f..5d34da6bb 100644 --- a/.gitignore +++ b/.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/ \ No newline at end of file diff --git a/GEMstack/.DS_Store b/GEMstack/.DS_Store new file mode 100644 index 000000000..1db5bc5b2 Binary files /dev/null and b/GEMstack/.DS_Store differ diff --git a/GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml b/GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml index a0c611b37..b9e719261 100644 --- a/GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml +++ b/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 - \ No newline at end of file + gnss: /novatel/inspva diff --git a/GEMstack/offboard/calibration/camera_info.py b/GEMstack/offboard/calibration/camera_info.py new file mode 100644 index 000000000..5610efb7f --- /dev/null +++ b/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) diff --git a/GEMstack/offboard/calibration/capture_ouster_oak.py b/GEMstack/offboard/calibration/capture_ouster_oak.py new file mode 100644 index 000000000..3c9e1e6af --- /dev/null +++ b/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) diff --git a/GEMstack/offboard/calibration/icp.py b/GEMstack/offboard/calibration/icp.py new file mode 100644 index 000000000..376c285a2 --- /dev/null +++ b/GEMstack/offboard/calibration/icp.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python3 + +import os +import glob +import numpy as np +import cv2 +import open3d as o3d + +# --- Adjust these to match your intrinsics and data details --- +INTRINSICS = np.array([ + [684.83331299, 0. , 573.37109375], + [ 0. , 684.60968018, 363.70092773], + [ 0. , 0. , 1. ] +], dtype=np.float32) + +# Depth scale: how to go from raw .tif values to actual "Z" in meters. +# In your capture code, you used: dimage = (dimage/4000*0xffff) +# Possibly each pixel is stored with range 0..65535. Adjust as needed. +DEPTH_SCALE = 4000.0 # Example factor if your depth was in mm or a certain scale. + +def depth_to_points(depth_img: np.ndarray, intrinsics: np.ndarray): + """ + Convert a single-channel depth image into an Nx3 array of 3D points + in the camera coordinate system. + - depth_img: 2D array of type uint16 or float with depth values + - intrinsics: 3x3 camera matrix + """ + fx = intrinsics[0,0] + fy = intrinsics[1,1] + cx = intrinsics[0,2] + cy = intrinsics[1,2] + + # Indices of each pixel + h, w = depth_img.shape + i_range = np.arange(h) + j_range = np.arange(w) + jj, ii = np.meshgrid(j_range, i_range) # shape (h,w) + + # Flatten + ii = ii.flatten().astype(np.float32) + jj = jj.flatten().astype(np.float32) + depth = depth_img.flatten().astype(np.float32) + + # Filter out zero / invalid depths + valid_mask = (depth > 0) + ii = ii[valid_mask] + jj = jj[valid_mask] + depth = depth[valid_mask] + + # Reproject to 3D (camera frame) + # X = (x - cx) * Z / fx, Y = (y - cy) * Z / fy, Z = depth + # Note: Z must be in meters or consistent units + z = depth / DEPTH_SCALE # Convert from your .tif scale to real meters + x = (jj - cx) * z / fx + y = (ii - cy) * z / fy + points3d = np.stack((x, y, z), axis=-1) # shape (N,3) + + return points3d + +def load_lidar_points(npz_file: str): + """ + Load the Nx3 LiDAR points from a .npz file created by 'np.savez'. + """ + data = np.load(npz_file) + # The capture code used: np.savez(lidar_fn, pc) + # So 'data' might have the default key 'arr_0' or 'pc' if named + # Inspect data.files to see. Let's assume 'arr_0' or single key: + arr_key = data.files[0] + points = data[arr_key] + # shape check, must be Nx3 or Nx4, ... + return points + +def make_open3d_pcd(points: np.ndarray): + """ + Convert Nx3 numpy array into an Open3D point cloud object. + """ + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + return pcd + +def perform_icp(camera_pcd: o3d.geometry.PointCloud, + lidar_pcd: o3d.geometry.PointCloud): + """ + Perform local ICP alignment of camera_pcd (source) to lidar_pcd (target). + Returns a transformation 4x4 that maps camera -> lidar (or vice versa). + """ + # 1) Estimate normals if you want point-to-plane + lidar_pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid( + radius=1.0, max_nn=30)) + camera_pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid( + radius=1.0, max_nn=30)) + + # 2) Initial guess: identity or something better if you have one + init_guess = np.eye(4) + + # 3) Choose a threshold for inlier distance + threshold = 0.5 # adjust based on your environment scale + + # 4) Run ICP (point-to-plane or point-to-point) + print("[ICP] Running ICP alignment ...") + result = o3d.pipelines.registration.registration_icp( + camera_pcd, # source + lidar_pcd, # target + threshold, + init_guess, + estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPlane() + ) + + print("[ICP] Done. Fitness = %.4f, RMSE = %.4f" % (result.fitness, result.inlier_rmse)) + print("[ICP] Transformation:\n", result.transformation) + return result.transformation + +def main(folder='data'): + color_files = sorted(glob.glob(os.path.join(folder, "color*.png"))) + icp_results = [] # to store (index, transform, fitness, rmse) + + for color_path in color_files: + # Extract index from filename, e.g. color10.png -> 10 + basename = os.path.basename(color_path) + idx_str = basename.replace("color","").replace(".png","") + + if int(idx_str) in range(77): + depth_path = os.path.join(folder, f"depth{idx_str}.tif") + lidar_path = os.path.join(folder, f"lidar{idx_str}.npz") + + if not (os.path.exists(depth_path) and os.path.exists(lidar_path)): + continue + + # Load depth / convert to 3D + depth_img = cv2.imread(depth_path, cv2.IMREAD_UNCHANGED) + if depth_img is None: + continue + camera_points = depth_to_points(depth_img, INTRINSICS) + camera_pcd = make_open3d_pcd(camera_points) + + # Load LiDAR + lidar_points = load_lidar_points(lidar_path) + lidar_pcd = make_open3d_pcd(lidar_points) + + lidar_pcd.estimate_normals( + search_param=o3d.geometry.KDTreeSearchParamHybrid( + radius=1.0, # Adjust based on your scene scale + max_nn=30 + ) + ) + + # Also estimate normals on the Camera (source) cloud (recommended) + camera_pcd.estimate_normals( + search_param=o3d.geometry.KDTreeSearchParamHybrid( + radius=1.0, + max_nn=30 + ) + ) + + # Now you can run Point-to-Plane ICP + init_guess = np.eye(4) + threshold = 0.5 # or some appropriate distance + result_icp = o3d.pipelines.registration.registration_icp( + camera_pcd, + lidar_pcd, + threshold, + init_guess, + o3d.pipelines.registration.TransformationEstimationPointToPlane() + ) + + print("ICP result:", result_icp.transformation) + print("Fitness:", result_icp.fitness, " RMSE:", result_icp.inlier_rmse) + + # After processing all frames, we can analyze or average + if len(icp_results) > 0: + # Example: pick the best frame by highest fitness + best_frame = max(icp_results, key=lambda x: x[2]) # x[2] is fitness + print("\nBest frame by fitness was index=", best_frame[0], + " with fitness=", best_frame[2], " rmse=", best_frame[3]) + else: + print("No frames processed properly.") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/GEMstack/offboard/calibration/klampt_lidar_ouster_show.py b/GEMstack/offboard/calibration/klampt_lidar_ouster_show.py new file mode 100644 index 000000000..11a9fcbcc --- /dev/null +++ b/GEMstack/offboard/calibration/klampt_lidar_ouster_show.py @@ -0,0 +1,183 @@ +from klampt import vis +from klampt.math import so3,se3 +from klampt.vis.colorize import colorize +from klampt import PointCloud,Geometry3D +from klampt.io import numpy_convert +from klampt.model.sensing import image_to_points +from klampt.model.create import bbox +import klampt +import cv2 +import os +import numpy as np +import math +import time + +#uncalibrated values -- TODO: load these from a calibration file +zed_K = np.array([[684.8333, 0.0, 573.371], + [0.0, 684.609, 363.7], + [0.0, 0.0, 1.0]]) +zed_intrinsics = [zed_K[0,0],zed_K[1,1],zed_K[0,2],zed_K[1,2]] +zed_w = 1152 +zed_h = 720 + + +def main(folder): + lidar_xform = se3.identity() + zed_xform = (so3.from_ndarray(np.array([[0,0,1],[-1,0,0],[0,-1,0]])),[0,0,0]) + lidar_pattern = os.path.join(folder,"lidar{}.npz") + color_pattern = os.path.join(folder,"color{}.png") + depth_pattern = os.path.join(folder,"depth{}.tif") + data = {} + def load_and_show_scan(idx): + arr_compressed = np.load(lidar_pattern.format(idx)) + arr = arr_compressed['arr_0'] + arr_compressed.close() + pc = numpy_convert.from_numpy(arr,'PointCloud') + pc = colorize(pc,'z','plasma') + data['lidar'] = Geometry3D(pc) + + try: # might need some modifications to work with our code + color = cv2.imread(color_pattern.format(idx)) + depth = cv2.imread(depth_pattern.format(idx),cv2.IMREAD_UNCHANGED) + depth = depth.astype(np.float32) + print("depth range",np.min(depth),np.max(depth)) + zed_xfov = 2*np.arctan(zed_w/(2*zed_intrinsics[0])) + zed_yfov = 2*np.arctan(zed_h/(2*zed_intrinsics[1])) + print("estimated zed horizontal FOV",math.degrees(zed_xfov),"deg") + print(f"Depth image shape: {depth.shape}, dtype: {depth.dtype}, min: {np.min(depth)}, max: {np.max(depth)}") + print(f"Color image shape: {color.shape}, dtype: {color.dtype}, min: {np.min(color)}, max: {np.max(color)}") + image_to_points( + depth, + color, + intrinsics=None, + xfov=zed_xfov, + yfov=zed_yfov, + depth_scale=4000.0 / 0xffff + ) + + except Exception as e: + print("Error loading zed data:",e) + pc = PointCloud() + + data['oak'] = Geometry3D(pc) + data['lidar'].setCurrentTransform(*lidar_xform) + data['oak'].setCurrentTransform(*zed_xform) + vis.add('lidar',data['lidar']) + vis.add('oak',data['oak']) + + data['index'] = 1 + def increment_index(): + data['index'] += 1 + try: + load_and_show_scan(data['index']) + except Exception: + data['index'] -= 1 + return + def decrement_index(): + data['index'] -= 1 + try: + load_and_show_scan(data['index']) + except Exception: + data['index'] += 1 + return + def print_xforms(): + print("lidar:") + print("rotation:",so3.ndarray(lidar_xform[0])) + print("position:",lidar_xform[1]) + print("zed:") + print("rotation:",so3.ndarray(zed_xform[0])) + print("position:",zed_xform[1]) + + point_selection = (4,0,0) + box_selection = [(3.5,-0.5,-0.5),(4.5,0.5,0.5)] + box_geometry = bbox(box_selection[0],box_selection[1],type='GeometricPrimitive') + data['selection_widget'] = None + + def select_point(): + if data['selection_widget'] is not None: + vis.remove(data['selection_widget']) + data['selection_widget'] = 'point_widget' + vis.add('point_widget',point_selection) + vis.edit('point_widget') + + def select_box(): + if data['selection_widget'] is not None: + vis.remove(data['selection_widget']) + data['selection_widget'] = 'box_widget' + vis.add('box_widget',box_geometry,color=(1.0,0.5,0,0.5)) + vis.add('box_widget_handle1',box_selection[0]) + vis.edit('box_widget_handle1') + vis.add('box_widget_handle2',box_selection[1]) + vis.edit('box_widget_handle2') + + def print_selection(): + if data['selection_widget'] is not None: + if data['selection_widget'] == 'point_widget': + print("Target point:",point_selection) + lidar = data['lidar'] # type: Geometry3D + pts = lidar.getPointCloud().getPoints() + pts = pts @ so3.ndarray(lidar_xform[0]).T + np.array(lidar_xform[1]) + dist_2 = np.sum((pts - point_selection)**2, axis=1) + closest_dist2 = np.min(dist_2) + closest_ind = np.argmin(dist_2) + print("Closest lidar point is",pts[closest_ind],"index",closest_ind,"at distance",math.sqrt(closest_dist2)) + else: + print("Target box:",box_selection) + lidar = data['lidar'] # type: Geometry3D + pts = lidar.getPointCloud().getPoints() + pts = pts @ so3.ndarray(lidar_xform[0]).T + np.array(lidar_xform[1]) + mask = np.logical_and(np.all(pts >= box_selection[0],axis=1),np.all(pts <= box_selection[1],axis=1)) + print("Number of lidar points in box:",np.sum(mask)) + print("Indices of lidar points in box:",np.where(mask)[0]) + print("Points in box",pts[mask]) + else: + print("No selection") + + vis.addAction(select_point,'Select with point','s','Select a point by dragging a point widget') + vis.addAction(select_box,'Select with box','b','Select multiple points with a box widget') + vis.addAction(print_selection,'Print selection','q') + + vis.addAction(increment_index,"Increment index",'=') + vis.addAction(decrement_index,"Decrement index",'-') + vis.addAction(print_xforms,'Print transforms','p') + load_and_show_scan(1) + vis.add('zed_xform',zed_xform) + vis.add('lidar_xform',lidar_xform) + vis.edit('zed_xform') + vis.edit('lidar_xform') + vis.show() + while vis.shown(): + lidar_xform = vis.getItemConfig('lidar_xform') + lidar_xform = lidar_xform[:9],lidar_xform[9:] + zed_xform = vis.getItemConfig('zed_xform') + zed_xform = zed_xform[:9],zed_xform[9:] + if data['selection_widget'] == 'point_widget': + point_selection = vis.getItemConfig('point_widget') + elif data['selection_widget'] == 'box_widget': + handle1 = vis.getItemConfig('box_widget_handle1') + handle2 = vis.getItemConfig('box_widget_handle2') + lower = [min(handle1[i],handle2[i]) for i in range(3)] + upper = [max(handle1[i],handle2[i]) for i in range(3)] + box_selection = [lower,upper] + box_geometry = bbox(lower,upper,type='GeometricPrimitive') + vis.add('box_widget',box_geometry,color=(1.0,0.5,0,0.5)) + data['lidar'].setCurrentTransform(*lidar_xform) + data['oak'].setCurrentTransform(*zed_xform) + time.sleep(0.02) + vis.kill() + +if __name__ == '__main__': + import sys + folder = 'data' + if len(sys.argv) >= 2: + folder = sys.argv[1] + if len(sys.argv) >= 3: + calib = sys.argv[2] + import yaml + with open(calib,'r') as f: + config = yaml.load(f,yaml.SafeLoader) + zed_K = np.array(config['K']).reshape((3,3)) + zed_intrinsics = [zed_K[0,0],zed_K[1,1],zed_K[0,2],zed_K[1,2]] + zed_w = config['width'] + zed_height = config['height'] + main(folder) diff --git a/GEMstack/offboard/calibration/lidar_to_camera_manual.py b/GEMstack/offboard/calibration/lidar_to_camera_manual.py new file mode 100644 index 000000000..4d3cc9719 --- /dev/null +++ b/GEMstack/offboard/calibration/lidar_to_camera_manual.py @@ -0,0 +1,191 @@ +#%% +import cv2 +from scipy.spatial.transform import Rotation as R +import matplotlib.pyplot as plt +import numpy as np + +rgb_path = '/mount/wp/GEMstack/data/color21.png' +depth_path = '/mount/wp/GEMstack/data/depth21.tif' +lidar_path = '/mount/wp/GEMstack/data/lidar21.npz' + +img = cv2.imread(rgb_path, cv2.IMREAD_UNCHANGED) + +lidar_points = np.load(lidar_path)['arr_0'] +lidar_points = lidar_points[~np.all(lidar_points== 0, axis=1)] # remove (0,0,0)'s + +rx,ry,rz = 0.006898647163954201, 0.023800082245145304, -0.025318355743942974 +tx,ty,tz = -1.1, 0.037735827433173136, 1.953202227766785 +rot = R.from_euler('xyz',[rx,ry,rz]).as_matrix() +lidar_ex = np.hstack([rot,[[tx],[ty],[tz]]]) +lidar_ex = np.vstack([lidar_ex,[0,0,0,1]]) + +camera_in = np.array([ + [684.83331299, 0. , 573.37109375], + [ 0. , 684.60968018, 363.70092773], + [ 0. , 0. , 1. ] +], dtype=np.float32) + +#%% +# blurred = cv2.GaussianBlur(img, (5, 5), 0) +# edges = cv2.Canny(blurred, threshold1=50, threshold2=150) +# plt.imshow(edges) +plt.imshow(cv2.cvtColor(img,cv2.COLOR_BGR2RGB)) + +#%% +import pyvista as pv +def vis(title='', ratio=1): + print(title) + pv.set_jupyter_backend('client') + plotter = pv.Plotter(notebook=True) + plotter.show_axes() + class foo: + def set_cam(self,pos=(-20*ratio,0,20*ratio),foc=(0,0,0)): + plotter.camera.position = pos + plotter.camera.focal_point = foc + return self + def add_pc(self,pc,ratio=ratio,**kargs): + plotter.add_mesh( + pv.PolyData(pc*ratio), + render_points_as_spheres=True, + point_size=2, + **kargs) + return self + def add_line(self,p1,p2,ratio=ratio,**kargs): + plotter.add_mesh( + pv.Line(p1*ratio,p2*ratio), + **kargs, + line_width=1) + return self + def add_box(self,bound,trans,ratio=ratio): + l,w,h = map(lambda x:x*ratio,bound) + box = pv.Box(bounds=(-l/2,l/2,-w/2,w/2,-h/2,h/2)) + box = box.translate(list(map(lambda x:x*ratio,trans))) + plotter.add_mesh(box, color='yellow') + return self + def show(self): + plotter.show() + return self + def close(self): + plotter.close() + return None + + return foo().set_cam() +def crop(pc,ix=None,iy=None,iz=None): + # crop a subrectangle in a pointcloud + # usage: crop(pc, ix = (x_min,x_max), ...) + # return: array(N,3) + mask = True + for dim,intervel in zip([0,1,2],[ix,iy,iz]): + if not intervel: continue + d,u = intervel + mask &= pc[:,dim] >= d + mask &= pc[:,dim] <= u + print(f'points left after cropping {dim}\'th dim',mask.sum()) + return pc[mask] + + +lidar_post = np.pad(lidar_points,((0,0),(0,1)),constant_values=1) @ lidar_ex.T[:,:3] +lidar_post = crop(lidar_post,ix=(0,8),iy=(-5,5)) +vis().add_pc(lidar_post).show() + +#%% +def pick_4_img(img): + corners = [] # Reset the corners list + def click_event(event, x, y, flags, param): + if event == cv2.EVENT_LBUTTONDOWN: + corners.append((x, y)) + cv2.circle(param, (x, y), 5, (0, 255, 0), -1) + cv2.imshow('Image', param) + + cv2.imshow('Image', img) + cv2.setMouseCallback('Image', click_event, img) + + while True: + if len(corners) == 4: + break + if cv2.waitKey(1) & 0xFF == ord('q'): + return None + + cv2.destroyAllWindows() + + return corners +cpoints = np.array(pick_4_img(img)).astype(float) +print(cpoints) + +#%% +def pick_4_pc(point_cloud): + points = [] + def cb(pt,*args): + points.append(pt) + while len(points)!=4: + points = [] + cloud = pv.PolyData(point_cloud) + plotter = pv.Plotter(notebook=False) + plotter.camera.position = (-20,0,20) + plotter.camera.focal_point = (0,0,0) + plotter.add_mesh(cloud, color='lightblue', point_size=5, render_points_as_spheres=True) + plotter.enable_point_picking(cb) + plotter.show() + return points + +lpoints = np.array(pick_4_pc(lidar_post)) +print(lpoints) +# %% +success, rvec, tvec = cv2.solvePnP(lpoints, cpoints, camera_in, None) +R, _ = cv2.Rodrigues(rvec) + +T = np.eye(4) +T[:3, :3] = R +T[:3, 3] = tvec.flatten() +print(T) + + +#%% +def depth_to_points(depth_img: np.ndarray, intrinsics: np.ndarray): + """ + Convert a single-channel depth image into an Nx3 array of 3D points + in the camera coordinate system. + - depth_img: 2D array of type uint16 or float with depth values + - intrinsics: 3x3 camera matrix + """ + fx = intrinsics[0,0] + fy = intrinsics[1,1] + cx = intrinsics[0,2] + cy = intrinsics[1,2] + + # Indices of each pixel + h, w = depth_img.shape + i_range = np.arange(h) + j_range = np.arange(w) + jj, ii = np.meshgrid(j_range, i_range) # shape (h,w) + + # Flatten + ii = ii.flatten().astype(np.float32) + jj = jj.flatten().astype(np.float32) + depth = depth_img.flatten().astype(np.float32) + + # Filter out zero / invalid depths + valid_mask = (depth > 0) + ii = ii[valid_mask] + jj = jj[valid_mask] + depth = depth[valid_mask] + + z = depth / 10000 + x = (jj - cx) * z / fx + y = (ii - cy) * z / fy + points3d = np.stack((x, y, z), axis=-1) # shape (N,3) + + return points3d + + + +depth_img = cv2.imread(depth_path, cv2.IMREAD_UNCHANGED) +camera_points = depth_to_points(depth_img, camera_in) + +v=vis() +v.add_pc(np.pad(lidar_points,((0,0),(0,1)),constant_values=1)@lidar_ex.T@T.T[:,:3],color='blue') +v.add_pc(camera_points,color='red') +v.show() + +#%% +print(np.vstack([(lidar_ex.T@T.T[:,:3]).T,[0,0,0,1]])) \ No newline at end of file diff --git a/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py b/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py new file mode 100644 index 000000000..6e0b25940 --- /dev/null +++ b/GEMstack/offboard/calibration/make_gem_e4_ouster_v2.py @@ -0,0 +1,243 @@ +#%% +import os +import sys +import math +import numpy as np +from scipy.spatial.transform import Rotation as R +os.getcwd() +VIS = False # True to show visuals + +#%% things to extract +tx,ty,tz,rx,ry,rz = [None] * 6 + +#%%============================================================== +#======================= util functions ========================= +#================================================================ +if VIS: + import pyvista as pv + import matplotlib.pyplot as plt +def vis(title='', ratio=1): + print(title) + pv.set_jupyter_backend('client') + plotter = pv.Plotter(notebook=True) + plotter.camera.position = (-20*ratio,0,20*ratio) + plotter.camera.focal_point = (0,0,0) + plotter.show_axes() + class foo: + def add_pc(self,pc,ratio=ratio,**kargs): + plotter.add_mesh( + pv.PolyData(pc*ratio), + render_points_as_spheres=True, + point_size=2, + **kargs) + return self + def add_line(self,p1,p2,ratio=ratio,**kargs): + plotter.add_mesh( + pv.Line(p1*ratio,p2*ratio), + **kargs, + line_width=1) + return self + def add_box(self,bound,trans,ratio=ratio): + l,w,h = map(lambda x:x*ratio,bound) + box = pv.Box(bounds=(-l/2,l/2,-w/2,w/2,-h/2,h/2)) + box = box.translate(list(map(lambda x:x*ratio,trans))) + plotter.add_mesh(box, color='yellow') + return self + def show(self): + plotter.show() + return self + + return foo() +def load_scene(path): + sc = np.load(path)['arr_0'] + sc = sc[~np.all(sc == 0, axis=1)] # remove (0,0,0)'s + return sc +def crop(pc,ix=None,iy=None,iz=None): + # crop a subrectangle in a pointcloud + # usage: crop(pc, ix = (x_min,x_max), ...) + # return: array(N,3) + mask = True + for dim,intervel in zip([0,1,2],[ix,iy,iz]): + if not intervel: continue + d,u = intervel + mask &= pc[:,dim] >= d + mask &= pc[:,dim] <= u + print(f'points left after cropping {dim}\'th dim',mask.sum()) + return pc[mask] + +from sklearn.linear_model import RANSACRegressor +from sklearn.linear_model import LinearRegression +def fit_plane_ransac(pc,tol=0.01): + # fit a line/plane/hyperplane in a pointcloud + # pc: np array (N,D). the pointcloud + # tol: the tolerance. default 0.01 + model = RANSACRegressor(LinearRegression(), residual_threshold=tol) + model.fit(pc[:,:-1], pc[:,-1]) + a = model.estimator_.coef_ + inter = model.estimator_.intercept_ + class foo: + def plot(self): + inliers = pc[model.inlier_mask_] + if pc.shape[1] == 2: + plt.title('ransac fitting line') + plt.scatter(pc[:,0], pc[:,1], color='blue', marker='o', s=1) + plt.scatter(inliers[:,0], inliers[:,1], color='red', marker='o', s=1) + x_line = np.linspace(0, max(pc[:,0]), 100).reshape(-1,1) + plt.plot(x_line, x_line * a[0] + inter, color='red', label='Fitted Line') + elif pc.shape[1] == 3: + vis('ransac fitting a plane').add_pc(pc).add_pc(inliers,color='red').show() + return self + + def results(self): + # return: array(D-1), float, array(N,3) + # ^: , coeffs, intercept toward the plane, inliers of the fit + return a,inter + return foo() + +from scipy.spatial import cKDTree +def pc_diff(pc0,pc1,tol=0.1): + # remove points in pc0 from pc1 s.t. euclidian distance is within tolerance + # return: array(N,3) + tree = cKDTree(pc0) + diff = [] + for i, point in enumerate(pc1): + _, idx = tree.query(point) + distance = np.linalg.norm(point - pc0[idx]) # Compute the distance + if distance > tol: + diff.append(point) + tree = cKDTree(pc1) + for i, point in enumerate(pc0): + _, idx = tree.query(point) + distance = np.linalg.norm(point - pc1[idx]) # Compute the distance + if distance > tol: + diff.append(point) + return np.array(diff) + + + + + + +#%%============================================================== +#========================= tz rx ry ============================= +#================================================================ + +#%% load scene for ground plane +sc = load_scene('/mount/wp/GEMstack/data/lidar70.npz') + +# %% we crop to keep the ground +cropped_sc = crop(sc,iz = (-3,-2)) +if VIS: + vis('ground points cropped').add_pc(cropped_sc,color='blue').show() + +#%% +from math import sqrt +fit = fit_plane_ransac(cropped_sc,tol=0.01) # small tol because the ground is very flat +c, inter = fit.results() +normv = np.array([c[0], c[1], -1]) +normv /= np.linalg.norm(normv) +nx,ny,nz = normv +height = nz * inter +# TODO MAGIC NUMBER WARNING +height_axel = 0.2794 # 11 inches that we measured +tz = height - height_axel +if VIS: + fit.plot() +rx = -math.atan2(ny,-nz) +ry = -math.atan2(-nx,-nz) + + +if VIS: + from scipy.spatial.transform import Rotation as R + rot = R.from_euler('xyz',[rx,ry,0]).as_matrix() + cal_sc = sc @ rot.T + [0,0,tz] + vis('yz projection').add_pc(cal_sc*[0,1,1],color='blue').show() + vis('xz projection').add_pc(cal_sc*[1,0,1],color='blue').show() + +#%%============================================================== +#========================== tx ty rz ============================ +#================================================================ + +rot = R.from_euler('xyz',[rx,ry,0]).as_matrix() + +if False: # True to use the diff method to extract object. + # load data + sc0 = load_scene('/mount/wp/GEMstack/data/lidar70.npz') + sc1 = load_scene('/mount/wp/GEMstack/data/lidar78.npz') + + sc0 = sc0 @ rot.T + [0,0,tz] + sc1 = sc1 @ rot.T + [0,0,tz] + + # crop to only keep a frontal box area + area = (-0,7),(-1,1),(-3,1) + cropped0 = crop(sc0,*area) + cropped1 = crop(sc1,*area) + print(cropped0.shape) + print(cropped1.shape) + + # Take difference to only keep added object + objects = pc_diff(cropped0,cropped1) + +else: #False to use only cropping + sc1 = load_scene('/mount/wp/GEMstack/data/lidar1.npz') + + objects = sc1 @ rot.T + [0,0,tz] + + # crop to only keep a frontal box area + area = (-0,20),(-1,1),(0,1) + objects = crop(objects,*area) + print(objects.shape) + + +#%% +if VIS: + vis().add_pc(objects,color='blue').show() #visualize diff, hopefully the added objects + +# %% use the added objects to find rz. +# TODO after dataset retake +# right now we assume tx = ty = 0 and \ +# just use median to find a headding direction +fit = fit_plane_ransac(objects[:,:2],tol=1) # tol=1 because 1m^3 foam boxes +c,inter = fit.results() +if VIS: + fit.plot() +# tx = ty = 0 +# hx,hy = np.median(diff,axis=0)[:2] +# rz = -np.arctan2(hy,hx) +tx = - (2.56 - 1.46) # https://publish.illinois.edu/robotics-autonomy-resources/gem-e4/hardware/ +ty = - inter +rz = - math.atan(c) + +if VIS: + p1 = (0,inter,0) + p2 = max(objects[:,0])*np.array([1,c[0],0])+np.array([0,inter,0]) + vis().add_pc(sc1*np.array([1,1,0]),color='blue').add_line(p1,p2,color='red').show() + + from scipy.spatial.transform import Rotation as R + rot = R.from_euler('xyz',[0,0,rz]).as_matrix() + cal_sc1 = sc1 @ rot.T + [tx,ty,0] + vis().add_pc(cal_sc1,color='blue').show() + + +#%% visualize calibrated pointcloud +if VIS: + rot = R.from_euler('xyz',[rx,ry,rz]).as_matrix() + + cal_sc1 = sc1 @ rot.T + [tx,ty,tz] + # projection + # cal_sc1[:,1] = 0 + v = vis(ratio=100) + v.add_pc(cal_sc1*[0,1,1],color='blue') + v.add_box((2.56,.61*2,2.03+height_axel),[2.56/2,0,(2.03+height_axel)/2]) + v.show() + # the yellow box should be 11 inches above the ground + # rear-axel center should be at (0,0,0) +# %% +print(f""" +translation: ({tx,ty,tz}) +rotation: ({rx,ry,rz}) +""") + + + +# %% diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index e13ff817e..fc6431f21 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -26,6 +26,7 @@ import numpy as np from ...utils import conversions + class GEMHardwareInterface(GEMInterface): """Interface for connnecting to the physical GEM e2 vehicle.""" def __init__(self): diff --git a/GEMstack/onboard/interface/gem_simulator.py b/GEMstack/onboard/interface/gem_simulator.py index 2abfde71f..d1574e7f7 100644 --- a/GEMstack/onboard/interface/gem_simulator.py +++ b/GEMstack/onboard/interface/gem_simulator.py @@ -207,7 +207,7 @@ def simulate(self, T : float, command : Optional[GEMVehicleCommand]): reading.brake_pedal_position = brake_pedal_position reading.accelerator_pedal_position = 0 reading.speed = v - if v > 0: + if v >= 0: reading.gear = 1 else: reading.gear = -1 diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py new file mode 100644 index 000000000..3134b1e35 --- /dev/null +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -0,0 +1,84 @@ +from ...state import AllState,VehicleState,ObjectPose,ObjectFrameEnum,AgentState,AgentEnum,AgentActivityEnum +from ..interface.gem import GEMInterface +from ..component import Component +#from ultralytics import YOLO +#import cv2 +from typing import Dict + +def box_to_fake_agent(box): + """Creates a fake agent state from an (x,y,w,h) bounding box. + + The location and size are pretty much meaningless since this is just giving a 2D location. + """ + x,y,w,h = box + pose = ObjectPose(t=0,x=x+w/2,y=y+h/2,z=0,yaw=0,pitch=0,roll=0,frame=ObjectFrameEnum.CURRENT) + dims = (w,h,0) + return AgentState(pose=pose,dimensions=dims,outline=None,type=AgentEnum.PEDESTRIAN,activity=AgentActivityEnum.MOVING,velocity=(0,0,0),yaw_rate=0) + + +class PedestrianDetector2D(Component): + """Detects pedestrians.""" + def __init__(self,vehicle_interface : GEMInterface): + self.vehicle_interface = vehicle_interface + #self.detector = YOLO('../../knowledge/detection/yolov8n.pt') + self.last_person_boxes = [] + + def rate(self): + return 4.0 + + def state_inputs(self): + return ['vehicle'] + + def state_outputs(self): + return ['agents'] + + def initialize(self): + #tell the vehicle to use image_callback whenever 'front_camera' gets a reading, and it expects images of type cv2.Mat + #self.vehicle_interface.subscribe_sensor('front_camera',self.image_callback,cv2.Mat) + pass + + #def image_callback(self, image : cv2.Mat): + # detection_result = self.detector(image) + # self.last_person_boxes = [] + # #uncomment if you want to debug the detector... + # #for bb in self.last_person_boxes: + # # x,y,w,h = bb + # # cv2.rectangle(image, (int(x-w/2), int(y-h/2)), (int(x+w/2), int(y+h/2)), (255, 0, 255), 3) + # #cv2.imwrite("pedestrian_detections.png",image) + + def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: + res = {} + for i,b in enumerate(self.last_person_boxes): + x,y,w,h = b + res['pedestrian'+str(i)] = box_to_fake_agent(b) + if len(res) > 0: + print("Detected",len(res),"pedestrians") + return res + + +class FakePedestrianDetector2D(Component): + """Triggers a pedestrian detection at some random time ranges""" + def __init__(self,vehicle_interface : GEMInterface): + self.vehicle_interface = vehicle_interface + self.times = [(5.0,20.0),(30.0,35.0)] + self.t_start = None + + def rate(self): + return 4.0 + + def state_inputs(self): + return ['vehicle'] + + def state_outputs(self): + return ['agents'] + + def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: + if self.t_start is None: + self.t_start = self.vehicle_interface.time() + t = self.vehicle_interface.time() - self.t_start + res = {} + for times in self.times: + if t >= times[0] and t <= times[1]: + res['pedestrian0'] = box_to_fake_agent((0,0,0,0)) + print("Detected a pedestrian") + return res diff --git a/GEMstack/onboard/perception/person_detector.py b/GEMstack/onboard/perception/person_detector.py new file mode 100644 index 000000000..8f4fa3588 --- /dev/null +++ b/GEMstack/onboard/perception/person_detector.py @@ -0,0 +1,48 @@ +#from ultralytics import YOLO +import cv2 +import sys + +def person_detector(img : cv2.Mat): + #TODO: implement me to produce a list of (x,y,w,h) bounding boxes of people in the image + return [] + +def main(fn): + image = cv2.imread(fn) + bboxes = person_detector(image) + print("Detected",len(bboxes),"people") + for bb in bboxes: + x,y,w,h = bb + if not isinstance(x,(int,float)) or not isinstance(y,(int,float)) or not isinstance(w,(int,float)) or not isinstance(h,(int,float)): + print("WARNING: make sure to return Python numbers rather than PyTorch Tensors") + print("Corner",(x,y),"size",(w,h)) + cv2.rectangle(image, (int(x-w/2), int(y-h/2)), (int(x+w/2), int(y+h/2)), (255, 0, 255), 3) + cv2.imshow('Results', image) + cv2.waitKey(0) + +def main_webcam(): + cap = cv2.VideoCapture(0) + cap.set(3, 640) + cap.set(4, 480) + + print("Press space to exit") + while True: + _, image = cap.read() + + bboxes = person_detector(image) + for bb in bboxes: + x,y,w,h = bb + cv2.rectangle(image, (int(x-w/2), int(y-h/2)), (int(x+w/2), int(y+h/2)), (255, 0, 255), 3) + + cv2.imshow('Person detection', image) + if cv2.waitKey(1) & 0xFF == ord(' '): + break + + cap.release() + + +if __name__ == '__main__': + fn = sys.argv[1] + if fn != 'webcam': + main(fn) + else: + main_webcam() \ No newline at end of file diff --git a/GEMstack/onboard/perception/state_estimation.py b/GEMstack/onboard/perception/state_estimation.py index 4aef25659..db96ad38f 100644 --- a/GEMstack/onboard/perception/state_estimation.py +++ b/GEMstack/onboard/perception/state_estimation.py @@ -37,7 +37,8 @@ def state_outputs(self) -> List[str]: return ['vehicle'] def healthy(self): - return self.gnss_pose is not None + return True + # return self.gnss_pose is not None TODO: fix def update(self) -> VehicleState: if self.gnss_pose is None: diff --git a/GEMstack/onboard/planning/blink_component.py b/GEMstack/onboard/planning/blink_component.py index 9ee6a9164..acd40ed0e 100644 --- a/GEMstack/onboard/planning/blink_component.py +++ b/GEMstack/onboard/planning/blink_component.py @@ -1,6 +1,8 @@ from ..component import Component from ..interface.gem import GEMInterface,GEMVehicleCommand,GEMVehicleReading +from ...state import AllState,VehicleIntent import time +from typing import List TURN_OFF = 0 TURN_LEFT = 1 @@ -32,27 +34,30 @@ def cleanup(self): self.send_turn_command(TURN_OFF) # pass - def update(self): + def state_inputs(self) -> List[str]: + """Returns the list of AllState inputs this component requires.""" + return ["intent"] + + def update(self, state: AllState): """Run in a loop""" # we need to set up a GEMVehicleCommand which encapsulates all commands that will be # sent to the drive-by-wire system, simultaneously. To avoid doing arbitrary things # to the vehicle, let's maintain the current values (e.g., accelerator, brake pedal, # steering angle) from its current readings. - - #if Allstate.intent.intent == "HALTING" : - #return current_time = time.time() - if current_time - self.last_update_time >= 2: # Change signal every 2 seconds - if self.turn_state == TURN_OFF: - self.turn_state = TURN_LEFT - elif self.turn_state == TURN_LEFT: - self.turn_state = TURN_RIGHT - else: - self.turn_state = TURN_OFF + if state.intent == 2: # check if halting + + if current_time - self.last_update_time >= 2: # Change signal every 2 seconds + if self.turn_state == TURN_OFF: + self.turn_state = TURN_LEFT + elif self.turn_state == TURN_LEFT: + self.turn_state = TURN_RIGHT + else: + self.turn_state = TURN_OFF - self.send_turn_command(self.turn_state) - self.last_update_time = current_time - + self.send_turn_command(self.turn_state) + self.last_update_time = current_time + # Read vehicle sensor data vehicle_reading = self.vehicle_interface.get_reading() print(f"Vehicle Speed: {vehicle_reading.speed:.2f} m/s") diff --git a/GEMstack/onboard/planning/driving_logic_component.py b/GEMstack/onboard/planning/driving_logic_component.py new file mode 100644 index 000000000..157fb90ad --- /dev/null +++ b/GEMstack/onboard/planning/driving_logic_component.py @@ -0,0 +1,42 @@ +from typing import List +from ..component import Component +from ...state import AllState,VehicleIntent + +class DrivingLogic(Component): + # count of times blinked + """Base class for top-level components in the execution stack.""" + def rate(self) -> float: + """Returns the rate in Hz at which this component should be updated.""" + return .5 + def state_inputs(self) -> List[str]: + """Returns the list of AllState inputs this component requires.""" + return ['all'] + def state_outputs(self) -> List[str]: + """Returns the list of AllState outputs this component generates.""" + return ['intent'] + def healthy(self): + """Returns True if the element is in a stable state.""" + return True + def initialize(self): + """Initialize the component. This is called once before the first + update.""" + self.count = 0 + return + def cleanup(self): + """Cleans up resources used by the component. This is called once after + the last update.""" + pass + def update(self, state: AllState): + """Update the component.""" + + state.intent.intent = 2 + + return + def debug(self, item, value): + """Debugs a streaming value within this component""" + if hasattr(self, 'debugger'): + self.debugger.debug(item, value) + def debug_event(self, label): + """Debugs an event within this component""" + if hasattr(self, 'debugger'): + self.debugger.debug_event(label) \ No newline at end of file diff --git a/GEMstack/onboard/planning/longitudinal_planning.py b/GEMstack/onboard/planning/longitudinal_planning.py new file mode 100644 index 000000000..d967d688e --- /dev/null +++ b/GEMstack/onboard/planning/longitudinal_planning.py @@ -0,0 +1,928 @@ +from typing import List, Tuple +from ..component import Component +from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, ObjectFrameEnum, AgentState +from ...utils import serialization +from ...mathutils.transforms import vector_madd + +import time +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.patches as patches +import math + +def detect_collision(curr_x: float, curr_y: float, curr_v: float, obj: AgentState, min_deceleration: float, max_deceleration: float) -> Tuple[bool, float]: + """Detects if a collision will occur with the given object and return deceleration to avoid it.""" + + # Get the object's position and velocity + obj_x = obj.pose.x + obj_y = obj.pose.y + obj_v_x = obj.velocity[0] + obj_v_y = obj.velocity[1] + + pedestrian_length = 0.5 + pedestrian_width = 0.5 + + vehicle_length = 3.5 + vehicle_width = 2 + + vehicle_buffer_x = 3.0 + vehicle_buffer_y = 1.5 + + yield_buffer_y = 1.0 + + vehicle_front = curr_x + vehicle_length + vehicle_back = curr_x + vehicle_left = curr_y + vehicle_width / 2 + vehicle_right = curr_y - vehicle_width / 2 + + pedestrian_front = obj_x + pedestrian_length / 2 + pedestrian_back = obj_x - pedestrian_length / 2 + pedestrian_left = obj_y + pedestrian_width / 2 + pedestrian_right = obj_y - pedestrian_width / 2 + + # Check if the object is in front of the vehicle + if vehicle_front > pedestrian_back: + if vehicle_back > pedestrian_front: + # The object is behind the vehicle + print("Object is behind the vehicle") + return False, 0.0 + if vehicle_right - vehicle_buffer_y > pedestrian_left or vehicle_left + vehicle_buffer_y < pedestrian_right: + # The object is to the side of the vehicle + print("Object is to the side of the vehicle") + return False, 0.0 + # The object overlaps with the vehicle + return True, max_deceleration + + if vehicle_right - vehicle_buffer_y > pedestrian_left and obj_v_y <= 0: + # The object is to the right of the vehicle and moving away + print("Object is to the right of the vehicle and moving away") + return False, 0.0 + + if vehicle_left + vehicle_buffer_y < pedestrian_right and obj_v_y >= 0: + # The object is to the left of the vehicle and moving away + print("Object is to the left of the vehicle and moving away") + return False, 0.0 + + if vehicle_front + vehicle_buffer_x >= pedestrian_back: + # The object is in front of the vehicle and within the buffer + print("Object is in front of the vehicle and within the buffer") + return True, max_deceleration + + # Calculate the deceleration needed to avoid a collision + print("Object is in front of the vehicle and outside the buffer") + distance = pedestrian_back - vehicle_front - vehicle_buffer_x + + relative_v = curr_v - obj_v_x + if relative_v <= 0: + return False, 0.0 + + print(relative_v, distance) + + if obj_v_y > 0 and ((obj_y - curr_y) / relative_v) < ((vehicle_right - vehicle_buffer_y - yield_buffer_y - pedestrian_left) / abs(obj_v_y)): + # The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle + print("The object is to the right of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle") + return False, 0.0 + if obj_v_y < 0 and ((obj_y - curr_y) / relative_v) < ((pedestrian_right - vehicle_left - vehicle_buffer_y - yield_buffer_y) / abs(obj_v_y)): + # The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle + print("The object is to the left of the vehicle and moving towards it, but the vehicle will pass before the object reaches the vehicle") + return False, 0.0 + + if obj_v_y != 0: + if obj_v_y < 0: + # The object is moving toward the right side of the vehicle + distance_to_pass = obj_y - (vehicle_right - vehicle_buffer_y - yield_buffer_y) + pedestrian_width / 2 + elif obj_v_y > 0: + # The object is moving toward the left side of the vehicle + distance_to_pass = (vehicle_left + vehicle_buffer_y + yield_buffer_y) - obj_y + pedestrian_width / 2 + + time_to_pass = distance_to_pass / abs(obj_v_y) + + distance_to_move = pedestrian_back - vehicle_front - vehicle_buffer_x + time_to_pass * obj_v_y + + + # if curr_v**2/2*distance_to_pass >= 1.5: + # return True, curr_v**2/2*distance_to_pass + time_to_max_v = (5 - curr_v)/0.5 + + if time_to_max_v > time_to_pass: + if curr_v*time_to_pass + 0.25*time_to_pass**2 > distance_to_move: + return False, 0.0 + else: + if (25 - curr_v**2)*4 + (time_to_pass - time_to_max_v) * 5 >= distance_to_move: + return False, 0.0 + + + return True, [distance_to_move, time_to_pass] + + else: + deceleration = relative_v ** 2 / (2 * distance) + if deceleration > max_deceleration: + return True, max_deceleration + if deceleration < min_deceleration: + return False, 0.0 + + return True, deceleration + +def longitudinal_plan(path : Path, acceleration : float, deceleration : float, max_speed : float, current_speed : float, + method : str) -> Trajectory: + """Generates a longitudinal trajectory for a path with a + trapezoidal velocity profile. + + 1. accelerates from current speed toward max speed + 2. travel along max speed + 3. if at any point you can't brake before hitting the end of the path, + decelerate with accel = -deceleration until velocity goes to 0. + """ + + if method == "milestone": + return longitudinal_plan_milestone(path, acceleration, deceleration, max_speed, current_speed) + elif method == "dt": + return longitudinal_plan_dt(path, acceleration, deceleration, max_speed, current_speed) + elif method == "dx": + return longitudinal_plan_dx(path, acceleration, deceleration, max_speed, current_speed) + else: + raise NotImplementedError("Invalid method, only milestone, dt, adn dx are implemented.") + + +def longitudinal_plan_milestone(path : Path, acceleration : float, deceleration : float, max_speed : float, current_speed : float) -> Trajectory: + """Generates a longitudinal trajectory for a path with a + trapezoidal velocity profile. + + 1. accelerates from current speed toward max speed + 2. travel along max speed + 3. if at any point you can't brake before hitting the end of the path, + decelerate with accel = -deceleration until velocity goes to 0. + """ + path_normalized = path.arc_length_parameterize() + + #TODO: actually do something to points and times + points = [p for p in path_normalized.points] + times = [t for t in path_normalized.times] + #============================================= + + print("-----LONGITUDINAL PLAN-----") + print("path length: ", path.length()) + length = path.length() + + # If the path is too short, just return the path for preventing sudden halt of simulation + if length < 0.05: + return Trajectory(path.frame, points, times) + + # This assumes that the time denomination cannot be changed + + # Starting point + x0 = points[0][0] + cur_point = points[0] + cur_time = times[0] + cur_index = 0 + + new_points = [] + new_times = [] + velocities = [] # for graphing and debugging purposes + + while current_speed > 0 or cur_index == 0: + # we want to iterate through all the points and add them + # to the new points. However, we also want to add "critical points" + # where we reach top speed, begin decelerating, and stop + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + print("=====================================") + # print("new points: ", new_points) + # print("current index: ", cur_index) + # print("current speed: ", current_speed) + # print("current position: ", cur_point) + + # Information we will need: + # Calculate how much time it would take to stop + # Calculate how much distance it would take to stop + min_delta_t_stop = current_speed/deceleration + min_delta_x_stop = current_speed*min_delta_t_stop - 0.5*deceleration*min_delta_t_stop**2 + # print(min_delta_x_stop) + assert min_delta_x_stop >= 0 + + + # Check if we are done + + # If we cannot stop before or stop exactly at the final position requested + if cur_point[0] + min_delta_x_stop >= points[-1][0]: + print("In case one") + # put on the breaks + + # Calculate the next point in a special manner because of too-little time to stop + if cur_index == len(points)-1: + # the next point in this instance would be when we stop + next_point = (cur_point[0] + min_delta_x_stop, 0) + else: + next_point = points[cur_index+1] + + # keep breaking until the next milestone in path + if next_point[0] <= points[-1][0]: + # print("continuing to next point") + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = next_point + current_speed -= deceleration*delta_t_to_next_x + cur_index += 1 + else: + # continue to the point in which we would stop (current_velocity = 0) + # update to the next point + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_point = next_point + cur_time += delta_t_to_next_x + # current_speed would not be exactly zero error would be less than 1e-4 but perfer to just set to zero + #current_speed -= delta_t_to_next_x*deceleration + current_speed = 0 + assert current_speed == 0 + + # This is the case where we are accelerating to max speed + # because the first if-statement covers for when we decelerating, + # the only time current_speed < max_speed is when we are accelerating + elif current_speed < max_speed: + # print("In case two") + # next point + next_point = points[cur_index+1] + # accelerate to max speed + + # calculate the time it would take to reach max speed + delta_t_to_max_speed = (max_speed - current_speed)/acceleration + # calculate the distance it would take to reach max speed + delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2 + + delta_t_to_stop_from_max_speed = max_speed/deceleration + delta_x_to_stop_from_max_speed = max_speed*delta_t_to_stop_from_max_speed - 0.5*deceleration*delta_t_to_stop_from_max_speed**2 + + delta_t_to_next_point = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) + velocity_at_next_point = current_speed + delta_t_to_next_point*acceleration + time_to_stop_from_next_point = velocity_at_next_point/deceleration + delta_x_to_stop_from_next_point = velocity_at_next_point*time_to_stop_from_next_point - 0.5*deceleration*time_to_stop_from_next_point**2 + # if we would reach max speed after the next point, + # just move to the next point and update the current speed and time + if next_point[0] + delta_x_to_stop_from_next_point < points[-1][0] and \ + cur_point[0] + delta_x_to_max_speed >= next_point[0]: + # ("go to next point") + # accelerate to max speed + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed += delta_t_to_next_x*acceleration + cur_index += 1 + + # This is the case where we would need to start breaking before reaching + # top speed and before the next point (i.e. triangle shape velocity) + elif cur_point[0] + delta_x_to_max_speed + delta_x_to_stop_from_max_speed >= points[-1][0]: + # print(delta_x_to_max_speed) + # print(delta_x_to_stop_from_max_speed) + # Add a new point at the point where we should start breaking + # print("Adding new point to start breaking") + delta_t_to_next_x = compute_time_triangle(cur_point[0], points[-1][0], current_speed, 0, acceleration, deceleration) + # print(delta_t_to_next_x) + #delta_t_to_next_x = compute_time_to_x(cur_point[0], points[-1][0] - min_delta_x_stop, current_speed, acceleration) + next_x = cur_point[0] + current_speed*delta_t_to_next_x + 0.5*acceleration*delta_t_to_next_x**2 + cur_time += delta_t_to_next_x + cur_point = [next_x, 0] + current_speed += delta_t_to_next_x*acceleration + + # this is the case where we would reach max speed before the next point + # we need to create a new point where we would reach max speed + else: + # print("adding new point") + # we would need to add a new point at max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + # This is the case where we are at max speed + # special functionality is that this block must + # add a point where we would need to start declerating to reach + # the final point + elif current_speed == max_speed: + next_point = points[cur_index+1] + # continue on with max speed + # print("In case three") + + # add point to start decelerating + if next_point[0] + min_delta_x_stop >= points[-1][0]: + # print("Adding new point to start decelerating") + cur_time += (points[-1][0] - min_delta_x_stop - cur_point[0])/current_speed + cur_point = [points[-1][0] - min_delta_x_stop,0] + current_speed = max_speed + else: + # Continue on to next point + # print("Continuing on to next point") + cur_time += (next_point[0] - cur_point[0])/current_speed + cur_point = next_point + cur_index += 1 + + # This is an edge case and should only be reach + # if the initial speed is greater than the max speed + elif current_speed > max_speed: + # We need to hit the breaks + + next_point = points[cur_index+1] + # print("In case four") + # slow down to max speed + delta_t_to_max_speed = (current_speed - max_speed)/deceleration + delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2 + + # If we would reach the next point before slowing down to max speed + # keep going until we reach the next point + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed -= delta_t_to_next_x*deceleration + cur_index += 1 + else: + # We would reach max speed before the next point + # we need to add a new point at the point where we + # would reach max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + else: + # not sure what falls here + raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here") + + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + + points = new_points + times = new_times + print("[PLAN] Computed points:", points) + print("[TIME] Computed time:", times) + print("[Velocities] Computed velocities:", velocities) + + #============================================= + + trajectory = Trajectory(path.frame,points,times,velocities) + return trajectory + +def compute_time_to_x(x0 : float, x1 : float, v : float, a : float) -> float: + """Computes the time to go from x0 to x1 with initial velocity v0 and final velocity v1 + with constant acceleration a. I am assuming that we will always have a solution by settings + discriminant equal to zero, i'm not sure if this is an issue.""" + + """Consider changing the system to use linear operators instead of explicitly calculating because of instances here""" + # x1 = x0 + v0*t + 0.5*a*t^2 + # x1 - x0 = v0*t + 0.5*a*t^2 + # 0.5*a*t^2 + v0*t + x0 - x1 = 0 + # t = (-v0 + sqrt(v0^2 - 4*0.5*a*(x0-x1)))/(2*0.5*a) + # t = (-v0 + sqrt(v0^2 + 2*a*(x1-x0)))/a + # print("x0: ", x0) + # print("x1: ", x1) + # print("v: ", v) + # print("a: ", a) + + t1 = (-v + max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a + t2 = (-v - max(0,(v**2 - 2*a*(x0-x1)))**0.5)/a + + # print("t1: ", t1) + # print("t2: ", t2) + + if math.isnan(t1): t1 = 0 + if math.isnan(t2): t2 = 0 + + # print("t1: ", t1) + # print("t2: ", t2) + + valid_times = [n for n in [t1, t2] if n > 0] + if valid_times: + return min(valid_times) + else: + return 0.0 + +def compute_time_triangle(x0 : float, xf: float, v0: float, vf : float, acceleration : float, deceleration : float) -> float: + """ + Compute the time to go from current point assuming we are accelerating to the point at which + we would need to start breaking in order to reach the final point with velocity 0.""" + # xf = v0*t1 + .5a*t1^2 + v1t2 -0.5d*t2^2 + # 0 = v1 - d*t2 = v0 + a*t1 - d * t2 + # t1 = (d*t2 - v0)/a + # xf = v0*(d*t2 - v0)/a + 0.5*a*(d*t2 - v0)^2/a^2 + v1*t2 - 0.5*d*t2^2 + # xf = v0*d*t2/a - v0^2/a + 0.5*a*(d*t2^2 - 2*v0*d*t2 + v0^2)/a^2 + v1*t2 - 0.5*d*t2^2 + # 0 = t2^2(0.5*a*d/a^2 - 0.5*d) + t2(v0*d/a - v0*d*a/a^2 + v1) -v0^2/a +0.5*a*v0^2/a^2 -xf + roots = quad_root(0.5*acceleration + acceleration**2/deceleration - 0.5*acceleration**2/deceleration, + v0+2*acceleration*v0/deceleration - acceleration*v0/deceleration, + x0 - xf + v0**2/deceleration - 0.5*v0**2/deceleration) + print(roots) + t1 = max(roots) + assert t1 > 0 + return t1 + +def quad_root(a : float, b : float, c : float) -> float: + x1 = (-b + max(0,(b**2 - 4*a*c))**0.5)/(2*a) + x2 = (-b - max(0,(b**2 - 4*a*c))**0.5)/(2*a) + + # print("t1: ", t1) + # print("t2: ", t2) + + if math.isnan(x1): x1 = 0 + if math.isnan(x2): x2 = 0 + + # print("t1: ", t1) + # print("t2: ", t2) + + valid_roots = [n for n in [x1, x2] if not type(n) is complex] + # print(f"Valid roots {valid_roots}") + return valid_roots + +def solve_for_v_peak(v0: float, acceleration: float, deceleration: float, total_length: float) -> float: + + if acceleration <= 0 or deceleration <= 0: + raise ValueError("Acceleration and deceleration cant be negative") + + #Formuala: (v_peak^2 - v0^2)/(2*a) + v_peak^2/(2*d) = total_length + numerator = deceleration * v0**2 + 2 * acceleration * deceleration * total_length + denominator = acceleration + deceleration + v_peak_sq = numerator / denominator + + if v_peak_sq < 0: + return 0.0 + + return math.sqrt(v_peak_sq) + +def compute_dynamic_dt(acceleration, speed, k=0.01, a_min=0.5): + position_step = k * max(speed, 1.0) # Ensures position step is speed-dependent + return np.sqrt(2 * position_step / max(acceleration, a_min)) + +def longitudinal_plan_dt(path, acceleration: float, deceleration: float, max_speed: float, current_speed: float): + + + # 1 parametrizatiom. + path_norm = path.arc_length_parameterize(speed=1.0) + total_length = path.length() + + # ------------------- + # If the path is too short, just return the path for preventing sudden halt of simulation + if total_length < 0.05: + points = [p for p in path_norm.points] + times = [t for t in path_norm.times] + return Trajectory(path.frame, points, times) + # ------------------- + + # 2. Compute distances for d_accel,d_decel + if max_speed > current_speed: + d_accel = (max_speed**2 - current_speed**2) / (2 * acceleration) + else: + d_accel = 0.0 # Already at or above max_speed + + d_decel = (max_speed**2) / (2 * deceleration) + + # 3. trapezoidal or triangle? + if d_accel + d_decel <= total_length: + t_accel = (max_speed - current_speed) / acceleration if max_speed > current_speed else 0.0 + t_decel = max_speed / deceleration + d_cruise = total_length - d_accel - d_decel + t_cruise = d_cruise / max_speed if max_speed != 0 else 0.0 + t_final = t_accel + t_cruise + t_decel + profile_type = "trapezoidal" + else: + # Triangular profile: not enough distance to reach max_speed so we will calculate peak speed. + peak_speed = solve_for_v_peak(current_speed, acceleration, deceleration, total_length) + # choose the min just in case + peak_speed = min(peak_speed, max_speed) + t_accel = (peak_speed - current_speed) / acceleration if peak_speed > current_speed else 0.0 + t_decel = peak_speed / deceleration + t_final = t_accel + t_decel + profile_type = "triangular" + + t = 0 + times = [] + s_vals = [] + num_time_steps = 0 + while t < t_final: + times.append(t) + if profile_type == "trapezoidal": + if t < t_accel: + # Acceleration phase. + s = current_speed * t + 0.5 * acceleration * t**2 + elif t < t_accel + t_cruise: + # Cruise phase. + s = d_accel + max_speed * (t - t_accel) + else: + # Deceleration phase. + t_decel_phase = t - (t_accel + t_cruise) + s = total_length - 0.5 * deceleration * (t_decel - t_decel_phase)**2 + else: # Triangular profile. + if t < t_accel: + # Acceleration phase. + s = current_speed * t + 0.5 * acceleration * t**2 + else: + t_decel_phase = t - t_accel + s_accel = current_speed * t_accel + 0.5 * acceleration * t_accel**2 + s = s_accel + peak_speed * t_decel_phase - 0.5 * deceleration * t_decel_phase**2 + + s_vals.append(min(s, total_length)) + if s >= total_length: + break + + dt = compute_dynamic_dt(acceleration if t < t_accel else deceleration,current_speed) + t = t + dt + + num_time_steps +=1 + + # Compute trajectory points + points = [path_norm.eval(s) for s in s_vals] + print("Number of time steps is --------------------", num_time_steps) + + # return Trajectory(path_norm.frame, points, times) + + + # # Plot: update a single window + # import matplotlib.pyplot as plt + # plt.figure("Distance vs Time") + # plt.clf() # Clear the current figure + # plt.plot(times, s_vals) + # plt.xlabel("Time (s)") + # plt.ylabel("Distance (m)") + # plt.title("Distance vs Time") + # plt.draw() + # plt.pause(0.001) + + + + # 4. Create a time grid. + # dt = 0.1 # adjust based on computation + # times = np.arange(0, t_final + dt, dt) + # num_time_steps = 0 + + # # 5. Compute the distance s(t) for each time step. + # s_vals = [] + # for t in times: + # if profile_type == "trapezoidal": + # if t < t_accel: + # # Acceleration phase. + # s = current_speed * t + 0.5 * acceleration * t**2 + # elif t < t_accel + t_cruise: + # # Cruise phase. + # s = d_accel + max_speed * (t - t_accel) + # else: + # # Deceleration phase. + # t_decel_phase = t - (t_accel + t_cruise) + # # Compute the remaining distance using the deceleration equation. + # s = total_length - 0.5 * deceleration * (t_decel - t_decel_phase)**2 + # else: # Triangular profile. + # if t < t_accel: + # # Acceleration phase. + # s = current_speed * t + 0.5 * acceleration * t**2 + # else: + # t_decel_phase = t - t_accel + # s_accel = current_speed * t_accel + 0.5 * acceleration * t_accel**2 + # s = s_accel + peak_speed * t_decel_phase - 0.5 * deceleration * t_decel_phase**2 + # num_time_steps +=1 + + # # should not exceed total path length + # s_vals.append(min(s, total_length)) + # print("NUmber of time steps -----------",num_time_steps) + # print("T FInal ----------------------------", t_final) + # points = [path_norm.eval(s) for s in s_vals] + + + + trajectory = Trajectory(path_norm.frame, points, list(times)) + return trajectory + +def longitudinal_plan_dx(path : Path, acceleration : float, deceleration : float, max_speed : float, current_speed : float) -> Trajectory: + """Generates a longitudinal trajectory for a path with a + trapezoidal velocity profile. + + 1. accelerates from current speed toward max speed + 2. travel along max speed + 3. if at any point you can't brake before hitting the end of the path, + decelerate with accel = -deceleration until velocity goes to 0. + """ + path_normalized = path.arc_length_parameterize() + #TODO: actually do something to points and times + points = [p for p in path_normalized.points] + times = [t for t in path_normalized.times] + #============================================= + # Adjust these two numbers to choose between computation speed or smoothness + rq = 0.1 # Smaller, smoother + multi = 5 # Larger, smoother + print("-----LONGITUDINAL PLAN-----") + print("path length: ", path.length()) + length = path.length() + + # If the path is too short, just return the path for preventing sudden halt of simulation + if length < 0.05: + return Trajectory(path.frame, points, times) + + # This assumes that the time denomination cannot be changed + + # Starting point + x0 = points[0][0] + cur_point = points[0] + cur_time = times[0] + cur_index = 0 + acc = 0 + + new_points = [] + new_times = [] + velocities = [] # for graphing and debugging purposes + + while current_speed > 0 or cur_index == 0: + # we want to iterate through all the points and add them + # to the new points. However, we also want to add "critical points" + # where we reach top speed, begin decelerating, and stop + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + print("=====================================") + print("new points: ", new_points) + print("current index: ", cur_index) + print("current speed: ", current_speed) + + # Information we will need: + # Calculate how much time it would take to stop + # Calculate how much distance it would take to stop + min_delta_t_stop = current_speed/deceleration + min_delta_x_stop = current_speed*min_delta_t_stop - 0.5*deceleration*min_delta_t_stop**2 + assert min_delta_x_stop >= 0 + + + # Check if we are done + + # If we cannot stop before or stop exactly at the final position requested + if cur_point[0] + min_delta_x_stop >= points[-1][0] - 0.0001: + acc = deceleration + flag = 1 + print("In case one") + # put on the breaks + # Calculate the next point in a special manner because of too-little time to stop + if cur_index >= len(points)-1: + # the next point in this instance would be when we stop + print(1) + if min_delta_x_stop < rq * acc: + next_point = (cur_point[0] + min_delta_x_stop, 0) + else: + next_point = (cur_point[0] + (min_delta_x_stop / (acc * multi)), 0) + flag = 0 + else: + print(2) + next_point = points[cur_index+1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + + # keep breaking until the next milestone in path + print("continuing to next point") + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = next_point + current_speed -= deceleration*delta_t_to_next_x + if flag: + cur_index += 1 + + # This is the case where we are accelerating to max speed + # because the first if-statement covers for when we decelerating, + # the only time current_speed < max_speed is when we are accelerating + elif current_speed < max_speed: + print("In case two") + print(current_speed) + acc = acceleration + flag = 1 + # next point + next_point = points[cur_index+1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + # accelerate to max speed + + # calculate the time it would take to reach max speed + delta_t_to_max_speed = (max_speed - current_speed)/acceleration + # calculate the distance it would take to reach max speed + delta_x_to_max_speed = current_speed*delta_t_to_max_speed + 0.5*acceleration*delta_t_to_max_speed**2 + + # if we would reach max speed after the next point, + # just move to the next point and update the current speed and time + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + print("go to next point") + # accelerate to max speed + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, acceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed += delta_t_to_next_x*acceleration + if flag: + cur_index += 1 + + # this is the case where we would reach max speed before the next point + # we need to create a new point where we would reach max speed + else: + print("adding new point") + # we would need to add a new point at max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + # This is the case where we are at max speed + # special functionality is that this block must + # add a point where we would need to start declerating to reach + # the final point + elif current_speed == max_speed: + next_point = points[cur_index+1] + # continue on with max speed + print("In case three") + + # add point to start decelerating + if next_point[0] + min_delta_x_stop >= points[-1][0]: + print("Adding new point to start decelerating") + cur_time += (points[-1][0] - min_delta_x_stop - cur_point[0])/current_speed + cur_point = [points[-1][0] - min_delta_x_stop,0] + current_speed = max_speed + else: + # Continue on to next point + print("Continuing on to next point") + cur_time += (next_point[0] - cur_point[0])/current_speed + cur_point = next_point + cur_index += 1 + + # This is an edge case and should only be reach + # if the initial speed is greater than the max speed + elif current_speed > max_speed: + # We need to hit the breaks + acc = deceleration + flag = 1 + # next point + next_point = points[cur_index+1] + if next_point[0] - cur_point[0] > rq * acc: + tmp = cur_point[0] + (next_point[0] - cur_point[0]) / (acc * multi) + flag = 0 + next_point = [tmp, next_point[1]] + print("In case four") + # slow down to max speed + delta_t_to_max_speed = (current_speed - max_speed)/deceleration + delta_x_to_max_speed = current_speed*delta_t_to_max_speed - 0.5*deceleration*delta_t_to_max_speed**2 + + # If we would reach the next point before slowing down to max speed + # keep going until we reach the next point + if cur_point[0] + delta_x_to_max_speed >= next_point[0]: + delta_t_to_next_x = compute_time_to_x(cur_point[0], next_point[0], current_speed, -deceleration) + cur_time += delta_t_to_next_x + cur_point = [next_point[0], 0] + current_speed -= delta_t_to_next_x*deceleration + cur_index += 1 + else: + # We would reach max speed before the next point + # we need to add a new point at the point where we + # would reach max speed + cur_time += delta_t_to_max_speed + cur_point = [cur_point[0] + delta_x_to_max_speed, 0] + current_speed = max_speed + + else: + # not sure what falls here + raise ValueError("LONGITUDINAL PLAN ERROR: Not sure how we ended up here") + + new_points.append(cur_point) + new_times.append(cur_time) + velocities.append(current_speed) + + points = new_points + times = new_times + print("[PLAN] Computed points:", points) + print("[TIME] Computed time:", times) + print("[Velocities] Computed velocities:", velocities) + + #============================================= + + trajectory = Trajectory(path.frame,points,times) + return trajectory + +def longitudinal_brake(path : Path, deceleration : float, current_speed : float) -> Trajectory: + """Generates a longitudinal trajectory for braking along a path.""" + path_normalized = path.arc_length_parameterize() + + #TODO: actually do something to points and times + points = [p for p in path_normalized.points] + times = [t for t in path_normalized.times] + + #============================================= + + print("=====LONGITUDINAL BRAKE=====") + print("path length: ", path.length()) + length = path.length() + + x0 = points[0][0] + t_stop = current_speed / deceleration + x_stop = x0 + current_speed * t_stop - 0.5 * deceleration * t_stop**2 + + new_points = [] + velocities = [] + + for t in times: + if t <= t_stop: + x = x0 + current_speed * t - 0.5 * deceleration * t**2 + else: + x = x_stop + new_points.append([x, 0]) + velocities.append(current_speed - deceleration * t) + points = new_points + print("[BRAKE] Computed points:", points) + + #============================================= + + trajectory = Trajectory(path.frame,points,times,velocities) + return trajectory + +class YieldTrajectoryPlanner(Component): + """Follows the given route. Brakes if you have to yield or + you are at the end of the route, otherwise accelerates to + the desired speed. + """ + + def __init__(self): + self.route_progress = None + self.t_last = None + self.acceleration = 0.5 + self.desired_speed = 1.0 + self.deceleration = 2.0 + + self.min_deceleration = 1.0 + self.max_deceleration = 8.0 + + def state_inputs(self): + return ['all'] + + def state_outputs(self) -> List[str]: + return ['trajectory'] + + def rate(self): + return 10.0 + + def update(self, state : AllState): + start_time = time.time() + + vehicle = state.vehicle # type: VehicleState + route = state.route # type: Route + t = state.t + + if self.t_last is None: + self.t_last = t + dt = t - self.t_last + + # Position in vehicle frame (Start (0,0) to (15,0)) + curr_x = vehicle.pose.x + curr_y = vehicle.pose.y + curr_v = vehicle.v + + abs_x = curr_x + state.start_vehicle_pose.x + abs_y = curr_y + state.start_vehicle_pose.y + + #figure out where we are on the route + if self.route_progress is None: + self.route_progress = 0.0 + closest_dist,closest_parameter = state.route.closest_point_local((curr_x,curr_y),[self.route_progress-5.0,self.route_progress+5.0]) + self.route_progress = closest_parameter + + lookahead_distance = max(10, curr_v**2 / (2 * self.deceleration)) + route_with_lookahead = route.trim(closest_parameter,closest_parameter + lookahead_distance) + print("Lookahead distance:", lookahead_distance) + + route_to_end = route.trim(closest_parameter, len(route.points) - 1) + + should_yield = False + yield_deceleration = 0.0 + + print("Current Speed: ", curr_v) + + for r in state.relations: + if r.type == EntityRelationEnum.YIELDING and r.obj1 == '': + #get the object we are yielding to + obj = state.agents[r.obj2] + + detected, deceleration = detect_collision(abs_x, abs_y, curr_v, obj, self.min_deceleration, self.max_deceleration) + if isinstance(deceleration, list): + print("@@@@@ INPUT", deceleration) + time_collision = deceleration[1] + distance_collision = deceleration[0] + b = 3*time_collision - 2*curr_v + c = curr_v**2 - 3*distance_collision + desired_speed = (-b + (b**2 - 4*c)**0.5)/2 + deceleration = 1.5 + print("@@@@@ YIELDING", desired_speed) + route_with_lookahead = route.trim(closest_parameter,closest_parameter + distance_collision) + traj = longitudinal_plan(route_with_lookahead, self.acceleration, deceleration, desired_speed, curr_v) + return traj + else: + if detected and deceleration > 0: + yield_deceleration = max(deceleration, yield_deceleration) + should_yield = True + + print("should yield: ", should_yield) + + should_accelerate = (not should_yield and curr_v < self.desired_speed) + + #choose whether to accelerate, brake, or keep at current velocity + if should_accelerate: + traj = longitudinal_plan(route_with_lookahead, self.acceleration, self.deceleration, self.desired_speed, curr_v, "dt") + elif should_yield: + traj = longitudinal_brake(route_with_lookahead, yield_deceleration, curr_v) + else: + traj = longitudinal_plan(route_with_lookahead, 0.0, self.deceleration, self.desired_speed, curr_v, "dt") + + return traj diff --git a/GEMstack/onboard/planning/pedestrian_yield_logic.py b/GEMstack/onboard/planning/pedestrian_yield_logic.py new file mode 100644 index 000000000..2567c0093 --- /dev/null +++ b/GEMstack/onboard/planning/pedestrian_yield_logic.py @@ -0,0 +1,22 @@ +from ...state import AgentState,AgentEnum,EntityRelation,EntityRelationEnum +from ..component import Component +from typing import List,Dict + +class PedestrianYielder(Component): + """Yields for all pedestrians in the scene. + + Result is stored in the relations graph. + """ + def rate(self): + return None + def state_inputs(self): + return ['agents'] + def state_outputs(self): + return ['relations'] + def update(self,agents : Dict[str,AgentState]) -> List[EntityRelation]: + res = [] + for n,a in agents.items(): + if a.type == AgentEnum.PEDESTRIAN: + #relation: ego-vehicle yields to pedestrian + res.append(EntityRelation(type=EntityRelationEnum.YIELDING,obj1='',obj2=n)) + return res diff --git a/GEMstack/onboard/planning/pure_pursuit.py b/GEMstack/onboard/planning/pure_pursuit.py index 37ee25c39..cbf5ef9e7 100644 --- a/GEMstack/onboard/planning/pure_pursuit.py +++ b/GEMstack/onboard/planning/pure_pursuit.py @@ -189,6 +189,11 @@ def compute(self, state : VehicleState, component : Component = None): output_accel = -self.max_decel self.t_last = t + + if desired_speed == 0 and speed == 0 and output_accel < 0.0: + print("Stopping. Set accel", output_accel, "to 0") + output_accel = 0.0 + return (output_accel, f_delta) diff --git a/GEMstack/scripts/__init__.py b/GEMstack/scripts/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/GEMstack/state/trajectory.py b/GEMstack/state/trajectory.py index d7a57db00..45e90b214 100644 --- a/GEMstack/state/trajectory.py +++ b/GEMstack/state/trajectory.py @@ -169,6 +169,7 @@ def trim(self, start : float, end : float) -> Path: class Trajectory(Path): """A timed, piecewise linear path.""" times : List[float] + velocities : Optional[List[float]] = None def domain(self) -> Tuple[float,float]: """Returns the time parameter domain""" diff --git a/README.md b/README.md index 51f26e290..463597875 100644 --- a/README.md +++ b/README.md @@ -11,25 +11,100 @@ GEMstack uses Python 3.7+ and ROS Noetic. (It is possible to do some offline and simulation work without ROS, but it is highly recommended to install it if you are working on any onboard behavior or training for rosbag files.) You should also have the following Python dependencies installed, which you can install from this folder using `pip install -r requirements.txt`: +- GEMstack Dependencies + - numpy + - scipy + - matplotlib + - opencv-python + - torch + - klampt==0.9.2 + - shapely + - dacite + - pyyaml +- Perception Dependencies + - ultralytics -- numpy -- scipy -- matplotlib -- opencv-python -- torch -- klampt -- shapely -- dacite -- pyyaml +In order to interface with the actual GEM e2 vehicle, you will need [PACMOD2](https://github.com/astuff/pacmod2) - Autonomoustuff's low level interface to vehicle. You will also need Autonomoustuff's [sensor message packages](https://github.com/astuff/astuff_sensor_msgs). The onboard computer uses Ubuntu 20.04 with Python 3.8, CUDA 11.6, and NVIDIA driver 515, so to minimize compatibility issues you should ensure that these are installed on your development system. +## Running the stack on Ubuntu 20.04 without Docker +### Checking CUDA Version -In order to interface with the actual GEM e2 vehicle, you will need [PACMOD2](https://github.com/astuff/pacmod2) - Autonomoustuff's low level interface to vehicle. You will also need Autonomoustuff's [sensor message packages](https://github.com/astuff/astuff_sensor_msgs). The onboard computer uses Ubuntu 20.04 with Python 3.8, CUDA 11.6, and NVIDIA driver 515, so to minimize compatibility issues you should ensure that these are installed on your development system. +Before proceeding, check your Nvidia Driver and supported CUDA version: +```bash +nvidia-smi +``` +This will show your NVIDIA driver version and the maximum supported CUDA version. Make sure you have CUDA 11.8 or 12+ installed. + +From Ubuntu 20.04 install [CUDA 11.6](https://gist.github.com/ksopyla/bf74e8ce2683460d8de6e0dc389fc7f5) or [CUDA 12+](https://gist.github.com/ksopyla/ee744bf013c83e4aa3fc525634d893c9) based on your current Nvidia Driver versio. + +To check the currently installed CUDA version: +```bash +nvcc --version +``` +you can install the dependencies or GEMstack by running `setup/setup_this_machine.sh` from the top-level GEMstack folder. + +## Running the stack on Ubuntu 20.04 or 22.04 with Docker +> [!NOTE] +> Make sure to check the Nvidia Driver and supported CUDA version before proceeding by following the steps in the previous section. + +## Prerequisites +- Docker (In Linux - Make sure to follow the post-installation steps from [here](https://docs.docker.com/engine/install/linux-postinstall/)) +- Nvidia Container Toolkit + +Try running the sample workload from the [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/sample-workload.html) to check if your system is compatible. + +```bash +sudo docker run --rm --runtime=nvidia --gpus all ubuntu nvidia-smi +``` +You should see the nvidia-smi output similar to [this](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/sample-workload.html#:~:text=all%20ubuntu%20nvidia%2Dsmi-,Your%20output%20should%20resemble%20the%20following%20output%3A,-%2B%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2D%2B%0A%7C%20NVIDIA%2DSMI%20535.86.10). + +If you see the output, you are good to go. Otherwise, you will need to install the Docker and NVidia Container Toolkit by following the instructions. +- For **Docker**, follow the instructions [here](https://docs.docker.com/engine/install/ubuntu/#install-using-the-repository). + +- For **Nvidia Container Toolkit**, run `setup/get_nvidia_container.sh` from this directory to install, or see [this](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html) for more details. -From a fresh Ubuntu 20.04 with ROS Noetic and [CUDA 11.6 installed](https://gist.github.com/ksopyla/bf74e8ce2683460d8de6e0dc389fc7f5), you can install these dependencies by running `setup/setup_this_machine.sh` from the top-level GEMstack folder. +## Building the Docker image -To build a Docker container with all these prerequisites, you can use the provided Dockerfile by running `docker build -t gem_stack setup/`. For GPU support you will need the NVidia Container Runtime (run `setup/get_nvidia_container.sh` from this directory to install, or see [this tutorial](https://collabnix.com/introducing-new-docker-cli-api-support-for-nvidia-gpus-under-docker-engine-19-03-0-beta-release/) to install) and run `docker run -it --gpus all gem_stack /bin/bash`. +To build a Docker image with all these prerequisites, you can use the provided Dockerfile by running. +```bash +bash setup/build_docker_image.sh +``` + +## Running the Docker container + +To run the container, you can use the provided Docker Compose file by running. +> [!NOTE] +> If you want to open multiple terminals to run the container, you can use the same command. It will automatically start a new terminal inside the same container. +```bash +bash run_docker_container.sh +``` +## Usage Tips and Instructions + +### Using Host Volume + +You can use the host volume under the container's home directory inside the `` folder. This allows you to build and run files that are on the host machine. For example, if you have a file on the host machine at `/home//project`, you can access it inside the container at `/home//host/project`. + +### Using Dev Containers Extension in VSCode +To have a good developer environment inside the Docker container, you can use the Dev Containers extension in VSCode. Follow these steps: + +1. Install the Dev Containers extension in VSCode. +2. Open the cloned repository in VSCode. +3. Press `ctrl+shift+p`(or select the remote explorer icon from the left bar) and select `Dev-Containers: Attach to Running Container...`. +4. Select the container name `gem_stack-container`. +5. Once attached, Select `File->Open Folder...`. +6. Select the folder/workspace you want to open in the container. + +This will set up the development environment inside the Docker container, allowing you to use VSCode features seamlessly. + +## Stopping the Docker container + +To stop the container, you can use the provided stop script by running. + +```bash +bash stop_docker_container.sh +``` ## In this folder diff --git a/data/.gitignore b/data/.gitignore old mode 100644 new mode 100755 diff --git a/data/README.md b/data/README.md old mode 100644 new mode 100755 diff --git a/homework/blink.py b/homework/blink.py index 89de04639..b83d3c49c 100644 --- a/homework/blink.py +++ b/homework/blink.py @@ -47,20 +47,20 @@ def cleanup(self): def get_dir_distress(self): pass - def update(self, Allstate): + def update(self): """Run in a loop""" # TODO: Implement your control loop here # You will need to publish a PacmodCmd() to /pacmod/as_rx/turn_cmd. Read the documentation to see # what the data in the message indicates. #self.turn_cmd = PacmodCmd() # TODO change to actual direction in Part 2 - if Allstate.intent.intent == "HALTING": - if self.turn_cmd.ui16_cmd == TURN_NONE: - self.turn_cmd.ui16_cmd = TURN_LEFT - elif self.turn_cmd.ui16_cmd == TURN_LEFT: - self.turn_cmd.ui16_cmd = TURN_RIGHT - else: - self.turn_cmd.ui16_cmd = TURN_NONE + + if self.turn_cmd.ui16_cmd == TURN_NONE: + self.turn_cmd.ui16_cmd = TURN_LEFT + elif self.turn_cmd.ui16_cmd == TURN_LEFT: + self.turn_cmd.ui16_cmd = TURN_RIGHT + else: + self.turn_cmd.ui16_cmd = TURN_NONE self.turn_blink_pub.publish(self.turn_cmd) pass diff --git a/homework/blink_launch.yaml b/homework/blink_launch.yaml deleted file mode 100644 index 0b17ac054..000000000 --- a/homework/blink_launch.yaml +++ /dev/null @@ -1,64 +0,0 @@ -description: "Launch just the blink distress code" -mode: hardware -vehicle_interface: gem_hardware.GEMHardwareInterface -mission_execution: StandardExecutor -require_engaged: False -# Recovery behavior after a component failure -recovery: - perception: - state_estimation : GNSSStateEstimator - perception_normalization : StandardPerceptionNormalizer - planning: - signaling: blink_component.BlinkDistress -# Driving behavior for the GEM vehicle. Runs perception and planner but doesn't execute anything (no controller). -drive: - perception: - state_estimation : GNSSStateEstimator - perception_normalization : StandardPerceptionNormalizer - planning: - signaling: blink_component.BlinkDistress -log: - # Specify the top-level folder to save the log files. Default is 'logs' - folder : 'logs' - # If prefix is specified, then the log folder will be named with the prefix followed by the date and time. Default no prefix - prefix : 'hw1_' - # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix - #suffix : 'test3' - # Specify which ros topics to record to vehicle.bag. - #ros_topics : ['/pacmod/as_rx/turn_cmd','/pacmod/as_tx/turn_rpt'] - # Specify options to pass to rosbag record. Default is no options. - #rosbag_options : '--split --size=1024' - # If True, then record all readings / commands of the vehicle interface. Default False - vehicle_interface : True - # Specify which components to record to behavior.json. Default records nothing - components : [] - # Specify which components of state to record to state.json. Default records nothing - #state: ['all'] - # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate - #state_rate: 10 -replay: # Add items here to set certain topics / inputs to be replayed from logs - # Specify which log folder to replay from - log: - ros_topics : [] - components : [] - -#usually can keep this constant -computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" - -variants: - sim: - run: - description: "Runs the distress signal, but in simulation" - mode: simulation - vehicle_interface: - type: gem_simulator.GEMDoubleIntegratorSimulationInterface - args: - scene: !relative_path '../scenes/xyhead_demo.yaml' - require_engaged: False - visualization: !include "klampt_visualization.yaml" - recovery: - perception: - state_estimation : OmniscientStateEstimator - drive: - perception: - state_estimation : OmniscientStateEstimator diff --git a/homework/capture_ouster_oak.py b/homework/capture_ouster_oak.py new file mode 100644 index 000000000..4e946d931 --- /dev/null +++ b/homework/capture_ouster_oak.py @@ -0,0 +1,108 @@ +# 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 + +lidar_points = None +camera_image = 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,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)) + 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/rgb/image_raw/compressedDepth", 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("Press Escape or Ctrl+C to quit") + while True: + if camera_image: + cv2.imshow("result",bridge.imgmsg_to_cv2(camera_image)) + key = cv2.waitKey(30) + if key == -1: + #nothing + pass + elif key == 27: + #escape + break + 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))] + 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) diff --git a/launch/blink_launch.yaml b/launch/blink_launch.yaml index 41174f692..f8b60eb2d 100644 --- a/launch/blink_launch.yaml +++ b/launch/blink_launch.yaml @@ -9,14 +9,16 @@ recovery: state_estimation : GNSSStateEstimator perception_normalization : StandardPerceptionNormalizer planning: - trajectory_tracking : blink_component.BlinkDistress + driving_logic: driving_logic_component.DrivingLogic + signaling: blink_component.BlinkDistress # Driving behavior for the GEM vehicle. Runs perception and planner but doesn't execute anything (no controller). drive: perception: state_estimation : GNSSStateEstimator perception_normalization : StandardPerceptionNormalizer planning: - trajectory_tracking : blink_component.BlinkDistress + driving_logic: driving_logic_component.DrivingLogic + signaling: blink_component.BlinkDistress log: # Specify the top-level folder to save the log files. Default is 'logs' folder : 'logs' diff --git a/launch/launch_all.sh b/launch/launch_all.sh new file mode 100644 index 000000000..c75fbecc9 --- /dev/null +++ b/launch/launch_all.sh @@ -0,0 +1,24 @@ +#!/bin/bash +<< COMMENTOUT +########## +- This script opens three new tabs in GNOME Terminal. +- Each tab will change to the GEMstack directory, source the ROS environment, and then run the specified command. + +- Run these commands at each tab: +cd ~/catkin_ws/src/GEMstack +source ~/catkin_ws/devel/setup.bash && roslaunch basic_launch sensor_init.launch +source ~/catkin_ws/devel/setup.bash && roslaunch basic_launch dbw_joystick.launch +source ~/catkin_ws/devel/setup.bash && python3 main.py --variant=fake_real launch/pedestrian_detection.yaml + +- If you don't want to detect virtual pedestrian, run this command instead: +source ~/catkin_ws/devel/setup.bash && python3 main.py launch/pedestrian_detection.yaml + +- If roscore cannot run, run this command to kill all roscore: +killall -9 roscore rosmaster +########## +COMMENTOUT + +cd ~/catkin_ws/src/GEMstack +gnome-terminal --tab -- bash -c "source ~/catkin_ws/devel/setup.bash && roslaunch basic_launch sensor_init.launch; exec bash" +gnome-terminal --tab -- bash -c "source ~/catkin_ws/devel/setup.bash && roslaunch basic_launch dbw_joystick.launch; exec bash" +gnome-terminal --tab -- bash -c "source ~/catkin_ws/devel/setup.bash && python3 main.py --variant=fake_real launch/pedestrian_detection.yaml; exec bash" \ No newline at end of file diff --git a/launch/pedestrian_detection.yaml b/launch/pedestrian_detection.yaml new file mode 100644 index 000000000..98f49a9aa --- /dev/null +++ b/launch/pedestrian_detection.yaml @@ -0,0 +1,99 @@ +description: "Run the yielding trajectory planner on the real vehicle with real perception" +mode: hardware +vehicle_interface: gem_hardware.GEMHardwareInterface +mission_execution: StandardExecutor + +# Recovery behavior after a component failure +recovery: + planning: + trajectory_tracking : recovery.StopTrajectoryTracker + +# Driving behavior for the GEM vehicle. Runs real pedestrian perception, yield planner, but does not send commands to real vehicle. +drive: + perception: + state_estimation : GNSSStateEstimator + agent_detection : pedestrian_detection.PedestrianDetector2D + perception_normalization : StandardPerceptionNormalizer + planning: + relations_estimation: pedestrian_yield_logic.PedestrianYielder + route_planning: + type: StaticRoutePlanner + args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv','start'] + motion_planning: longitudinal_planning.YieldTrajectoryPlanner + trajectory_tracking: + type: pure_pursuit.PurePursuitTrajectoryTracker + print: False + + +log: + # Specify the top-level folder to save the log files. Default is 'logs' + #folder : 'logs' + # If prefix is specified, then the log folder will be named with the prefix followed by the date and time. Default no prefix + #prefix : 'fixed_route_' + # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix + #suffix : 'test3' + # Specify which ros topics to record to vehicle.bag. Default records nothing. This records the "standard" ROS topics. + ros_topics : + # Specify options to pass to rosbag record. Default is no options. + #rosbag_options : '--split --size=1024' + # If True, then record all readings / commands of the vehicle interface. Default False + vehicle_interface : True + # Specify which components to record to behavior.json. Default records nothing + components : ['state_estimation','agent_detection','motion_planning'] + # Specify which components of state to record to state.json. Default records nothing + #state: ['all'] + # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate + #state_rate: 10 +replay: # Add items here to set certain topics / inputs to be replayed from logs + # Specify which log folder to replay from + log: + ros_topics : [] + components : [] + +#usually can keep this constant +computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" + +variants: + detector_only: + run: + description: "Run the pedestrian detection code" + drive: + planning: + trajectory_tracking: + real_sim: + run: + description: "Run the pedestrian detection code with real detection and fake simulation" + mode: hardware + vehicle_interface: + type: gem_mixed.GEMRealSensorsWithSimMotionInterface + args: + scene: !relative_path '../scenes/xyhead_demo.yaml' + mission_execution: StandardExecutor + require_engaged: False + visualization: !include "klampt_visualization.yaml" + drive: + perception: + state_estimation : OmniscientStateEstimator + + + fake_real: + run: + description: "Run the yielding trajectory planner on the real vehicle with faked perception" + drive: + perception: + agent_detection : pedestrian_detection.FakePedestrianDetector2D + + fake_sim: + run: + description: "Run the yielding trajectory planner in simulation with faked perception" + mode: simulation + vehicle_interface: + type: gem_simulator.GEMDoubleIntegratorSimulationInterface + args: + scene: !relative_path '../scenes/xyhead_demo.yaml' + visualization: !include "klampt_visualization.yaml" + drive: + perception: + # agent_detection : pedestrian_detection.FakePedestrianDetector2D + agent_detection : OmniscientAgentDetector #this option reads agents from the simulator + state_estimation : OmniscientStateEstimator diff --git a/requirements.txt b/requirements.txt index 94db8ba43..aba104c78 100644 --- a/requirements.txt +++ b/requirements.txt @@ -4,6 +4,9 @@ scipy matplotlib torch shapely -klampt +klampt==0.9.2 pyyaml dacite + +# Perception +ultralytics diff --git a/run_docker_container.sh b/run_docker_container.sh new file mode 100644 index 000000000..c987a9016 --- /dev/null +++ b/run_docker_container.sh @@ -0,0 +1,8 @@ +#!/bin/bash +# Check if container is already running +if [ "$(docker ps -q -f name=gem_stack-container)" ]; then + docker exec -it gem_stack-container bash +else + docker compose -f setup/docker-compose.yaml up -d + docker exec -it gem_stack-container bash +fi \ No newline at end of file diff --git a/setup/Dockerfile b/setup/Dockerfile.cuda11.8 similarity index 54% rename from setup/Dockerfile rename to setup/Dockerfile.cuda11.8 index b35c3b85d..687533803 100644 --- a/setup/Dockerfile +++ b/setup/Dockerfile.cuda11.8 @@ -1,7 +1,28 @@ FROM nvidia/cuda:11.8.0-cudnn8-devel-ubuntu20.04 + +ARG USER_UID=1000 +ARG USER_GID=1000 +ARG USER=$(whoami) + +ENV DEBIAN_FRONTEND=noninteractive + #use bash instead of sh -RUN rm /bin/sh && ln -s /bin/bash /bin/sh +SHELL ["/bin/bash", "-c"] + RUN apt-get update && apt-get install -y git python3 python3-pip wget zstd + +# Set time zone non-interactively +ENV TZ=America/Chicago +RUN ln -fs /usr/share/zoneinfo/$TZ /etc/localtime \ + && echo $TZ > /etc/timezone \ + && apt-get update && apt-get install -y tzdata \ + && rm -rf /var/lib/apt/lists/* + +# install Zed SDK +RUN wget https://download.stereolabs.com/zedsdk/4.0/cu118/ubuntu20 -O zed_sdk.run +RUN chmod +x zed_sdk.run +RUN ./zed_sdk.run -- silent + # Install ROS Noetic RUN apt-get update && apt-get install -y lsb-release gnupg2 RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list' @@ -13,19 +34,18 @@ RUN apt-get install -y python3-rosdep python3-rosinstall python3-rosinstall-gene RUN rosdep init RUN rosdep update -#Install Cuda 11.8 -#RUN wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/cuda-ubuntu2004.pin -#RUN sudo mv cuda-ubuntu2004.pin /etc/apt/preferences.d/cuda-repository-pin-600 -##add public keys -#RUN sudo apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/3bf863cc.pub -#RUN sudo add-apt-repository "deb http://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/ /" -#RUN install cuda-toolkit-11-8 +ARG USER +ARG USER_UID +ARG USER_GID +# Create user more safely +RUN groupadd -g ${USER_GID} ${USER} || groupmod -n ${USER} $(getent group ${USER_GID} | cut -d: -f1) +RUN useradd -l -m -u ${USER_UID} -g ${USER_GID} ${USER} || usermod -l ${USER} -m -u ${USER_UID} -g ${USER_GID} $(getent passwd ${USER_UID} | cut -d: -f1) +RUN echo "${USER} ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers -# install Zed SDK -RUN wget https://download.stereolabs.com/zedsdk/4.0/cu118/ubuntu20 -O zed_sdk.run -RUN chmod +x zed_sdk.run -RUN ./zed_sdk.run -- silent +# Fix permissions for Python packages +RUN chown -R ${USER}:${USER} /usr/local/lib/python3.8/dist-packages/ \ + && chmod -R u+rw /usr/local/lib/python3.8/dist-packages/ # create ROS Catkin workspace RUN mkdir -p /catkin_ws/src @@ -42,8 +62,22 @@ RUN cd /catkin_ws/src && git clone https://github.com/ros-perception/radar_msgs. RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && rosdep install --from-paths src --ignore-src -r -y RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && catkin_make -DCMAKE_BUILD_TYPE=Release -# install GEMstack Python dependencies -RUN git clone https://github.com/krishauser/GEMstack.git -RUN cd GEMstack && pip3 install -r requirements.txt +# Copy requirements.txt from host (now relative to parent directory) +COPY requirements.txt /tmp/requirements.txt + +# Install Python dependencies +RUN pip3 install -r /tmp/requirements.txt + +# Install other Dependencies +RUN apt-get install -y ros-noetic-septentrio-gnss-driver + +USER ${USER} + +# Add ROS and GEMstack paths to bashrc +RUN echo "source /opt/ros/noetic/setup.bash" >> /home/${USER}/.bashrc +RUN echo "source /catkin_ws/devel/setup.bash" >> /home/${USER}/.bashrc + +# BASE END CONFIG +WORKDIR /home/${USER} -RUN echo /catkin_ws/devel/setup.sh +ENTRYPOINT ["/bin/bash"] \ No newline at end of file diff --git a/setup/Dockerfile.cuda12 b/setup/Dockerfile.cuda12 new file mode 100644 index 000000000..ffafaaa3f --- /dev/null +++ b/setup/Dockerfile.cuda12 @@ -0,0 +1,83 @@ +FROM nvidia/cuda:12.0.0-cudnn8-devel-ubuntu20.04 + +ARG USER_UID=1000 +ARG USER_GID=1000 +ARG USER=$(whoami) + +ENV DEBIAN_FRONTEND=noninteractive + +#use bash instead of sh +SHELL ["/bin/bash", "-c"] + +RUN apt-get update && apt-get install -y git python3 python3-pip wget zstd + +# Set time zone non-interactively +ENV TZ=America/Chicago +RUN ln -fs /usr/share/zoneinfo/$TZ /etc/localtime \ + && echo $TZ > /etc/timezone \ + && apt-get update && apt-get install -y tzdata \ + && rm -rf /var/lib/apt/lists/* + +# install Zed SDK +RUN wget https://stereolabs.sfo2.cdn.digitaloceanspaces.com/zedsdk/4.2/ZED_SDK_Ubuntu20_cuda12.1_v4.2.4.zstd.run -O zed_sdk.run +RUN chmod +x zed_sdk.run +RUN ./zed_sdk.run -- silent + +# Install ROS Noetic +RUN apt-get update && apt-get install -y lsb-release gnupg2 +RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list' +RUN wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc +RUN apt-key add ros.asc +RUN apt-get update +RUN DEBIAN_FRONTEND=noninteractive apt-get install -y ros-noetic-desktop +RUN apt-get install -y python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential python3-catkin-tools +RUN rosdep init +RUN rosdep update + +ARG USER +ARG USER_UID +ARG USER_GID + +# Create user more safely +RUN groupadd -g ${USER_GID} ${USER} || groupmod -n ${USER} $(getent group ${USER_GID} | cut -d: -f1) +RUN useradd -l -m -u ${USER_UID} -g ${USER_GID} ${USER} || usermod -l ${USER} -m -u ${USER_UID} -g ${USER_GID} $(getent passwd ${USER_UID} | cut -d: -f1) +RUN echo "${USER} ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers + +# Fix permissions for Python packages +RUN chown -R ${USER}:${USER} /usr/local/lib/python3.8/dist-packages/ \ + && chmod -R u+rw /usr/local/lib/python3.8/dist-packages/ + +# create ROS Catkin workspace +RUN mkdir -p /catkin_ws/src + +# install ROS dependencies and packages +RUN cd /catkin_ws/src && git clone https://github.com/krishauser/POLARIS_GEM_e2.git +RUN cd /catkin_ws/src && git clone --recurse-submodules https://github.com/stereolabs/zed-ros-wrapper.git +RUN cd /catkin_ws/src && git clone https://github.com/astuff/pacmod2.git + #for some reason the ibeo messages don't work? +RUN cd /catkin_ws/src && git clone https://github.com/astuff/astuff_sensor_msgs.git && rm -rf astuff_sensor_msgs/ibeo_msgs +RUN cd /catkin_ws/src && git clone https://github.com/ros-perception/radar_msgs.git \ + && cd radar_msgs && git checkout noetic + +RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && rosdep install --from-paths src --ignore-src -r -y +RUN source /opt/ros/noetic/setup.bash && cd /catkin_ws && catkin_make -DCMAKE_BUILD_TYPE=Release + +# Copy requirements.txt from host (now relative to parent directory) +COPY requirements.txt /tmp/requirements.txt + +# Install Python dependencies +RUN pip3 install -r /tmp/requirements.txt + +# Install other Dependencies +RUN apt-get install -y ros-noetic-septentrio-gnss-driver + +USER ${USER} + +# Add ROS and GEMstack paths to bashrc +RUN echo "source /opt/ros/noetic/setup.bash" >> /home/${USER}/.bashrc +RUN echo "source /catkin_ws/devel/setup.bash" >> /home/${USER}/.bashrc + +# BASE END CONFIG +WORKDIR /home/${USER} + +ENTRYPOINT ["/bin/bash"] \ No newline at end of file diff --git a/setup/build_docker_image.sh b/setup/build_docker_image.sh new file mode 100644 index 000000000..0852f27ab --- /dev/null +++ b/setup/build_docker_image.sh @@ -0,0 +1,22 @@ +#!/bin/bash + +echo "Select CUDA version:" +echo "1) CUDA 11.8" +echo "2) CUDA 12+" +read -p "Enter choice [1-2]: " choice + +case $choice in + 1) + DOCKERFILE=setup/Dockerfile.cuda11.8 + ;; + 2) + DOCKERFILE=setup/Dockerfile.cuda12 + ;; + *) + echo "Invalid choice" + exit 1 + ;; +esac + +export DOCKERFILE +UID=$(id -u) GID=$(id -g) docker compose -f setup/docker-compose.yaml build \ No newline at end of file diff --git a/setup/docker-compose.yaml b/setup/docker-compose.yaml new file mode 100644 index 000000000..d0b745c9e --- /dev/null +++ b/setup/docker-compose.yaml @@ -0,0 +1,44 @@ +version: '3.9' + +services: + gem-stack-ubuntu-20.04-CUDA: + image: gem_stack + container_name: gem_stack-container + build: + context: .. + dockerfile: ${DOCKERFILE:-setup/Dockerfile.cuda11.8} # Default to cuda11.8 if not specified + args: + USER: ${USER} + USER_UID: ${UID:-1000} # Pass host UID + USER_GID: ${GID:-1000} # Pass host GID + stdin_open: true + tty: true + volumes: + - "/etc/group:/etc/group:ro" + - "/etc/passwd:/etc/passwd:ro" + - "/etc/shadow:/etc/shadow:ro" + - "/etc/sudoers.d:/etc/sudoers.d:ro" + - "~:/home/${USER}/host" + - "/tmp/.X11-unix:/tmp/.X11-unix:rw" + - "/run/user/${UID}/bus:/run/user/${UID}/bus:ro" + - "/tmp/runtime-${USER}:/tmp/runtime-${USER}:rw" + environment: + - DISPLAY=${DISPLAY} + - XDG_RUNTIME_DIR=/tmp/runtime-${USER} + - DBUS_SYSTEM_BUS_ADDRESS=unix:path=/var/run/dbus/system_bus_socket + - DBUS_SESSION_BUS_ADDRESS=unix:path=/run/user/${UID}/bus + - NVIDIA_DRIVER_CAPABILITIES=all + - NVIDIA_VISIBLE_DEVICES=all + # - LIBGL_ALWAYS_SOFTWARE=1 # Uncomment if you want to use software rendering (No GPU) + network_mode: host + ipc: host + user: "${USER}:${USER}" + # Un-Comment the following lines if you want to use Nvidia GPU + runtime: nvidia + deploy: + resources: + reservations: + devices: + - driver: nvidia + count: all # alternatively, use `count: all` for all GPUs + capabilities: [gpu] diff --git a/setup/get_nvidia_container.sh b/setup/get_nvidia_container.sh index 466b1989e..4f738e2c8 100755 --- a/setup/get_nvidia_container.sh +++ b/setup/get_nvidia_container.sh @@ -1,9 +1,14 @@ #!/bin/bash -curl -s -L https://nvidia.github.io/nvidia-container-runtime/gpgkey | \ - sudo apt-key add - -distribution=$(. /etc/os-release;echo $ID$VERSION_ID) -curl -s -L https://nvidia.github.io/nvidia-container-runtime/$distribution/nvidia-container-runtime.list | \ - sudo tee /etc/apt/sources.list.d/nvidia-container-runtime.list +curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg \ + && curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \ + sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \ + sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list + +sed -i -e '/experimental/ s/^#//g' /etc/apt/sources.list.d/nvidia-container-toolkit.list + sudo apt-get update -sudo apt-get install nvidia-container-runtime \ No newline at end of file +sudo apt-get install -y nvidia-container-toolkit + +sudo nvidia-ctk runtime configure --runtime=docker +sudo systemctl restart docker \ No newline at end of file diff --git a/setup/setup_this_machine.sh b/setup/setup_this_machine.sh index e30b3aa23..00da094c9 100755 --- a/setup/setup_this_machine.sh +++ b/setup/setup_this_machine.sh @@ -1,30 +1,68 @@ #!/bin/bash sudo apt update -sudo apt-get install git python3 python3-pip wget zstd +sudo apt-get install -y git python3 python3-pip wget zstd + +if [ ! -f /opt/ros/noetic/setup.bash ]; then + echo "ROS Noetic not found. Installing ROS Noetic..." + sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' + wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc + sudo apt-key add ros.asc + sudo apt update + sudo DEBIAN_FRONTEND=noninteractive apt install -y ros-noetic-desktop + sudo apt install -y python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential python3-catkin-tools + sudo rosdep init + rosdep update +fi source /opt/ros/noetic/setup.bash #install Zed SDK -wget https://download.stereolabs.com/zedsdk/4.0/cu121/ubuntu20 -O ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run -chmod +x ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run -./ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run -- silent +echo "Select CUDA version:" +echo "1) CUDA 11.8" +echo "2) CUDA 12+" +read -p "Enter choice [1-2]: " choice + +case $choice in + 1) + wget https://download.stereolabs.com/zedsdk/4.0/cu118/ubuntu20 -O zed_sdk.run + ;; + 2) + wget https://stereolabs.sfo2.cdn.digitaloceanspaces.com/zedsdk/4.2/ZED_SDK_Ubuntu20_cuda12.1_v4.2.4.zstd.run -O zed_sdk.run + ;; + *) + echo "Invalid choice" + exit 1 + ;; +esac + +chmod +x zed_sdk.run +./zed_sdk.run -- silent #create ROS Catkin workspace -mkdir catkin_ws -mkdir catkin_ws/src +mkdir -p ~/catkin_ws/src + +# Store current working directory +CURRENT_DIR=$(pwd) #install ROS dependencies and packages -cd catkin_ws/src +cd ~/catkin_ws/src git clone https://github.com/krishauser/POLARIS_GEM_e2.git git clone https://github.com/astuff/pacmod2.git -git clone https://github.com/astuff/astuff_sensor_msgs.git -git clone https://github.com/ros-perception/radar_msgs.git -cd radar_msgs; git checkout noetic; cd .. +git clone https://github.com/astuff/astuff_sensor_msgs.git && rm -rf astuff_sensor_msgs/ibeo_msgs +git clone https://github.com/ros-perception/radar_msgs.git && cd radar_msgs; git checkout noetic; cd .. + +# Remove the ibeo_msgs folder so it is not included +rm -rf astuff_sensor_msgs/ibeo_msgs cd .. #back to catkin_ws +rosdep update rosdep install --from-paths src --ignore-src -r -y catkin_make -DCMAKE_BUILD_TYPE=Release source devel/setup.bash -cd .. #back to GEMstack +cd $CURRENT_DIR +cd .. #install GEMstack Python dependencies -python3 -m pip install -r requirements.txt \ No newline at end of file +python3 -m pip install -r requirements.txt + +#install other dependencies +sudo apt-get install -y ros-noetic-septentrio-gnss-driver \ No newline at end of file diff --git a/stop_docker_container.sh b/stop_docker_container.sh new file mode 100644 index 000000000..1b1bad2c8 --- /dev/null +++ b/stop_docker_container.sh @@ -0,0 +1,3 @@ +#! /bin/bash + +docker compose -f setup/docker-compose.yaml down \ No newline at end of file diff --git a/test_longitudinal_plan.py b/test_longitudinal_plan.py new file mode 100644 index 000000000..400f34fea --- /dev/null +++ b/test_longitudinal_plan.py @@ -0,0 +1,80 @@ +#needed to import GEMstack from top level directory +import sys +import os +sys.path.append(os.getcwd()) + +from GEMstack.state import Path, ObjectFrameEnum +from GEMstack.onboard.planning.longitudinal_planning import longitudinal_plan,longitudinal_brake +import matplotlib.pyplot as plt + +def test_longitudinal_planning(): + test_path = Path(ObjectFrameEnum.START,[(0,0),(1,0),(2,0),(3,0),(4,0),(5,0),(6,0)]) + test_path2 = Path(ObjectFrameEnum.START,[(5,0),(6,1),(7,2),(9,4)]) + + test_traj = longitudinal_brake(test_path, 2.0, 0.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Braking from 0 m/s (should just stay still)") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + test_traj = longitudinal_brake(test_path, 2.0, 2.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Braking from 2 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + test_traj = longitudinal_plan(test_path, 1.0, 2.0, 3.0, 0.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Accelerating from 0 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + + test_traj = longitudinal_plan(test_path, 1.0, 2.0, 3.0, 2.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Accelerating from 2 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + test_traj = longitudinal_plan(test_path, 0.0, 2.0, 3.0, 3.1) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Keeping constant velocity at 3.1 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + test_traj = longitudinal_plan(test_path, 2.0, 2.0, 20.0, 10.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Too little time to stop, starting at 10 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + test_traj = longitudinal_brake(test_path, 2.0, 10.0) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Too little time to stop, braking at 10 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + test_traj = longitudinal_plan(test_path2, 1.0, 2.0, 3.0, 0.0) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Nonuniform planning") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + + +if __name__ == '__main__': + test_longitudinal_planning() diff --git a/testing/test_longitudinal_plan.py b/testing/test_longitudinal_plan.py new file mode 100644 index 000000000..ab16ca5e6 --- /dev/null +++ b/testing/test_longitudinal_plan.py @@ -0,0 +1,128 @@ +#needed to import GEMstack from top level directory +import sys +import os +sys.path.append(os.getcwd()) + +from GEMstack.state import Path, ObjectFrameEnum +from GEMstack.onboard.planning.longitudinal_planning import longitudinal_plan,longitudinal_brake +import matplotlib.pyplot as plt + +def test_longitudinal_planning(): + test_path = Path(ObjectFrameEnum.START,[(0,0),(1,0),(2,0),(3,0),(4,0),(5,0),(6,0)]) + test_path2 = Path(ObjectFrameEnum.START,[(5,0),(6,1),(7,2),(9,4)]) + + test_traj = longitudinal_brake(test_path, 2.0, 0.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Braking from 0 m/s (should just stay still)") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Braking from 0 m/s (should just stay still)") + plt.xlabel('time') + plt.ylabel('velocity') + plt.show() + + test_traj = longitudinal_brake(test_path, 2.0, 2.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Braking from 2 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Braking from 2 m/s") + plt.xlabel('time') + plt.ylabel('velocity') + plt.show() + + test_traj = longitudinal_plan(test_path, 1.1, 2.0, 3.0, 0.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Accelerating from 0 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Accelerating from 0 m/s") + plt.xlabel('time') + plt.ylabel('velocity') + plt.show() + + + test_traj = longitudinal_plan(test_path, 1.0, 2.0, 3.0, 2.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Accelerating from 2 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Accelerating from 2 m/s") + plt.xlabel('time') + plt.ylabel('velocity') + plt.show() + + + test_traj = longitudinal_plan(test_path, 0.0, 2.0, 3.0, 3.1) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Keeping constant velocity at 3.1 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Keeping constant velocity at 3.1 m/s") + plt.xlabel('time') + plt.ylabel('velocity') + plt.show() + + test_traj = longitudinal_plan(test_path, 2.0, 2.0, 20.0, 10.0) + assert (t1 < t2 for (t1,t2) in zip(test_traj.times[:-1],test_traj.times[1:]) ) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Too little time to stop, starting at 10 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Too little time to stop, starting at 10 m/s") + plt.xlabel('time') + plt.ylabel('velocity') + plt.show() + + test_traj = longitudinal_brake(test_path, 2.0, 10.0) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Too little time to stop, braking at 10 m/s") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Too little time to stop, braking at 10 m/s") + plt.xlabel('time') + plt.ylabel('velocity') + plt.show() + + test_traj = longitudinal_plan(test_path2, 1.0, 2.0, 3.0, 0.0) + plt.plot(test_traj.times,[p[0] for p in test_traj.points]) + plt.title("Nonuniform planning") + plt.xlabel('time') + plt.ylabel('position') + plt.show() + + plt.plot(test_traj.times,[v for v in test_traj.velocities]) + plt.title("Nonuniform planning") + plt.xlabel('time') + plt.ylabel('velocity') + plt.show() + + +if __name__ == '__main__': + test_longitudinal_planning()