simple_arm_tasks.py

Go to the documentation of this file.

#! /usr/bin/env python

import roslib; roslib.load_manifest('tidyup_arm_services')
import rospy
import actionlib

import sys, traceback, math, copy
import std_srvs
from tidyup_msgs import msg
from tidyup_msgs import srv
from pr2_tasks.arm_tasks import ArmTasks
from arm_navigation_msgs.msg import ArmNavigationErrorCodes
from geometry_msgs.msg import PoseStamped, Quaternion, Point, Pose
from arm_navigation_msgs.msg import CollisionObject, AttachedCollisionObject, CollisionOperation
from pr2_python import exceptions
import pr2_python.geometry_tools as gt
from pr2_python.hand_description import HandDescription
from tidyup_arm_services.find_free_space import free_spots_on_table
from pr2_python.exceptions import ManipulationError, AllYourBasePosesAreBelongToUs, ActionFailedError
from pr2_python.planning_scene_interface import PlanningSceneInterface, get_planning_scene_interface
from pr2_python.arm_planner import ArmPlanner
from pr2_tasks.pickplace import PickPlace, get_wrist_pose_from_grasp, PickupResult
from pr2_tasks.pickplace_definitions import PlaceGoal, PickupGoal
from pr2_tasks.tableware_detection import TablewareDetection, TablewareDetectionResult
from pr2_tasks.exceptions import DetectionError
from tidyup_arm_services.planning_scene_sync import PlanningSceneSync
from pr2_python import trajectory_tools as tt
from pr2_python import visualization_tools as vt
from pr2_python import head
from visualization_msgs.msg import Marker, MarkerArray
from pr2_python.world_interface import WorldInterface
from hardcoded_facts.geometry_pose_loader import read_table_file
import tf
import numpy as np
import object_manipulation_msgs
import tabletop_object_detector

DEFAULT_CARRY_JOINT_TRAJECTORY = {'left_arm': [[0.968, 0.129, 0.554, -1.891, 1.786, -1.127, 0.501], 
                                              [2.135, -0.02, 1.64, -2.07, 1.64, -1.68, 1.398],
                                              [2.110, 1.230, 2.06, -1.69, -0.3, -1.32, 1.57]],
                                 'right_arm': [[-0.968, 0.129, -0.554, -1.891, -1.786, -1.127, 0.501],
                                               [-2.135, -0.02, -1.64, -2.07, -1.64, -1.68, 1.398],
                                               [-2.110, 1.230, -2.06, -1.69, 0.3, -1.32, 1.57]]}

class SimpleArmTasks():
  def __init__(self):
    self._detector = TablewareDetection()
    self._tasks = ArmTasks()
    self._psi = get_planning_scene_interface()
    self._wi = self._detector._wi
    # reset world interface
    self._reset_world_interface()

    self._pick_place = PickPlace()
    self._known_objects = dict()
    self._known_wipe_goals = dict()
    self._head = head.Head()

    # to side
    self._side_joint_trajectory = self._tasks.side_joint_trajectory
    self._toSideActionServer = actionlib.SimpleActionServer('/tidyup/side_position_action', msg.ArmToSideAction, self._to_side_execute, False)
    self._toSideActionServer.start() 
    # to carry 
    self._carry_joint_trajectory = DEFAULT_CARRY_JOINT_TRAJECTORY
