Igvc 2024

cv/lane_detection/config/lane_detection_params.yaml

@@ -1 +1 @@
-lane_detection_mode: 1 # 0 => DEEP LEARNING, 1 => CLASSICAL
+lane_detection_mode: 0 # 0 => DEEP LEARNING, 1 => CLASSICAL

cv/lane_detection/launch/lane_detection_server.launch

@@ -24,13 +24,13 @@
     </node> -->
 
 
-    <!-- <node
+    <node
         name="lane_viz"
         pkg="lane_detection"
         type="lane_viz.py"
         output="screen"
     >
-    </node> -->
+    </node>
 
     <node
         name="lane_scan_cpp"

cv/lane_detection/src/depth_sensing.py

@@ -0,0 +1,66 @@
+########################################################################
+#
+# Copyright (c) 2022, STEREOLABS.
+#
+# All rights reserved.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+########################################################################
+
+import pyzed.sl as sl
+import math
+import numpy as np
+import sys
+import math
+
+def main():
+    # Create a Camera object
+    zed = sl.Camera()
+
+    # Create a InitParameters object and set configuration parameters
+    init_params = sl.InitParameters(depth_mode=sl.DEPTH_MODE.NEURAL,
+                                    coordinate_units=sl.UNIT.METER,
+                                    coordinate_system=sl.COORDINATE_SYSTEM.IMAGE)
+
+    # Open the camera
+    status = zed.open(init_params)
+    if status != sl.ERROR_CODE.SUCCESS: #Ensure the camera has opened succesfully
+        print("Camera Can't Open.... "+repr(status)+". Exit program.")
+        exit()
+
+    # Create and set RuntimeParameters after opening the camera
+    runtime_parameters = sl.RuntimeParameters()
+
+    point_cloud = sl.Mat()
+
+    mirror_ref = sl.Transform()
+    mirror_ref.set_translation(sl.Translation(2.75,4.0,0))
+
+    # A new image is available if grab() returns SUCCESS
+    if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
+        # Retrieve colored point cloud. Point cloud is aligned on the left image.
+        zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)
+
+    np.save('/home/tsyh/Documents/num',point_cloud.get_data()[:, :, 0:3])
+    err = point_cloud.write('/home/tsyh/Documents/point.xyz')
+    if(err == sl.ERROR_CODE.SUCCESS):
+        print("point cloud saved")
+    else:
+        print("the point cloud has not been saved")
+
+    # Close the camera
+    zed.close()
+
+if __name__ == "__main__":
+    main()

cv/lane_detection/src/lane_detection_inference.py

@@ -10,6 +10,7 @@
 import onnx
 import onnxruntime as ort 
 import torch
+import pandas as pd
 
 from cv_bridge import CvBridge
 from sensor_msgs.msg import Image
@@ -23,13 +24,13 @@
 from cv_utils import camera_projection
 
 from line_fitting import fit_lanes
-from unet_lane.Inference import Inference
 from ultralytics import YOLO
 
 from threshold_lane.threshold import lane_detection
 
 # import open3d as o3d
-from sensor_msgs.msg import CameraInfo
+from sensor_msgs.msg import CameraInfo, LaserScan, PointCloud2, PointField
+from sensor_msgs import point_cloud2
 
 
 class CVModelInferencer:
@@ -38,25 +39,21 @@ def __init__(self):
 
         self.pub = rospy.Publisher('cv/lane_detections', FloatArray, queue_size=10)
         self.pub_raw = rospy.Publisher('cv/model_output', Image, queue_size=10)
+        self.pub_pt = rospy.Publisher('cv/lane_detections_cloud', PointCloud2, queue_size=10)
+        # self.pub_scan = rospy.Publisher('cv/lane_detections_scan', LaserScan, queue_size=10)
 
         self.bridge = CvBridge()
         self.projection = camera_projection.CameraProjection()
 
         rospack = rospkg.RosPack()
-        # self.model_path = rospack.get_path('lane_detection') + '/models/competition_model_4c_128.pt'
         self.model_path = rospack.get_path('lane_detection') + '/models/best.pt'
