feat(#211): Add rear filter

ll7 · Mar 16, 2023 · e9dd931 · e9dd931
1 parent 4fbc35f
commit e9dd931
Show file tree

Hide file tree

Showing 3 changed files with 92 additions and 12 deletions.
diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz
@@ -5,6 +5,8 @@ Panels:
  Property Tree Widget:
  Expanded:
  - /PointCloud23
+ - /PointCloud24
+ - /PointCloud25
  Splitter Ratio: 0.5
  Tree Height: 308
  - Class: rviz/Selection
@@ -70,13 +72,13 @@ Visualization Manager:
  - Alpha: 1
  Class: rviz_plugin_tutorials/Imu
  Color: 204; 51; 204
- Enabled: true
+ Enabled: false
  History Length: 1
  Name: Imu
  Queue Size: 10
  Topic: /carla/hero/IMU
  Unreliable: false
- Value: true
+ Value: false
  - Alpha: 1
  Autocompute Intensity Bounds: true
  Autocompute Value Bounds:
@@ -89,7 +91,7 @@ Visualization Manager:
  Color: 255; 255; 255
  Color Transformer: Intensity
  Decay Time: 0
- Enabled: false
+ Enabled: true
  Invert Rainbow: false
  Max Color: 255; 255; 255
  Min Color: 0; 0; 0
@@ -104,7 +106,7 @@ Visualization Manager:
  Unreliable: false
  Use Fixed Frame: true
  Use rainbow: true
- Value: false
+ Value: true
  - Alpha: 1
  Autocompute Intensity Bounds: true
  Autocompute Value Bounds:
@@ -185,6 +187,62 @@ Visualization Manager:
  Use Fixed Frame: true
  Use rainbow: true
  Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.20000000298023224
+ Style: Flat Squares
+ Topic: /carla/hero/LIDAR_filtered_rear_left
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.20000000298023224
+ Style: Flat Squares
+ Topic: /carla/hero/LIDAR_filtered_rear_right
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
  Enabled: true
  Global Options:
  Background Color: 48; 48; 48
@@ -213,7 +271,7 @@ Visualization Manager:
  Views:
  Current:
  Class: rviz/Orbit
- Distance: 5.234607219696045
+ Distance: 18.93408966064453
  Enable Stereo Rendering:
  Stereo Eye Separation: 0.05999999865889549
  Stereo Focal Distance: 1
@@ -229,9 +287,9 @@ Visualization Manager:
  Invert Z Axis: false
  Name: Current View
  Near Clip Distance: 0.009999999776482582
- Pitch: 0.1753983497619629
+ Pitch: 0.5053982734680176
  Target Frame: <Fixed Frame>
- Yaw: 4.805431365966797
+ Yaw: 2.630427598953247
  Saved: ~
 Window Geometry:
  Camera:

diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch
@@ -30,10 +30,30 @@
  <node pkg="perception" type="lidar_distance.py" name="lidar_distance" output="screen">
  <param name="max_y" value="1.5"/>
  <param name="min_y" value="-1.5"/>
- <param name="min_x" value="2.5"/>
+ <param name="min_x" value="3"/>
  <param name="min_z" value="-1.5"/>
  <param name="max_z" value="0"/>
  <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="-1.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="1.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
@@ -5,6 +5,7 @@
 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
@@ -27,11 +28,11 @@ def callback(self, data):
  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),
+ 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),
+ min_y=rospy.get_param('~min_y', -np.inf),
  max_z=rospy.get_param('~max_z', np.inf),
- min_z=rospy.get_param('~min_z', np.inf),
+ min_z=rospy.get_param('~min_z', -np.inf),
  )
 
  # Filter coordinates based in generated bit_mask
@@ -82,7 +83,8 @@ def listener(self):
  Range
  )
 
- rospy.Subscriber(rospy.get_param('~source_topic', "/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()