[Personalrobots-commit] SF.net SVN: personalrobots:[4682] pkg/trunk

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Revision: 4682
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4682&view=rev
Author:   hsujohnhsu
Date:     2008-09-25 20:07:06 +0000 (Thu, 25 Sep 2008)

Log Message:
-----------
camera test working.

Modified Paths:
--------------
    pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.py
    pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.xml
    pkg/trunk/drivers/simulator/gazebo_plugin/test/testscan.xml
    pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world

Added Paths:
-----------
    pkg/trunk/drivers/simulator/gazebo_plugin/test/testcamera.valid.ppm

Removed Paths:
-------------
    pkg/trunk/drivers/simulator/gazebo_plugin/test/testcamera.valid.jpg


Property changes on: pkg/trunk/drivers/simulator/gazebo_plugin/test/testcamera.valid.ppm
___________________________________________________________________
Added: svn:mime-type
   + application/octet-stream

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.py
===================================================================

--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.py	2008-09-25 19:07:59 UTC (rev 4681)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.py	2008-09-25 20:07:06 UTC (rev 4682)
@@ -37,15 +37,23 @@
 PKG = 'gazebo_plugin'
 NAME = 'testcameras'
 
+import math
 import rostools
 rostools.update_path(PKG)
+rostools.update_path('rostest')
+rostools.update_path('std_msgs')
+rostools.update_path('robot_msgs')
+rostools.update_path('rostest')
+rostools.update_path('rospy')
 
 import sys, unittest
 import os, os.path, threading, time
 import rospy, rostest
 from std_msgs.msg import *
+from PIL import Image      as pili
+from PIL import ImageChops as pilic
 
-FRAME_TARGET = "cam_sen-0050.jpg"
+FRAME_TARGET = "cam_sen-0050.ppm"
 FRAME_DIR = "testcameraframes"
 
 class PollCameraThread(threading.Thread):
@@ -69,11 +77,12 @@
     def __init__(self, *args):
         super(TestCameras, self).__init__(*args)
         self.success = False
-        self.pollThread = PollCameraThread(self, FRAME_DIR)
+        self.tested  = False
+        #self.pollThread = PollCameraThread(self, FRAME_DIR)
 
     def onTargetFrame(self):
         time.sleep(0.5) #Safety, to make sure the image is really done being written.
-        ps = "diff -q " + FRAME_DIR + "/" + FRAME_TARGET + " test/testcamera.valid.jpg"
+        ps = "diff -q " + FRAME_DIR + "/" + FRAME_TARGET + " test/testcamera.valid.ppm"
         #print "CMD: " + ps + "\n"
         result = os.system(ps)
         if (result == 0):
@@ -82,26 +91,68 @@
         else:
             self.success = False
             #print "Failure\n"
-        os.system("killall gazebo") #Huge temp hack to kill gazebo.
         rospy.signal_shutdown('test done')
-        
-    def test_cameras(self):
+
+    def images_are_the_same(self,i0,i1):
+        # assume len(i0)==len(i1)
+        print "lengths ",len(i0), len(i1)
+        for i in range(len(i0)):
+          (r0,g0,b0) = i0[i-1]
+          (r1,g1,b1) = i1[i-1]
+          if abs(r0-r1) > 0 or abs(g0-g1) > 0 or abs(b0-b1) > 0:
+            print "debug errors ",i,abs(r0-r1),abs(g0-g1),abs(b0-b1)
+          tol = 10
+          if abs(r0-r1) > tol or abs(g0-g1) > tol or abs(b0-b1) > tol:
+            return False
+        return True
+
+    def imageInput(self,image):
+        print " got image from ROS, begin comparing images "
+        print "  - load validation image from file testcamera.valid.ppm "
+        im0 = pili.open("testcamera.valid.ppm")
+        print "  - load image from ROS "
+        size = image.width,image.height
+        im1 = pili.new("RGBA",size)
+        im1 = pili.frombuffer("RGB",size,str(image.data));
+        im1 = im1.transpose(pili.FLIP_LEFT_RIGHT).rotate(180);
+        imc = pilic.difference(im0,im1)
+        print "  - comparing images "
+        #im1.save("testsave.ppm") # to capture a new valid frame when things change
+        #im1.show()
+        #im0.show()
+        #imc.show()
+        comp_result = self.images_are_the_same(im0.getdata(),im1.getdata())
+        print "test comparison ", comp_result
+        #print "proofcomparison ", self.images_are_the_same(im1.getdata(),im1.getdata())
+        if (comp_result == 1):
+          print "  - images are the Same "
+          self.success = True
+        else:
+          print "  - images differ "
+          self.success = False
+        self.tested  = True
+
+    def testcameras(self):
+        print " wait 3 sec for objects to settle "
+        time.sleep(3)
+        print " subscribe image from ROS "
+        rospy.subscribe_topic("test_camera/image", Image, self.imageInput)
         rospy.ready(NAME, anonymous=True)
