tabletop_manipulation_demo_exec_test.py

Go to the documentation of this file.

#!/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 collision validation 
PKG = 'pr2_tabletop_manipulation_gazebo_demo'

import roslib
roslib.load_manifest(PKG)

import unittest, sys, os, math
import time
import rospy, rostest
from nav_msgs.msg import *
from std_msgs.msg import String
from pr2_controllers_msgs.msg import PointHeadGoal, PointHeadAction, SingleJointPositionAction, SingleJointPositionActionGoal
from pr2_common_action_msgs.msg import TuckArmsGoal, TuckArmsAction
from actionlib import *

# import pick and place class
from pr2_pick_and_place_demos.pick_and_place_manager import *
from object_manipulator.convert_functions import *
from pr2_tabletop_manipulation_gazebo_demo import simple_pick_and_place_example

from gazebo.msg import *
from gazebo.srv import *

TEST_DURATION = 60000.0
TARGET_POSE_X = 0.6
TARGET_POSE_Y = -0.15

class TestTabletopManipulationNoExecutive(unittest.TestCase):
    def __init__(self, *args):
        super(TestTabletopManipulationNoExecutive, self).__init__(*args)


    def stringOutput(self, str):
        print str.data
        
    
    def test_tabletop_manipulation_no_executive(self):
      # print 'Waiting for tuck arms action server'
      # tuck_arms_ac = SimpleActionClient('tuck_arms', TuckArmsAction)
      # tuck_arms_ac.wait_for_server()
      # tuck_arms_goal = TuckArmsGoal(False,False)
      # self.assert_(tuck_arms_ac.send_goal_and_wait(tuck_arms_goal, rospy.Duration(TEST_DURATION), rospy.Duration(5.0)))
    
      print 'Waiting for point head action server'
      point_head_ac = SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction)
      point_head_ac.wait_for_server()
      head_goal = PointHeadGoal()
      head_goal.target.header.frame_id = 'base_link'
      head_goal.target.point.x = 1.0
      head_goal.target.point.y = 0.0
      head_goal.target.point.z = 0.25
      head_goal.max_velocity = 0.1
      self.assert_(point_head_ac.send_goal_and_wait(head_goal, rospy.Duration(TEST_DURATION), rospy.Duration(5.0)))

      # print 'Raising torso', this is broken
      # torso_ac = SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction)
      # torso_ac.wait_for_server()
      # torso_goal = SingleJointPositionActionGoal()
      # torso_goal.header.frame_id = 'base_link'
      # torso_goal.goal.position = 0.3
      # self.assert_(torso_ac.send_goal_and_wait(torso_goal, rospy.Duration(TEST_DURATION), rospy.Duration(5.0)))

      #TODO call pick_and_place_tutorial service/action
      sppe = simple_pick_and_place_example.SimplePickAndPlaceExample()

      #adjust for your table 
      table_height = .505
      #.6 m in front of robot, centered
      target_point_xyz = [.7, 0, table_height]
      target_point = create_point_stamped(target_point_xyz, '/base_link')
      success = sppe.pick_up_object_near_point(target_point, 0)   #right arm
      self.assert_(success)
      if success:
          #square of size 30 cm by 30 cm
          place_rect_dims = [.06, .06]
          #.7 m in front of robot, to the right
          center_xyz = [TARGET_POSE_X, TARGET_POSE_Y, table_height]
          #aligned with axes of frame_id
          center_quat = [0,0,0,1]
          place_rect_center = create_pose_stamped(center_xyz+center_quat, '/base_link')
          self.assert_(sppe.place_object(0, place_rect_dims, place_rect_center))

      # check can pose and 
      print 'Checking object pose to see if it is near goal'
      rospy.wait_for_service('/gazebo/get_model_state')

      get_model_state = rospy.ServiceProxy('/gazebo/get_model_state',GetModelState)
      resp = get_model_state('object_1','pr2::base_link')
      print 'check object_1 pose status: ',resp
      self.assert_(abs(resp.pose.position.x - TARGET_POSE_X) < 0.06 and \
                   abs(resp.pose.position.y - TARGET_POSE_Y) < 0.06 and \
                   abs(resp.pose.position.z - table_height)  < 0.06)
        

if __name__ == '__main__':
    rospy.init_node('tabletop_manipulation_test', anonymous=True)

    # hack, wait for pipeline to start
    time.sleep(50.0)

    rostest.run(PKG, sys.argv[0], TestTabletopManipulationNoExecutive, sys.argv) #, text_mode=True)