Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16550] pkg/trunk
|
From: <hsu...@us...> - 2009-06-01 19:09:54
|
Revision: 16550
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16550&view=rev
Author: hsujohnhsu
Date: 2009-06-01 19:09:42 +0000 (Mon, 01 Jun 2009)
Log Message:
-----------
fixing test.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/manifest.xml
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/set_pose.py
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.py
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.valid.ppm
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_camera.world
Modified: pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/manifest.xml 2009-06-01 19:09:14 UTC (rev 16549)
+++ pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/manifest.xml 2009-06-01 19:09:42 UTC (rev 16550)
@@ -12,7 +12,7 @@
<depend package="pr2_gazebo" />
<depend package="gazebo_robot_description" />
<depend package="laser_scan" />
- <depend package="deprecated_msgs" />
+ <depend package="image_msgs" />
<depend package="stereo_image_proc" />
<sysdepend os="ubuntu" version="7.04-feisty" package="xvfb"/>
</package>
Modified: pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/set_pose.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/set_pose.py 2009-06-01 19:09:14 UTC (rev 16549)
+++ pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/set_pose.py 2009-06-01 19:09:42 UTC (rev 16550)
@@ -32,7 +32,7 @@
# POSSIBILITY OF SUCH DAMAGE.
#
-## Gazebo tug arms for navigation
+## Set head and gripper poses
PKG = 'test_pr2_sensors_gazebo'
NAME = 'set_pose'
@@ -50,29 +50,21 @@
from pr2_mechanism_controllers.msg import *
-CMD_POS_1 = 0.0
-CMD_POS_2 = 0.0
-CMD_POS_3 = 0.0
-CMD_POS_4 = 0.0
-CMD_POS_5 = 0.0
-CMD_POS_6 = 0.0
-CMD_POS_7 = 0.0
-CMD_POS_8 = 0.0
-CMD_POS_9 = 0.2
+CMD_POS_1 = 0.02
+CMD_POS_2 = -0.2
+CMD_POS_3 = 0.2
if __name__ == '__main__':
- pub_l_arm = rospy.Publisher("/left_arm_commands", JointPosCmd)
pub_l_gripper = rospy.Publisher("/l_gripper_controller/set_command", Float64)
- pub_r_arm = rospy.Publisher("/right_arm_commands", JointPosCmd)
pub_r_gripper = rospy.Publisher("/r_gripper_controller/set_command", Float64)
+ pub_head_pan = rospy.Publisher("/head_pan_controller/set_command", Float64)
pub_head_tilt = rospy.Publisher("/head_tilt_controller/set_command", Float64)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 20.0 #publish for 20 seconds then stop
while time.time() < timeout_t:
- pub_l_arm.publish(JointPosCmd(['l_shoulder_pan_joint','l_shoulder_lift_joint','l_upper_arm_roll_joint','l_elbow_flex_joint','l_forearm_roll_joint','l_wrist_flex_joint','l_wrist_roll_joint'],[CMD_POS_1,CMD_POS_2,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_l_gripper.publish(Float64(CMD_POS_8))
- pub_r_arm.publish(JointPosCmd(['r_shoulder_pan_joint','r_shoulder_lift_joint','r_upper_arm_rolr_joint','r_elbow_flex_joint','r_forearm_roll_joint','r_wrist_flex_joint','r_wrist_roll_joint'],[CMD_POS_1,CMD_POS_2,CMD_POS_3,CMD_POS_4,CMD_POS_5,CMD_POS_6,CMD_POS_7],[0,0,0,0,0,0,0],0))
- pub_r_gripper.publish(Float64(CMD_POS_8))
- pub_head_tilt.publish(Float64(CMD_POS_9))
+ pub_l_gripper.publish(Float64(CMD_POS_1))
+ pub_r_gripper.publish(Float64(CMD_POS_1))
+ pub_head_pan.publish(Float64(CMD_POS_2))
+ pub_head_tilt.publish(Float64(CMD_POS_3))
time.sleep(0.2)
Modified: pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.py 2009-06-01 19:09:14 UTC (rev 16549)
+++ pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.py 2009-06-01 19:09:42 UTC (rev 16550)
@@ -45,15 +45,16 @@
import sys, unittest
import os, os.path, threading, time
import rospy, rostest
-from deprecated_msgs.msg import Image as image_msg
+from image_msgs.msg import Image as image_msg
+from image_msgs.msg import CamInfo as caminfo_msg
from PIL import Image as pili
from PIL import ImageChops as pilic
FRAME_TARGET = "cam_sen-0050.ppm"
FRAME_DIR = "test_camera_frames"
TOTAL_ERROR_TOL = 5
-TEST_DURATION = 30
-TEST_INIT_WAIT = 70
+TEST_DURATION = 10
+TEST_INIT_WAIT = 1
class PollCameraThread(threading.Thread):
def __init__(self, target, dir):
@@ -76,7 +77,9 @@
def __init__(self, *args):
super(TestCameras, self).__init__(*args)
self.success = False
- #self.pollThread = PollCameraThread(self, FRAME_DIR)
+ self.got_caminfo = False
+ self.caminfo_height = 0
+ self.caminfo_width = 0
def onTargetFrame(self):
time.sleep(0.5) #Safety, to make sure the image is really done being written.
@@ -116,15 +119,13 @@
return False
print "thumbnail lengths ",len(i0d), len(i1d)
- #compare thumbnails only
+ #compare pixel by pixel for thumbnails
for i in range(len(i0d)):
- (r0,g0,b0) = i0d[i]
- (r1,g1,b1) = i1d[i]
- #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)
- if abs(r0-r1) > pixel_tol or abs(g0-g1) > pixel_tol or abs(b0-b1) > pixel_tol:
+ (p0) = i0d[i]
+ (p1) = i1d[i]
+ if abs(p0-p1) > pixel_tol:
error_count = error_count + 1
- error_total = error_total + abs(r0-r1) + abs(g0-g1) + abs(b0-b1)
+ error_total = error_total + abs(p0-p1)
error_avg = float(error_total)/float(len(i0d))
print "total error count:",error_count
print "average error: ",error_avg
@@ -133,58 +134,65 @@
else:
return True
+ def caminfoInput(self,caminfo):
+ self.got_caminfo = True
+ self.caminfo_width = caminfo.width
+ self.caminfo_height = caminfo.height
+ print " got cam info (",caminfo.width,"X",caminfo.height,")"
+
def imageInput(self,image):
- print " got image message from ROS, begin comparing images "
+ if (self.got_caminfo):
+ print " got image and caminfo from ROS, begin comparing images."
+ else:
+ print " got image but no caminfo."
+ return
- print " - load validation image from file test_camera.valid.ppm "
- fname = roslib.packages.get_pkg_dir('test_pr2_sensors_gazebo') + '/test_camera.valid.ppm'
- if os.path.isfile(fname):
- im0 = pili.open(fname)
- #elif os.path.isfile("test/test_camera.valid.ppm"):
- # im0 = pili.open("test/test_camera.valid.ppm")
- else:
- print "cannot find validation file: test_camera.valid.ppm"
- self.success = False
- return
+ print " - load validation image from file test_camera.valid.ppm "
+ fname = roslib.packages.get_pkg_dir('test_pr2_sensors_gazebo') + '/test_camera.valid.ppm'
+ if os.path.isfile(fname):
+ im0 = pili.open(fname)
+ else:
+ print "cannot find validation file: test_camera.valid.ppm"
+ self.success = False
+ return
- 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 " - load image from ROS "
+ size = self.caminfo_width,self.caminfo_height
+ im1 = pili.new("L",size)
+ im1 = pili.frombuffer("L",size,str(image.uint8_data.data));
+ im1 = im1.transpose(pili.FLIP_LEFT_RIGHT).rotate(180);
+ imc = pilic.difference(im0,im1)
- print " - comparing images "
- im1.save("test_1.ppm") # uncomment this line to capture a new valid frame when things change
- im0.save("test_0.ppm")
- imc.save("test_d.ppm")
- #im1.show()
- #im0.show()
- #imc.show()
- comp_result = self.images_are_the_same(im0,im1)
- print "test comparison ", comp_result
- #print "proofcomparison ", self.images_are_the_same(im1.getdata(),im1.getdata())
- if (self.success == False): # test if we have not succeeded
- if (comp_result == 1):
- print " - images are the Same "
- self.success = True
- else:
- print " - images differ "
- self.success = False
+ print " - comparing images "
+ im1.save("test_1.ppm") # uncomment this line to capture a new valid frame when things change
+ im0.save("test_0.ppm")
+ imc.save("test_d.ppm")
+ #im1.show()
+ #im0.show()
+ #imc.show()
+ comp_result = self.images_are_the_same(im0,im1)
+ print "test comparison ", comp_result
+ #print "proofcomparison ", self.images_are_the_same(im1.getdata(),im1.getdata())
+ if (self.success == False): # test if we have not succeeded
+ if (comp_result == 1):
+ print " - images are the Same "
+ self.success = True
+ else:
+ print " - images differ "
+ self.success = False
def test_camera(self):
print " wait ",TEST_INIT_WAIT," sec for objects and head tilt to settle."
time.sleep(TEST_INIT_WAIT)
print " subscribe stereo left image from ROS "
- #rospy.Subscriber("test_camera/image", image_msg, self.imageInput) # this is a test camera, simply looking at the cups
- rospy.Subscriber("/stereo_l/image", image_msg, self.imageInput) # this is a camera mounted on PR2 head (left stereo camera)
+ rospy.Subscriber("/stereo/left/image", image_msg, self.imageInput) # this is a camera mounted on PR2 head (left stereo camera)
+ rospy.Subscriber("/stereo/left/cam_info", caminfo_msg, self.caminfoInput) # this is a camera mounted on PR2 head (left stereo camera)
rospy.init_node(NAME, anonymous=True)
- #self.pollThread.start()
timeout_t = time.time() + TEST_DURATION
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
- time.sleep(0.1)
+ time.sleep(1.0)
self.assert_(self.success)
Modified: pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.valid.ppm
===================================================================
(Binary files differ)
Modified: pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.xml 2009-06-01 19:09:14 UTC (rev 16549)
+++ pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.xml 2009-06-01 19:09:42 UTC (rev 16550)
@@ -1,7 +1,7 @@
<launch>
<master auto="start" />
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/test/test_camera.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/test/test_camera.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
@@ -15,17 +15,17 @@
<!-- load controllers -->
<include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
+ <include file="$(find stereo_image_proc)/stereo.launch" />
<!-- send arm a command -->
<node pkg="robot_mechanism_controllers" type="control.py" args="set head_tilt_controller 0.2" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
<!-- set head_tilt and arm pose -->
<node pkg="test_pr2_sensors_gazebo" type="set_pose.py" output="screen"/>
+ <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="b" output="screen"/>
+ <node pkg="mechanism_control" type="spawner.py" args="$(find arm_gazebo)/l_gripper_controller.xml" output="screen"/>
+ <node pkg="mechanism_control" type="spawner.py" args="$(find arm_gazebo)/r_gripper_controller.xml" output="screen"/>
- <!-- visualization -->
- <!--
- <node pkg="wx_camera_panel" type="standalone_visualizer.py" args="/stereo_l" respawn="false" output="screen" />
- -->
-
+ <!-- test -->
<test test-name="test_pr2_sensors_gazebo_test_camera" pkg="test_pr2_sensors_gazebo" type="test_camera.py" time-limit="110" />
</launch>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_camera.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_camera.world 2009-06-01 19:09:14 UTC (rev 16549)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_camera.world 2009-06-01 19:09:42 UTC (rev 16550)
@@ -97,26 +97,27 @@
<normal>0 0 1</normal>
<size>51.3 51.3</size>
<!-- map3.png -->
- <material>Gazebo/White</material>
+ <material>Gazebo/Rocky</material>
</geom:plane>
</body:plane>
</model:physical>
<!-- The small cuboidal "cup" -->
- <model:physical name="cylinder1_model">
- <xyz> 0.78 1.0 0.075</xyz>
+ <model:physical name="box1_model">
+ <xyz> 0.78 1.0 0.5</xyz>
<rpy> 0.0 0.0 0.0</rpy>
- <body:box name="cylinder1_body">
- <geom:box name="cylinder1_geom">
+ <static>true</static>
+ <body:box name="box1_body">
+ <geom:box name="box1_geom">
<mesh>default</mesh>
- <size>0.05 0.05 0.15</size>
+ <size>0.2 0.2 1.0</size>
<mass> 0.5</mass>
<cfm>0.000001</cfm>
<erp>0.8</erp>
<visual>
- <size> 0.05 0.05 0.15</size>
- <material>Gazebo/PioneerBody</material>
+ <size>0.2 0.2 1.0</size>
+ <material>PR2/Blue</material>
<mesh>unit_box</mesh>
</visual>
</geom:box>
@@ -125,18 +126,19 @@
<!-- The small cylindrical "cup" -->
<model:physical name="cylinder1_model">
- <xyz> 0.78 0.0 0.075</xyz>
+ <xyz> 0.78 0.0 0.5</xyz>
<rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
<body:cylinder name="cylinder1_body">
<geom:cylinder name="cylinder1_geom">
<mesh>default</mesh>
- <size>0.05 0.15</size>
+ <size>0.2 1.0</size>
<mass> 0.5</mass>
<cfm>0.000001</cfm>
<erp>0.8</erp>
<visual>
- <size> 0.05 0.05 0.15</size>
- <material>Gazebo/PioneerBody</material>
+ <size>0.2 0.2 1.0</size>
+ <material>PR2/Green</material>
<mesh>unit_cylinder</mesh>
</visual>
</geom:cylinder>
@@ -145,13 +147,16 @@
<!-- The trimesh cup -->
<model:physical name="cup1_model">
- <xyz> 0.78 -1.0 0</xyz>
- <rpy> 90.0 0.0 90.0</rpy>
+ <xyz> 0.78 -1.0 0.0</xyz>
+ <rpy> 90.0 0.0 90.0</rpy>
+ <static>true</static>
<body:trimesh name="cup1_body">
+ <static>true</static>
<geom:trimesh name="cup1_geom">
+ <static>true</static>
<kp>1000000000.0</kp>
- <kd>1.3</kd>
- <scale> 0.1 0.1 0.1</scale>
+ <kd>1.0</kd>
+ <scale>0.2 1.0 0.2</scale>
<mesh>cup.mesh</mesh>
<massMatrix>true</massMatrix>
@@ -167,8 +172,8 @@
<cz>0.0</cz>
<visual>
- <scale> 0.1 0.1 0.1</scale>
- <material>Gazebo/PioneerBody</material>
+ <scale>0.2 1.0 0.2</scale>
+ <material>PR2/Red</material>
<mesh>cup.mesh</mesh>
</visual>
</geom:trimesh>
@@ -176,7 +181,6 @@
</model:physical>
<!-- White Directional light -->
- <!--
<model:renderable name="directional_white">
<light>
<type>directional</type>
@@ -184,9 +188,9 @@
<diffuseColor>0.4 0.4 0.4</diffuseColor>
<specularColor>0.0 0.0 0.0</specularColor>
<attenuation>1 0.0 1.0 0.4</attenuation>
+ <range>20</range>
</light>
</model:renderable>
- -->
<model:empty name="factory_model">
<model_thread>false</model_thread>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