-        self.pollThread.start()
-        timeout_t = time.time() + 30.0 #30 seconds
-        while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
+        #self.pollThread.start()
+        timeout_t = time.time() + 60 #60 seconds delay for processing comparison
+        while not rospy.is_shutdown() and not self.tested and time.time() < timeout_t:
             time.sleep(0.1)
-        os.system("killall gazebo")
         self.assert_(self.success)
         
     
 
 
 if __name__ == '__main__':
-    while (os.path.isfile(FRAME_DIR + "/" + FRAME_TARGET)):
-        print "Old test case still alive."
-        time.sleep(0.00001)
+    #while (os.path.isfile(FRAME_DIR + "/" + FRAME_TARGET)):
+    #    print "Old test case still alive."
+    #    time.sleep(1)
     
-    rostest.run(PKG, sys.argv[0], TestCameras, sys.argv) #, text_mode=True)
+    print " starting test "
+    rostest.run(PKG, sys.argv[0], TestCameras, sys.argv)
 
 

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.xml	2008-09-25 19:07:59 UTC (rev 4681)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.xml	2008-09-25 20:07:06 UTC (rev 4682)
@@ -1,6 +1,12 @@
 <launch>
-  <master start="auto" />
-  <include file="$(find wg_robot_description)/send_test.xml"/>
-  <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/testcameras.world" />
-  <test test-name="gazebo_plugin_testcameras" pkg="gazebo_plugin" type="testcameras.py" />
+  <master auto="start" />
+    <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge &quot;$(find wg_robot_description)/pr2/pr2.xml&quot;" />
+    <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/testcameras.world" respawn="false" output="screen">
+      <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
+      <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+      <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+      <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+    </node>
+    <!--<node pkg="gazebo_plugin" type="groundtruthtransform" args="" respawn="true" />-->
+    <test test-name="gazebo_plugin_testcameras" pkg="gazebo_plugin" type="testcameras.py" />
 </launch>

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testscan.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testscan.xml	2008-09-25 19:07:59 UTC (rev 4681)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testscan.xml	2008-09-25 20:07:06 UTC (rev 4682)
@@ -1,6 +1,12 @@
 <launch>
-  <master start="auto" />
-  <include file="$(find wg_robot_description)/send_test.xml"/>
-  <node pkg="gazebo" type="run-gazebo.sh" args="$(find gazebo_robot_description)/world/testscan.world" respawn="true" />
-  <test test-name="gazebo_plugin_testpointclouds" pkg="gazebo_plugin" type="testscan.py" />
+  <master auto="start" />
+    <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge &quot;$(find wg_robot_description)/pr2/pr2.xml&quot;" />
+    <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/testscan.world" respawn="false" output="screen">
+      <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
+      <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+      <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+      <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+    </node>
+    <!--<node pkg="gazebo_plugin" type="groundtruthtransform" args="" respawn="true" />-->
+    <test test-name="gazebo_plugin_testpointclouds" pkg="gazebo_plugin" type="testscan.py" />
 </launch>

Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world	2008-09-25 19:07:59 UTC (rev 4681)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world	2008-09-25 20:07:06 UTC (rev 4682)
@@ -40,17 +40,19 @@
 	  <rpy>0 45 0</rpy>
 	  <saveFrames>true</saveFrames>
 	  <startSaveFrames>true</startSaveFrames>
-	  <saveFramePath>testcameraframes</saveFramePath>
+	  <saveFramePath>testguicameraframes</saveFramePath>
 	</camera>
       </row>
     </frames>
   </rendering:gui>
 