#    for arm in ['left_arm', 'right_arm']:
#      self._carry_joint_trajectory[arm] = rospy.get_param('/arm_configurations/side_carry/trajectory/'+arm, DEFAULT_CARRY_JOINT_TRAJECTORY[arm])
    self._toCarryActionServer = actionlib.SimpleActionServer('/tidyup/post_grasp_position_action', msg.PostGraspPositionAction, self._to_carry_execute, False)
    self._toCarryActionServer.start()
    # 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() 
    # compute putdown pose
    self._find_putdown_pose_service = rospy.Service('/tidyup/request_putdown_pose', srv.GetPutdownPose, self._find_putdown_pose_callback)
    # putdown 
    self._putDownActionServer = actionlib.SimpleActionServer('/tidyup/placing_action', msg.PlaceObjectAction, self._put_down_execute, False)
    self._putDownActionServer.start()
    # Reset world interface
    self._resetWorldInterface = rospy.Service('/tidyup/reset_world_interface', std_srvs.srv.Empty, self._reset_world_interface)
    self._attachSponge = rospy.Service('/tidyup/attach_sponge', std_srvs.srv.Empty, self._add_sponge_to_right_gripper)
    self._detachSponge = rospy.Service('/tidyup/detach_sponge', std_srvs.srv.Empty, self._add_sponge_to_base)
    # debug visualization
    self._visualization_publisher = rospy.Publisher('/tidyup/visualization', MarkerArray)
    #self._planning_scene_sync = PlanningSceneSync()
    rospy.loginfo('tidyup_arm_services: initialized.')
 

  def _to_carry_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: move {0} to carry.'.format(arm_name))
    self._tasks.side_joint_trajectory = self._carry_joint_trajectory
    self._tasks.move_arm_to_side(arm_name)
    self._psi.reset()
    self._toCarryActionServer.set_succeeded()

  def _to_side_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: move {0} to side.'.format(arm_name))
    self._tasks.side_joint_trajectory = self._side_joint_trajectory
    self._tasks.move_arm_to_side(arm_name)
    self._psi.reset()
    self._toSideActionServer.set_succeeded()

  def _reset_world_interface(self, request=None):
    rospy.loginfo("Resetting world interface")
    self._wi.reset(True)
    self._known_objects = dict()
    self._known_wipe_goals = dict()
    self._add_tables_to_wi()
    self._add_sponge_to_base()
    self._psi.reset()
    return {}

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

  def _point_to_str(self, pt):
      ps = "position: (%.2f, %.2f, %.2f)" % \
        (pt.x, pt.y, pt.z)
      return ps

  def _dump_planning_scene_objects(self, planning_scene=None):
      """ Output a condensed printable form of the planning scene.
          If the planning_scene is None, the current world scene is retrieved
          from the world interface."""
      if planning_scene is None:
          planning_scene = self._wi.get_planning_scene().planning_scene

      s = "Collision Objects:"
      if not planning_scene.collision_objects:
          s += "\nNone"
      for co in planning_scene.collision_objects:
          s += "\nid: %s op: %d frame: %s pose: %s" % (co.id, co.operation.operation, co.header.frame_id, \
              self._pose_to_str(co.poses[0]))
      s += "\nAttached Collision Objects:"
      if not planning_scene.attached_collision_objects:
          s += "\nNone"
      for aco in planning_scene.attached_collision_objects:
          s += "\nid: %s attach_link: %s op: %d frame: %s pose: %s" % (aco.object.id, aco.link_name, \
               aco.object.operation.operation, \
               aco.object.header.frame_id, self._pose_to_str(aco.object.poses[0]))
      rospy.loginfo("Planning Scene Objects:\n%s" % s)

  def _pickup_goal_to_str(self, pg):
      if pg is None:
          return "Pickup Goal is None"
      s = "Pickup Goal for %s (collision_object) on collision_support %s using arm \"%s\".\n" % \
          (pg.collision_object_name, pg.collision_support_surface_name, pg.arm_name)
      s += "label: %s graspable_object collision name: %s object pose: %s %s\n" % \
          (pg.label, pg.target.collision_name, pg.object_pose_stamped.header.frame_id, self._pose_to_str(pg.object_pose_stamped.pose))
      if hasattr(pg, 'collision_object_header') and hasattr(pg, 'collision_object_poses'):
          s += "collision_object_pose: %s %s" % (pg.collision_object_header.frame_id, self._pose_to_str(pg.collision_object_poses[0]))
      else:
          s += "no collision_object_pose"
      #s += "%d desired grasps" % len(pg.desired_grasps)
      return s

  def _place_goal_to_str(self, pg, print_locations=True):
      if pg is None:
          return "Place Goal is None"
      s = "Place Goal for %s (collision_object) on collision_support %s using arm \"%s\"." % \
              (pg.collision_object_name, pg.collision_support_surface_name, pg.arm_name)
      if print_locations:
          s += "\nPlace Locations:"
          for loc in pg.place_locations:
              s += "\nframe: %s pose: %s" % (loc.header.frame_id, self._pose_to_str(loc.pose))
      return s

  def _dump_known_objects(self):
      s = ""
      if not self._known_objects:
          s += " None"
      for so, known in self._known_objects.iteritems():
          s += "\nOn Static Object %s:" % so
          for pg in known.pickup_goals:
              s += "\n" + self._pickup_goal_to_str(pg)
      rospy.loginfo("Known Objects:%s" % s)

  def _wipe_goal_to_str(self, wg, valid):
      if wg is None:
          return "Wipe Goal is None"
      validity = "validity unknown"
      if valid is not None:
          if valid:
              validity = "valid"
          else:
              validity = "invalid"
      s = "Wipe Goal %s (box_size %s) at %s, %s (%s)." % \
              (wg.name, wg.box_size, wg.spot.header.frame_id, self._point_to_str(wg.spot.point), validity)
      return s

  def _dump_known_wipe_goals(self):
      s = ""
      if not self._known_wipe_goals:
          s += " None"
      for so, wgs in self._known_wipe_goals.iteritems():
          s += "\nOn Static Object %s:" % so
          for (wg, valid) in wgs:
              s += "\n" + self._wipe_goal_to_str(wg, valid)
      rospy.loginfo("Known Wipe Goals:%s" % s)

  def _remove_known_objects(self, known):
    """ Remove known objects from the world."""
    if known:
      for pg in known.pickup_goals:
        self._wi.remove_collision_object(pg.collision_object_name)

  def _remove_objects_in_fov(self, cur_collision_objects):
    """ Remove objects from the world that are in the FOV.
        The cur_collision_objects contain the collision objects as they were in
        the world right before calling this function."""

    rospy.loginfo("Removing collision objects in FOV for re-detection")
    # Also remove all objects that are in the FOV of the robot
    # for now, we'll just assume FOV is all objects in a within a radius in front of the robot
    fov_point = PoseStamped()
    fov_point.header.frame_id = "base_link"
    fov_point.header.stamp = rospy.Time.now()
    fov_point.pose.position.x = 0.7
    fov_point.pose.position.y = 0.0
    fov_point.pose.position.z = 0.7
    fov_point.pose.orientation.x = 0.0
    fov_point.pose.orientation.y = 0.0
    fov_point.pose.orientation.z = 0.0
    fov_point.pose.orientation.w = 1.0
    map_fov_point = self._psi.transform_pose_stamped("map", fov_point)
    clearing_radius = 1.0
    for co in cur_collision_objects:
        if co.id.startswith("table"):
            continue
        distance = gt.euclidean_distance(map_fov_point.pose.position, co.poses[0].position)
        if distance < clearing_radius:
            self._wi.remove_collision_object(co.id)

  def _compute_redetected_objects(self, known, det, cur_collision_objects):
    """ Compute which objects have been redetected, which are newly detected, and
        which known objects have not been redetected.
        known and det are detection results of the previsouly known objects (known)
        and the current detection (det).
        The cur_collision_objects contain the collision objects as they were in
        the world right before calling this function.
        Returns a tuple (known_not_redetected, newly_detected, redetected) that
        contains sets of the respective pickup goals. Redetected contains a set
        of tuples (known_pg, detected_pg) that are redetected matches."""

    rospy.loginfo("Updating/merging world interface/known objects.")
    # First only determine the merging before applying anything
    known_not_redetected = set()    # should be here, but aren't any more
    redetected = set()              # did know this, detection redetected it here (known, detected)
    if known:
        for known_pg in known.pickup_goals:
          # for each known object find corresponding detected object
          min_distance = 0.1 # max distance variance 10cm
          closest_new_pg = None
          for det_pg in det.pickup_goals:
            det_co = None
            for co in cur_collision_objects:
                if co.id == det_pg.collision_object_name:
                    det_co = co
                    break
            if det_co is None:
                rospy.error("Could not find collision object for pickup goal %s" % det_pg.collision_object_name)
            assert det_co.header.frame_id == known_pg.collision_object_header.frame_id
            distance = gt.euclidean_distance(known_pg.collision_object_poses[0].position, det_co.poses[0].position)
            if distance < min_distance:
              min_distance = distance
              closest_new_pg = det_pg
          if closest_new_pg is None:
            known_not_redetected.add(known_pg)
          else:
            # Assumption: name is 'cup_1', 'bottle_3' and the stuff before '_' is the type
            # warn if the type changes
            known_type = known_pg.collision_object_name.split("_")[0]
            new_detected_type = closest_new_pg.collision_object_name.split("_")[0]
            if known_type != new_detected_type:
                rospy.logwarn('Possible type redetection mismatch: known: %s new_detected: %s' % \
                    (self._pickup_goal_to_str(known_pg), self._pickup_goal_to_str(closest_new_pg)))
                rospy.logwarn('Treating as non-match/new detection')
                known_not_redetected.add(known_pg)
            else:
                redetected.add((known_pg, closest_new_pg))

    # The newly detected ones are those that are not redetected
    newly_detected = set()          # didn't know about this before, detection just found it
    redetected_only_new = [new_pg for (known_pg, new_pg) in redetected]
    # As a (common) corner case: If known was empty, redetected is empty, thus none of the
    # det_pg in det.pickup_goals will be in redetected_only_new and all are newly_detected.
    for det_pg in det.pickup_goals:
        if det_pg not in redetected_only_new:
            newly_detected.add(det_pg)

    # output merges
    for pg in known_not_redetected:
        rospy.logwarn('not able to re-detect known object {0}.'.format(self._pickup_goal_to_str(pg)))
    for pg in newly_detected:
        rospy.loginfo('Newly detected object: %s' % self._pickup_goal_to_str(pg))
    for (known_pg, detected_pg) in redetected:
        rospy.loginfo('known object {0} re-detected as {1}.'.format(
            self._pickup_goal_to_str(known_pg), self._pickup_goal_to_str(detected_pg)))

    return (known_not_redetected, newly_detected, redetected)

  def _compute_detection_name_mappings(self, det_names, world_names, newly_detected, redetected):
    """ Determine name mappings in the form newly_detected_name -> old/known_name.
        The name mappings should include a mapping for every detected object and
        thus might include trivial ones like: cup_1 -> cup_1.
        Args:
            det_names - set of names in the new detection
            world_names - set of names that exist in the world (from cur_collision_objects)
            newly_detected - pickup goals for objects that have been newly detected
            redetected - set of pickup goal tuples of redetected objects: (known_pg, redetected_pg)
        """
    # First extract name mappings and check for possible conflicts
    name_mappings = dict()  # det_name -> known_name (need to rename det_name to known_name)
    new_names = set()       # every name we will assign to

    for (known_pg, detected_pg) in redetected:
        name_mappings[detected_pg.collision_object_name] = known_pg.collision_object_name
        if detected_pg.collision_object_name != known_pg.collision_object_name:
            new_names.add(known_pg.collision_object_name)

    # conflicts arise when we want to assign a new name that already exists in the world
    # if it is somewhere else in the world that is very bad and shouldn't happen as
    # our new names are chosen from the known objects that were at this location before
    # and thus before this detection run also were in the world.
    # Nobody should have been able to insert an object with that name somewhere else
    world_other = world_names - det_names
    world_conflicts = world_other & new_names
    for wc_name in world_conflicts:
        rospy.logerror('Naming conflict: Found %s in world, but should be reassigned from known' % wc_name)
    # if it is in our det set that is OK as we'll just change that
    # all redetected ones get their old name in name_mappings, but some newly detected ones
    # might block that, so we'll just change their name as they are new
    for new_pg in newly_detected:
        if new_pg.collision_object_name in new_names:
            # bad, already assigned this name to some other thing, we are new, so just invent a new name
            type_name_parts = new_pg.collision_object_name.split("_")
            type_name = type_name_parts[0]
            if(len(type_name_parts) > 1):
                type_name = '_'.join(type_name_parts[:-1])
            # cup, cup_1, cup_c_1 -> cup, cup, cup_c
            cand = ""
            for cand_id in range(1, 100000):
                cand = type_name + "_" + str(cand_id)
                if cand not in world_other and cand not in new_names:
                    break 
            if not cand:
                rospy.logerror('Could not find new name for %s - this is bad.' % new_pg.collision_object_name)
            else:
                name_mappings[new_pg.collision_object_name] = cand
                new_names.add(cand)
        else:
            name_mappings[new_pg.collision_object_name] = new_pg.collision_object_name
            new_names.add(new_pg.collision_object_name)

    rospy.loginfo("Determined the following renamings: %s" % str(name_mappings))
    return name_mappings

  def _remove_objects_for_renaming(self, name_mappings):
    """ Remove all objects from the world that should be renamed. """

    rospy.loginfo('Removing old objects for renaming')
    for old_name, new_name in name_mappings.iteritems():
        if old_name != new_name:
            self._wi.remove_collision_object(old_name)

  def _readd_objects_for_renaming(self, name_mappings, cur_collision_objects):
    """ Readd all objects from the world (with a new name) that were 
        previously removed for renaming.
        The cur_collision_objects are the object as they were in the world
        before _remove_objects_for_renaming was called!"""

    rospy.loginfo('Add new objects for renaming')
    for old_name, new_name in name_mappings.iteritems():
        if old_name != new_name:
            match_co = None
            for co in cur_collision_objects:
                if co.id == old_name:
                    match_co = co
                    break
            if match_co is None:
                rospy.logerror('_readd_objects_for_renaming: Could not find collision object for %s in current collision objects.' % old_name)
            else:
                match_co.id = new_name
                self._wi.add_object(match_co)

  def _rename_and_update_pickup_goals(self, name_mappings, cur_collision_objects, pickup_goals):
    """ Rename the pickup goals according to name mappings and also update/assign a
        collision_object_header/collision_object_poses to them.
        The cur_collision_objects contain the collision objects as they were in
        the world right before calling this function."""

    rospy.loginfo('Renaming known pickup goals')
    for pg in pickup_goals:
        pg.target.collision_name = name_mappings[pg.collision_object_name]
        pg.collision_object_name = name_mappings[pg.collision_object_name]  # do this last as it changes the name!

    rospy.loginfo("Adding collision_object_poses to pickup goals")
    for pg in pickup_goals:
        match_co = None
        for co in cur_collision_objects:
            if co.id == pg.collision_object_name:
                match_co = co
                break
        if match_co is None:
            rospy.logerror('_rename_and_update_pickup_goals: Could not find collision object for %s in current collision objects.' % pg.collision_object_name)
        else:
            pg.collision_object_header = match_co.header
            pg.collision_object_poses = match_co.poses
            assert pg.collision_object_poses

  def _compute_new_wipe_goals(self, pickup_goals):
    """ Compute tentative wiping goals from pickup_goals (using their obj locations)
        pickup_goals is a list of PickupGoal with collision_object_poses + names
        Returns: list of (msg.WipeGoal, valid) - Here valid is None = unknown."""
    wgs = []
    for pg in pickup_goals:
        wg = msg.WipeGoal()
        wg.name = "wipe_goal_" + pg.collision_object_name
        wg.box_size = 0.3
        wg.spot.header = pg.collision_object_header
        wg.spot.point = pg.collision_object_poses[0].position
        wgs.append((wg, None))
    return wgs

  def _filter_wipe_goals(self, new_wgs, known_wgs):
    """ Filter wiping goals for this location.
        new_wg is a list of wiping goals computed for this location.
        known_wgs is a list of the known wiping goals already at this location.
        Returns a filtered list of WipeGoals where all new and known wipe goals
        are contained, unless a new WipeGoal is too close to an already known WipeGoal."""
    # We never want to create a WipeGoal for an object we placed ourselves.
    # So, we only remember WipeGoals the first time we detect an object.
    # From then on the WipeGoal is remembered, but only a valid WipeGoal,
    # if it is not too close to a known one.
    def wg_distance(wg1, wg2):
        return gt.euclidean_distance(wg1.spot.point, wg2.spot.point)

    # we should only have one wg per object once it is first detected
    # maybe add valid flag?
    all_known_wipe_goal_names = set()
    for so, wgs in self._known_wipe_goals.iteritems():
        for (wg, valid) in wgs:
            all_known_wipe_goal_names.add(wg.name)
    # problem here: if we reject one because it is too close, it is not known at all,
    # thus we'll readd it after putdown + detect

    filtered_wgs = []
    # Initialize the known ones, we always keep the old ones
    if known_wgs:
        for (wg, valid) in known_wgs:
            filtered_wgs.append((wg, valid))
    # TODO: Self filter the new detection, some in there might be too close
    for (wg, _) in new_wgs:
        # we already have this one somewhere, do not readd
        if wg.name in all_known_wipe_goal_names:
            continue
        # This is an unknown one! We will definitely remember this one,
        # but it is only valid if it is not too near to another valid one
        valid = True
        if known_wgs:
            for (known_wg, known_wg_valid) in known_wgs:
                if not known_wg_valid:
                    continue
                if wg_distance(wg, known_wg) <= 0.1:
                    valid = False
                    break
        filtered_wgs.append((wg, valid))
    return filtered_wgs

  def _create_detected_visible_objects(self, pickup_goals):
    """ Create a reply of visible_objects for an object detection request from the pickup_goals. """

    self._psi.reset()   # for _check_trajectory_to_pickup_goal

    # convert objects to tidyup_objects and return results
    visible_objects = list()
    for pg in pickup_goals:
      object = msg.GraspableObject()
      object.name = pg.collision_object_name
      object.pose = pg.object_pose_stamped
      # if we have a global pose from collision object, use this one instead of the
      # pickup goal pose, which is usually in base_link
      if hasattr(pg, 'collision_object_header') and hasattr(pg, 'collision_object_poses'):
        object.pose = PoseStamped()
        object.pose.header = pg.collision_object_header
        object.pose.pose = pg.collision_object_poses[0]
      else:
        rospy.logwarn("Pickup goal for %s did not have collision_object_header/poses" % pg.collision_object_name)
      object.reachable_right_arm = True
      object.reachable_left_arm = True
      #pg.only_perform_feasibility_test = True
