diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt
index 99a7f50c5..abe50c897 100644
--- a/gazebo_plugins/CMakeLists.txt
+++ b/gazebo_plugins/CMakeLists.txt
@@ -396,6 +396,7 @@ if (CATKIN_ENABLE_TESTING)
target_link_libraries(set_model_state-test ${catkin_LIBRARIES})
add_rostest(test/range/range_plugin.test)
+ add_rostest(test/block_laser_clipping.test)
if (ENABLE_DISPLAY_TESTS)
add_rostest_gtest(depth_camera-test
diff --git a/gazebo_plugins/scripts/test_block_laser_clipping.py b/gazebo_plugins/scripts/test_block_laser_clipping.py
new file mode 100755
index 000000000..169d87e84
--- /dev/null
+++ b/gazebo_plugins/scripts/test_block_laser_clipping.py
@@ -0,0 +1,53 @@
+#!/usr/bin/env python
+
+"""
+Test plugin gazebo_ros_block_laser using gazebo_ros_block_laser_clipping.world.
+"""
+
+import math
+import rospy
+from sensor_msgs.msg import PointCloud
+
+import unittest
+
+class TestBlockLaserClippingPlugin(unittest.TestCase):
+ MIN_RANGE = 0.4
+ MAX_RANGE = 1.0
+
+ def _ranges(self):
+ msg = rospy.wait_for_message('test_block_laser', PointCloud)
+ for point in msg.points:
+ yield math.sqrt(point.x**2 + point.y**2 + point.z**2)
+
+ def test_points_at_all_depths(self):
+ # Make sure there are points at all depths
+ BIN_SIZE = 0.01
+ num_bins = int((self.MAX_RANGE - self.MIN_RANGE) / BIN_SIZE)
+ bins = [b * BIN_SIZE + self.MIN_RANGE for b in range(num_bins)]
+ bin_counts = dict(zip(bins, [0] * len(bins)))
+
+ for r in self._ranges():
+ closest_bin = None
+ closest_dist = float('inf')
+ for b in bins:
+ dist = abs(r - b)
+ if abs(r - b) < closest_dist:
+ closest_dist = dist
+ closest_bin = b
+ bin_counts[closest_bin] += 1
+
+ self.assertTrue(0 not in bin_counts.values(), msg=repr(bin_counts))
+
+ def test_between_min_max(self):
+ ALLOWED_ERROR = 0.00001
+ for r in self._ranges():
+ self.assertGreater(r, self.MIN_RANGE - ALLOWED_ERROR)
+ self.assertLess(r, self.MAX_RANGE + ALLOWED_ERROR)
+
+
+if __name__ == '__main__':
+ import rostest
+ PKG_NAME = 'gazebo_plugins'
+ TEST_NAME = PKG_NAME + 'block_laser_clipping'
+ rospy.init_node(TEST_NAME)
+ rostest.rosrun(PKG_NAME, TEST_NAME, TestBlockLaserClippingPlugin)
diff --git a/gazebo_plugins/test/block_laser_clipping.test b/gazebo_plugins/test/block_laser_clipping.test
new file mode 100644
index 000000000..bdcf1cb0f
--- /dev/null
+++ b/gazebo_plugins/test/block_laser_clipping.test
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
diff --git a/gazebo_plugins/test/test_worlds/gazebo_ros_block_laser_clipping.world b/gazebo_plugins/test/test_worlds/gazebo_ros_block_laser_clipping.world
new file mode 100644
index 000000000..54f415f2e
--- /dev/null
+++ b/gazebo_plugins/test/test_worlds/gazebo_ros_block_laser_clipping.world
@@ -0,0 +1,90 @@
+
+
+
+
+ 0 0 0.8 0.0 1.5707 0.0
+ true
+
+
+ 0.007 0 -0.033 0 0 0
+
+
+ 0.07 0.065 0.065
+
+
+
+
+
+
+ 0.07 0.065 0.065
+
+
+
+
+ 5
+ 0 0 0.0 0 0 0
+
+ /
+ test_block_laser_frame
+ test_block_laser
+ true
+
+
+
+ 0.4
+ 1.0
+ 0.001
+
+
+
+ 50
+ 1
+ -0.3
+ 0.3
+
+
+ 50
+ 1
+ -0.4
+ 0.4
+
+
+
+ gaussian
+ 0.0
+ 0.001
+
+
+
+ 1
+ True
+
+ false
+
+ 0.1
+
+
+
+
+
+ true
+ 0.0991 -0.002 0.0909 -0.239 0.789 0
+
+
+
+
+ 1 0.1 0.1
+
+
+
+
+
+
+ 1 0.1 0.1
+
+
+
+
+
+
+