-  <model:physical name="cam_mod">
-    <body:empty name="cam_bod">
+  <!-- camera used to test Ros_Camera plugin -->
+  <model:physical name="test_camera_model">
+    <body:empty name="test_camera_body">
         <xyz>-1 0 3</xyz>	
         <rpy>0 45 0</rpy>
-        <sensor:camera name="cam_sen">
+        <sensor:camera name="test_camera_sensor">
+            <alwaysOn>true</alwaysOn>
             <size>1024 800</size>
             <hfov>60</hfov>
             <nearClip>0.1</nearClip>
@@ -58,11 +60,12 @@
             <updateRate>15.0</updateRate>
             <saveFrames>true</saveFrames>
             <saveFramePath>testcameraframes</saveFramePath>
-            <controller:ros_camera name="cam_controller" plugin="libRos_Camera.so">
+            <controller:ros_camera name="test_camera_controller" plugin="libRos_Camera.so">
+                <alwaysOn>true</alwaysOn>
                 <updateRate>15.0</updateRate>
-                <topicName>cam_test</topicName>
-                <frameName>cam_test</frameName>
-                <interface:camera name="cam_iface" />
+                <topicName>test_camera/image</topicName>
+                <frameName>test_camera_body</frameName>
+                <interface:camera name="test_camera_iface" />
             </controller:ros_camera>
         </sensor:camera>
     </body:empty>
@@ -71,7 +74,7 @@
   <rendering:ogre>
     <ambient>1.0 1.0 1.0 1.0</ambient>
     <sky>
-      <material>Gazebo/CloudySky</material>
+      <material>Gazebo/White</material>
     </sky>
     <gazeboPath>media</gazeboPath>
     <grid>false</grid>
@@ -92,46 +95,12 @@
         <normal>0 0 1</normal>
         <size>51.3 51.3</size>
         <!-- map3.png -->
-        <material>PR2/floor_texture</material>
+        <material>Gazebo/White</material>
       </geom:plane>
     </body:plane>
   </model:physical>
 
 
-  <!-- The "desk"-->
-  <model:physical name="desk1_model">
-    <xyz>   2.28  -0.21  -0.10</xyz>
-    <rpy>   0.0    0.0    0.50</rpy>
-    <body:box name="desk1_legs_body">
-      <geom:box name="desk1_legs_geom">
-        <kp>100000000.0</kp>
-        <kd>1.0</kd>
-        <xyz>   0.0    0.0    0.50</xyz>
-        <mesh>default</mesh>
-        <size>0.5 1.0  0.75</size>
-        <mass> 10.0</mass>
-        <visual>
-          <size> 0.5 1.0 0.75</size>
-          <material>Gazebo/Rocky</material>
-          <mesh>unit_box</mesh>
-        </visual>
-      </geom:box>
-      <geom:box name="desk1_top_geom">
-        <kp>100000000.0</kp>
-        <kd>0.1</kd>
-        <xyz>   0.0    0.0    0.90</xyz>
-        <mesh>default</mesh>
-        <size>0.75 1.5  0.05</size>
-        <mass> 10.0</mass>
-        <visual>
-          <size> 0.75 1.5 0.05</size>
-          <material>Gazebo/Rocky</material>
-          <mesh>unit_box</mesh>
-        </visual>
-      </geom:box>
-    </body:box>
-  </model:physical>
-
   <!-- The small cuboidal "cup" -->
   <model:physical name="cylinder1_model">
     <xyz>   0.78   1.0    0.075</xyz>
@@ -215,10 +184,6 @@
     </light>
   </model:renderable>
   
-  <geo:origin>
-    <lat>37.4270909558</lat><lon>-122.077919338</lon>
-  </geo:origin>
-
   <model:physical name="robot_model1">
 
     <controller:ros_time name="ros_time" plugin="libRos_Time.so">
@@ -227,8 +192,8 @@
       <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
     </controller:ros_time>
 
-    <xyz>0.0     0.0   0.0 </xyz>
-    <rpy>0.0     0.0   0.0 </rpy>
+    <xyz> -3.0     0.0   0.0 </xyz>
+    <rpy>  0.0     0.0   0.0 </rpy>
 
     <!-- base, torso and arms -->
     <include embedded="true">
@@ -237,5 +202,4 @@
 
   </model:physical>
 
-
 </gazebo:world>


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.


