Merge pull request #247 from una-auxme/236-feature-documentation-visi…

…onnode-lidarnode 236 feature documentation visionnode lidarnode
una-auxme · Apr 1, 2024 · 8cb891c · 8cb891c
2 parents 179dd07 + da3f852
commit 8cb891c
Show file tree

Hide file tree

Showing 13 changed files with 361 additions and 329 deletions.
diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch
@@ -1,4 +1,3 @@
-<!-- TODO: Insert components of component-->
 <launch>
   <arg name="role_name" default="hero" />
   <arg name="control_loop_rate" default="0.1" />
@@ -80,25 +79,4 @@
     <param name="point_cloud_topic" value="/carla/hero/LIDAR_filtered"/>
     <param name="range_topic" value="/carla/hero/LIDAR_range"/>
   </node>
-
- <!--<node pkg="perception" type="lidar_distance.py" name="lidar_distance_rear_right" output="screen">
-    <param name="min_y" value="-5"/>
-    <param name="max_y" value="-2.5"/>
-    <param name="max_x" value="0"/>
-    <param name="min_z" value="-1.5"/>
-    <param name="max_z" value="0"/>
-    <param name="point_cloud_topic" value="/carla/hero/LIDAR_filtered_rear_right"/>
-    <param name="range_topic" value="/carla/hero/LIDAR_range_rear_right"/>
-  </node>
-
-  <node pkg="perception" type="lidar_distance.py" name="lidar_distance_rear_left" output="screen">
-    <param name="min_y" value="2.5"/>
-    <param name="max_y" value="5"/>
-    <param name="max_x" value="0"/>
-    <param name="min_z" value="-1.5"/>
-    <param name="max_z" value="0"/>
-    <param name="point_cloud_topic" value="/carla/hero/LIDAR_filtered_rear_left"/>
-    <param name="range_topic" value="/carla/hero/LIDAR_range_rear_left"/>
-  </node>
--->
 </launch>
diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py
@@ -20,10 +20,9 @@ def callback(self, data):
         """ Callback function, filters a PontCloud2 message
             by restrictions defined in the launchfile.
 
-            Publishes a range message containing the farest and
-            the closest point of the filtered result. Additionally,
-            publishes the filtered result as PointCloud2
-            on the topic defined in the launchfile.
+            Publishes a Depth image for the specified camera angle.
+            Each angle has do be delt with differently since the signs of the
+            coordinate system change with the view angle.
 
         :param data: a PointCloud2
         """
@@ -119,7 +118,8 @@ def callback(self, data):
         self.dist_array_right_publisher.publish(dist_array_right_msg)
 
     def listener(self):
-        """ Initializes the node and it's publishers
+        """
+        Initializes the node and it's publishers
         """
         # run simultaneously.
         rospy.init_node('lidar_distance')
@@ -180,8 +180,17 @@ def listener(self):
         rospy.spin()
 
     def reconstruct_img_from_lidar(self, coordinates_xyz, focus):
-        # reconstruct 3d LIDAR-Data and calculate 2D Pixel
-        # according to Camera-World
+        """
+        reconstruct 3d LIDAR-Data and calculate 2D Pixel
+        according to Camera-World
+
+        Args:
+            coordinates_xyz (np.array): filtered lidar points
+            focus (String): Camera Angle
+
+        Returns:
+            image: depth image for camera angle
+        """
 
         # intrinsic matrix for camera:
         # width -> 300, height -> 200, fov -> 100 (agent.py)
@@ -201,6 +210,7 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus):
         img = np.zeros(shape=(720, 1280), dtype=np.float32)
         dist_array = np.zeros(shape=(720, 1280, 3), dtype=np.float32)
         for c in coordinates_xyz:
+            # center depth image
             if focus == "Center":
                 point = np.array([c[1], c[2], c[0], 1])
                 pixel = np.matmul(m, point)
@@ -209,6 +219,8 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus):
                     img[719-y][1279-x] = c[0]
                     dist_array[719-y][1279-x] = \
                         np.array([c[0], c[1], c[2]], dtype=np.float32)
+
+            # back depth image
             if focus == "Back":
                 point = np.array([c[1], c[2], c[0], 1])
                 pixel = np.matmul(m, point)
@@ -217,6 +229,8 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus):
                     img[y][1279-x] = -c[0]
                     dist_array[y][1279-x] = \
                         np.array([-c[0], c[1], c[2]], dtype=np.float32)
+
+            # left depth image
             if focus == "Left":
                 point = np.array([c[0], c[2], c[1], 1])
                 pixel = np.matmul(m, point)
@@ -225,6 +239,8 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus):
                     img[719-y][1279-x] = c[1]
                     dist_array[y][1279-x] = \
                         np.array([c[0], c[1], c[2]], dtype=np.float32)
+
+            # right depth image
             if focus == "Right":
                 point = np.array([c[0], c[2], c[1], 1])
                 pixel = np.matmul(m, point)

diff --git a/code/perception/src/lidar_filter_utility.py b/code/perception/src/lidar_filter_utility.py
@@ -31,10 +31,6 @@ def bounding_box(points, min_x=-np.inf, max_x=np.inf, min_y=-np.inf,
 
     """
 
-    """print(min_x, max_x, "X")
-    print(min_y, max_y, "Y")
-    print(min_z, max_z, "Z")"""
-
     bound_x = np.logical_and(points['x'] > min_x, points['x'] < max_x)
     bound_y = np.logical_and(points['y'] > min_y, points['y'] < max_y)
     bound_z = np.logical_and(points['z'] > min_z, points['z'] < max_z)