+        self.depth_map_path = rospack.get_path('lane_detection') + '/config/num.npy'
+
 
         # Get the parameter to decide between deep learning and classical
         self.classical_mode = rospy.get_param('/lane_detection_inference/lane_detection_mode')
         self.Inference = None
         self.lane_detection = None
-
-        # Load in the depth matrix
-        # depth_dir = rospack.get_path('lane_detection') + '/config/depth_sim.npy'
-        # depth_matrix_np = np.load(depth_dir)
-        # depth_matrix_np = depth_matrix_np * 1000 # Convert from (m) to (mm)
-        # depth_matrix_np = cv2.resize(depth_matrix_np, (330, 180))
-        # self.depth_matrix = o3d.geometry.Image(depth_matrix_np.astype(np.float32))
 
         if self.classical_mode == 1:
             self.lane_detection = lane_detection
@@ -67,10 +64,10 @@ def __init__(self):
 
             rospy.loginfo("Lane Detection node initialized with DEEP LEARNING...\nCUDA status: %s ", torch.cuda.is_available())
 
-        # self.hack = cv2.imread(r'/home/ammarvora/utra/espresso-ws/src/Espresso/cv/lane_detection/src/lane.png')
-
-        # print(self.hack.shape)
-
+        # listen for transform from camera to lidar frames
+        # self.listener = tf.TransformListener()
+        # self.listener.waitForTransform("/left_camera_link_optical", "/base_laser", rospy.Time(), rospy.Duration(10.0))
+        
 
 
     def run(self):
@@ -93,9 +90,6 @@ def lane_transform(self, img):
                                 [int(width/2)-new_width,length]])
         M2 = cv2.getPerspectiveTransform(input_pts,output_pts)
         out = cv2.warpPerspective(img,M2,(width, length),flags=cv2.INTER_LINEAR)
-        # plt.imshow(out)
-        # cv2.imshow('test', out)
-        # cv2.waitKey(0)
         return out
 
 
@@ -104,15 +98,14 @@ def process_image(self, data):
             return
 
         raw = self.bridge.imgmsg_to_cv2(data, desired_encoding='passthrough')
+        projected_lanes = np.load(self.depth_map_path)
+
 
         if raw is not None:
             # Get the image
             input_img = raw.copy()
-            # input_img = self.hack
-
-            # input_img = cv2.resize(raw.shape[1], raw.shape[0])
+            input_img = cv2.resize(input_img, (330, 180))
 
-            # cv2.imwrite(r'/home/ammarvora/utra/espresso-ws/src/Espresso/cv/lane_detection/src' + 'frame.png', input_img)
             # Do model inference 
             output = None
             mask = None
@@ -122,48 +115,29 @@ def process_image(self, data):
 
                 mask = np.where(output > 0.5, 1., 0.)
                 mask = mask.astype(np.uint8)
-                mask = cv2.resize(mask, (330, 180))
 
             else:
                 # output = self.Inference.inference(input_img)
-                # input_img = cv2.resize(input_img, (330, 180))
-                cv2.rectangle(input_img, (0,0), (input_img.shape[1],int(input_img.shape[0] / 10)), (0,0,0), -1) 
-                # cv2.imwrite(r'/home/tsyh/Downloads/test.jpg', input_img.squeeze())
+                # cv2.rectangle(input_img, (0,0), (input_img.shape[1],int(input_img.shape[0] / 9)), (0,0,0), -1) 
 
                 output = self.Inference(input_img)
                 confidence_threshold = 0.5
-                # number_masks = sum(1 for box in results[0].boxes if float(box.conf) > confidence_threshold)
-                # print("number masks: ", number_masks)
 
-                labels = {}
                 output_image = np.zeros_like(input_img[:,:,0], dtype=np.uint8)
+                # output_image = np.zeros_like(projected_lanes[:,:,0], dtype=np.uint8)
 
                 if output[0].masks:
                     for k in range(len(output[0].masks)):
                         mask = np.array(output[0].masks[k].data.cpu() if torch.cuda.is_available() else output[0].masks[k].data)  # Convert tensor to numpy array
                         label = output[0].names[int(output[0].boxes[k].cls)]
 
