feat(#211): Added parametrization

code/perception/src/lidar_distance.py

@@ -5,11 +5,13 @@
 import lidar_filter_utility
 from sensor_msgs.msg import PointCloud2, Range
 
+
 class LidarDistance():
 
  def callback(self, data):
- """ Callback function to process PointCloud2 and publish a Range message
- from the contained points and a PointCloud2
+ """ Callback function to process PointCloud2 and
+ publish a Range message from the contained points
+ and a PointCloud2
 
  :param data:
  :return:
@@ -19,8 +21,8 @@ def callback(self, data):
  # https://stackoverflow.com/questions/44295375/how-to-slice-a-numpy-ndarray-made-up-of-numpy-void-numbers
  bit_mask = lidar_filter_utility.bounding_box(
  coordinates,
- max_x = rospy.get_param('~max_x', np.inf),
- min_x= rospy.get_param('~min_x', np.inf),
+ max_x=rospy.get_param('~max_x', np.inf),
+ min_x=rospy.get_param('~min_x', np.inf),
  max_y=rospy.get_param('~max_y', np.inf),
  min_y=rospy.get_param('~min_y', np.inf),
  max_z=rospy.get_param('~max_z', np.inf),
@@ -33,15 +35,20 @@ def callback(self, data):
  rospy.loginfo(len(coordinates))
 
  # Create pointcloud from manipulated data
- coordinates_manipulated = ros_numpy.point_cloud2.array_to_pointcloud2(coordinates)
+ coordinates_manipulated = ros_numpy \
+ .point_cloud2.array_to_pointcloud2(coordinates)
  coordinates_manipulated.header = data.header
 
  # Publish manipulated pointCloud2
  self.pub_pointcloud.publish(coordinates_manipulated)
 
  # https://stackoverflow.com/questions/1401712/how-can-the-euclidean-distance-be-calculated-with-numpy
- coordinates_xyz = np.array(lidar_filter_utility.remove_field_name(coordinates, 'intensity').tolist())
- distances = np.array([np.linalg.norm(c - [0, 0, 0]) for c in coordinates_xyz])
+ coordinates_xyz = np.array(
+ lidar_filter_utility.remove_field_name(coordinates, 'intensity')
+ .tolist()
+ )
+ distances = np.array(
+ [np.linalg.norm(c - [0, 0, 0]) for c in coordinates_xyz])
 
  if len(distances) > 0:
  range_msg = Range()
@@ -59,10 +66,20 @@ def listener(self):
  # run simultaneously.
  rospy.init_node('lidar_distance')
 
- namespace = rospy.get_namespace()
-
- self.pub_pointcloud = rospy.Publisher(rospy.get_param('~point_cloud_topic', '/carla/hero/' + rospy.get_namespace() + '_filtered'), PointCloud2)
- self.pub_range = rospy.Publisher(rospy.get_param('~range_topic', '/carla/hero/' + rospy.get_namespace() + '_range'), Range)
+ self.pub_pointcloud = rospy.Publisher(
+ rospy.get_param(
+ '~point_cloud_topic',
+ '/carla/hero/' + rospy.get_namespace() + '_filtered'
+ ),
+ PointCloud2
+ )
+ self.pub_range = rospy.Publisher(
+ rospy.get_param(
+ '~range_topic',
+ '/carla/hero/' + rospy.get_namespace() + '_range'
+ ),
+ Range
+ )
 
  rospy.Subscriber("/carla/hero/LIDAR", PointCloud2, self.callback)
 

code/perception/src/lidar_filter_utility.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python
 import numpy as np
 
+
 # https://gist.github.com/bigsnarfdude/bbfdf343cc2fc818dc08b58c0e1374ae
 def bounding_box(points, min_x=-np.inf, max_x=np.inf, min_y=-np.inf,
  max_y=np.inf, min_z=-np.inf, max_z=np.inf):
@@ -9,21 +10,24 @@ def bounding_box(points, min_x=-np.inf, max_x=np.inf, min_y=-np.inf,
  Parameters
  ----------
  points: (n,3) array
- The array containing all the points's coordinates. Expected format:
+ The array containing all the points's coordinates.
+ Expected format:
  array([
  [x1,y1,z1],
  ...,
  [xn,yn,zn]])
 
  min_i, max_i: float
- The bounding box limits for each coordinate. If some limits are missing,
- the default values are -infinite for the min_i and infinite for the max_i.
+ The bounding box limits for each coordinate.
+ If some limits are missing, the default values
+ are -infinite for the min_i and infinite for the max_i.
 
  Returns
  -------
  bb_filter : boolean array
- The boolean mask indicating wherever a point should be keeped or not.
- The size of the boolean mask will be the same as the number of given points.
+ The boolean mask indicating wherever a point should be
+ keeped or not. The size of the boolean mask will be the
+ same as the number of given points.
 
  """
 
@@ -35,6 +39,7 @@ def bounding_box(points, min_x=-np.inf, max_x=np.inf, min_y=-np.inf,
 
  return bb_filter
 
+
 # https://stackoverflow.com/questions/15575878/how-do-you-remove-a-column-from-a-structured-numpy-array
 def remove_field_name(a, name):
  names = list(a.dtype.names)