#      pg.arm_name = 'left_arm'
#      if self._check_trajectory_to_pickup_goal(pg):
#        object.reachable_left_arm = True
#      else:
#        object.reachable_left_arm = False
#      pg.arm_name = 'right_arm'
#      if self._check_trajectory_to_pickup_goal(pg):
#        object.reachable_right_arm = True
#      else:
#        object.reachable_right_arm = False
      visible_objects.append(object)

    return visible_objects

  def _detect_objects_callback(self, request):
    """ Call the object detection and based on that:
        update the world interface so the newly detected objects are now in the world and
        already known objects are updated. The set of objects that we know to be on this
        static_object should be exaclty the set of detected objects, i.e. not re-detected objects are removed,
        new ones added, re-detected ones updated.
        Objects in the world not on this static_object should never be touched.

        In addition update our known_objects on this static_object with this information.
        We will also forget about any other known_objects on this location
        and the planner state should do the same.
        Any picked up object should also be removed."""

    rospy.loginfo('request: detect objects at %s.' % request.static_object)
    self._psi.reset()

    self._head.look_at_relative_point(0.8, 0.0, 0.7)

    self._dump_planning_scene_objects()
    self._dump_known_objects()

    rospy.loginfo("Removing known objects at %s from world interface for re-detection." % request.static_object)

    cur_collision_objects = self._wi.collision_objects()
    # remove known objects from this position in the world map
    known = self._known_objects.get(request.static_object)
    self._remove_known_objects(known)

    self._dump_planning_scene_objects()

    self._remove_objects_in_fov(cur_collision_objects)

    self._dump_planning_scene_objects()
    self._dump_known_objects()

    rospy.loginfo("Calling object detection.")

    try:
      # det is a tableware_detection.TablewareDetectionResult
      det = self._detector.detect_objects(add_objects_to_map='recognized', add_table_to_map=False, reset_collision_objects=False, table_name=request.static_object)
    except DetectionError, last_ex:
      rospy.logerr('object detection FAILED')
      return False #{ 'objects': list()}

    self._dump_planning_scene_objects()
    self._dump_known_objects()

    # filter unrecognized and compute grasps
    recognized_pgs = list()
    for pg in det.pickup_goals:
      if pg.label != 'graspable':
        recognized_pgs.append(pg)
    det.pickup_goals = recognized_pgs
    rospy.loginfo("Recognized pickup goals from detection:")
    for pg in det.pickup_goals:
        rospy.loginfo(self._pickup_goal_to_str(pg))

    cur_collision_objects = self._wi.collision_objects()
    (known_not_redetected, newly_detected, redetected) = self._compute_redetected_objects(known, det, cur_collision_objects)

    # Initialize the new known with det to get an updated table
    # pickup goals will be updated now
    new_known = det
    # we will keep all the objects in det, but might rename the matched ones in the new det
    # so they will have the 'old' known name

    # those should be the new names in the detection at static_object
    det_names = set([pg.collision_object_name for pg in det.pickup_goals])   # every name we just got from det
    world_names = set([co.id for co in cur_collision_objects])   # every name in wi right now

    name_mappings = self._compute_detection_name_mappings(det_names, world_names, newly_detected, redetected)

    # As we might have cyclic conflicts (e.g. cup_1 -> cup_2, cup_2 -> cup_1), we'll do this the easy way:
    # Remove all objects and reinsert them
    self._dump_planning_scene_objects()
    self._remove_objects_for_renaming(name_mappings)
    self._dump_planning_scene_objects()
    self._readd_objects_for_renaming(name_mappings, cur_collision_objects)
    self._dump_planning_scene_objects()

    # Reget from wi as we renamed stuff above
    cur_collision_objects = self._wi.collision_objects()
    self._rename_and_update_pickup_goals(name_mappings, cur_collision_objects, new_known.pickup_goals)

    self._known_objects[request.static_object] = new_known

    rospy.loginfo("Final world scene/known objects")
    self._dump_planning_scene_objects()
    self._dump_known_objects()

    rospy.loginfo("Update/Merge done.")

    rospy.loginfo("Computing Wipe Goals")
    self._dump_known_wipe_goals()

    # Compute tentative wiping goals (all obj locations at static_object (in /map))
    new_wgs = self._compute_new_wipe_goals(self._known_objects[request.static_object].pickup_goals)
    rospy.loginfo("Tentative new wipe goals at %s" % request.static_object)
    for (wg, valid) in new_wgs:
        rospy.loginfo(self._wipe_goal_to_str(wg, valid))
    # Remove extra wiping goals (knowing the old ones)
    filtered_wgs = self._filter_wipe_goals(new_wgs, self._known_wipe_goals.get(request.static_object))
    rospy.loginfo("Filtered wipe goals at %s" % request.static_object)
    for (wg, valid) in filtered_wgs:
        rospy.loginfo(self._wipe_goal_to_str(wg, valid))

    self._known_wipe_goals[request.static_object] = filtered_wgs

    self._dump_known_wipe_goals()

    # Prepare the response
    visible_objects = self._create_detected_visible_objects(self._known_objects[request.static_object].pickup_goals)
    valid_wipe_goals = [wg for (wg, valid) in filtered_wgs if valid]

    obj_names = ", ".join([obj.name for obj in visible_objects])
    wg_names = ", ".join([wg.name for wg in valid_wipe_goals])
    rospy.loginfo('detect objects finished. sending {0} objects ({1})'.format(len(visible_objects), obj_names))
    rospy.loginfo('determined %d wipe goals (%s)' % (len(valid_wipe_goals), wg_names))
    return { 'objects': visible_objects, 'wipe_goals': valid_wipe_goals}

  def _request_graspability_callback(self, request):
    rospy.loginfo('DEPRECATED: request object graspability.')
    return { 'objects': request.objects}

  def _pick_up_execute(self, goal):
    rospy.loginfo('pick up execute: pickup {0} on {1} with {2}'.format(goal.pickup_object, goal.static_object, goal.arm))
    self._psi.reset()
    action_result = msg.GraspObjectResult()
    action_result.grasp.pose.orientation.w = 1 

    self._dump_known_objects()

    # find pickup_goal by id
    known = self._known_objects.get(goal.static_object)
    if not known:
      rospy.logerr('pickup_execute: FAILED: detection data not available for this static_object {0}'.format(goal.static_object))
      action_result.error_code.val = action_result.error_code.SENSOR_INFO_STALE
      self._pickUpActionServer.set_aborted(result=action_result, text='execution failed: detection data not available for this static_object {0}'.format(goal.static_object))
      return
    pickup_goal = None
    for pg in known.pickup_goals:
      if pg.collision_object_name == goal.pickup_object:
        pickup_goal = pg
        break
    if pickup_goal == None:
      rospy.logerr('pickup_execute: FAILED: detection data not available for this pickup_object {0}'.format(goal.pickup_object))
      action_result.error_code.val = action_result.error_code.SENSOR_INFO_STALE
      self._pickUpActionServer.set_aborted(result=action_result, text='execution failed: detection data not available for this pickup_object {0}'.format(goal.pickup_object))
      return

    # compute grasps
    #pickup_goal = PickupGoal()
    pickup_goal.arm_name = goal.arm
    #pickup_result = PickupResult()
    try:
      pickup_result = self._pick_place.pickup(pickup_goal)
    except ManipulationError, last_ex:
      rospy.logerr('pickup_execute: FAILED: {0}'.format(last_ex))
      #action_result.error_code.val = last_ex.arm_navigation_error_code.val
      self._pickUpActionServer.set_aborted(result=action_result, text='execution failed: {0}'.format(last_ex))
      return

    # Success: Remove this pickup goal from known as
    # we can't pick this up any more
    known.pickup_goals.remove(pickup_goal)

    self._dump_known_objects()

    action_result.error_code.val = action_result.error_code.SUCCESS
    #action_result.grasp.pose = pickup_result.frame_free_grasp
    # TODO: frame_id for grasp pose correct?! no, fill this correct
    rospy.loginfo('pickup_execute: succeded')
    action_result.grasp.header.frame_id = self._tasks._arm_planners[goal.arm].hand.attach_link
    self._pickUpActionServer.set_succeeded(result=action_result, text='pickup successful')

  def _find_putdown_pose_callback(self, request):
    rospy.loginfo('find putdown pose request: place {0} held in {2} on {1}'.format(request.putdown_object, request.static_object, request.arm))
    psi = self._psi
    psi.current_diff.planning_scene_diff = request.planning_scene
    psi.send_diff()
    #self._planning_scene_sync.update_planning_scene(psi)
    #scene = psi.send_diff().planning_scene
    place_goal, grasp = self._create_place_goal(request)
    response = srv.GetPutdownPoseResponse()
    if not place_goal:
      rospy.logerr('find putdown pose FAILED: no object attached to gripper.')
      response.error_code.val = ArmNavigationErrorCodes.INCOMPLETE_ROBOT_STATE
      return response
    place_pose = self._check_trajectory_to_place_goal(place_goal)
    if not place_pose:
      rospy.logerr('find putdown pose FAILED: no valid pose found.')
      response.error_code.val = ArmNavigationErrorCodes.PLANNING_FAILED
      return response
    response.error_code.val = ArmNavigationErrorCodes.SUCCESS
    response.putdown_pose = place_pose
    rospy.loginfo('found putdown pose: {0}'.format(response.putdown_pose))
    return response

  def _put_down_execute(self, goal):
    rospy.loginfo('find putdown pose request: place {0} held in {2} on {1}'.format(goal.putdown_object, goal.static_object, goal.arm))
    self._psi.reset()

    place_goal, grasp = self._create_place_goal(goal)
    rospy.loginfo(self._place_goal_to_str(place_goal))

    self._pick_place._place_client.send_goal_and_wait(place_goal)
    result = self._pick_place._place_client.get_result()
    if result.manipulation_result.value == result.manipulation_result.SUCCESS:
      rospy.loginfo('planner selected pose: {0}'.format(result.place_location))

      self._dump_known_objects()

      rospy.loginfo('Inserting new pickup goal for %s at %s' % (goal.putdown_object, goal.static_object))
      # Insert this placed object into known, so it can be
      # matched by object detection in the next step, instead of getting a new name
      # All data should come from object det anyways, so we only need to make
      # sure it can be matched
      known = self._known_objects.get(goal.static_object)
      putdown_pg = None
      # First make sure that known is a valid TablewareDetectionResult in self._known_objects
      # which we can use to add our new pickup goal for the placed object to
      if known:
          for pg in known.pickup_goals:
              if pg.collision_object_name == goal.putdown_object:
                  putdown_pg = pg
                  rospy.logwarn('Found old pickup goal (%s) for putdown object already at this location' % _pickup_goal_to_str(putdown_pg))
                  break
      else:
          table = tabletop_object_detector.msg.Table()
          known = TablewareDetectionResult([], table, goal.static_object)
          self._known_objects[goal.static_object] = known

      # PoseStamped of the actual object pose, not the place wrist pose
      place_object_location = self.get_object_pose_from_grasp(result.place_location, grasp)

      # create or update the pickup goal for the placed object
      if putdown_pg is None:
          grasp_target = object_manipulation_msgs.msg.GraspableObject()
          grasp_target.collision_name = goal.putdown_object
          # FIXME: place_location is in map frame, this should still be OK for object_pose_stamped
          putdown_pg = PickupGoal("", grasp_target, object_pose_stamped=place_object_location, collision_object_name=goal.putdown_object)
          known.pickup_goals.append(putdown_pg) # This is new, so add it.
      else:
          putdown_pg.target.collision_name = goal.putdown_object
          putdown_pg.object_pose_stamped = place_object_location

      putdown_pg.collision_object_header = place_object_location.header
      putdown_pg.collision_object_poses = [place_object_location.pose]
      putdown_pg.collision_support_surface_name = goal.static_object

      self._dump_known_objects()

      # send result
      self._psi.reset()
      action_result = msg.PlaceObjectResult()
      action_result.putdown_pose = place_object_location
      action_result.error_code.val = result.manipulation_result.value
      self._putDownActionServer.set_succeeded(text='putdown successful', result=action_result)
      rospy.loginfo("putdown successful")
    else:
      rospy.logerr('pickplace.place pose FAILED: error code: {0}'.format(result.manipulation_result.value))
      rospy.logerr('Attempted Locations:')
      for ps in result.attempted_locations:
          rospy.logerr("frame: %s pose: %s" % (ps.header.frame_id, self._pose_to_str(ps.pose)))
      rospy.logerr('PlaceLocationResults:')
      for plr in result.attempted_location_results:
          rospy.logerr("Result Code: %d" % plr.result_code)
      self._putDownActionServer.set_aborted(text='find putdown pose FAILED: no valid pose.')
    #self._psi.reset()

  def get_object_pose_from_grasp(self, wrist_pose_stamped, frame_free_grasp_pose):
    '''
    Function for finding the end effector pose given a grasp and a desired place for the object.

    **Args:**

        **wrist_pose_stamped (geometry_msgs.msg.PoseStamped):** The pose as a geometry_msgs.msg.PoseStamped
            of the end effector to place the object at place_pose_stamped.
            For the PR2 this is the wrist pose.

        **frame_free_grasp (pickplace_definitions.FrameFreeGrasp):** Grasp with which the object is held

    **Returns:**
        place_pose_stamped (geometry_msgs.msg.PoseStamped): The Place for an object
    '''

    origin = Pose()
    origin.orientation.w = 1.0
    pose_stamped = PoseStamped()
    pose_stamped.header.frame_id = wrist_pose_stamped.header.frame_id
    pose_stamped.header.stamp = wrist_pose_stamped.header.stamp
    pose_stamped.pose = gt.transform_pose(gt.transform_pose(origin, frame_free_grasp_pose), 
                                          wrist_pose_stamped.pose)
    return pose_stamped

  def _create_place_goal(self, request):
    rospy.loginfo('create_place_goal')
    #scene = self._planning_scene_sync._latest_planning_scene
    psi = self._psi
    scene = psi.current_planning_scene()

    # check object id and arm
    putdown_object = None
    for attached in scene.attached_collision_objects:
      if attached.object.id == request.putdown_object and attached.link_name[0] == request.arm[0]:
        putdown_object = attached
