diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index f8722593..f657445e 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -5,16 +5,21 @@ import lidar_filter_utility from sensor_msgs.msg import PointCloud2, Range - class LidarDistance(): + """ See doc/06_perception/02_lidar_distance_utility.md on + how to configute this node + """ def callback(self, data): - """ Callback function to process PointCloud2 and - publish a Range message from the contained points - and a PointCloud2 + """ 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. - :param data: - :return: + :param data: a PointCloud2 """ coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) @@ -29,10 +34,8 @@ def callback(self, data): min_z=rospy.get_param('~min_z', np.inf), ) - rospy.loginfo(len(coordinates)) # Filter coordinates based in generated bit_mask coordinates = coordinates[bit_mask] - rospy.loginfo(len(coordinates)) # Create pointcloud from manipulated data coordinates_manipulated = ros_numpy \ @@ -59,10 +62,8 @@ def callback(self, data): self.pub_range.publish(range_msg) def listener(self): - # In ROS, nodes are uniquely named. If two nodes with the same - # name are launched, the previous one is kicked off. The - # anonymous=True flag means that rospy will choose a unique - # name for our 'listener' node so that multiple listeners can + """ Initializes the node and it's publishers + """ # run simultaneously. rospy.init_node('lidar_distance') @@ -81,7 +82,7 @@ def listener(self): Range ) - rospy.Subscriber("/carla/hero/LIDAR", PointCloud2, self.callback) + rospy.Subscriber(rospy.get_param('~source_topic', "/carla/hero/LIDAR"), PointCloud2, self.callback) # spin() simply keeps python from exiting until this node is stopped rospy.spin() diff --git a/code/perception/src/lidar_filter_utility.py b/code/perception/src/lidar_filter_utility.py index d0bf0e2a..4cd50260 100644 --- a/code/perception/src/lidar_filter_utility.py +++ b/code/perception/src/lidar_filter_utility.py @@ -42,6 +42,12 @@ def bounding_box(points, min_x=-np.inf, max_x=np.inf, min_y=-np.inf, # https://stackoverflow.com/questions/15575878/how-do-you-remove-a-column-from-a-structured-numpy-array def remove_field_name(a, name): + """ Removes a column from a structured numpy array + + :param a: structured numoy array + :param name: name of the column to remove + :return: structured numpy array without column + """ names = list(a.dtype.names) if name in names: names.remove(name) diff --git a/doc/06_perception/02_lidar_distance_utility.md b/doc/06_perception/02_lidar_distance_utility.md index e69de29b..27fbc5ea 100644 --- a/doc/06_perception/02_lidar_distance_utility.md +++ b/doc/06_perception/02_lidar_distance_utility.md @@ -0,0 +1,62 @@ +# Lidar Distance Utility + +**Summary:** + +This node can filter points from a [PoinCloud2](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointCloud2.html) +which are located in a given bounding box. +It then publishes a filtered [PoinCloud2](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointCloud2.html) containing +the points located in the given restriction. +Additionally, it publishes a [Range](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Range.html) message +containing the closest and the farest point. +This can then be used to detect the distance to the closest object in front of us. + +--- + +## Author + +Tim Dreier + +## Date + +16.03.2023 + +--- + +* [Title of wiki page](#title-of-wiki-page) + * [Author](#author) + * [Date](#date) + * [Prerequisite](#prerequisite) + * [Some Content](#some-content) + * [more Content](#more-content) + * [Sources](#sources) + + +## Configuration + +The possible params for this node are summed up in the following table: + +| Parameter | Description | Default value | +|-------------------|-------------------------------------------------------|-----------------------------------| +| min_x | Minimum x value of the coordinate | `np.inf` | +| max_x | Maximum x value of the coordinate | `np.inf` | +| min_y | Minimum y value of the coordinate | `np.inf` | +| max_y | Maximum y value of the coordinate | `np.inf` | +| min_z | Minimum z value of the coordinate | `np.inf` | +| max_z | Maximum z value of the coordinate | `np.inf` | +| point_cloud_topic | Topic to publish the filtered PointCloud2 | `/carla/hero/filtered` | +| range_topic | Topic to publish the Range | `/carla/hero/_range` | +| source_topic | Topic on which the PointCloud2 messages are read from | `/carla/hero/LIDAR` | + +### Example + +```xml + + + + + + + + + +```