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 actionlib

import sys, traceback
import copy
import math
from hardcoded_facts.geometry_pose_loader import read_pose_stamped_file
from visualization_msgs import msg as visualization
from geometry_msgs import msg as geometry
from tidyup_msgs import srv
from tidyup_msgs import msg
from arm_navigation_msgs.msg import CollisionObject, Shape, OrderedCollisionOperations, CollisionOperation, ArmNavigationErrorCodes
from pr2_python.world_interface import WorldInterface
from pr2_python.arm_mover import ArmMover
from pr2_python.arm_planner import ArmPlanner
from pr2_python.planning_scene_interface import get_planning_scene_interface
from pr2_python import transform_listener
from pr2_python.exceptions import ArmNavError
from pr2_python.arm_mover import ArmMover
import tf

class SimulatedDoor():
  def __init__(self, name):
    self.open = False
    self._poses = dict()
    self.name = name
  def current_pose(self):
    return self._poses[self.open]
  def add_pose(self, open, pose):
    self._poses[open] = pose

class ObjectTracker():

  def __init__(self, simulate_moving):
    self.simulate_movable_objects = simulate_moving
    # world interface
    #self._tasks = Tasks()
    self._wi = WorldInterface()
    self._wi.reset()
    self._psi = get_planning_scene_interface()
    self._psi.reset()
    self._transformListener = transform_listener.get_transform_listener()
    self._arm_mover = ArmMover()
    self._right_arm_planner = self._arm_mover._planners['right_arm']
    self._left_arm_planner = self._arm_mover._planners['left_arm']
    # object_name -> SimulatedDoor
    self._doors = dict()
    # object_name -> geometry_msgs/PoseStamped
    self._movable_objects = dict()
    # actions and services
    if self.simulate_movable_objects:
        # detect objects
        self._detectGraspableObjectsService = rospy.Service('/tidyup/detect_objects', srv.DetectGraspableObjects, self._detect_objects_callback)
        # graspability
        #self._requestObjectGraspabilityService = rospy.Service('/tidyup/request_graspability', srv.RequestObjectsGraspability, self._request_graspability_callback)
        # pickup
        #self._pickUpActionServer = actionlib.SimpleActionServer('/tidyup/grasping_action', msg.GraspObjectAction, self._pick_up_execute, False)
        #self._pickUpActionServer.start() 
        # door
        self._detectDoorService = rospy.Service('/tidyup/detect_door_state', srv.DetectDoorState, self._detect_door_state_callback)
        self._openDoorActionServer = actionlib.SimpleActionServer('/tidyup/open_door', msg.OpenDoorAction, self._open_door_execute, False)
        self._openDoorActionServer.start() 

    rospy.loginfo('[{0}]initialized.'.format(rospy.get_name()))

  def _detect_objects_callback(self, request):
    self._psi.reset()
    rospy.loginfo('request: detect objects.')
    # add objects not yet in the in the planning scene
    visibles = [self._create_tidyup_object(name, pose_stamped) for name, pose_stamped in self._movable_objects.items() if self._pose_in_view(pose_stamped)]
    for object in visibles:
      # once added to the planning scene, remove them from our list
      self._movable_objects.pop(object.name) 
      if object.name.startswith('plate'):
        rospy.loginfo('added plate {0} to environment.'.format(object.name))
        object.pose.pose.position.z += 0.015 # place above table
        self._wi.add_collision_box(object.pose, [0.25, 0.25, 0.03], object.name)
      elif object.name.startswith('bowl'):
        rospy.loginfo('added bowl {0} to environment.'.format(object.name))
        object.pose.pose.position.z += 0.035 # place above table
        self._wi.add_collision_box(object.pose, [0.17, 0.17, 0.07], object.name)
      elif object.name.startswith('cup'):
        rospy.loginfo('added cup {0} to environment.'.format(object.name))
        object.pose.pose.position.z += 0.075 # place above table
        self._wi.add_collision_box(object.pose, [0.07, 0.07, 0.15], object.name)
    # find object already in the planning scene
    ps_visibles = [self._convert_collision_object_to_tidyup_object(object) for object in self._psi.collision_objects() if self._object_in_view(object)]
    visibles.extend(ps_visibles)
    self._psi.reset()
    return { 'objects': visibles}

  def _request_graspability_callback(self, request):
    rospy.loginfo('request: object graspability.')
    for object in request.objects:
      object.reachable_right_arm = self._object_in_reach(object, self._right_arm_planner) 
      object.reachable_left_arm = self._object_in_reach(object, self._left_arm_planner) 
    return { 'objects': request.objects}

  def _pick_up_execute(self, goal):
    self._psi.reset()
    arm_name = 'no_arm'
    if goal.right_arm:
      arm_name = 'right_arm'
    elif goal.left_arm:
      arm_name = 'left_arm'
    target = self._wi.collision_object(goal.target.name)
    if not target:
      self._pickUpActionServer.set_aborted(text='pick-up: specified object {0} does not exist.'.format(goal.target.name))
      return

    rospy.loginfo('executing: pick up of {0} with {1}.'.format(target.id, arm_name))
    # allow collisions between object and anything
    collisions = OrderedCollisionOperations()
    collision = CollisionOperation()
    collision.object1 = goal.target.name
    collision.object2 = collision.COLLISION_SET_ALL # arm_planner.joint_names[6]
    collision.operation = collision.DISABLE
    collisions.collision_operations.append(collision)
    gripper_pose = geometry.PoseStamped()
    gripper_pose.pose = transform_listener.transform_pose('/torso_lift_link', '/map', goal.target.pose.pose)
    gripper_pose.header.frame_id = '/torso_lift_link'
    gripper_pose.pose.position.z += 0.2
    gripper_pose.pose.orientation.x = 0.7071
    gripper_pose.pose.orientation.y = 0.0
    gripper_pose.pose.orientation.z = -0.7071
    gripper_pose.pose.orientation.w = 0.0
    reached_goal = self._arm_mover.move_to_goal(arm_name, gripper_pose, ordered_collisions=collisions)
    self._psi.reset()
    if reached_goal:
      self._wi.attach_object_to_gripper(arm_name, target.id)
      self._psi.reset()
      self._pickUpActionServer.set_succeeded(text='pickup successful')
      return
    self._pickUpActionServer.set_aborted(text='execution failed')

  def _create_tidyup_object(self, name, pose_stamped):
    tidyup_object = msg.GraspableObject()
    tidyup_object.name = name
    tidyup_object.pose = pose_stamped
    return tidyup_object

  def _convert_collision_object_to_tidyup_object(self, object):
    tidyup_object = msg.GraspableObject()
    tidyup_object.name = object.id
    tidyup_object.pose = geometry.PoseStamped()
    tidyup_object.pose.pose = object.poses[0]
    tidyup_object.pose.header.frame_id = object.header.frame_id
    return tidyup_object

  def _detect_door_state_callback(self, request):
    rospy.loginfo('request: detect door state.')
    for door in self._doors.values():
      if self._pose_in_view(door.current_pose()):
        return { 'door_open': door.open}
    # no door in view
    rospy.loginfo('detect door: no door in view. assuming door is open.')
    return { 'door_open': True}

  def _open_door_execute(self, goal):
    self._psi.reset()
    arm_name = 'no_arm'
    if goal.right_arm:
      arm_name = 'right_arm'
    elif goal.left_arm:
      arm_name = 'left_arm'
    rospy.loginfo('executing: open door with {0}.'.format(arm_name))
    # find door
    for door in self._doors.values():
      if self._pose_in_view(door.current_pose()):
        # open door
        door.open = True
        # set new coordinates
        self._update_door_pose(door)
        self._psi.reset()
        self._openDoorActionServer.set_succeeded(text='succeeded: door is open')
        return
    rospy.logerr('request failed: no door in view')
    self._openDoorActionServer.set_aborted(text='failed: not in front of door')

  def _pose_in_view(self, pose_stamped):
    # transform to head frame
    object_pose = transform_listener.transform_pose('/head_pan_link', pose_stamped.header.frame_id, pose_stamped.pose)
    # check angle and distance
    yawAngle = math.atan2(object_pose.position.y, object_pose.position.x)
    maxYawAngle = math.pi/4.0
    minX = 0.2
    maxX = 2.0
    if math.fabs(yawAngle) < maxYawAngle and minX < object_pose.position.x < maxX: 
      return True
    return False

  def _object_in_view(self, object):
    # ignore doors and tables
    if object.id.startswith('table') or object.id.startswith('door'): 
      return False

    # transform to head frame
    object_pose = transform_listener.transform_pose('/head_pan_link', object.header.frame_id, object.poses[0])
    # check angle and distance
    yawAngle = math.atan2(object_pose.position.y, object_pose.position.x)
    maxYawAngle = math.pi/4.0
    minX = 0.2
    maxX = 2.0
    if math.fabs(yawAngle) < maxYawAngle and minX < object_pose.position.x < maxX: 
      #print '{0} is in view: coords={1}|{2} yaw={3}'.format(object.id, object_pose.position.x, object_pose.position.y, yawAngle)
      return True
    #print '{0} is NOT in view: coords={1}|{2} yaw={3}'.format(object.id, object_pose.position.x, object_pose.position.y, yawAngle)
    return False

  def _object_in_reach(self, object, arm_planner):
    # transform to head frame
    self._psi.reset()
    # allow collisions between gripper and object
    collisions = OrderedCollisionOperations()
    collision = CollisionOperation()
    collision.object1 = object.name
    collision.object2 = collision.COLLISION_SET_ALL # arm_planner.joint_names[6]
    collision.operation = collision.DISABLE
    collisions.collision_operations.append(collision)
    gripper_pose = geometry.PoseStamped()
    gripper_pose.pose = transform_listener.transform_pose('/torso_lift_link', '/map', object.pose.pose)
    gripper_pose.header.frame_id = '/torso_lift_link'
    gripper_pose.pose.position.z += 0.2
    gripper_pose.pose.orientation.x = 0.7071
    gripper_pose.pose.orientation.y = 0.0
    gripper_pose.pose.orientation.z = -0.7071
    gripper_pose.pose.orientation.w = 0.0
    response = arm_planner.get_ik(gripper_pose, ordered_collisions=collisions)
    self._psi.reset()
    return response.error_code.val == ArmNavigationErrorCodes.SUCCESS

  def poseStampToStr(self, pose):
      (_, _, yaw) = tf.transformations.euler_from_quaternion([pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w])
      ps = "position: (%.2f, %.2f, %.2f) - quat: (%.2f, %.2f, %.2f, %.2f) - yaw: %.2f deg" % \
              (pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, \
              pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w, 180.0/math.pi*yaw)
      return ps

  def create_objects(self, dataFileName):
    # read from file
    poses = read_pose_stamped_file(dataFileName)
    rospy.loginfo("Creating virtual objects:")
    for name, pose in poses.items():
      if name.startswith('table'):
        rospy.loginfo("Adding table: " + name + " at " + self.poseStampToStr(pose))
        self.add_table(pose, name)
      elif name.startswith('door'):
        rospy.loginfo("Adding door: " + name + " at " + self.poseStampToStr(pose))
        self.add_door(pose, name)
      elif self.simulate_movable_objects:
        rospy.loginfo("Adding movable object: " + name + " at " + self.poseStampToStr(pose))
        self._movable_objects[name] = pose
    self._psi.reset()

  def add_door(self, pose, pose_name):
    if pose_name.endswith('_closed'):
      open_key = False
      name = pose_name[:-len('_closed')]
    elif pose_name.endswith('_open'):
      name = pose_name[:-len('_open')]
      open_key = True
    if self._doors.has_key(name):
      door = self._doors[name]
    else:
      door = SimulatedDoor(name)
      self._doors[name] = door
    door.add_pose(open_key, pose)
    if door.open == open_key:
      self._update_door_pose(door)

  def _update_door_pose(self, door):
    self._wi.add_collision_box(door.current_pose(), [0.05, 1.0, 2.0], door.name)

  def add_table(self, pose, id):
#    co = CollisionObject()
#    co.header.frame_id = pose.header.frame_id
#    co.padding = -1
#    co.id = id
#    
#    xmin = -0.35
#    xmax = 0.35
#    ymin = -0.5
#    ymax = 0.5
#    bbox = Shape()
#    bbox.type = bbox.BOX
#    bbox.dimensions = [abs(xmax - xmin), abs(ymax - ymin), 0.05]
#    co.shapes.append(bbox)
#    co.poses.append(copy.deepcopy(pose.pose))
#    self._wi.add_object(co)
    self._wi.add_collision_box(pose, [0.7, 1.0, 0.05], id)
#    return co