#    putdown_object = psi.attached_collision_object(request.putdown_object)
    if not putdown_object:
      rospy.logerr('no attached object.')
      return None, None
    rospy.loginfo('attached object: {0}'.format(putdown_object.object.id))

    # table / shelf
    table = None
    for co in scene.collision_objects:
        if co.id == request.static_object:
            table = co
    #table = psi.collision_object(request.static_object)
    collision_objects = []
    for object in scene.collision_objects:
      if object.id != request.static_object and object.id != request.putdown_object:
        collision_objects.append(object)

    # find possible poses
    orientation = scene.robot_state.multi_dof_joint_state.poses[0].orientation
    object_poses = free_spots_on_table(table, putdown_object.object, orientation, collision_objects, orientation_count=8)
    rospy.loginfo('finished: free_spots_on_table ({1}). first pose: {0}'.format(object_poses[0], len(object_poses)))

    # prioritize putdown poses
    preferred_putdown_point = Point()
    preferred_putdown_point.x = 0.7
    preferred_putdown_point.y = 0.0
    preferred_putdown_point.z = 0
    if request.arm == 'right_arm':
      preferred_putdown_point.y *= -1;
    preferred_putdown_point = psi.transform_point('/map', '/torso_lift_link', preferred_putdown_point)
    object_poses.sort(key=lambda object_pose: math.sqrt((preferred_putdown_point.x - object_pose.pose.position.x)**2 + (preferred_putdown_point.y - object_pose.pose.position.y)**2))
    shoulder_point = Point()
    shoulder_point.x = 0
    shoulder_point.y = 0.3
    shoulder_point.z = 0
    if request.arm == 'right_arm':
      shoulder_point.y *= -1;
    shoulder_point = psi.transform_point('/map', '/torso_lift_link', shoulder_point)
    rospy.loginfo('should point at: {0}'.format(shoulder_point))
    object_poses = filter(lambda object_pose: math.sqrt((shoulder_point.x - object_pose.pose.position.x)**2 + (shoulder_point.y - object_pose.pose.position.y)**2) <= 1.0, object_poses)
    if object_poses:
        rospy.loginfo('finished: prioritizing/filtering ({1}). first pose: {0}'.format(object_poses[0], len(object_poses)))
    else:
        rospy.logwarn('finished: prioritizing/filtering: 0 poses.')

    # compute grasp
