test_object_tracker.py

Go to the documentation of this file.

#! /usr/bin/env python

import roslib; roslib.load_manifest('simulation_object_tracker')
import rospy
import sys, traceback
from simulation_object_tracker.object_tracker import ObjectTracker
from geometry_msgs import msg as geometry
from tidyup_msgs import srv
from tidyup_msgs import msg
from pr2_python import hand_description
from pr2_python import arm_mover
from tidyup_arm_services.simple_arm_tasks import SimpleArmTasks

if __name__ == '__main__':
  try:
    rospy.init_node('simulation_object_tracker')
    arm_tasks = SimpleArmTasks()
    objectTracker = ObjectTracker()
    objectTracker.create_objects(dataFileName = '../configs/simulated_objects.dat')
    
    # TEST
#    pose = geometry.PoseStamped()
#    pose.header.frame_id = '/map'
#    pose.pose.position.x = 0.60 
#    pose.pose.position.y = -0.15
#    pose.pose.position.z = 0.7
#    pose.pose.orientation.x = 0.0
#    pose.pose.orientation.y = 0.012 
#    pose.pose.orientation.z = 0.254 
#    pose.pose.orientation.w = 0.967
#    objectTracker._worldInterface.add_collision_box(pose, [0.15, 0.15, 0.08], 'test_bowl')
    rospy.loginfo('starting test.')
    side_request = msg.ArmToSideGoal()
    side_request.right_arm = True
    arm_tasks._to_side_execute(side_request)
    side_request.right_arm = False
    side_request.left_arm = True
    arm_tasks._to_side_execute(side_request)
    response = objectTracker._detect_graspables_callback(srv.DetectGraspableObjects())
    print response['objects']
    graspability_request = srv.RequestObjectsGraspabilityRequest()
    graspability_request.objects = response['objects']
    graspability_response = objectTracker._request_graspability_callback(graspability_request)
    print graspability_response['objects']
    grasp_request = msg.GraspObjectGoal()
    for object in response['objects']:
      if object.reachable_right_arm:
        grasp_request.target = object
        grasp_request.right_arm = True
        break
      elif object.reachable_left_arm:
        grasp_request.target = object
        grasp_request.left_arm = True
        break
    if not grasp_request.right_arm and not grasp_request.left_arm:
      rospy.logwarn('no objects in range, aborting test.')
      sys.exit(1)
    objectTracker._pick_up_execute(grasp_request)
    carry_request = msg.PostGraspPositionGoal()
    carry_request.right_arm = True
    arm_tasks._to_carry_execute(carry_request)
    putdown_request = msg.PlaceObjectGoal()
    putdown_request.right_arm = True
    putdown_request.position = grasp_request.target.pose
    putdown_request.position.pose.position.z += 0.05
    objectTracker._put_down_execute(putdown_request)
    side_request = msg.ArmToSideGoal()
    side_request.right_arm = True
    arm_tasks._to_side_execute(side_request)
  except:
    traceback.print_exc(file=sys.stdout)