Merge pull request #33 from sloretz/fix_dont_clip_intensity

Allows ranger intensities to be greater than 256

Author: gerkey

CMakeLists.txt

@@ -62,4 +62,5 @@ install(DIRECTORY world
 if(CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   add_rostest(test/hztest.xml)
+  add_rostest(test/intensity_test.xml)
 endif()

src/stageros.cpp

@@ -457,7 +457,7 @@ StageNode::WorldCallback()
                 for(unsigned int i = 0; i < sensor.ranges.size(); i++)
                 {
                     msg.ranges[i] = sensor.ranges[i];
-                    msg.intensities[i] = (uint8_t)sensor.intensities[i];
+                    msg.intensities[i] = sensor.intensities[i];
                 }
 
                 if (robotmodel->lasermodels.size() > 1)

test/intensity_test.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python
+
+import unittest
+import rospy
+import time
+import Queue as queue
+from sensor_msgs.msg import LaserScan
+
+PKG = "stage_ros"
+
+
+class RangerIntensityTests(unittest.TestCase):
+    def setUp(self):
+        """Called before every test. Set up a LaserScan subscriber
+        """
+        rospy.init_node('ranger_intensity_test', anonymous=True)
+        self._scan_q = queue.Queue()
+        self._scan_topic = rospy.get_param("scan_topic", "base_scan")
+        self._subscriber = rospy.Subscriber(self._scan_topic,
+                                            LaserScan, self._scan_callback)
+
+    def tearDown(self):
+        """Called after every test. Cancel the scan subscription
+        """
+        self._subscriber.unregister()
+        self._scan_q = None
+        self._subscriber = None
+
+    def _scan_callback(self, scan_msg):
+        """Called every time a scan is received
+        """
+        self._scan_q.put(scan_msg)
+
+    def _wait_for_scan(self, timeout=1.0):
+        """Wait for a laser scan to be received
+        """
+        # Use wall clock time for timeout
+        end_time = time.time() + timeout
+        while time.time() < end_time:
+            try:
+                return self._scan_q.get(True, 0.1)
+            except queue.Empty:
+                pass
+        return None
+
+    def test_intensity_greater_than_256(self):
+        """Make sure stage_ros returns intensity values higher than 256
+        """
+        scan = self._wait_for_scan()
+        self.assertIsNotNone(scan)
+        self.assertGreater(max(scan.intensities), 256.9)
+
+
+if __name__ == '__main__':
+    import rosunit
+    rosunit.unitrun(PKG, 'test_intensity', RangerIntensityTests)

test/intensity_test.xml

@@ -0,0 +1,8 @@
+<launch>
+    <param name="/use_sim_time" value="true"/>
+    <node pkg="stage_ros" name="stageros" type="stageros" args="-g $(find stage_ros)/world/intense.world"/>
+
+    <test test-name="intensity_check" pkg="stage_ros" type="intensity_test.py" name="scan_intensity_test">
+        <param name="scan_topic" value="base_scan" />
+    </test>
+</launch>

world/erratic-inc.world

@@ -0,0 +1,26 @@
+# A simple robot for inclusion in other world files
+
+define topurg ranger
+(
+    sensor
+    (
+        range [ 0.0  30.0 ]
+        fov 270.25
+        samples 1081
+    )
+    # generic model properties
+    color "black"
+    size [ 0.05 0.05 0.1 ]
+)
+
+define erratic position
+(
+    size [ 0.35 0.35 0.25 ]
+    origin [ -0.05 0 0 0 ]
+    gui_nose 1
+    drive "diff"
+    topurg
+    (
+        pose [ 0 0 0 0 ]
+    )
+)

world/intense.world

@@ -0,0 +1,63 @@
+# A world containing objects with varying ranger insensity values
+
+include "erratic-inc.world"
+
+resolution 0.02
+interval_sim 100
+
+define block model
+(
+    size [0.5 0.5 0.5]
+    gui_nose 0
+)
+
+erratic
+(
+    pose [ 0 0 0 0 ]
+    name "era"
+    color "blue"
+)
+
+# Insert blocks with different intensity values
+block
+(
+    pose [ 1.0 -3 0 0 ]
+    ranger_return 0
+    color "gray0"
+)
+block
+(
+    pose [ 1.0 -2 0 0 ]
+    ranger_return 50
+    color "gray5"
+)
+block
+(
+    pose [ 1.0 -1 0 0 ]
+    ranger_return 100
+    color "gray10"
+)
+block
+(
+    pose [ 1.0 0 0 0 ]
+    ranger_return 250
+    color "gray25"
+)
+block
+(
+    pose [ 1.0 1 0 0 ]
+    ranger_return 4000
+    color "gray40"
+)
+block
+(
+    pose [ 1.0 2 0 0 ]
+    ranger_return 80000
+    color "gray80"
+)
+block
+(
+    pose [ 1.0 3 0 0 ]
+    ranger_return 1000000
+    color "gray100"
+)