#    rospy.loginfo('robot state: {0}'.format(psi.get_robot_state()))
#    pose_markers = MarkerArray()
    grasp = psi.transform_pose(\
      psi.hands[request.arm].hand_frame, \
      putdown_object.object.header.frame_id, \
      putdown_object.object.poses[0])
    rospy.loginfo('grasp: {0}'.format(grasp))

    # compute wrist poses
    wrist_poses = []
    for object_pose in object_poses:
      wrist_poses.append(get_wrist_pose_from_grasp(object_pose, grasp))
#      pose_markers.markers.append(vt.marker_at(wrist_poses[-1], mid=len(wrist_poses), ns='putdown_wrist_poses'))
 #   self._visualization_publisher.publish(pose_markers)
    if object_poses:
        rospy.loginfo('finished creating place goal. firts pose: {0}'.format(wrist_poses[0]))
    else:
        rospy.logwarn('place goal does not have any poses')
    return PlaceGoal(request.arm, wrist_poses, collision_object_name=putdown_object.object.id, collision_support_surface_name=table.id), grasp

  def _check_trajectory_to_place_goal(self, place_goal):
    rospy.loginfo('check putdown for {0} with {1}'.format(place_goal.collision_object_name, place_goal.arm_name))
#    arm_planner = self._tasks._arm_planners[place_goal.arm_name]
    # disable collisions between putdown object and anything else
    collision = CollisionOperation()
    collision.object1 = place_goal.collision_object_name
    collision.object2 = collision.COLLISION_SET_ALL # arm_planner.joint_names[6]
    collision.operation = collision.DISABLE
    place_goal.additional_collision_operations.collision_operations.append(collision)
    self._psi.add_ordered_collisions(place_goal.additional_collision_operations)
    marker_ns = place_goal.collision_object_name+"_"+place_goal.arm_name+'_putdown'
    result = self._check_trajectory_to_wrist_poses_list(place_goal.place_locations, place_goal.arm_name, marker_ns=marker_ns)
    self._psi.remove_ordered_collisions(place_goal.additional_collision_operations)
    # request failed. visualize situation
    #robot_marker = vt.robot_marker(robot_state=self._psi.get_robot_state(), ns=marker_ns+'_failed', r=0.9, g=0.0, b=0.0, a=0.5)
    #self._visualization_publisher.publish(robot_marker)
    return result

  def _check_trajectory_to_pickup_goal(self, pickup_goal):
    rospy.loginfo('check pickup for {0} with {1}'.format(pickup_goal.collision_object_name, pickup_goal.arm_name))
    #arm_planner = self._tasks._arm_planners[pickup_goal.arm_name]
    grasps = self._pick_place.get_grasps(pickup_goal)

    # disable collisions between putdown object and anything else
    collision = CollisionOperation()
    collision.object1 = pickup_goal.collision_object_name
    collision.object2 = collision.COLLISION_SET_ALL # arm_planner.joint_names[6]
    collision.operation = collision.DISABLE
    pickup_goal.additional_collision_operations.collision_operations.append(collision)
    self._psi.add_ordered_collisions(pickup_goal.additional_collision_operations)
    grasp_markers = MarkerArray()
    marker_ns = pickup_goal.collision_object_name+"_"+pickup_goal.arm_name+"_pickup"
    #rospy.loginfo('potential models: \n{0}'.format(pickup_goal.target.potential_models))
    rospy.loginfo('checking grasps: {0}'.format(len(grasps)))
    wrist_poses = list()
    for grasp in grasps:
      #rospy.loginfo('grasp: \n{0}'.format(grasp))
      gripper_pose = PoseStamped() 
      gripper_pose.pose = grasp.grasp_pose
      gripper_pose.header.frame_id = '/base_link'
      wrist_poses.append(self._psi.transform_pose_stamped('/map', gripper_pose))
    result = self._check_trajectory_to_wrist_poses_list(wrist_poses, pickup_goal.arm_name, marker_ns)
    self._psi.remove_ordered_collisions(pickup_goal.additional_collision_operations)
    pickup_goal.additional_collision_operations.collision_operations.remove(collision)
    return result
    
  def _check_trajectory_to_wrist_poses_list(self, wrist_poses, arm_name, marker_ns=None):
    if not marker_ns: 
      marker_ns='wrist_'+arm_name
    rospy.loginfo('check to wrist poses {0} for {1}'.format(len(wrist_poses), arm_name))
    # disable collisions between putdown object and anything else
    arm_planner = self._tasks._arm_planners[arm_name]
    robot_state = self._psi.get_robot_state()
    wrist_markers = MarkerArray()
    found_trajectory = False
    trajectory_pose = None
    for wrist_pose in wrist_poses:
      # HACK: plan to 0.1m above the table surface to avoid colisions
      # this is somewhat correct, since the pickplace planner first moves to approach pose, 
      # before moving to the "real" wrist pose
      wrist_pose.pose.position.z += 0.1
      # 1) check collision-aware ik
