Add block laser clipping test

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

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

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)

gazebo_plugins/test/block_laser_clipping.test

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+ <param name="/use_sim_time" value="true" />
+
+ <node name="gazebo" pkg="gazebo_ros" type="gzserver"
+ respawn="false" output="screen"
+ args="--verbose $(find gazebo_plugins)/test/test_worlds/gazebo_ros_block_laser_clipping.world" />
+
+ <test test-name="block_laser_clipping" pkg="gazebo_plugins" type="test_block_laser_clipping.py"
+ clear_params="true" time-limit="30.0" />
+</launch>

gazebo_plugins/test/test_worlds/gazebo_ros_block_laser_clipping.world

@@ -0,0 +1,90 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+ <world name="default">
+ <model name="test_block_laser">
+ <pose>0 0 0.8 0.0 1.5707 0.0</pose>
+ <static>true</static>
+ <link name="link">
+ <visual name="visual">
+ <pose>0.007 0 -0.033 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.07 0.065 0.065</size>
+ </box>
+ </geometry>
+ </visual>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.07 0.065 0.065</size>
+ </box>
+ </geometry>
+ </collision>
+ <sensor name="laser_profiler" type="ray">
+ <update_rate>5</update_rate>
+ <pose>0 0 0.0 0 0 0</pose>
+ <plugin name="proximity_ray_plugin" filename="libgazebo_ros_block_laser.so" >
+ <robotNamespace>/</robotNamespace>
+ <frameName>test_block_laser_frame</frameName>
+ <topicName>test_block_laser</topicName>
+ <alwaysOn>true</alwaysOn>
+ </plugin>
+ <ray>
+ <range>
+ <min>0.4</min>
+ <max>1.0</max>
+ <resolution>0.001</resolution>
+ </range>
+ <scan>
+ <horizontal>
+ <samples>50</samples>
+ <resolution>1</resolution>
+ <min_angle>-0.3</min_angle>
+ <max_angle>0.3</max_angle>
+ </horizontal>
+ <vertical>
+ <samples>50</samples>
+ <resolution>1</resolution>
+ <min_angle>-0.4</min_angle>
+ <max_angle>0.4</max_angle>
+ </vertical>
+ </scan>
+ <noise>
+ <type>gaussian</type>
+ <mean>0.0</mean>
+ <stddev>0.001</stddev>
+ </noise>
+ </ray>
+
+ <always_on>1</always_on>
+ <visualize>True</visualize>
+ </sensor>
+ <gravity>false</gravity>
+ <inertial>
+ <mass>0.1</mass>
+ </inertial>
+ </link>
+ </model>
+
+ <model name="slope">
+ <static>true</static>
+ <pose>0.0991 -0.002 0.0909 -0.239 0.789 0</pose>
+ <link name="link">
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>1 0.1 0.1</size>
+ </box>
+ </geometry>
+ </visual>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>1 0.1 0.1</size>
+ </box>
+ </geometry>
+ </collision>
+ </link>
+ </model>
+ </world>
+</sdf>