-                        if label not in labels:
-                            labels[label] = np.zeros((180,330), dtype=np.uint8)
-
                         if float(output[0].boxes[k].conf) > confidence_threshold:  # Check confidence level
                             if label == 'lane':
                                 img = np.where(mask > 0.5, 255, 0).astype(np.uint8)
                                 img = cv2.resize(img.squeeze(), (output_image.shape[1], output_image.shape[0]))
                                 output_image = np.maximum(output_image, img)
 
-                            resize_mask = np.where(mask > 0.5, 1., 0.).astype(np.uint8)
-                            resize_mask = cv2.resize(resize_mask.squeeze(), (330, 180))
-
-                            labels[label] = np.maximum(labels[label], resize_mask)
                 output = output_image
-                mask = labels['lane'] if 'lane' in labels else np.zeros((330,180), dtype=np.uint8)
-
-
-
-            # mask = np.where(output > 0.5, 1., 0.)
-            # mask = mask.astype(np.uint8)
-            # mask = cv2.resize(mask, (330, 180))
 
             # Publish to /cv/model_output
             img_msg = self.bridge.cv2_to_imgmsg(output, encoding='passthrough')
@@ -172,32 +146,22 @@ def process_image(self, data):
             if img_msg is not None:
                 self.pub_raw.publish(img_msg)
 
-
-            '''The following code is needed for virtual layers'''
-            rows = np.where(mask==1)[0].reshape(-1,1)
-            cols = np.where(mask==1)[1].reshape(-1,1)
-            lane_table = np.concatenate((cols,rows),axis=1)
-
-            # print(lane_table)
-
-            # ta = time.time()
-            projected_lanes = self.projection(lane_table)
-            # tb = time.time()
-
-            # print(f'PROJECTION FPS: {1 / (tb - ta)}')
-
             # Build the message
             lane_msg = FloatList()
             pts_msg = []
+            cloud = []
 
-            for pt in projected_lanes:
+            for i in range(output.shape[0]):
+                for j in range(output.shape[1]):
+                    if((output[i][j])==255):
+                        pt_msg = Point()
+                        pt_msg.x = projected_lanes[i][j][0]
+                        pt_msg.y = projected_lanes[i][j][1]
+                        pt_msg.z = projected_lanes[i][j][2]
 
-                pt_msg = Point()
-                pt_msg.x = pt[0]
-                pt_msg.y = pt[1]
-                pt_msg.z = pt[2]
+                        pts_msg.append(pt_msg)
+                        cloud.append(projected_lanes[i][j])
 
-                pts_msg.append(pt_msg)
             lane_msg.elements = pts_msg
 
 
@@ -206,6 +170,11 @@ def process_image(self, data):
             msg.header.stamp = data.header.stamp
             self.pub.publish(msg)
 
+            pt_header = Header(frame_id='left_camera_link_optical')
+            pt_header.stamp = data.header.stamp
+            pt_cloud = point_cloud2.create_cloud_xyz32(header=pt_header, points=cloud)
+            self.pub_pt.publish(pt_cloud)
+
 
 
 

cv/lane_detection/src/lane_viz.py

@@ -19,7 +19,7 @@ class FloatArrayToPointCloud2Node:
     def __init__(self):
         rospy.init_node("float_array_to_pc2_node")
         self.points2_pub = rospy.Publisher(
-            "/cv/lane_detections_cloud", PointCloud2, queue_size=1
+            "/cv/lane_cloud", PointCloud2, queue_size=1
         )
 
         # listen for transform from camera to lidar frames

description/config/cartographer.lua

@@ -35,10 +35,10 @@ end
 
 -- check if cv published
 local topic_name = "/cv/lane_detections\n" -- DON'T REMOVE THE SPACE AFTER. WE NEED IT
-num_scan = 1
+num_scan = 0
 if is_topic_published(topic_name) then
   print("......................cv published")
-    num_scan = 2
+    num_scan = 1
 end
 
 print("NUMBER SCAN: ", num_scan)