#      rospy.loginfo('checking ik for wrist pose: {0} {1}'.format(wrist_pose.header.frame_id, self._pose_to_str(wrist_pose.pose)))
#      relative_pose = self._psi.transform_pose_stamped('base_link', wrist_pose)
#      rospy.loginfo('       relative wrist pose: {0} {1}'.format(relative_pose.header.frame_id, self._pose_to_str(relative_pose.pose)))
      ik_result = arm_planner.get_ik(wrist_pose, starting_state=robot_state, handle_collision_operations=False)
      if ik_result.error_code.val == ik_result.error_code.SUCCESS:
        wrist_markers.markers.append(vt.marker_at(pose_stamped=wrist_pose, ns=marker_ns+'_'+str(ik_result.error_code.val), mid=len(wrist_markers.markers), mtype=Marker.ARROW, r=0.1, g=0.3, b=0.8, a=1.0))
        if not found_trajectory:
          rospy.loginfo('found ik solution, checking trajectory')
          try:
            # 2) check full trajectory
            trajectory = arm_planner.plan_collision_free(wrist_pose, starting_state=robot_state)
            found_trajectory = True
            trajectory_pose = wrist_pose
          except exceptions.ArmNavError, e:
            wrist_markers.markers.append(vt.marker_at(pose_stamped=wrist_pose, ns=marker_ns+'_'+str(ik_result.error_code.val), mid=len(wrist_markers.markers), mtype=Marker.ARROW, r=0.1, g=0.8, b=0.8, a=1.0))
      else:
        wrist_markers.markers.append(vt.marker_at(pose_stamped=wrist_pose, ns=marker_ns+'_'+str(ik_result.error_code.val), mid=len(wrist_markers.markers), mtype=Marker.ARROW, r=0.8, g=0.3, b=0.1, a=1.0))
    if found_trajectory:
      # visualize result
      final_arm_state = tt.last_state_on_joint_trajectory(trajectory)
      final_robot_state = tt.set_joint_state_in_robot_state(final_arm_state, copy.deepcopy(robot_state))
      robot_marker = vt.robot_marker(robot_state=final_robot_state, ns=marker_ns+'_succeeded', r=0.2, g=0.5, b=0.9, a=0.5)
      self._visualization_publisher.publish(robot_marker)
      self._visualization_publisher.publish(wrist_markers)
      rospy.loginfo('trajectory ok, wrist pose reachable')
      return wrist_pose
    else:
      # request failed. visualize situation
      robot_marker = vt.robot_marker(robot_state=robot_state, ns=marker_ns+'_failed', r=0.8, g=0.3, b=0.1, a=0.5)
      self._visualization_publisher.publish(robot_marker)
      self._visualization_publisher.publish(wrist_markers)
      rospy.loginfo('trajectory check FAILED')
      return None

  def _add_tables_to_wi(self):
    table_file = rospy.get_param('~tables')
    tables = read_table_file(table_file)
    for table in tables:
      self._wi.add_collision_box(table[0], table[1], table[2])

  def _add_sponge_to_base(self, req=None):
    link_name = 'base_link'
    object_name = 'sponge'
    touch_links = ['base_link']
    stamped = PoseStamped()
    pose = Pose()
    pose.position.x = 0.25
    pose.position.y = 0.0
    pose.position.z = 0.33
    pose.orientation.x = 0
    pose.orientation.y = 0
    pose.orientation.z = 0
    pose.orientation.w = 1
    stamped.pose = pose
    stamped.header.frame_id = link_name
    self._attach_object(object_name, link_name, touch_links, stamped)
    return {}
    
  def _add_sponge_to_right_gripper(self, req=None):
    wi = self._wi
    hand = arm_planner = self._tasks._arm_planners['right_arm'].hand
    link_name = hand.attach_link
    object_name = 'sponge'
    touch_links = hand.touch_links
    stamped = PoseStamped()
    pose = Pose()
    pose.position.x = 0.025
    pose.position.y = 0.025
    pose.position.z = 0
    quat_list = tf.transformations.quaternion_from_euler(np.pi/2.0, 0, -np.pi/2.0)
    pose.orientation.x = quat_list[0]
    pose.orientation.y = quat_list[1]
    pose.orientation.z = quat_list[2]
    pose.orientation.w = quat_list[3]
    stamped.pose = pose
    stamped.header.frame_id = hand.attach_link
    self._attach_object(object_name, link_name, touch_links, stamped)
    return {}

  def _attach_object(self, object_name, link_name, touch_links, stamped):
    wi = self._wi
    # detach, if attached
    attached = wi.attached_collision_object(object_name)
    if attached:
      attached.object.operation.operation = attached.object.operation.DETACH_AND_ADD_AS_OBJECT
      wi._publish(attached, wi._attached_object_pub)
    
    object = wi.collision_object(object_name)
    # create object, if does not exist 
    if not object:
      rospy.loginfo('creating sponge')
      wi.add_collision_box(stamped, [0.10, 0.15, 0.05], object_name)
      object = wi.collision_object(object_name)
    
    # update object pose
    object.header.frame_id = stamped.header.frame_id
    object.poses[0] = stamped.pose
    object.operation.operation = object.operation.ADD
    wi.add_object(object)
    
    # attach to new link
    attached = AttachedCollisionObject()
    attached.object = wi.collision_object(object_name)
    attached.link_name = link_name
    attached.touch_links = touch_links
    attached.object.operation.operation = attached.object.operation.ATTACH_AND_REMOVE_AS_OBJECT
    wi._publish(attached, wi._attached_object_pub)
    
    self._psi.reset()