Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7574] pkg/trunk
|
From: <hsu...@us...> - 2008-12-04 15:13:53
|
Revision: 7574
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7574&view=rev
Author: hsujohnhsu
Date: 2008-12-04 15:13:40 +0000 (Thu, 04 Dec 2008)
Log Message:
-----------
* added simple test for 2dnav stack.
* update initial camera position for tables.world.
Modified Paths:
--------------
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables.launch
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/tables.world
Added Paths:
-----------
pkg/trunk/demos/2dnav_gazebo/test/
pkg/trunk/demos/2dnav_gazebo/test/test_2dnav.py
pkg/trunk/demos/2dnav_gazebo/test/test_2dnav.xml
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables_no_x.launch
Added: pkg/trunk/demos/2dnav_gazebo/test/test_2dnav.py
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/test/test_2dnav.py (rev 0)
+++ pkg/trunk/demos/2dnav_gazebo/test/test_2dnav.py 2008-12-04 15:13:40 UTC (rev 7574)
@@ -0,0 +1,266 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+## Gazebo test 2dnav stack
+
+PKG = '2dnav_gazebo'
+NAME = 'test_2dnav'
+
+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('rostools')
+rostools.update_path('rospy')
+
+
+import sys, unittest
+import os, os.path, threading, time
+import rospy, rostest, rostools
+from std_msgs.msg import *
+from rostools.msg import *
+
+
+TARGET_DURATION = 2.0
+TARGET_TOL = 1.0
+TEST_TIMEOUT = 90.0
+
+TARGET_X = 25.0
+TARGET_Y = 19.0
+TARGET_T = 0.0
+
+class E:
+ def __init__(self,x,y,z):
+ self.x = x
+ self.y = y
+ self.z = z
+
+ def shortest_euler_distance(self, e_from, e_to):
+ # takes two sets of euler angles, finds minimum transform between the two, FIXME: return some hacked norm for now
+ # start from the euler-from, and apply reverse euler-to transform, see how far we are from 0,0,0
+ r0 = e_from.x
+ p0 = e_from.y
+ y0 = e_from.z
+
+ r1 = math.cos(e_to.z)*r0 + math.sin(e_to.z)*p0
+ p1 = -math.sin(e_to.z)*r0 + math.cos(e_to.z)*p0
+ y1 = y0
+
+ r2 = math.cos(e_to.y)*r1 - math.sin(e_to.y)*y1
+ p2 = p1
+ y2 = math.sin(e_to.y)*r1 + math.cos(e_to.y)*y1
+
+ self.x = r2
+ self.y = math.cos(e_to.x)*p1 + math.sin(e_to.x)*y1
+ self.z = -math.sin(e_to.x)*p1 + math.cos(e_to.x)*y1
+
+class Q:
+ def __init__(self,x,y,z,u):
+ self.x = x
+ self.y = y
+ self.z = z
+ self.u = u
+
+ def normalize(self):
+ s = math.sqrt(self.u * self.u + self.x * self.x + self.y * self.y + self.z * self.z)
+ self.u /= s
+ self.x /= s
+ self.y /= s
+ self.z /= s
+
+ def getEuler(self):
+ self.normalize()
+ squ = self.u
+ sqx = self.x
+ sqy = self.y
+ sqz = self.z
+ # init euler angles
+ vec = E(0,0,0)
+ # Roll
+ vec.x = math.atan2(2 * (self.y*self.z + self.u*self.x), squ - sqx - sqy + sqz);
+ # Pitch
+ vec.y = math.asin(-2 * (self.x*self.z - self.u*self.y));
+ # Yaw
+ vec.z = math.atan2(2 * (self.x*self.y + self.u*self.z), squ + sqx - sqy - sqz);
+ return vec
+
+
+
+class NavStackTest(unittest.TestCase):
+ def __init__(self, *args):
+ super(NavStackTest, self).__init__(*args)
+ self.success = False
+ self.reached_target_vw = False
+ self.duration_start = 0
+
+ self.odom_xi = 0;
+ self.odom_yi = 0;
+ self.odom_ei = E(0,0,0)
+ self.odom_initialized = False;
+
+ self.odom_x = 0;
+ self.odom_y = 0;
+ self.odom_e = E(0,0,0)
+
+ self.p3d_xi = 0;
+ self.p3d_yi = 0;
+ self.p3d_ei = E(0,0,0)
+ self.p3d_initialized = False;
+
+ self.p3d_x = 0;
+ self.p3d_y = 0;
+ self.p3d_e = E(0,0,0)
+
+ def normalize_angle_positive(self, angle):
+ return math.fmod(math.fmod(angle, 2*math.pi) + 2*math.pi, 2*math.pi)
+
+ def normalize_angle(self, angle):
+ anorm = self.normalize_angle_positive(angle)
+ if anorm > math.pi:
+ anorm -= 2*math.pi
+ return anorm
+
+ def shortest_angular_distance(self, angle_from, angle_to):
+ angle_diff = self.normalize_angle_positive(angle_to) - self.normalize_angle_positive(angle_from)
+ if angle_diff > math.pi:
+ angle_diff = -(2*math.pi - angle_diff)
+ return self.normalize_angle(angle_diff)
+
+ def printBaseOdom(self, odom):
+ print "odom received"
+ print "odom pos " + "x: " + str(odom.pos.x)
+ print "odom pos " + "y: " + str(odom.pos.y)
+ print "odom pos " + "t: " + str(odom.pos.th)
+ print "odom vel " + "x: " + str(odom.vel.x)
+ print "odom vel " + "y: " + str(odom.vel.y)
+ print "odom vel " + "t: " + str(odom.vel.th)
+
+ def printBaseP3D(self, p3d):
+ print "base pose ground truth received"
+ print "P3D pose translan: " + "x: " + str(p3d.pos.position.x)
+ print " " + "y: " + str(p3d.pos.position.y)
+ print " " + "z: " + str(p3d.pos.position.z)
+ print "P3D pose rotation: " + "x: " + str(p3d.pos.orientation.x)
+ print " " + "y: " + str(p3d.pos.orientation.y)
+ print " " + "z: " + str(p3d.pos.orientation.z)
+ print " " + "w: " + str(p3d.pos.orientation.w)
+ print "P3D rate translan: " + "x: " + str(p3d.vel.vel.x)
+ print " " + "y: " + str(p3d.vel.vel.y)
+ print " " + "z: " + str(p3d.vel.vel.z)
+ print "P3D rate rotation: " + "x: " + str(p3d.vel.ang_vel.vx)
+ print " " + "y: " + str(p3d.vel.ang_vel.vy)
+ print " " + "z: " + str(p3d.vel.ang_vel.vz)
+
+
+
+ def odomInput(self, odom):
+ #self.printBaseOdom(odom)
+ if self.odom_initialized == False:
+ self.odom_initialized = True
+ self.odom_xi = odom.pos.x
+ self.odom_yi = odom.pos.y
+ self.odom_ei = E(0,0,odom.pos.th)
+ self.odom_x = odom.pos.x - self.odom_xi
+ self.odom_y = odom.pos.y - self.odom_yi
+ self.odom_e.shortest_euler_distance(self.odom_ei, E(0,0,odom.pos.th))
+
+
+ def p3dInput(self, p3d):
+ q = Q(p3d.pos.orientation.x , p3d.pos.orientation.y , p3d.pos.orientation.z , p3d.pos.orientation.w)
+ q.normalize()
+ v = q.getEuler()
+
+ if self.p3d_initialized == False:
+ self.p3d_initialized = True
+ self.p3d_xi = p3d.pos.position.x
+ self.p3d_yi = p3d.pos.position.y
+ self.p3d_ei = E(v.x,v.y,v.z)
+
+ self.p3d_x = p3d.pos.position.x - self.p3d_xi
+ self.p3d_y = p3d.pos.position.y - self.p3d_yi
+ self.p3d_e.shortest_euler_distance(self.p3d_ei,E(v.x,v.y,v.z))
+ #print "p3d initial:" + str(self.p3d_ei.x) + "," + str(self.p3d_ei.y) + "," + str(self.p3d_ei.z) \
+ # + " p3d final:" + str(v.x) + "," + str(v.y) + "," + str(v.z)
+
+
+ def test_2dnav(self):
+ print "LNK\n"
+ #pub_base = rospy.Publisher("cmd_vel", BaseVel)
+ pub_goal = rospy.Publisher("goal", Planner2DGoal) #received by wavefront_player or equivalent
+ rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
+ rospy.Subscriber("odom", RobotBase2DOdom, self.odomInput)
+ rospy.init_node(NAME, anonymous=True)
+ timeout_t = time.time() + TEST_TIMEOUT
+ # wait for result
+ while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
+ # send goal
+ h = Header();
+ h.stamp = rospy.get_rostime();
+ h.frame_id = "map"
+ pub_goal.publish(Planner2DGoal(h,Pose2DFloat32(TARGET_X,TARGET_Y,TARGET_T),1))
+ time.sleep(1.0)
+ # display what odom thinks
+ #print " odom " + " x: " + str(self.odom_e.x) + " y: " + str(self.odom_e.y) + " t: " + str(self.odom_e.t)
+ # display what ground truth is
+ #print " p3d " + " x: " + str(self.p3d_e.x) + " y: " + str(self.p3d_e.y) + " t: " + str(self.p3d_e.t)
+ # display what the odom error is
+ odom_yaw_error = E(0,0,0)
+ odom_yaw_error.shortest_euler_distance(self.p3d_e,self.odom_e)
+ nav_yaw_error = E(0,0,0)
+ nav_yaw_error.shortest_euler_distance(self.p3d_ei,E(0,0,TARGET_T))
+ nav_yaw_error.shortest_euler_distance(self.p3d_e,nav_yaw_error)
+ print " error " + " odom_x:" + str(self.odom_x - self.p3d_x) \
+ + " odom_y:" + str(self.odom_y - self.p3d_y) \
+ + " odom_e:" + str(odom_yaw_error.x) + "," + str(odom_yaw_error.y) + "," + str(odom_yaw_error.z) \
+ + " nav_x:" + str(self.p3d_x - (TARGET_X - self.p3d_xi )) \
+ + " nav_y:" + str(self.p3d_y - (TARGET_Y - self.p3d_yi )) \
+ + " nav_e:" + str(nav_yaw_error.x) + "," + str(nav_yaw_error.y) + "," + str(nav_yaw_error.z)
+ # check total error
+ odom_error = abs(self.odom_x - self.p3d_x) + abs(self.odom_y - self.p3d_y) \
+ + abs(odom_yaw_error.x) + abs(odom_yaw_error.y) + abs(odom_yaw_error.z)
+ nav_error = abs(self.p3d_x - (TARGET_X - self.p3d_xi )) \
+ + abs(self.p3d_y - (TARGET_Y - self.p3d_yi )) \
+ + abs(nav_yaw_error.x) + abs(nav_yaw_error.y) + abs(nav_yaw_error.z)
+ print "nav error:" + str(nav_error) + " tol:" + str(TARGET_TOL) + " odom error:" + str(odom_error)
+ if nav_error < TARGET_TOL:
+ self.success = True
+
+ self.assert_(self.success)
+
+if __name__ == '__main__':
+ rostest.run(PKG, sys.argv[0], NavStackTest, sys.argv) #, text_mode=True)
+
+
Property changes on: pkg/trunk/demos/2dnav_gazebo/test/test_2dnav.py
___________________________________________________________________
Added: svn:executable
+ *
Added: pkg/trunk/demos/2dnav_gazebo/test/test_2dnav.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/test/test_2dnav.xml (rev 0)
+++ pkg/trunk/demos/2dnav_gazebo/test/test_2dnav.xml 2008-12-04 15:13:40 UTC (rev 7574)
@@ -0,0 +1,30 @@
+<launch>
+ <master auto="start" />
+
+ <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_tables_no_x.launch"/>
+
+ <!-- Common parameter settings /-->
+ <include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
+ <param name="/trex/ping_frequency" value="1"/>
+
+ <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map_blank.png 0.1" respawn="false" />
+ <node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
+ <node pkg="world_3d_map" type="world_3d_map" args="/robotdesc/pr2 scan:=tilt_scan cloud:=full_cloud" respawn="false" />
+
+ <!--node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" /-->
+ <node pkg="highlevel_controllers" type="move_base_sbpl" args="" respawn="false" />
+
+ <!--
+ <node pkg="highlevel_controllers" type="recharge_controller" args="" respawn="false" />
+ <node pkg="kinematic_planning" type="kinematic_planning" args="robotdesc/pr2"/>
+ <include file="$(find world_3d_map)/run.xml"/>
+ <node pkg="highlevel_controllers" type="move_arm" args="right" respawn="false"/>
+ <node pkg="highlevel_controllers" type="move_arm" args="left" respawn="false" />
+ <node pkg="highlevel_controllers" type="move_end_effector" args="right" respawn="false"/>
+ -->
+
+ <!--node pkg="nav_view" type="nav_view" respawn="false" /-->
+
+ <!-- test -->
+ <test test-name="2dnav_gazebo_test_2dnav" pkg="2dnav_gazebo" type="test_2dnav.py" />
+</launch>
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables.launch 2008-12-04 14:17:54 UTC (rev 7573)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables.launch 2008-12-04 15:13:40 UTC (rev 7574)
@@ -16,7 +16,7 @@
</node>
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 -5 -15 0 0 0 90" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 -5 -15 0 0 0 0" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- use mech.py to spawn all controllers listed in controllers.xml -->
<include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_controllers.launch" />
Added: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables_no_x.launch (rev 0)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables_no_x.launch 2008-12-04 15:13:40 UTC (rev 7574)
@@ -0,0 +1,25 @@
+<launch>
+ <!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/wg_robot_description/pr2 -->
+ <!-- if needed, group tag allows pushing components into namespace via ns="namespace" -->
+ <group name="wg">
+
+ <!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
+ (Mechanism Control, BaseControllerNode, etc...) -->
+ <include file="$(find wg_robot_description)/pr2_prototype1/send_description.xml" />
+
+ <!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
+ <node pkg="gazebo" type="gazebo" args="-r $(find gazebo_robot_description)/gazebo_worlds/tables.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/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>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 -5 -15 0 0 0 0" respawn="false" output="screen" /> <!-- load default arm controller -->
+
+ <!-- use mech.py to spawn all controllers listed in controllers.xml -->
+ <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_controllers.launch" />
+ </group>
+</launch>
+
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/tables.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/tables.world 2008-12-04 14:17:54 UTC (rev 7573)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/tables.world 2008-12-04 15:13:40 UTC (rev 7574)
@@ -44,7 +44,7 @@
<frames>
<row height="100%">
<camera width="100%">
- <xyz>0 0 20</xyz>
+ <xyz>-5 -10 20</xyz>
<rpy>0 90 90</rpy>
</camera>
</row>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