@@ -47,7 +47,7 @@ options = {
   map_builder = MAP_BUILDER,
   trajectory_builder = TRAJECTORY_BUILDER,
   map_frame = "map",
-  tracking_frame = "imu_link",
+  tracking_frame = "base_link",
   published_frame = "base_link",
   odom_frame = "odom",
   provide_odom_frame = true,
@@ -56,10 +56,10 @@ options = {
   use_odometry = true,
   use_nav_sat = true,
   use_landmarks = false,
-  num_laser_scans = num_scan,
+  num_laser_scans = 1-num_scan,
   num_multi_echo_laser_scans = 0,
   num_subdivisions_per_laser_scan = 1,
-  num_point_clouds = 0,
+  num_point_clouds = num_scan,
   lookup_transform_timeout_sec = 1.,
   submap_publish_period_sec = 0.3,
   pose_publish_period_sec = 5e-3,
@@ -76,8 +76,8 @@ MAP_BUILDER.use_trajectory_builder_2d = true
 TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10
 
 TRAJECTORY_BUILDER_2D.min_range = 0.1
-TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.
-TRAJECTORY_BUILDER_2D.use_imu_data = true
+TRAJECTORY_BUILDER_2D.missing_data_ray_length = 15.
+TRAJECTORY_BUILDER_2D.use_imu_data = false
 TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10
 TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 15
 
@@ -97,7 +97,7 @@ POSE_GRAPH.global_constraint_search_after_n_seconds = 30 -- Increase
 
 ---------Global/Local SLAM---------
 TRAJECTORY_BUILDER_2D.submaps.num_range_data = 100 -- Decrease
-TRAJECTORY_BUILDER_2D.max_range = 3.5 -- Decrease
+TRAJECTORY_BUILDER_2D.max_range = 10 -- Decrease
 
 -------------------------------------------------------------------------------------
 

description/launch/cartographer.launch

@@ -8,11 +8,11 @@
     <node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" args="-configuration_directory $(find description)/config -configuration_basename cartographer.lua" output="screen">
 
         <remap from="imu" to="/imu/data" />
-        <remap from="odom" to="/odom" />
+        <remap from="odom" to="/wheel_odom/euler" />
         <remap from="scan" to="/scan_modified" />
-        <remap from="scan_1" to="/cv/lane_detections_scan"/>
-        <remap from="scan_2" to="/scan_modified" />
-        <!-- <remap from="points2" to="/cv/lane_detections_cloud" />  -->
+        <!-- <remap from="scan_1" to="/cv/lane_detections_scan"/>
+        <remap from="scan_2" to="/scan_modified" /> -->
+        <remap from="points2" to="/cv/lane_detections_cloud" /> 
         <remap from="fix" to="/gps/fix"/>
     </node>
 

description/launch/simulate.launch

@@ -6,7 +6,7 @@
 
     <!-- IGVC worlds: full, walls, ramp, plain -->
     <!-- IGVC world types: pavement (2022 IGVC) -->
-    <arg name="world" default="full"/>
+    <arg name="world" default="ramp"/>
     <arg name="world_type" default="pavement"/>
 
     <!-- Simulate 'world' in Gazebo -->
@@ -44,6 +44,6 @@
     args="-19.5 0 0 1.5707 0 0 world ground_truth" />
 
     <!-- Cartographer (Mapping), publishes odom and map frame transforms -->
-    <include file="$(find description)/launch/cartographer.launch" />
+    <!-- <include file="$(find description)/launch/cartographer.launch" /> -->
 
 </launch>

description/src/poor_mans_scheduler.py

@@ -58,7 +58,7 @@ def __init__(self):
         self.merged_scan_set = False
         self.assign_topic('/pause_navigation', 'manual_default_set', Bool, True)
         self.assign_topic('/scan_modified', 'scan_override_set', LaserScan)
-        #self.assign_topic('/cv/lane_detections_cloud','cv_cloud_set',PointCloud2)
+        #self.assign_topic('/cv/lane_cloud','cv_cloud_set',PointCloud2)
         self.assign_topic('/cv/lane_detections_scan','cv_lane_scan_set',LaserScan)
         self.assign_topic('/scan_merged','merged_scan_set',LaserScan)