Esempio n. 1
0
def main():
    obj_type = sys.argv[1]
    my_world = WorldInterface()

    #    print "quat="+str(gt.euler_to_quaternion(math.pi/2,math.pi/2,0))

    if obj_type == "plate" or obj_type == "paper_plate" or obj_type == "plastic_plate":
        obj = create_plate(my_world)
    elif obj_type == "box":
        obj = create_box(my_world)

    my_world.attach_object_to_gripper('left_arm', obj)
Esempio n. 2
0
    def _initialize(self):
        '''Connect up all services and action clients.
        '''
        self._world_interface = WorldInterface()
        self._controller_manager = ControllerManagerClient()
        if self._arm_name in ['right_arm', 'left_arm']:
            self._group_name = self._arm_name
            self._planner = ArmPlanner(self._arm_name)
            self._hand_description = HandDescription(self._arm_name)

            arm_abbr = self._arm_name[0]
            self._joint_controller = '%s_arm_controller' % arm_abbr                
            self._cartesian_controller = '%s_cart' % arm_abbr
            
            self._move_arm_client = actionlib.SimpleActionClient(
                'move_%s' % self._arm_name, arm_navigation_msgs.msg.MoveArmAction)
            self._wait_for_action_server(self._move_arm_client)
            
            jt_action_name = '/%s_arm_controller/joint_trajectory_action' % arm_abbr
            self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, JointTrajectoryAction)
            self._wait_for_action_server(self._joint_trajectory_client)
            
            self._cart_interface = CartesianControllerInterface(self._arm_name)
        elif self._arm_name == 'both':
            self._joint_controller = 'two_arm_controller'
            jt_two_arm_action_name = '/two_arm_controller/joint_trajectory_action'
            self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, JointTrajectoryAction)
            self._wait_for_action_server(self._joint_trajectory_client)
        else:
            raise ValueError('Invalid arm name for worker: %s' % self._arm_name)
Esempio n. 3
0
def pickplace_main():

    rospy.loginfo('Waiting for action')
    rospy.loginfo('Doing detection')
    detector = TablewareDetection()
    det = detector.detect_objects(add_objects_as_mesh=False)
    if not det.pickup_goals:
        rospy.loginfo('Nothing to pick up!')
        return
    wi = WorldInterface()
    psi = get_planning_scene_interface()
    add_map_tables(wi)
    psi.reset()
    client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
    client.wait_for_server()
    
    goal = DARRTGoal()
    
    goal.pickup_goal = det.pickup_goals[0]
    goal.pickup_goal.lift_distance = 0.3
    goal.pickup_goal.arm_name = 'right_arm'
    
    place_pose_stamped = PoseStamped()
    place_pose_stamped.header.frame_id = wi.world_frame
    place_pose_stamped.pose.position.x = -1.3
    place_pose_stamped.pose.position.y = 0.7
    if 'graspable' in goal.pickup_goal.collision_object_name:
        place_pose_stamped.pose.position.z = 0.8
    else:
        place_pose_stamped.pose.position.z = 0.75
    place_pose_stamped.pose.orientation.w = 1.0
    goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
                                collision_support_surface_name = 'serving_table',
                                collision_object_name = goal.pickup_goal.collision_object_name)
    goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT]
    goal.object_sampling_fraction = 0.3
    goal.retreat_distance = 0.3
    goal.debug_level = 2
    goal.do_pause = False
    goal.execute = False
    goal.planning_time = 30
    goal.tries = 10
    goal.goal_bias = 0.2

    average_time = 0.0
    rospy.loginfo('First place goal is\n'+str(goal.place_goal.place_locations[0]))
    rospy.loginfo('Restart after '+str(goal.planning_time))
    for i in range(NTRIALS):
        rospy.loginfo('Sending goal')
        client.send_goal_and_wait(goal)
        rospy.loginfo('Returned')
        result = client.get_result()
        if result.error_code != result.SUCCESS:
            rospy.logerr('Something went wrong!  Error '+str(result.error_code)+'.  We had '+str(i)+
                         ' successful trials with an average time of '+str(average_time/float(i)))
            return
        average_time += result.planning_time
        rospy.loginfo('TRIAL '+str(i)+': '+str(result.planning_time))
    rospy.loginfo(str(NTRIALS)+' averaged '+str(average_time/float(NTRIALS))+' seconds to solve')
Esempio n. 4
0
def main():
    rospy.init_node("touring", anonymous=True)

    sm = smach.StateMachine(outcomes=["success",
                                      "failure"])
    base_mover = base.Base()
    detector = TablewareDetection()
    pickplace = PickPlace()
    pickplace.min_open_gripper_pos = 0.0014
    world = two_arms_states.WorldState()
    interface = WorldInterface()
    tasks = Tasks()
    placer_r = SensingPlacer('right_arm')
    placer_l = SensingPlacer('left_arm')

    interface.reset()
    psi = get_planning_scene_interface()
    psi.reset()
    tasks.move_arms_to_side()

    rospy.loginfo("Creating State Machine")

    with sm:
        detect = create_detect_sm(world, base_mover, detector)
        smach.StateMachine.add("detect", detect,
                transitions = {"success":"clean_table",
                               "failure":"failure"}
                )

        clean_table = clean_table_sm(world, tasks, base_mover, placer_l, placer_r, interface)
        smach.StateMachine.add("clean_table", clean_table,
                transitions = {"success":"setup_table",
                               "failure":"failure"
                               }
                )
        
        setup_table = setup_table_sm(world, tasks, base_mover, placer_l, placer_r, interface)
        smach.StateMachine.add("setup_table", setup_table,
                transitions = {"success":"success",
                               "failure":"failure"
                               }
                )


    outcome = sm.execute()
    rospy.loginfo("Outcome: %s", outcome)
 def __init__(self):
     arms = ['right_arm', 'left_arm']
     self.grippers = {}
     self.arm_planners = {}
     for arm in arms:
         self.grippers[arm] = Gripper(arm)
         self.arm_planners[arm] = ArmPlanner(arm)
     self.arm_mover = ArmMover()
     self.arm_tasks = ArmTasks()
     self.base = Base()
     self.wi = WorldInterface()
     self.psi = get_planning_scene_interface()
     self.base_action = SimpleActionClient('base_trajectory_action',
                                           BaseTrajectoryAction)
     rospy.loginfo('Waiting for base trajectory action')
     self.base_action.wait_for_server()
     rospy.loginfo('Found base trajectory action')
     rospy.loginfo('DARRT trajectory executor successfully initialized!')
Esempio n. 6
0
 def __init__(self,
              world,
              screenshot,
              fake_walls=False,
              box_plate=False,
              real=False):
     self.world = world
     self.screenshot = screenshot
     self.wi = WorldInterface()
     self.psi = get_planning_scene_interface()
     self.pose_pub = rospy.Publisher('/initialpose',
                                     PoseWithCovarianceStamped)
     self.arms = ArmTasks()
     self.torso = Torso()
     self.gripper = {
         'right_arm': Gripper('right_arm'),
         'left_arm': Gripper('left_arm')
     }
     self.box_plate = box_plate
     self.real = real
     self.fake_walls = fake_walls
Esempio n. 7
0
def time_plate_main():
    
    rospy.loginfo('Reading file')
    [result, goal, collision_objects, robot_state] = pickle.load(open(sys.argv[1], 'r'))
    rospy.loginfo('Original starting state was\n'+str(robot_state))
    markers = vt.robot_marker(robot_state, ns='robot_starting_state', a = 0.5)
    pub = rospy.Publisher('/darrt_planning/robot_state_markers', MarkerArray)
    for i in range(10):
        rospy.loginfo('Publishing starting state!')
        pub.publish(markers)
        rospy.sleep(0.1)
    wi = WorldInterface()
    wi.reset(repopulate=False)
    psi = get_planning_scene_interface()
    psi.reset()
    plate = None
    for co in collision_objects:
        wi.add_object(co)
        if (co.id == goal.pickup_goal.collision_object_name):
            plate = co
    psi.reset()
    ops = PoseStamped()
    ops.header = copy.deepcopy(plate.header)
    ops.pose = copy.deepcopy(plate.poses[0])
    #the old pickup goal may have used base link to generate grasps
    #i don't know what we do with things for which we need a point cloud
    goal.pickup_goal = PickupGoal('right_arm', om.GraspableObject(reference_frame_id=ops.header.frame_id), 
                                  object_pose_stamped=ops, collision_object_name=goal.pickup_goal.collision_object_name,
                                  collision_support_surface_name=goal.pickup_goal.collision_support_surface_name,
                                  desired_grasps=plate_grasps(ops, ops.header.frame_id))
    rospy.loginfo('Teleop to initial state.  Ready to go?')
    raw_input()

    client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
    client.wait_for_server()
    goal.execute = False
    goal.planning_time = 200
    goal.tries = 10000
    goal.debug_level = 1
    goal.do_pause = False
    goal.goal_bias = 0.2
#    goal.place_goal.place_locations[0].pose.position.x = 1
 #   goal.place_goal.place_locations[0].pose.position.y = 1
  #  goal.place_goal.place_locations[0].pose.position.z = 0.86
    
    average_time = 0.0
    rospy.loginfo('First place goal is\n'+str(goal.place_goal.place_locations[0]))
    rospy.loginfo('File: '+sys.argv[1]+', restart after '+str(goal.planning_time))
    for i in range(NTRIALS):
        rospy.loginfo('Sending goal')
        client.send_goal_and_wait(goal)
        rospy.loginfo('Returned')
        result = client.get_result()
        if result.error_code != result.SUCCESS:
            rospy.logerr('Something went wrong!  Error '+str(result.error_code)+'.  We had '+str(i)+
                         ' successful trials with an average time of '+str(average_time/float(i)))
            return
        average_time += result.planning_time
        rospy.loginfo('TRIAL '+str(i)+': '+str(result.planning_time))
    rospy.loginfo(str(NTRIALS)+' averaged '+str(average_time/float(NTRIALS))+' seconds to solve')
    def _initialize(self):
        '''Connect up all services and action clients.
        '''
        self._world_interface = WorldInterface()
        self._controller_manager = ControllerManagerClient()
        if self._arm_name in ['right_arm', 'left_arm']:
            self._group_name = self._arm_name
            self._planner = ArmPlanner(self._arm_name)
            self._hand_description = HandDescription(self._arm_name)

            arm_abbr = self._arm_name[0]
            self._joint_controller = '%s_arm_controller' % arm_abbr
            self._cartesian_controller = '%s_cart' % arm_abbr

            self._move_arm_client = actionlib.SimpleActionClient(
                'move_%s' % self._arm_name,
                arm_navigation_msgs.msg.MoveArmAction)
            self._wait_for_action_server(self._move_arm_client)

            #jt_action_name = '/%s_arm_controller/joint_trajectory_action' % arm_abbr
            #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, JointTrajectoryAction)
            #self._wait_for_action_server(self._joint_trajectory_client)

            jt_action_name = '/%s_arm_controller/follow_joint_trajectory' % arm_abbr
            self._joint_trajectory_client = actionlib.SimpleActionClient(
                jt_action_name, FollowJointTrajectoryAction)
            self._wait_for_action_server(self._joint_trajectory_client)

            self._cart_interface = CartesianControllerInterface(self._arm_name)
        elif self._arm_name == 'both':
            self._joint_controller = 'two_arm_controller'
            #jt_two_arm_action_name = '/two_arm_controller/joint_trajectory_action'
            #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, JointTrajectoryAction)
            jt_two_arm_action_name = '/two_arm_controller/follow_joint_trajectory'
            self._joint_trajectory_client = actionlib.SimpleActionClient(
                jt_two_arm_action_name, FollowJointTrajectoryAction)
            self._wait_for_action_server(self._joint_trajectory_client)
        else:
            raise ValueError('Invalid arm name for worker: %s' %
                             self._arm_name)
def main():
    [result, goal, collision_objects,
     robot_state] = pickle.load(open(sys.argv[1], 'r'))
    wi = WorldInterface()
    #set the planning scene up correctly
    wi.reset(repopulate=False)
    for co in collision_objects:
        #for running easy version
        if ('table' in co.id or co.id == 'plate'):
            wi.add_object(co)
    psi = get_planning_scene_interface()
    psi.reset()
    client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
    client.wait_for_server()
    average_time = 0
    goal.planning_time = 30
    goal.tries = 10000
    goal.debug_level = 0
    goal.do_pause = False
    goal.goal_bias = 0.2
    #i think that this does not work
    #goal.robot_state = robot_state
    goal.execute = False

    rospy.loginfo('First place goal is\n' +
                  str(goal.place_goal.place_locations[0]))
    rospy.loginfo('File: ' + sys.argv[1] + ', restart after ' +
                  str(goal.planning_time))
    for i in range(NTRIALS):
        rospy.loginfo('Sending goal')
        client.send_goal_and_wait(goal)
        rospy.loginfo('Returned')
        result = client.get_result()
        if result.error_code != result.SUCCESS:
            rospy.logerr('Something went wrong!  Error ' +
                         str(result.error_code) + '.  We had ' + str(i) +
                         ' successful trials with an average time of ' +
                         str(average_time / float(i)))
            return
        average_time += result.planning_time
        rospy.loginfo('TRIAL ' + str(i) + ': ' + str(result.planning_time))
    rospy.loginfo(
        str(NTRIALS) + ' averaged ' + str(average_time / float(NTRIALS)) +
        ' seconds to solve')
Esempio n. 10
0
def plate_main():
    pub = rospy.Publisher('darrt_trajectory', MarkerArray)
    rospy.loginfo('Waiting for action')
    rospy.loginfo('Doing detection')
    wi = WorldInterface()
    psi = get_planning_scene_interface()
    detector = TablewareDetection()
    plate = wi.collision_object('plate')
    good_detection = False
    goal = DARRTGoal()
    if plate:
        rospy.loginfo('Use last detection?')
        good_detection = (raw_input() == 'y')
        ops = PoseStamped()
        ops.header = copy.deepcopy(plate.header)
        ops.pose = copy.deepcopy(plate.poses[0])
        goal.pickup_goal = PickupGoal('right_arm', om.GraspableObject(reference_frame_id=ops.header.frame_id), 
                                      object_pose_stamped=ops, collision_object_name='plate',
                                      collision_support_surface_name='serving_table')
    while not good_detection:
        det = detector.detect_objects(add_table_to_map=False, 
                                      add_objects_as_mesh=False, table_name='serving_table')
        goal.pickup_goal = get_plate(det.pickup_goals, det.table.pose, wi)
        psi.reset()
        if not goal.pickup_goal:
            rospy.loginfo('Nothing to pick up!')
        rospy.loginfo('Good detection?')
        good_detection = (raw_input() == 'y')
    if not goal.pickup_goal:
        rospy.loginfo('Nothing to pick up!')
        return
    add_map_tables(wi)
    psi.reset()
    client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
    client.wait_for_server()

    goal.pickup_goal.arm_name = 'right_arm'
    goal.pickup_goal.desired_grasps = plate_grasps(goal.pickup_goal.object_pose_stamped, 
                                                   goal.pickup_goal.target.reference_frame_id)
    goal.pickup_goal.lift.desired_distance = 0.3
    place_pose_stamped = PoseStamped()
    place_pose_stamped.header.frame_id = wi.world_frame 
    # place_pose_stamped.pose.position.x = 1.3
    # place_pose_stamped.pose.position.y = -0.3
    # place_pose_stamped.pose.position.z = 1.01
    place_pose_stamped.pose.position.x = 0.0
    place_pose_stamped.pose.position.y = 0.0
    place_pose_stamped.pose.position.z = 0.76
    place_pose_stamped.pose.orientation.w = 1.0
    # place_pose_stamped.pose.orientation.x = 0.1
    # place_pose_stamped.pose.orientation.y = 0.1
    # place_pose_stamped.pose.orientation.w = np.sqrt(0.98)
    #place_pose_stamped = copy.deepcopy(goal.pickup_goal.object_pose_stamped)
    #place_pose_stamped.pose.position.x -= 0.2
    #place_pose_stamped.pose.position.y -= 0.2
    #place_pose_stamped.pose.position.z += 0.2
    goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
                                collision_support_surface_name = 'dirty_table',
                                collision_object_name = goal.pickup_goal.collision_object_name)
    goal.place_goal.approach.direction.header.frame_id = wi.world_frame
    goal.place_goal.approach.direction.vector.x = np.sqrt(0.18)
    goal.place_goal.approach.direction.vector.y = -np.sqrt(0.18)
    goal.place_goal.approach.direction.vector.z = -0.8
    goal.place_goal.approach.desired_distance = 0.2
    goal.primitives = [goal.PICK, goal.PLACE, goal.PUSH, goal.BASE_TRANSIT]
    goal.min_grasp_distance_from_surface = 0.19
    goal.object_sampling_fraction = 0.7
    goal.retreat_distance = 0.3
    goal.debug_level = 2
    goal.do_pause = False
    goal.execute = False
    goal.planning_time = 6000
    goal.tries = 1
    goal.goal_bias = 0.2
    rospy.loginfo('Sending goal')
    client.send_goal_and_wait(goal)
    rospy.loginfo('Returned')
    result = client.get_result()

    marray = MarkerArray()
    if result.error_code == result.SUCCESS:
        #pickle everything!! great excitement
        filename = 'trajectory_and_objects.pck'
        rospy.loginfo('Pickling to '+filename)
        #the last bit allows us to recreate the planning
        pickle.dump([client.get_result(), goal, wi.collision_objects(), wi.get_robot_state()], open(filename, 'wb'))
        rospy.loginfo('Successful write')
        for t in result.primitive_trajectories:
            marray.markers += vt.trajectory_markers(t, ns='trajectory', resolution=3).markers
        for (i, m) in enumerate(marray.markers):
            m.id = i
            (m.color.r, m.color.g, m.color.b) = vt.hsv_to_rgb(i/float(len(marray.markers))*300.0, 1, 1)
        while not rospy.is_shutdown():
            pub.publish(marray)
            rospy.sleep(0.1)
class Executor:
    def __init__(self):
        arms = ['right_arm', 'left_arm']
        self.grippers = {}
        self.arm_planners = {}
        for arm in arms:
            self.grippers[arm] = Gripper(arm)
            self.arm_planners[arm] = ArmPlanner(arm)
        self.arm_mover = ArmMover()
        self.arm_tasks = ArmTasks()
        self.base = Base()
        self.wi = WorldInterface()
        self.psi = get_planning_scene_interface()
        self.base_action = SimpleActionClient('base_trajectory_action',
                                              BaseTrajectoryAction)
        rospy.loginfo('Waiting for base trajectory action')
        self.base_action.wait_for_server()
        rospy.loginfo('Found base trajectory action')
        rospy.loginfo('DARRT trajectory executor successfully initialized!')

    def execute_trajectories(self, result):
        rospy.loginfo('There are ' + str(len(result.primitive_names)) +
                      ' segments')
        for (i, t) in enumerate(result.primitive_types):
            rospy.loginfo('Executing trajectory ' + str(i) + ' of type ' + t +
                          ' and length ' + str(
                              len(result.primitive_trajectories[i].
                                  joint_trajectory.points)))
            #print 'Trajectory is\n', result.primitive_trajectories[i]
            splits = t.split('-')
            if splits[0] == 'BaseTransit':
                self.execute_base_trajectory(result.primitive_trajectories[i])
                continue
            if splits[0] == 'BaseWarp':
                self.arm_tasks.move_arm_to_side('right_arm')
                self.arm_tasks.move_arm_to_side('left_arm')
                self.execute_warp_trajectory(result.primitive_trajectories[i])
                continue
            if splits[0] == 'ArmTransit':
                self.execute_arm_trajectory(splits[1],
                                            result.primitive_trajectories[i])
                continue
            #if splits[0] == 'Approach':
            #gripper.open()
            if splits[0] == 'Pickup':
                try:
                    self.grippers[splits[1]].close()
                except ActionFailedError:
                    pass
                self.wi.attach_object_to_gripper(splits[1], splits[2])
                #so the trajectory filter doesn't complain
                ops = OrderedCollisionOperations()
                op = CollisionOperation()
                op.operation = op.DISABLE

                op.object2 = splits[2]
                for t in self.wi.hands[splits[1]].touch_links:
                    op.object1 = t
                    ops.collision_operations.append(copy.deepcopy(op))
                self.psi.add_ordered_collisions(ops)
            self.execute_straight_line_arm_trajectory(
                splits[1], result.primitive_trajectories[i])
            if splits[0] == 'Place':
                self.grippers[splits[1]].open()
                self.wi.detach_and_add_back_attached_object(
                    splits[1], splits[2])
                self.psi.reset()

    def execute_warp_trajectory(self, trajectory):
        #figure out the last point on the trajectory
        tp = trajectory.multi_dof_joint_trajectory.points[-1]
        (phi, theta, psi) = gt.quaternion_to_euler(tp.poses[0].orientation)
        self.base.move_to(tp.poses[0].position.x, tp.poses[0].position.y, psi)

    def execute_base_trajectory(self, trajectory):
        goal = BaseTrajectoryGoal()
        #be a little slower and more precise
        goal.linear_velocity = 0.2
        goal.angular_velocity = np.pi / 8.0
        goal.linear_error = 0.01
        goal.angular_error = 0.01
        joint = -1
        for (i, n) in enumerate(
                trajectory.multi_dof_joint_trajectory.joint_names):
            if n == 'world_joint' or n == '/world_joint':
                goal.world_frame = trajectory.multi_dof_joint_trajectory.frame_ids[
                    i]
                goal.robot_frame = trajectory.multi_dof_joint_trajectory.child_frame_ids[
                    i]
                joint = i
                break
        if joint < 0:
            raise ActionFailedError('No world joint found in base trajectory')
        for p in trajectory.multi_dof_joint_trajectory.points:
            (phi, theta,
             psi) = gt.quaternion_to_euler(p.poses[joint].orientation)
            goal.trajectory.append(
                Pose2D(x=p.poses[joint].position.x,
                       y=p.poses[joint].position.y,
                       theta=psi))
        self.base_action.send_goal_and_wait(goal)

    def execute_arm_trajectory(self, arm_name, trajectory):
        arm_trajectory = self.arm_planners[arm_name].joint_trajectory(
            trajectory.joint_trajectory)
        arm_trajectory.header.stamp = rospy.Time.now()
        #try:
        #    arm_trajectory = self.arm_planners[arm_name].filter_trajectory(arm_trajectory)
        #except ArmNavError, e:
        #rospy.logwarn('Trajectory filter failed with error '+str(e)+'.  Executing anyway')
        arm_trajectory = tt.convert_path_to_trajectory(arm_trajectory,
                                                       time_per_pt=0.35)
        self.arm_mover.execute_joint_trajectory(arm_name, arm_trajectory)

    def execute_straight_line_arm_trajectory(self, arm_name, trajectory):
        arm_trajectory = self.arm_planners[arm_name].joint_trajectory(
            trajectory.joint_trajectory)
        arm_trajectory.header.stamp = rospy.Time.now()
        arm_trajectory = tt.convert_path_to_trajectory(arm_trajectory,
                                                       time_per_pt=0.4)
        self.arm_mover.execute_joint_trajectory(arm_name, arm_trajectory)
Esempio n. 12
0
def main():

    #Find an object to pick up
    psi = get_planning_scene_interface()
    psi.reset()
    rospy.loginfo('Doing detection')
    detector = TablewareDetection()
    det = detector.detect_objects(add_objects_as_mesh=False)
    if not det.pickup_goals:
        rospy.loginfo('Nothing to pick up!')
        return
    psi.reset()
    #DARRT action client
    client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
    rospy.loginfo('Waiting for DARRT action')
    client.wait_for_server()
    rospy.loginfo('Found DARRT action')

    #DARRTGoal
    goal = DARRTGoal()

    #pickup goal (from the tabletop detection)
    goal.pickup_goal = det.pickup_goals[0]
    goal.pickup_goal.arm_name = 'right_arm'

    #world interface to tell us whether we're using the
    #map or odom_combined frame
    wi = WorldInterface()

    #place goal (move far away)
    place_pose_stamped = PoseStamped()
    place_pose_stamped.header.frame_id = wi.world_frame
    place_pose_stamped.pose.position.x = -2
    place_pose_stamped.pose.position.y = 0
    place_pose_stamped.pose.position.z = 0.85
    place_pose_stamped.pose.orientation.w = 1.0
    goal.place_goal = PlaceGoal(
        goal.pickup_goal.arm_name, [place_pose_stamped],
        collision_support_surface_name=det.table_name,
        collision_object_name=goal.pickup_goal.collision_object_name)

    goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT]
    goal.planning_time = 30
    goal.tries = 3
    goal.debug_level = 1
    goal.object_sampling_fraction = 0.5

    #Send the goal to the action
    rospy.loginfo('Sending goal')
    client.send_goal_and_wait(goal)
    rospy.loginfo('Returned')

    #Check the result
    result = client.get_result()
    if result.error_code != result.SUCCESS:
        rospy.logerr('Planning failed.  Error code: ' + str(result.error_code))
        return
    rospy.loginfo('Planning succeeded!')

    #at this point the planning is done.
    #now we execute and visualize the plan

    #visualize trajectory
    pub = rospy.Publisher('darrt_trajectory', MarkerArray)
    marray = get_trajectory_markers(result.primitive_trajectories)
    for i in range(10):
        pub.publish(marray)
        rospy.sleep(0.05)

    #Executor is a Python class for executing DARRT plans
    executor = Executor()

    #execute trajectory
    rospy.loginfo('Press enter to execute')
    raw_input()
    executor.execute_trajectories(result)
    rospy.loginfo('Successfully executed!')
Esempio n. 13
0
#!/usr/bin/env python
import roslib
roslib.load_manifest('cob_arm_navigation_python')

import rospy
from MotionPlan import *
from MoveArm import *
from MoveHand import *
from tf.transformations import *
from copy import deepcopy
rospy.init_node("test")

from pr2_python.world_interface import WorldInterface

wi = WorldInterface()
grasp_pose = PoseStamped()
#grasp_pose.header.frame_id = "base_link"
grasp_pose.header.frame_id = "odom_combined"
grasp_pose.header.stamp = rospy.Time()
grasp_pose.pose = wi.collision_object('milk').poses[0]
p = grasp_pose.pose.position
#p.x, p.y, p.z = [-0.8,-0.2,0.9]
p.x += 0.02
p.y += 0.02
p.z += 0.02
o = grasp_pose.pose.orientation
o.x,o.y,o.z,o.w = quaternion_from_euler(-1.581, -0.019, 2.379)

lift_pose = deepcopy(grasp_pose)
p = lift_pose.pose.position
p.x += 0.0
Esempio n. 14
0
class SimSetup:
    def __init__(self,
                 world,
                 screenshot,
                 fake_walls=False,
                 box_plate=False,
                 real=False):
        self.world = world
        self.screenshot = screenshot
        self.wi = WorldInterface()
        self.psi = get_planning_scene_interface()
        self.pose_pub = rospy.Publisher('/initialpose',
                                        PoseWithCovarianceStamped)
        self.arms = ArmTasks()
        self.torso = Torso()
        self.gripper = {
            'right_arm': Gripper('right_arm'),
            'left_arm': Gripper('left_arm')
        }
        self.box_plate = box_plate
        self.real = real
        self.fake_walls = fake_walls

    def setup(self, arm):
        self.wi.reset(repopulate=False)
        self.set_robot_position()
        goal = DARRTGoal()
        goal.support_surfaces = self.add_tables()
        if self.fake_walls:
            self.add_walls()
        #all the primitives
        arm_transit = Primitive(name='ArmTransit', parameters=[arm])
        base_transit = Primitive(name='BaseTransit', parameters=['pr2_base'])
        approach = Primitive(name="Approach", parameters=[arm])
        retreat = Primitive(name="Retreat", parameters=[arm])
        rigid_transfer = Primitive(name="RigidTransfer", parameters=[arm])
        push = Primitive(name="Push", parameters=[arm])
        pickup = Primitive(name="Pickup", parameters=[arm])
        uspat = Primitive(name="UseSpatula",
                          parameters=[arm],
                          numeric_parameters=[0.22, 0.2, 0.2])
        strans = Primitive(name="SpatulaTransfer", parameters=[arm])
        if self.world < 0:
            obj = self.add_little_object()
            spatula, grasps = self.add_spatula(arm)
            block = self.add_block()
            if self.world < -8:
                self.add_barrier()
            goal.objects = [obj, spatula, block]
            rigid_transfer.parameters.append(spatula.collision_object.id)
            push.parameters += [
                obj.collision_object.id, arm[0] + '_end_effector'
            ]
            uspat.parameters += [
                spatula.collision_object.id, obj.collision_object.id,
                block.collision_object.id
            ]
            uspat.grasps = grasps
            lift = Vector3(z=1.0)
            uspat.directional_parameters.append(lift)
            strans.parameters += [
                spatula.collision_object.id, obj.collision_object.id
            ]
            goal.primitives = [
                arm_transit, base_transit, approach, retreat, rigid_transfer,
                push, uspat, strans
            ]
            goal.goals = [
                Goal(obj.collision_object.id, self.get_goal_poses(), False)
            ]
            self.psi.reset()
            return goal
        else:
            rospy.loginfo('Old world set up is broken!')
            return None

        #this no longer works right
        if self.world < 14:
            obj = self.add_plate()
        else:
            obj = self.add_box()
        if self.world > 7:
            self.add_barrier()
        self.psi.reset()
        if self.world < 6 or self.world > 7:
            ss_surface = ss_names[2]
        #elif self.world == 12:
        #   ss_surface = ss_names[1]
        else:
            ss_surface = ss_names[0]
        return World(obj, ss_surface, ss_names[1])

    def set_robot_position(self):
        #right now worlds 6 and above don't move the robot
        if self.world > 15:
            return
        ps = PoseWithCovarianceStamped()
        ps.pose.covariance = [0] * 36
        ps.header.frame_id = '/map'
        if abs(self.world) <= 2:
            #easier start pose
            ps.pose.pose.position.x = 2.7
            ps.pose.pose.position.y = -2.5
            ps.pose.pose.orientation.w = 1
        elif self.world < 0 or abs(self.world) < 6 or abs(self.world) == 14:
            ps.pose.pose.position.x = -1
            ps.pose.pose.position.y = 0
            ps.pose.pose.orientation.w = 1
        elif abs(self.world) == 12:
            ps.pose.pose.position.x = 0
            ps.pose.pose.position.y = 1.5
            ps.pose.pose.orientation.w = 1
        else:
            ps.pose.pose.position.x = 1
            ps.pose.pose.position.y = 1
            ps.pose.pose.orientation.z = -1.0 / np.sqrt(2.0)
            ps.pose.pose.orientation.w = 1.0 / np.sqrt(2.0)

        if not self.real:
            self.pose_pub.publish(ps)
        self.gripper['right_arm'].open()
        self.torso.move(0.3)

        self.arms.move_arm_to_side('left_arm')
        if abs(self.world) < 4:
            rospy.loginfo('Moving right arm to up position')
            self.arms._arm_mover.move_to_goal(
                'right_arm',
                self.arms._arm_planners['right_arm'].
                joint_positions_to_joint_state(
                    self.arms.side_joint_trajectory['right_arm'][1]),
                try_hard=True)
        else:
            self.arms.move_arm_to_side('right_arm')

    def add_tables(self):
        if self.screenshot:
            return ['', '', '']
        table = CollisionObject()
        table.header.frame_id = self.wi.world_frame
        table.operation.operation = table.operation.ADD

        shape = Shape()
        shape.type = shape.MESH
        #center table
        shape.vertices = [
            Point(x=-0.2, y=-0.4, z=CENTER_TABLE_HEIGHT),
            Point(x=0.97, y=-0.6, z=CENTER_TABLE_HEIGHT),
            Point(x=1.0, y=0.25, z=CENTER_TABLE_HEIGHT),
            Point(x=-0.25, y=0.3, z=CENTER_TABLE_HEIGHT)
        ]
        shape.triangles = [0, 1, 2, 2, 3, 0]

        pose = Pose()
        pose.orientation.w = 1.0
        poseb = copy.deepcopy(pose)
        poseb.position.z = -0.02
        poset = copy.deepcopy(pose)
        poset.position.z = 0.02
        table.shapes.append(shape)
        table.shapes.append(shape)
        table.shapes.append(shape)
        table.poses.append(poset)
        table.poses.append(pose)
        table.poses.append(poseb)
        table.id = 'center_table'
        self.wi.add_object(table)

        #table near the door
        table.id = 'door_table'
        shape.vertices = [
            Point(x=-2.4, y=1.84, z=DOOR_TABLE_HEIGHT),
            Point(x=-1.15, y=1.75, z=DOOR_TABLE_HEIGHT),
            Point(x=-1.15, y=2.5, z=DOOR_TABLE_HEIGHT),
            Point(x=-2.4, y=2.5, z=DOOR_TABLE_HEIGHT)
        ]

        self.wi.add_object(table)

        #table in far corner
        table.id = 'far_corner'
        shape.vertices = [
            Point(x=3, y=-2.7, z=FAR_TABLE_HEIGHT),
            Point(x=2.4, y=-3.8, z=FAR_TABLE_HEIGHT),
            Point(x=3.2, y=-4.3, z=FAR_TABLE_HEIGHT),
            Point(x=3.8, y=-3.2, z=FAR_TABLE_HEIGHT)
        ]
        self.wi.add_object(table)

        if self.fake_walls:
            #these are the table feet
            foot = CollisionObject()
            foot.header.frame_id = self.wi.world_frame
            foot.operation.operation = foot.operation.ADD
            foot.id = "far_corner_foot"

            shape = Shape()
            shape.type = shape.BOX
            shape.dimensions = [0.1, 0.5, FAR_TABLE_HEIGHT / 2.0]

            pose = Pose()
            pose.position.x = 3
            pose.position.y = -3.4
            pose.position.z = shape.dimensions[2] / 2.0
            angle = 0.5
            pose.orientation.z = np.cos(angle / 2.0)
            pose.orientation.w = np.sin(angle / 2.0)
            foot.shapes.append(shape)
            foot.poses.append(pose)
            self.wi.add_object(foot)

            foot.id = "center_table_foot1"
            shape.dimensions = [0.1, 0.75, 0.3]
            pose.position.x = 0.9
            pose.position.y = -0.1
            pose.position.z = shape.dimensions[2] / 2.0
            angle = 0
            pose.orientation.z = np.cos(angle / 2.0)
            pose.orientation.w = np.sin(angle / 2.0)
            self.wi.add_object(foot)

            foot.id = "center_table_foot2"
            pose.position.x = -0.2
            self.wi.add_object(foot)

            foot.id = "door_table_foot"
            pose.position.x = -1.25
            pose.position.y = 2.1
            self.wi.add_object(foot)

        return ['center_table', 'door_table', 'far_corner']

    def add_walls(self):
        window_wall = CollisionObject()
        window_wall.header.frame_id = self.wi.world_frame
        window_wall.operation.operation = window_wall.operation.ADD
        window_wall.id = "window_wall"

        shape = Shape()
        shape.type = shape.BOX
        shape.dimensions = [8, 0.1, 2.5]
        pose = Pose()
        pose.position.x = 0
        pose.position.y = -2.63
        pose.position.z = shape.dimensions[2] / 2.0 - 0.1
        angle = np.pi / 6.25
        pose.orientation.z = np.cos(angle / 2.0)
        pose.orientation.w = np.sin(angle / 2.0)
        window_wall.shapes.append(shape)
        window_wall.poses.append(pose)
        self.wi.add_object(window_wall)

        shape.dimensions[0] = shape.dimensions[1]
        shape.dimensions[1] = 5
        pose.position.x = -2.45
        pose.position.y = 1
        angle = 0.05
        pose.orientation.z = np.cos(angle / 2.0)
        pose.orientation.w = np.sin(angle / 2.0)
        window_wall.id = "door_wall"
        self.wi.add_object(window_wall)

        shape.dimensions[1] = shape.dimensions[0]
        shape.dimensions[0] = 8
        pose.position.x = 0
        pose.position.y = 2.3
        angle = 0.1
        pose.orientation.z = np.cos(angle / 2.0)
        pose.orientation.w = np.sin(angle / 2.0)
        window_wall.id = "cabinet_wall"
        self.wi.add_object(window_wall)

        shape.dimensions[0] = shape.dimensions[1]
        shape.dimensions[1] = 3
        pose.position.x = 4
        pose.position.y = -3
        angle = 0.5
        pose.orientation.z = np.cos(angle / 2.0)
        pose.orientation.w = np.sin(angle / 2.0)
        window_wall.id = "banner_wall"
        self.wi.add_object(window_wall)

        shape.dimensions[1] = 4
        shape.dimensions[2] = 1
        pose.position.x = 3.25
        pose.position.y = 0.65
        pose.position.z = shape.dimensions[2] / 2.0 - 0.05
        angle = 0
        pose.orientation.z = np.cos(angle / 2.0)
        pose.orientation.w = np.sin(angle / 2.0)
        window_wall.id = "computer_wall"
        self.wi.add_object(window_wall)

        shape.dimensions[1] = shape.dimensions[0]
        shape.dimensions[0] = 1.8
        pose.position.x = 3.9
        pose.position.y = -1.6
        angle = np.pi / 5.5
        pose.orientation.z = np.cos(angle / 2.0)
        pose.orientation.w = np.sin(angle / 2.0)
        window_wall.id = "nook_wall"
        self.wi.add_object(window_wall)

    def add_little_object(self):
        little_obj = CollisionObject()
        little_obj.header.frame_id = self.wi.world_frame
        little_obj.operation.operation = little_obj.operation.ADD
        little_obj.id = "little_obj"
        shape = Shape()
        shape.type = shape.CYLINDER
        shape.dimensions = [0.05, 0.02]

        pose = Pose()
        pose.orientation.w = 1.0

        little_obj.shapes.append(shape)
        little_obj.poses.append(pose)

        little_obj_p = copy.deepcopy(little_obj)
        little_obj_p.poses[0].position.x = 3.1
        little_obj_p.poses[0].position.y = -3.2
        little_obj_p.poses[
            0].position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[1] / 2.0
        self.wi.add_object(little_obj_p)
        return ObjectType(type="RoundObject",
                          collision_object=little_obj,
                          parameters=['far_corner'])

    def add_block(self):
        block = CollisionObject()
        block.header.frame_id = self.wi.world_frame
        block.operation.operation = block.operation.ADD
        block.id = "block"
        shape = Shape()
        shape.type = shape.BOX
        shape.dimensions = [0.1, 0.1, 0.13]

        pose = Pose()
        pose.orientation.w = 1.0

        block.shapes.append(shape)
        block.poses.append(pose)

        block_p = copy.deepcopy(block)
        block_p.poses[0].position.x = 2.9
        block_p.poses[0].position.y = -3.5
        block_p.poses[
            0].position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[2] / 2.0
        self.wi.add_object(block_p)
        return ObjectType(type="FixedObject",
                          collision_object=block,
                          parameters=['far_table'])

    def add_spatula(self, arm):
        spatula = CollisionObject()
        spatula.id = "spatula"
        spatula.header.frame_id = self.wi.world_frame
        spatula.operation.operation = spatula.operation.ADD

        paddle = Shape()
        handle = Shape()
        paddle.type = paddle.BOX
        paddle.dimensions = [0.11, 0.12, 0.005]
        handle.type = handle.CYLINDER
        handle.dimensions = [0.02, 0.24]

        paddle_pose = Pose()
        handle_pose = Pose()
        paddle_pose.position.y = paddle.dimensions[1] / 2.0
        paddle_pose.orientation.w = 1.0

        angle = np.pi / 5.0
        handle_pose.position.y = -1.0 * handle.dimensions[1] / 2.0 * np.sin(
            np.pi / 2.0 - angle)
        handle_pose.position.z = handle.dimensions[1] / 2.0 * np.cos(
            np.pi / 2.0 - angle)
        handle_pose.orientation.x = np.sin((np.pi / 2.0 - angle) / 2.0)
        handle_pose.orientation.w = np.cos((np.pi / 2.0 - angle) / 2.0)

        spatula.shapes = [paddle, handle]
        spatula.poses = [paddle_pose, handle_pose]

        #this is the grasp transformation
        pos_on_handle = handle.dimensions[1] - 0.1
        inv_grasp = Transform()
        grasp = RigidGrasp()
        #really should be calculating this...
        inv_grasp.translation.y = GRIPPER_LENGTH
        inv_grasp.translation.z = pos_on_handle / 2.0
        #flip 90 degrees
        inv_grasp.rotation.z = np.sin(-1.0 * np.pi / 4.0)
        inv_grasp.rotation.w = np.cos(-1.0 * np.pi / 4.0)
        g = gt.transform_pose(transform_to_pose(inv_grasp), handle_pose)
        origin = Pose()
        origin.orientation.w = 1.0
        grasp.transform = pose_to_transform(
            gt.inverse_transform_pose(origin, g))
        grasp.touch_links = [arm[0] + '_end_effector']
        grasp.attach_link = arm[0] + '_gripper_r_finger_tip_link'
        grasp.min_approach_distance = 0
        grasp.desired_approach_distance = 0.15
        grasp.min_distance_from_surface = -1

        spat_p = copy.deepcopy(spatula)

        wtrans = Pose()
        wtrans.orientation.x = np.sin(angle / 2.0)
        wtrans.orientation.w = np.cos(angle / 2.0)
        if self.world == -1:
            wtrans.position.x = 3
            wtrans.position.y = -2.8
            wtrans.position.z = FAR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
            ss = ['far_corner']
        elif self.world == -2 or self.world == -7 or self.world == -9 or self.world == -5:
            wtrans.position.x = -1.7
            wtrans.position.y = 2
            wtrans.position.z = DOOR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
            ss = ['door_table']
        else:
            wtrans.position.x = 0.6
            wtrans.position.y = -0.3
            wtrans.position.z = CENTER_TABLE_HEIGHT + 0.02 + handle.dimensions[
                0]
            ss = ['center_table']
            if self.world == -4 or self.world == -5:
                wtrans.position.y = 0
                rot = Quaternion()
                rot.z = np.sin(np.pi / 2.0)
                rot.w = np.cos(np.pi / 2.0)
                wtrans.orientation = gt.multiply_quaternions(
                    rot, wtrans.orientation)
                if self.world == -5:
                    wtrans.position.x = 0

        for i in range(len(spat_p.poses)):
            spat_p.poses[i] = gt.transform_pose(spat_p.poses[i], wtrans)

        self.wi.add_object(spat_p)
        return ObjectType(type="SpatulaObject",
                          collision_object=spatula,
                          parameters=ss,
                          numeric_parameters=paddle.dimensions +
                          handle.dimensions + [angle]), [grasp]

    def add_plate(self):
        plate = CollisionObject()
        plate.header.frame_id = self.wi.world_frame
        plate.operation.operation = plate.operation.ADD
        plate.id = "plate"

        shape = Shape()
        shape.type = shape.CYLINDER
        shape.dimensions = [0.15, 0.04]
        if self.box_plate:
            shape.type = shape.BOX
            shape.dimensions = [0.3, 0.3, 0.04]

        pose = Pose()
        if self.world < 6 or self.world == 8 or self.world == 9 or self.world == 12:
            pose.position.x = 3.1
            pose.position.y = -3.2
            pose.position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[
                1] / 2.0
        elif self.world == 6:
            pose.position.x = 0.6  #0.894692#0.5
            pose.position.y = -0.3  #-0.468198#-0.2
            pose.position.z = CENTER_TABLE_HEIGHT + shape.dimensions[1] / 2.0
        # elif self.world == 12:
        #     pose.position.x = -1.3
        #     pose.position.y = 2
        #     pose.position.z = DOOR_TABLE_HEIGHT + 0.02 + shape.dimensions[1]/2.0
        else:
            pose.position.x = 0.5  #0.894692#0.5
            pose.position.y = 0.1
            pose.position.z = CENTER_TABLE_HEIGHT + 0.02 + shape.dimensions[
                1] / 2.0

        pose.orientation.w = 1.0

        plate.shapes.append(shape)
        plate.poses.append(pose)

        self.wi.add_object(plate)

        return plate

    def add_box(self):
        box = CollisionObject()
        box.header.frame_id = self.wi.world_frame
        box.operation.operation = box.operation.ADD
        box.id = "box"

        shape = Shape()
        shape.type = shape.BOX
        shape.dimensions = [0.1, 0.05, 0.15]

        pose = Pose()
        pose.position.x = 3.1
        pose.position.y = -3.2
        pose.position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[2] / 2.0
        pose.orientation.w = 1.0

        box.shapes.append(shape)
        box.poses.append(pose)

        self.wi.add_object(box)

        return box

    def add_barrier(self):
        box = CollisionObject()
        box.header.frame_id = self.wi.world_frame
        box.operation.operation = box.operation.ADD
        box.id = "barrier1"

        shape = Shape()
        shape.type = shape.BOX
        shape.dimensions = [0.1, 2, 1]
        box.shapes.append(shape)
        pose = Pose()
        pose.position.x = 1
        pose.position.y = -1.5
        pose.position.z = 0
        pose.orientation.w = 1.0
        box.poses.append(pose)
        self.wi.add_object(box)

        # if self.world != 12:
        #     box.id = "barrier2"
        #     box.shapes[0].dimensions = [1, 0.1, 1]
        #     box.poses[0].position.x = 1.5
        #     box.poses[0].position.y = 0
        #     self.wi.add_object(box)

    def get_goal_poses(self):
        place_pose_stamped = PoseStamped()
        place_pose_stamped.header.frame_id = self.wi.world_frame

        if abs(self.world) < 0 or (self.world == -5 or abs(self.world) % 2 == 0
                                   and abs(self.world) != 12
                                   and abs(self.world) != 4):
            #easiest goal
            x = 1
            y = -2
            z = 0.85
        else:
            #slightly harder goal
            x = -1
            y = 1
            z = 0.85

        if abs(self.world) == 12:
            x = 0
            y = -1
            z - 0.85

        if abs(self.world) == 14:
            x = 0
            y = 1
            z = 0.85

        place_pose_stamped.pose.position.x = x
        place_pose_stamped.pose.position.y = y
        place_pose_stamped.pose.position.z = z
        place_pose_stamped.pose.orientation.w = 1.0
        return [place_pose_stamped]
Esempio n. 15
0
def main():
    wi = WorldInterface()
    wi.reset()
    psi = get_planning_scene_interface()
    psi.reset()
    rospy.sleep(2.0)
Esempio n. 16
0
def main():
    rospy.init_node("touring", anonymous=True)

    sm = smach.StateMachine(outcomes=["success",
                                      "failure"])
    base_mover = base.Base()
    detector = TablewareDetection()
    pickplace = PickPlace()
    pickplace.min_open_gripper_pos = 0.0014
    poses = basic_states.poses
    world = basic_states.WorldState()
    interface = WorldInterface()
    tasks = Tasks()
    placer = SensingPlacer('right_arm')

    interface.reset()
    psi = get_planning_scene_interface()
    psi.reset()
    tasks.move_arms_to_side()

    rospy.loginfo("Creating State Machine")
    nav_states = {}
    for name, pos in poses.iteritems():
        nav_states[name] = basic_states.NavigateTo(
        world, base_mover, pos[0], pos[1], pos[2])

    detect_state = basic_states.Detect(world, detector)
    
    top_pos_place_down = (-2.15, .5, .98)
    place_down_top = basic_states.PlaceDown(world, placer,
            top_pos_place_down)

    T = {"failure":"failure"}
    with sm:
        T["success"] = "detect"
        T["failure"] = "failure"
        
        smach.StateMachine.add("move_shelf", nav_states["shelf"],
                transitions=T.copy())
        T["success"] = "choose_bowl"
        T["failure"] = "failure"


        smach.StateMachine.add("detect", detect_state,
                transitions=T.copy()
                )
        T["success"] = "move_to_pickable"
        T["failure"] = "failure"
        
        smach.StateMachine.add("choose_bowl", 
                basic_states.ChooseItem(world, "bowl"),
                transitions=T.copy()
                )
        T["success"] = "pickup"
        T["failure"] = "pickup"
        
        smach.StateMachine.add("move_to_pickable", 
                basic_states.MoveToPickable(world, base_mover ),
                transitions=T.copy()
                )
        T["success"] = "move_arms_to_side"
        T["failure"] = "move_arms_to_side_after_pickup"
        
        smach.StateMachine.add("pickup", 
                basic_states.PickUp(world, pickplace, interface),
                transitions=T.copy()
                )
        T["success"] = "detect"
        T["failure"] = "failure"
        
        smach.StateMachine.add("move_arms_to_side_after_pickup", 
                basic_states.MoveArmsToSide(world, tasks),
                transitions=T.copy()
                )
        T["success"] = "move_top_table"
        T["failure"] = "failure"
        
        smach.StateMachine.add("move_arms_to_side", 
                basic_states.MoveArmsToSide(world, tasks),
                transitions=T.copy()
                )
        T["success"] = "approach_top_table"
        T["failure"] = "failure"
        
        smach.StateMachine.add("move_top_table", 
                nav_states["table_top_edge"],
                transitions=T.copy())
        T["success"] = "place_down_top"
        T["failure"] = "failure"
        
        smach.StateMachine.add("approach_top_table", 
                basic_states.MoveToReachable(world, base_mover, top_pos_place_down),
                transitions=T.copy())
        T["success"] = "success"
        T["failure"] = "failure"
        
        smach.StateMachine.add("place_down_top", 
                place_down_top,
                transitions=T.copy())

    outcome = sm.execute()
    rospy.loginfo("Outcome: %s", outcome)
 def execute(self):
     WorldInterface().detach_and_add_back_attached_object(
         self.arm, self.object_id)
     return MotionHandleDummy()
 def execute(self):
     WorldInterface().attach_object_to_gripper(self.arm, self.object_id)
     return MotionHandleDummy()
Esempio n. 19
0
    def __init__(self, allow_vertical_tables=False, vertical_threshold=DEFAULT_VERTICAL_THRESHOLD,
                 table_search_resolution=DEFAULT_TABLE_SEARCH_RESOLUTION, 
                 table_search_max=DEFAULT_TABLE_SEARCH_MAX, table_thickness=DEFAULT_TABLE_THICKNESS,
                 tableware_labels=None, object_padding=DEFAULT_OBJECT_PADDING):

        '''
        Constructor for TablewareDetection.
        
        **Args:**

            *allow_vertical_tables (boolean):* If True will do segmentation and return even if a vertical table is 
            found.
        
            *vertical_threshold (double):* The dot product of the normal to the table plane and the z axis must be 
            greater than this for the table to be considered horizontal.
        
            *table_search_resolution (double):* If detect_objects is initially given a point for the head to look at, 
            it will search along the world x axis near that point until it founds a horizontal table.  This is the 
            resolution in meters of that search.  Resolution cannot be smaller than a centimeter.
        
            *table_search_max (double):* The maximum distance in meters from the point the search should go.  It will 
            go this far in both directions before failing.
        
            *table_thickness (double):* If the table is added to the map, it will be added with this thickness in 
            meters.  This is helpful for placing as it will occlude points that the robot sees below the table.
        
            *tableware_labels ([string]):* A set of database tags that count as "tableware".  If objects have 
            multiple tags, they are considered tableware if anyone of these tags matches.

            *object_padding (double):* If objects are added to the map, this is the padding in centimeters that they 
            will be added with.  Padding is used only for replacing voxels in the collision map.  We recommend setting 
            this high so that during pick there is no collision between the object and the voxels where it used to be.
        '''

        self.allow_vertical_tables = allow_vertical_tables
        self.vertical_threshold = vertical_threshold
        self.table_search_resolution = table_search_resolution
        self.table_search_max = table_search_max
        self.table_thickness = table_thickness
        self.tableware_labels = tableware_labels
        if not tableware_labels:
            self.tableware_labels = copy.copy(DEFAULT_TABLEWARE_LABELS)
        self.object_padding = object_padding

        self._wi = WorldInterface()
        self._head = Head()

        self._detect_srv = rospy.ServiceProxy(OBJECT_DETECTION_SERVICE, TabletopDetection)
        rospy.loginfo('Waiting for object detection service')
        self._detect_srv.wait_for_service()
        self._bounding_box_srv = rospy.ServiceProxy(FIND_CLUSTER_BOUNDING_BOX_SERVICE, FindClusterBoundingBox)
        rospy.loginfo('Waiting for bounding box service to fill pickup goal.')
        self._bounding_box_srv.wait_for_service()
        rospy.loginfo('Waiting for object segmentation service')
        self.segmentation_service = rospy.ServiceProxy(SEGMENTATION_SERVICE,TabletopSegmentation)
        rospy.loginfo('Waiting for object recognition service')
        self.recognition_service = rospy.ServiceProxy(RECOGNITION_SERVICE,TabletopObjectRecognition) 

        self._get_model_mesh_srv = rospy.ServiceProxy(GET_MODEL_MESH_SERVICE, GetModelMesh)
        self.get_model_description = rospy.ServiceProxy(GET_MODEL_DESCRIPTION_SERVICE, GetModelDescription)
        try:
            rospy.loginfo('Waiting for get model mesh service')
            self._get_model_mesh_srv.wait_for_service(5.0)   
            rospy.loginfo('Waiting for get model description service')
            self.get_model_description.wait_for_service(5.0)
        except rospy.ROSException:
            rospy.logwarn('Did not find database services.  Likely will not be able to return recognition results')
            self._get_model_mesh_srv = None
            self.get_model_description = None
        
        self._object_id = 0


        rospy.loginfo('Ready to do object detections!')
class ArmMoverWorker(threading.Thread):
    def __init__(self, arm_name):
        '''Worker class that can move or query the arm. By making several of these,
        the arm mover class can do multiple movements/queries at once.
        '''
        self._arm_name = arm_name
        threading.Thread.__init__(self)
        self.daemon = True

        self._current_handle = None
        self._task_cv = threading.Condition()

    def assign_task(self, handle):
        ''' Assign a task to this worker.

        **Args:**
            handle (MovementHandle): Handle to the movement being assigned.

        **Raises:**
            ArmNavError if already doing another task.
        '''
        self._task_cv.acquire()
        try:
            if self._current_handle is not None:
                raise ArmNavError(
                    'Movement requested while other movement still runnning!')
            self._current_handle = handle
            self._task_cv.notify()
        finally:
            self._task_cv.release()

    def run(self):
        '''Loop and wait to be assigned a task.
        '''
        self._initialize()

        while not rospy.is_shutdown():
            self._task_cv.acquire()
            while (self._current_handle is None) and (not rospy.is_shutdown()):
                self._task_cv.wait(0.01)
            self._task_cv.release()

            if rospy.is_shutdown():
                break

            try:
                rospy.logdebug('ArmMoverWorker starting task: %s(%s)' %
                               (str(self._current_handle.task_func),
                                str(self._current_handle.task_args)))
                self._current_handle.task_func(self,
                                               *self._current_handle.task_args)
            except Exception as e:
                self._current_handle._add_error(e)

            self._task_cv.acquire()
            self._current_handle._set_in_progress(False)
            self._current_handle = None
            self._task_cv.release()

    def _initialize(self):
        '''Connect up all services and action clients.
        '''
        self._world_interface = WorldInterface()
        self._controller_manager = ControllerManagerClient()
        if self._arm_name in ['right_arm', 'left_arm']:
            self._group_name = self._arm_name
            self._planner = ArmPlanner(self._arm_name)
            self._hand_description = HandDescription(self._arm_name)

            arm_abbr = self._arm_name[0]
            self._joint_controller = '%s_arm_controller' % arm_abbr
            self._cartesian_controller = '%s_cart' % arm_abbr

            self._move_arm_client = actionlib.SimpleActionClient(
                'move_%s' % self._arm_name,
                arm_navigation_msgs.msg.MoveArmAction)
            self._wait_for_action_server(self._move_arm_client)

            #jt_action_name = '/%s_arm_controller/joint_trajectory_action' % arm_abbr
            #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, JointTrajectoryAction)
            #self._wait_for_action_server(self._joint_trajectory_client)

            jt_action_name = '/%s_arm_controller/follow_joint_trajectory' % arm_abbr
            self._joint_trajectory_client = actionlib.SimpleActionClient(
                jt_action_name, FollowJointTrajectoryAction)
            self._wait_for_action_server(self._joint_trajectory_client)

            self._cart_interface = CartesianControllerInterface(self._arm_name)
        elif self._arm_name == 'both':
            self._joint_controller = 'two_arm_controller'
            #jt_two_arm_action_name = '/two_arm_controller/joint_trajectory_action'
            #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, JointTrajectoryAction)
            jt_two_arm_action_name = '/two_arm_controller/follow_joint_trajectory'
            self._joint_trajectory_client = actionlib.SimpleActionClient(
                jt_two_arm_action_name, FollowJointTrajectoryAction)
            self._wait_for_action_server(self._joint_trajectory_client)
        else:
            raise ValueError('Invalid arm name for worker: %s' %
                             self._arm_name)

    def _wait_for_action_server(self,
                                action_client,
                                max_wait=10.,
                                wait_increment=0.1):
        '''
        Wait for the action server corresponding to this action client to be ready.
        
        **Args:**
            **action_client (actionlib.SimpleActionClient):** client for an action

            *max_wait (float):* Total number of seconds to wait before failing

            *wait_increment (float):* Number or seconds to wait between checks to rospy.is_shutdown()
        
        **Raises:**

            **exceptions.ActionFailedError:** if max_wait seconds elapses without server being ready.
        '''
        for ii in range(int(round(max_wait / wait_increment))):
            if rospy.is_shutdown():
                raise ActionFailedError(
                    'Could not connect to action server (rospy shutdown requested)'
                )

            if action_client.wait_for_server(rospy.Duration(wait_increment)):
                return
        raise ActionFailedError(
            'Could not connect to action server (timeout exceeeded)')

    def _move_to_goal(self,
                      goal,
                      try_hard=False,
                      collision_aware_goal=True,
                      planner_timeout=5.,
                      ordered_collisions=None,
                      bounds=None,
                      planner_id='',
                      cartesian_timeout=5.0):
        '''
        Move the specified arm to the given goal.
        
        This function should only get called indirectly by calling move_arm_to_goal.
        '''
        try:
            reached_goal = False

            # check which kind of goal we were given
            if type(goal) == PoseStamped:
                goal_is_pose = True
            elif type(goal) == JointState:
                goal_is_pose = False
            else:
                raise ArmNavError('Invalid goal type %s' % str(type(goal)))

            rospy.loginfo('Attempting to use move arm to get to goal')
            try:
                self._move_to_goal_using_move_arm(goal, planner_timeout,
                                                  ordered_collisions, bounds,
                                                  planner_id)
                reached_goal = True
            except ArmNavError as e:
                self._current_handle._add_error(e)
                rospy.loginfo('Move arm failed: %s' % str(e))

            if (not reached_goal) and try_hard:
                rospy.loginfo('Attempting to move directly to goal')
                try:
                    self._move_to_goal_directly(goal,
                                                planner_timeout,
                                                bounds,
                                                collision_aware=True)
                    reached_goal = True
                except ArmNavError as e:
                    self._current_handle._add_error(e)
                    rospy.loginfo('Collision aware IK failed: %s' % str(e))

            if (not reached_goal) and try_hard and (
                    not collision_aware_goal) and goal_is_pose:
                rospy.loginfo(
                    'Attempting to move directly to goal, ignoring collisions')
                try:
                    self._move_to_goal_directly(goal,
                                                planner_timeout,
                                                bounds,
                                                collision_aware=False)
                    reached_goal = True
                except ArmNavError as e:
                    self._current_handle._add_error(e)
                    rospy.loginfo('Non-collision aware IK failed: %s' % str(e))

            if (not reached_goal) and try_hard and (
                    not collision_aware_goal) and goal_is_pose:
                rospy.loginfo(
                    'Attempting to use cartesian controller to move towards goal'
                )
                try:
                    self._move_to_goal_using_cartesian_control(
                        goal, cartesian_timeout, bounds)
                    reached_goal = True
                except ArmNavError as e:
                    self._current_handle._add_error(e)
        finally:
            self._current_handle._set_reached_goal(reached_goal)

    def _move_into_joint_limits(self):
        '''
        Moves the arm into joint limits if it is outside of them.

        This cannot be a collision free move but it is almost always very very short.

        **Raises:**

            **exceptions.ArmNavError:** if IK fails

            **rospy.ServiceException:** if there is a problem calling the IK service

            **ValueError:** if the goal type is wrong
        '''
        joint_state = self._planner.get_closest_joint_state_in_limits()
        self._move_to_goal_directly(joint_state, trajectory_time=0.5)
        self._current_handle._set_reached_goal(True)

    def _move_out_of_collision(self, move_mag=0.3, num_tries=100):
        '''
        Tries to find a small movement that will take the arm out of collision.

        **Args:**

            *move_mag (float):* Max magnitude in radians of movement for each joint.
            
            *num_tries (int):* Number of random joint angles to try before giving up.
            
        **Returns:**
            succeeded (boolean): True if arm was sucessfully moved out of collision.
        '''
        req = GetStateValidityRequest()
        req.robot_state = self.world_interface.get_robot_state()
        req.check_collisions = True
        req.check_path_constraints = False
        req.check_joint_limits = False
        req.group_name = self._arm_name
        res = self._planner.get_state_validity_service(req)
        if res.error_code.val == ArmNavErrorCodes.SUCCESS:
            rospy.logdebug('Current state not in collision')
            return False

        joint_state = self._planner.arm_joint_state()
        current_joint_position = np.array(joint_state.position)
        for ii in range(num_tries):
            joint_position = current_joint_position + np.random.uniform(
                -move_mag, move_mag, (len(joint_state.position), ))
            joint_state.position = list(joint_position)
            trajectory_tools.set_joint_state_in_robot_state(
                joint_state, req.robot_state)
            res = self._planner.get_state_validity_service(req)
            in_collision = (res.error_code.val != ArmNavErrorCodes.SUCCESS)
            rospy.logdebug('%s in collision: %s' %
                           (str(joint_position), str(in_collision)))
            if not in_collision:
                self._move_to_goal_directly(joint_state,
                                            None,
                                            None,
                                            collision_aware=False)
                self._current_handle._set_reached_goal(True)
        self._current_handle._set_reached_goal(False)

    def _call_action(self, action_client, goal):
        '''
        Call an action and wait for it to complete.
        
        **Returns:** 
            Result of action.

        **Raises:** 
            
            **exceptions.ArmNavError:** if action fails.
        '''
        action_client.send_goal(goal)
        gs = actionlib_msgs.msg.GoalStatus()

        r = rospy.Rate(100)
        while True:
            if self._current_handle._get_cancel_requested():
                raise ActionFailedError('Preempted (cancel requested)')
            state = action_client.get_state()
            if state in [gs.PENDING, gs.ACTIVE, gs.PREEMPTING, gs.RECALLING]:
                # action is still going
                pass
            elif state in [gs.PREEMPTED, gs.REJECTED, gs.RECALLED, gs.LOST]:
                raise ArmNavError('Action call failed (%d)!' % (state, ))
            elif state in [gs.SUCCEEDED, gs.ABORTED]:
                return action_client.get_result()
            r.sleep()

    def _move_to_goal_directly(self,
                               goal,
                               planner_timeout=5.0,
                               bounds=None,
                               collision_aware=True,
                               trajectory_time=5.0):
        '''
        Move directly to the goal.
        
        No planning, just interpolated joint positions.
        
        If goal is a PoseStamped, does IK to find joint positions to put the end effector in that pose.
        Then executes a trajectory where the only point is the goal joint positions.
        
        Note: planner_timeout, collision_aware and bounds only apply to the IK, and so are not used when
        the goal is already a JointState

        **Raises:**

            **exceptions.ArmNavError:** if IK fails

            **rospy.ServiceException:** if there is a problem calling the IK service

            **ValueError:** if the goal type is wrong
        '''
        if type(goal) == JointState:
            joint_state = goal
        elif type(goal) == PoseStamped:
            ik_res = self._planner.get_ik(goal,
                                          collision_aware=collision_aware,
                                          starting_state=None,
                                          seed_state=None,
                                          timeout=planner_timeout)
            if not ik_res.error_code.val == ArmNavErrorCodes.SUCCESS:
                raise ArmNavError('Unable to get IK for pose',
                                  ik_res.error_code)
            joint_state = ik_res.solution.joint_state
        else:
            raise ValueError('Invalid goal type: %s' % str(type(goal)))

        trajectory = JointTrajectory()
        trajectory.joint_names = self._planner.joint_names
        jtp = JointTrajectoryPoint()
        jtp.positions = joint_state.position
        jtp.time_from_start = rospy.Duration(trajectory_time)
        trajectory.points.append(jtp)
        self._execute_joint_trajectory(trajectory)

        # should actually check this...
        self._current_handle._set_reached_goal(True)

    def _move_to_goal_using_move_arm(self,
                                     goal,
                                     planner_timeout,
                                     ordered_collisions,
                                     bounds,
                                     planner_id=''):
        '''
        Try using the MoveArm action to get to the goal.
        '''
        self._controller_manager.switch_controllers(
            start_controllers=[self._joint_controller])
        current_state = self._world_interface.get_robot_state()
        link_name = self._hand_description.hand_frame

        if type(goal) == JointState:
            mp_request = conversions.joint_state_to_motion_plan_request(
                goal,
                link_name,
                self._group_name,
                current_state,
                timeout=planner_timeout,
                bounds=bounds,
                planner_id=planner_id)
        elif type(goal) == PoseStamped:
            mp_request = conversions.pose_stamped_to_motion_plan_request(
                goal,
                link_name,
                self._group_name,
                starting_state=current_state,
                timeout=planner_timeout,
                bounds=bounds,
                planner_id=planner_id)
        else:
            raise ValueError('Invalid goal type %s' % str(type(goal)))

        ma_goal = arm_navigation_msgs.msg.MoveArmGoal()
        ma_goal.motion_plan_request = mp_request
        if ordered_collisions:
            ma_goal.operations = ordered_collisions
        ma_goal.planner_service_name = DEFAULT_PLANNER_SERVICE_NAME

        # send goal to move arm
        res = self._call_action(self._move_arm_client, ma_goal)
        if res == None:
            raise ArmNavError('MoveArm failed without setting result')
        elif not res.error_code.val == ArmNavErrorCodes.SUCCESS:
            raise ArmNavError('MoveArm failed', res.error_code)
        else:
            self._current_handle._set_reached_goal(True)

    def _move_to_goal_using_cartesian_control(self, goal, timeout, bounds):
        if type(goal) == PoseStamped:
            pose_stamped = goal
        else:
            raise ValueError('Invalid goal type for cartesian control: %s' %
                             str(type(goal)))
        self._controller_manager.switch_controllers(
            start_controllers=[self._cartesian_controller])
        self._cart_interface.set_desired_pose(pose_stamped)
        start_time = time.time()
        r = rospy.Rate(100)
        try:
            print 'Current handle'
            print self._current_handle._get_cancel_requested()
            while not self._current_handle._get_cancel_requested():
                print 'Inside while loop'
                # ignores bounds right now and uses defaults... fixme
                if self._cart_interface.reached_desired_pose():
                    self._current_handle._set_reached_goal(True)
                    return
                if (time.time() - start_time) > timeout:
                    raise ArmNavError(
                        'Cartesian control move time out',
                        ArmNavErrorCodes(ArmNavErrorCodes.TIMED_OUT))
                r.sleep()
        finally:
            self._cart_interface.cancel_desired_pose()

    def _execute_joint_trajectory(self, trajectory):
        '''
        Executes the given trajectory, switching controllers first if needed.

        **Args:**

            **trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to execute.
        '''
        self._controller_manager.switch_controllers(
            start_controllers=[self._joint_controller])
        #goal = JointTrajectoryGoal()
        goal = FollowJointTrajectoryGoal()
        goal.trajectory = trajectory
        jt_res = self._call_action(self._joint_trajectory_client, goal)

        # should actually check this
        self._current_handle._set_reached_goal(True)

        return jt_res

    def _execute_two_arm_trajectory(self, trajectory):
        '''
        Executes the given trajectory, switching controllers first if needed.

        **Args:**

        **trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to execute.
        '''
        self._controller_manager.switch_controllers(
            start_controllers=[self._joint_controller])
        #goal = JointTrajectoryGoal()
        goal = FollowJointTrajectoryGoal()
        goal.trajectory = trajectory
        jt_res = self._call_action(self._joint_trajectory_client, goal)

        # should actually check this
        self._current_handle._set_reached_goal(True)

        return jt_res
Esempio n. 21
0
	def execute(self, target=PoseStamped(), blocking=True):
		ah = ActionHandle("pick_up", "dummy", "", blocking)
		rospy.loginfo("Picking up object...")
		
		#ah = self.actions.grasp_object(target, blocking)
		wi = WorldInterface()
		wi.reset_attached_objects()
		wi.reset_collision_objects()
		
		# add table
		table_extent = (2.0, 2.0, 1.0)
		table_pose = conversions.create_pose_stamped([ -0.5 - table_extent[0]/2.0, 0 ,table_extent[2]/2.0 ,0,0,0,1], 'base_link')
		wi.add_collision_box(table_pose, table_extent, "table")
		
		mp = MotionPlan()
		mp += CallFunction(sss.move, 'torso','front')
		#mp += MoveArm('arm',['pregrasp'])
		mp += CallFunction(sss.move, 'sdh','cylopen', False)
		
		# OpenIssues:
		# - where to place the sdh_grasp_link to grasp the object located at target?
		# - there is no orientation in the target? -> hardcoded taken from pregrasp
		grasp_pose = PoseStamped()
		grasp_pose = deepcopy(target)
		grasp_pose.header.stamp = rospy.Time.now()
		grasp_pose.pose.orientation.x = 0.220
		grasp_pose.pose.orientation.y = 0.670
		grasp_pose.pose.orientation.z = -0.663
		grasp_pose.pose.orientation.w = -0.253
		mp += MoveArm('arm',[grasp_pose,['sdh_grasp_link']], seed = 'pregrasp')
		
		mp += CallFunction(sss.move, 'sdh','cylclosed', True)
		
		
		#ah = self.actions.lift_object(target, blocking)
		lift_pose = PoseStamped()
		lift_pose = deepcopy(target)
		lift_pose.header.stamp = rospy.Time.now()
		lift_pose.pose.position.z += 0.08
		lift_pose.pose.orientation.x = 0.220
		lift_pose.pose.orientation.y = 0.670
		lift_pose.pose.orientation.z = -0.663
		lift_pose.pose.orientation.w = -0.253
		mp += MoveArm('arm',[lift_pose,['sdh_grasp_link']])
		
		
		
		#ah = self.actions.retrieve_object(target, blocking)
		#mp += MoveArm('arm',['pregrasp'])
		mp += MoveArm('arm',['wavein'])
		
		
		
		
		planning_res = mp.plan(2)
		print planning_res

		if planning_res.success:
			for e in mp.execute():
				exec_res = e.wait()
				print exec_res
				if not exec_res.success:
					#rospy.logerr("Execution of MotionExecutable %s failed", e.name)
					ah.set_failed(4)
					break
			ah.set_succeeded()
		else:
			rospy.logerr("Planning failed")
			ah.set_failed(4)
		
		return ah
Esempio n. 22
0
#!/usr/bin/env python
import roslib
roslib.load_manifest('cob_arm_navigation_python')

import rospy
from cob_arm_navigation_python.MotionPlan import *
from cob_arm_navigation_python.MoveArm import *
from cob_arm_navigation_python.MoveHand import *
from tf.transformations import *
from copy import deepcopy
rospy.init_node("test")

from pr2_python.world_interface import WorldInterface

wi = WorldInterface()
grasp_pose = PoseStamped()
#grasp_pose.header.frame_id = "base_link"
grasp_pose.header.frame_id = "odom_combined"
grasp_pose.header.stamp = rospy.Time()
grasp_pose.pose = wi.collision_object('milk').poses[0]
p = grasp_pose.pose.position
#p.x, p.y, p.z = [-0.8,-0.2,0.9]
p.x += 0.02
p.y += 0.02
p.z += 0.02
o = grasp_pose.pose.orientation
o.x, o.y, o.z, o.w = quaternion_from_euler(-1.581, -0.019, 2.379)

lift_pose = deepcopy(grasp_pose)
p = lift_pose.pose.position
p.x += 0.0
Esempio n. 23
0
class ArmMoverWorker(threading.Thread):
    def __init__(self, arm_name):
        '''Worker class that can move or query the arm. By making several of these,
        the arm mover class can do multiple movements/queries at once.
        '''
        self._arm_name = arm_name
        threading.Thread.__init__(self)
        self.daemon = True

        self._current_handle = None
        self._task_cv = threading.Condition()


    def assign_task(self, handle):
        ''' Assign a task to this worker.

        **Args:**
            handle (MovementHandle): Handle to the movement being assigned.

        **Raises:**
            ArmNavError if already doing another task.
        '''
        self._task_cv.acquire()
        try:
            if self._current_handle is not None:
                raise ArmNavError('Movement requested while other movement still runnning!')
            self._current_handle = handle
            self._task_cv.notify()
        finally:
            self._task_cv.release()
    
    def run(self):
        '''Loop and wait to be assigned a task.
        '''
        self._initialize()

        while not rospy.is_shutdown():
            self._task_cv.acquire()
            while (self._current_handle is None) and (not rospy.is_shutdown()):
                self._task_cv.wait(0.01)
            self._task_cv.release()

            if rospy.is_shutdown():
                break

            try:
                rospy.logdebug('ArmMoverWorker starting task: %s(%s)' % (
                        str(self._current_handle.task_func), str(self._current_handle.task_args)))
                self._current_handle.task_func(self, *self._current_handle.task_args)
            except Exception as e:
                self._current_handle._add_error(e)
            
            self._task_cv.acquire()
            self._current_handle._set_in_progress(False)
            self._current_handle = None
            self._task_cv.release()

    def _initialize(self):
        '''Connect up all services and action clients.
        '''
        self._world_interface = WorldInterface()
        self._controller_manager = ControllerManagerClient()
        if self._arm_name in ['right_arm', 'left_arm']:
            self._group_name = self._arm_name
            self._planner = ArmPlanner(self._arm_name)
            self._hand_description = HandDescription(self._arm_name)

            arm_abbr = self._arm_name[0]
            self._joint_controller = '%s_arm_controller' % arm_abbr                
            self._cartesian_controller = '%s_cart' % arm_abbr
            
            self._move_arm_client = actionlib.SimpleActionClient(
                'move_%s' % self._arm_name, arm_navigation_msgs.msg.MoveArmAction)
            self._wait_for_action_server(self._move_arm_client)
            
            #jt_action_name = '/%s_arm_controller/joint_trajectory_action' % arm_abbr
            #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, JointTrajectoryAction)
            #self._wait_for_action_server(self._joint_trajectory_client)
            
            jt_action_name = '/%s_arm_controller/follow_joint_trajectory' % arm_abbr
            self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, FollowJointTrajectoryAction)
            self._wait_for_action_server(self._joint_trajectory_client)
            
            self._cart_interface = CartesianControllerInterface(self._arm_name)
        elif self._arm_name == 'both':
            self._joint_controller = 'two_arm_controller'
            #jt_two_arm_action_name = '/two_arm_controller/joint_trajectory_action'
            #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, JointTrajectoryAction)
            jt_two_arm_action_name = '/two_arm_controller/follow_joint_trajectory'
            self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, FollowJointTrajectoryAction)
            self._wait_for_action_server(self._joint_trajectory_client)
        else:
            raise ValueError('Invalid arm name for worker: %s' % self._arm_name)

    def _wait_for_action_server(self, action_client, max_wait=10., wait_increment=0.1):
        '''
        Wait for the action server corresponding to this action client to be ready.
        
        **Args:**
            **action_client (actionlib.SimpleActionClient):** client for an action

            *max_wait (float):* Total number of seconds to wait before failing

            *wait_increment (float):* Number or seconds to wait between checks to rospy.is_shutdown()
        
        **Raises:**

            **exceptions.ActionFailedError:** if max_wait seconds elapses without server being ready.
        '''
        for ii in range(int(round(max_wait / wait_increment))):
            if rospy.is_shutdown():
                raise ActionFailedError('Could not connect to action server (rospy shutdown requested)')
            
            if action_client.wait_for_server(rospy.Duration(wait_increment)):
                return
        raise ActionFailedError('Could not connect to action server (timeout exceeeded)')

    def _move_to_goal(self, goal,
                try_hard=False, collision_aware_goal=True, planner_timeout=5., ordered_collisions=None,
                bounds=None, planner_id='', cartesian_timeout=5.0):
        '''
        Move the specified arm to the given goal.
        
        This function should only get called indirectly by calling move_arm_to_goal.
        '''
        try:
            reached_goal = False
            
            # check which kind of goal we were given
            if type(goal) == PoseStamped:
                goal_is_pose = True
            elif type(goal) == JointState:
                goal_is_pose = False
            else:
                raise ArmNavError('Invalid goal type %s' % str(type(goal)))
    
            rospy.loginfo('Attempting to use move arm to get to goal')
            try:
                self._move_to_goal_using_move_arm(
                    goal, planner_timeout, ordered_collisions, bounds, planner_id)
                reached_goal = True
            except ArmNavError as e:
                self._current_handle._add_error(e)
                rospy.loginfo('Move arm failed: %s' % str(e))
    
            if (not reached_goal) and try_hard:
                rospy.loginfo('Attempting to move directly to goal')
                try:
                    self._move_to_goal_directly(goal, planner_timeout, bounds, collision_aware=True)
                    reached_goal = True
                except ArmNavError as e:
                    self._current_handle._add_error(e)
                    rospy.loginfo('Collision aware IK failed: %s' % str(e))
    
            if (not reached_goal) and try_hard and (not collision_aware_goal) and goal_is_pose:
                rospy.loginfo('Attempting to move directly to goal, ignoring collisions')
                try:
                    self._move_to_goal_directly(goal, planner_timeout, bounds, collision_aware=False)
                    reached_goal = True
                except ArmNavError as e:
                    self._current_handle._add_error(e)
                    rospy.loginfo('Non-collision aware IK failed: %s' % str(e))
    
            if (not reached_goal) and try_hard and (not collision_aware_goal) and goal_is_pose:
                rospy.loginfo('Attempting to use cartesian controller to move towards goal')
                try:
                    self._move_to_goal_using_cartesian_control(goal, cartesian_timeout, bounds)
                    reached_goal = True
                except ArmNavError as e:
                    self._current_handle._add_error(e)
        finally:
            self._current_handle._set_reached_goal(reached_goal)


    def _move_into_joint_limits(self):
        '''
        Moves the arm into joint limits if it is outside of them.

        This cannot be a collision free move but it is almost always very very short.

        **Raises:**

            **exceptions.ArmNavError:** if IK fails

            **rospy.ServiceException:** if there is a problem calling the IK service

            **ValueError:** if the goal type is wrong
        '''
        joint_state = self._planner.get_closest_joint_state_in_limits()
        self._move_to_goal_directly(joint_state, trajectory_time=0.5)
        self._current_handle._set_reached_goal(True)

    def _move_out_of_collision(self, move_mag=0.3, num_tries=100):
        '''
        Tries to find a small movement that will take the arm out of collision.

        **Args:**

            *move_mag (float):* Max magnitude in radians of movement for each joint.
            
            *num_tries (int):* Number of random joint angles to try before giving up.
            
        **Returns:**
            succeeded (boolean): True if arm was sucessfully moved out of collision.
        '''
        req = GetStateValidityRequest()
        req.robot_state = self.world_interface.get_robot_state()
        req.check_collisions = True
        req.check_path_constraints = False
        req.check_joint_limits = False
        req.group_name = self._arm_name
        res = self._planner.get_state_validity_service(req)
        if res.error_code.val == ArmNavErrorCodes.SUCCESS:
            rospy.logdebug('Current state not in collision')
            return False

        joint_state = self._planner.arm_joint_state()
        current_joint_position = np.array(joint_state.position)
        for ii in range(num_tries):
            joint_position = current_joint_position + np.random.uniform(
                -move_mag, move_mag, (len(joint_state.position),))
            joint_state.position = list(joint_position)
            trajectory_tools.set_joint_state_in_robot_state(joint_state, req.robot_state)
            res = self._planner.get_state_validity_service(req)
            in_collision = (res.error_code.val != ArmNavErrorCodes.SUCCESS)
            rospy.logdebug('%s in collision: %s' % (str(joint_position), str(in_collision)))
            if not in_collision:
                self._move_to_goal_directly(joint_state, None, None, collision_aware=False)
                self._current_handle._set_reached_goal(True)
        self._current_handle._set_reached_goal(False)
    
    def _call_action(self, action_client, goal):
        '''
        Call an action and wait for it to complete.
        
        **Returns:** 
            Result of action.

        **Raises:** 
            
            **exceptions.ArmNavError:** if action fails.
        '''
        action_client.send_goal(goal)
        gs = actionlib_msgs.msg.GoalStatus()

        r = rospy.Rate(100)
        while True:
            if self._current_handle._get_cancel_requested():
                raise ActionFailedError('Preempted (cancel requested)')
            state = action_client.get_state()
            if state in [gs.PENDING, gs.ACTIVE, gs.PREEMPTING, gs.RECALLING]:
                # action is still going
                pass
            elif state in [gs.PREEMPTED, gs.REJECTED, gs.RECALLED, gs.LOST]:
                raise ArmNavError('Action call failed (%d)!' % (state,))
            elif state in [gs.SUCCEEDED, gs.ABORTED]:
                return action_client.get_result() 
            r.sleep()

    def _move_to_goal_directly(self, goal, planner_timeout=5.0, bounds=None,
            collision_aware=True, trajectory_time=5.0):
        '''
        Move directly to the goal.
        
        No planning, just interpolated joint positions.
        
        If goal is a PoseStamped, does IK to find joint positions to put the end effector in that pose.
        Then executes a trajectory where the only point is the goal joint positions.
        
        Note: planner_timeout, collision_aware and bounds only apply to the IK, and so are not used when
        the goal is already a JointState

        **Raises:**

            **exceptions.ArmNavError:** if IK fails

            **rospy.ServiceException:** if there is a problem calling the IK service

            **ValueError:** if the goal type is wrong
        '''
        if type(goal) == JointState:
            joint_state = goal
        elif type(goal) == PoseStamped:
            ik_res = self._planner.get_ik(goal, collision_aware=collision_aware, starting_state=None,
                seed_state=None, timeout=planner_timeout)
            if not ik_res.error_code.val == ArmNavErrorCodes.SUCCESS:
                raise ArmNavError('Unable to get IK for pose', ik_res.error_code)
            joint_state = ik_res.solution.joint_state
        else:
            raise ValueError('Invalid goal type: %s' % str(type(goal)))
        
        trajectory = JointTrajectory()
        trajectory.joint_names = self._planner.joint_names
        jtp = JointTrajectoryPoint()
        jtp.positions = joint_state.position
        jtp.time_from_start = rospy.Duration(trajectory_time)
        trajectory.points.append(jtp)
        self._execute_joint_trajectory(trajectory)

        # should actually check this...
        self._current_handle._set_reached_goal(True)
        
    def _move_to_goal_using_move_arm(self, goal, planner_timeout, ordered_collisions, bounds, planner_id=''):
        '''
        Try using the MoveArm action to get to the goal.
        '''
        self._controller_manager.switch_controllers(start_controllers=[self._joint_controller])
        current_state = self._world_interface.get_robot_state()
        link_name = self._hand_description.hand_frame
        
        if type(goal) == JointState:
            mp_request = conversions.joint_state_to_motion_plan_request(
                goal, link_name, self._group_name, current_state,
                timeout=planner_timeout, bounds=bounds, planner_id=planner_id)
        elif type(goal) == PoseStamped:
            mp_request = conversions.pose_stamped_to_motion_plan_request(
                goal, link_name, self._group_name, starting_state=current_state, 
                timeout=planner_timeout, bounds=bounds, planner_id=planner_id)
        else:
            raise ValueError('Invalid goal type %s' % str(type(goal)))
        
        ma_goal = arm_navigation_msgs.msg.MoveArmGoal()
        ma_goal.motion_plan_request = mp_request
        if ordered_collisions:
            ma_goal.operations = ordered_collisions
        ma_goal.planner_service_name = DEFAULT_PLANNER_SERVICE_NAME
        
        # send goal to move arm
        res = self._call_action(self._move_arm_client, ma_goal)
        if res == None:
            raise ArmNavError('MoveArm failed without setting result')
        elif not res.error_code.val == ArmNavErrorCodes.SUCCESS:
            raise ArmNavError('MoveArm failed', res.error_code)
        else:
            self._current_handle._set_reached_goal(True)

    def _move_to_goal_using_cartesian_control(self, goal, timeout, bounds):
        if type(goal) == PoseStamped:
            pose_stamped = goal
        else:
            raise ValueError('Invalid goal type for cartesian control: %s' % str(type(goal)))
        self._controller_manager.switch_controllers(start_controllers=[self._cartesian_controller])
        self._cart_interface.set_desired_pose(pose_stamped)
        start_time = time.time()
        r = rospy.Rate(100)
        try:
            print 'Current handle'
            print self._current_handle._get_cancel_requested()
            while not self._current_handle._get_cancel_requested():
                print 'Inside while loop'
                # ignores bounds right now and uses defaults... fixme
                if self._cart_interface.reached_desired_pose():
                    self._current_handle._set_reached_goal(True)
                    return
                if (time.time() - start_time) > timeout:
                    raise ArmNavError('Cartesian control move time out', 
                                      ArmNavErrorCodes(ArmNavErrorCodes.TIMED_OUT))
                r.sleep()
        finally:
            self._cart_interface.cancel_desired_pose()
                                   
    def _execute_joint_trajectory(self, trajectory):
        '''
        Executes the given trajectory, switching controllers first if needed.

        **Args:**

            **trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to execute.
        '''
        self._controller_manager.switch_controllers(start_controllers=[self._joint_controller])
        #goal = JointTrajectoryGoal()
        goal = FollowJointTrajectoryGoal()
        goal.trajectory = trajectory
        jt_res = self._call_action(self._joint_trajectory_client, goal)

        # should actually check this
        self._current_handle._set_reached_goal(True)

        return jt_res
    
    def _execute_two_arm_trajectory(self, trajectory):
        '''
        Executes the given trajectory, switching controllers first if needed.

        **Args:**

        **trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to execute.
        '''
        self._controller_manager.switch_controllers(start_controllers=[self._joint_controller])
        #goal = JointTrajectoryGoal()
        goal = FollowJointTrajectoryGoal()
        goal.trajectory = trajectory
        jt_res = self._call_action(self._joint_trajectory_client, goal)

        # should actually check this
        self._current_handle._set_reached_goal(True)
        
        return jt_res
Esempio n. 24
0
class TablewareDetection:
    '''
    Calls tabletop detection allowing the user a lot of (optional) control over when and how objects are added
    to the collision map.  The usual use of this class is to instantiate an instance and then use detect_objects
    to find objects on a table in front of the robot:
    detector = TablewareDetection()
    res = detector.detect_objects([optional arguments])
    
    This class is a wrapper around the tabletop_object_detector, which assumes that the robot is standing in 
    front of a table on which objects are positioned at least 3cm apart.  It first locates the principle 
    horizontal plane in the image and then segments clusters above that plane based on Euclidean distance.  
    It runs those clusters through the object recognition pipeline and returns them in the form of pickup goals.
    
    The attributes of this class can all be changed by passing in different defaults although the default settings 
    should work well in most cases.  However, most of the parameters pertinent to detection must be changed 
    service side.  Look at the tabletop_object_detector package for that.
    
    Many of the parameters in this class are for working with "vertical tables".
    The detector finds the "principle plane" in the image.  Unfortunately, this plane may not be horizontal.  
    In general, it is best if any detection that does not return a table that is approximately perpendicular to 
    the z axis in the world is considered as failed.  If a point for the head to look at is passed to 
    detect_objects, the robot will move its head trying to find a horizontal table.
    
    **Attributes:**

        **allow_vertical_tables (boolean):** If True will do segmentation and return even if a vertical table is found.
        
        **vertical_threshold (double):** The dot product of the normal to the table plane and the z axis must be greater
        than this for the table to be considered horizontal.
        
        **table_search_resolution (double):** If detect_objects is initially given a point for the head to look at, it 
        will search along the world x axis near that point until it founds a horizontal table.  This is the 
        resolution in meters of that search.  Resolution cannot be smaller than a centimeter.
        
        **table_search_max (double):** The maximum distance in meters from the point the search should go.  It will go 
        this far in both directions before failing.
        
        **table_thickness (double):** If the table is added to the map, it will be added with this thickness in meters.
        This is helpful for placing as it will occlude points that the robot sees below the table.
        
        **tableware_labels ([string]):** A set of database tags that count as "tableware".  If objects have 
        multiple tags, they are considered tableware if anyone of these tags matches.

        **object_padding (double):** If objects are added to the map, this is the padding in centimeters that they will 
        be added with.  Padding is used only for replacing voxels in the collision map.  We recommend setting 
        this high so that during pick there is no collision between the object and the voxels where it used to be.
        
        **get_model_description (rospy.ServiceProxy):** Service for getting the description of a model from the model 
        ID.  See the household_database_msgs documentation for more information.
    '''

    def __init__(self, allow_vertical_tables=False, vertical_threshold=DEFAULT_VERTICAL_THRESHOLD,
                 table_search_resolution=DEFAULT_TABLE_SEARCH_RESOLUTION, 
                 table_search_max=DEFAULT_TABLE_SEARCH_MAX, table_thickness=DEFAULT_TABLE_THICKNESS,
                 tableware_labels=None, object_padding=DEFAULT_OBJECT_PADDING):

        '''
        Constructor for TablewareDetection.
        
        **Args:**

            *allow_vertical_tables (boolean):* If True will do segmentation and return even if a vertical table is 
            found.
        
            *vertical_threshold (double):* The dot product of the normal to the table plane and the z axis must be 
            greater than this for the table to be considered horizontal.
        
            *table_search_resolution (double):* If detect_objects is initially given a point for the head to look at, 
            it will search along the world x axis near that point until it founds a horizontal table.  This is the 
            resolution in meters of that search.  Resolution cannot be smaller than a centimeter.
        
            *table_search_max (double):* The maximum distance in meters from the point the search should go.  It will 
            go this far in both directions before failing.
        
            *table_thickness (double):* If the table is added to the map, it will be added with this thickness in 
            meters.  This is helpful for placing as it will occlude points that the robot sees below the table.
        
            *tableware_labels ([string]):* A set of database tags that count as "tableware".  If objects have 
            multiple tags, they are considered tableware if anyone of these tags matches.

            *object_padding (double):* If objects are added to the map, this is the padding in centimeters that they 
            will be added with.  Padding is used only for replacing voxels in the collision map.  We recommend setting 
            this high so that during pick there is no collision between the object and the voxels where it used to be.
        '''

        self.allow_vertical_tables = allow_vertical_tables
        self.vertical_threshold = vertical_threshold
        self.table_search_resolution = table_search_resolution
        self.table_search_max = table_search_max
        self.table_thickness = table_thickness
        self.tableware_labels = tableware_labels
        if not tableware_labels:
            self.tableware_labels = copy.copy(DEFAULT_TABLEWARE_LABELS)
        self.object_padding = object_padding

        self._wi = WorldInterface()
        self._head = Head()

        self._detect_srv = rospy.ServiceProxy(OBJECT_DETECTION_SERVICE, TabletopDetection)
        rospy.loginfo('Waiting for object detection service')
        self._detect_srv.wait_for_service()
        self._bounding_box_srv = rospy.ServiceProxy(FIND_CLUSTER_BOUNDING_BOX_SERVICE, FindClusterBoundingBox)
        rospy.loginfo('Waiting for bounding box service to fill pickup goal.')
        self._bounding_box_srv.wait_for_service()
        rospy.loginfo('Waiting for object segmentation service')
        self.segmentation_service = rospy.ServiceProxy(SEGMENTATION_SERVICE,TabletopSegmentation)
        rospy.loginfo('Waiting for object recognition service')
        self.recognition_service = rospy.ServiceProxy(RECOGNITION_SERVICE,TabletopObjectRecognition) 

        self._get_model_mesh_srv = rospy.ServiceProxy(GET_MODEL_MESH_SERVICE, GetModelMesh)
        self.get_model_description = rospy.ServiceProxy(GET_MODEL_DESCRIPTION_SERVICE, GetModelDescription)
        try:
            rospy.loginfo('Waiting for get model mesh service')
            self._get_model_mesh_srv.wait_for_service(5.0)   
            rospy.loginfo('Waiting for get model description service')
            self.get_model_description.wait_for_service(5.0)
        except rospy.ROSException:
            rospy.logwarn('Did not find database services.  Likely will not be able to return recognition results')
            self._get_model_mesh_srv = None
            self.get_model_description = None
        
        self._object_id = 0


        rospy.loginfo('Ready to do object detections!')
        
    def detect_objects(self, add_objects_to_map='all', add_table_to_map=True,
                       add_objects_as_mesh='all', reset_collision_objects=True,
                       table_name='', point_head_at=None, reset_collider=True, 
                       labels_callback=None):
        '''
        This is the function you should call to get detections of objects in the scene.  By default, it will 
        give you back a TablewareResult, remove all collision objects (although not attached objects) currently in 
        the collision map and add the objects detected and the table to the map.  This returns first pickup goals for 
        objects recognized as tableware, then for objects recognized but not as tableware and then for objects not
        recognized at all.  In order to use the pickup goals returned, you must fill in the arm_name field.  
        
        The object labels returned in pickup goal reflect the recognition result.  Labels are:
        For tableware: One of the keys in the tableware_labels list.
        For recognized objects that are not tableware: The first tag in the database
        For unrecognized objects: graspable

        **Args:**
        
            *add_objects_to_map (string or False):* This argument controls whether and which objects are added to the
            collision map during detection.  Passing 'all' will add all detected objects to map, 'recognized' 
            will add only recognized objects to map, 'tableware' will add only objects recognized as tableware,  
            and False or 'None' or 'none' will add nothing to the map.
            
            *add_table_to_map (boolean):* If true, this will add the detected table to the map.  If you do not already
            have a representation of the table in the map and you wish to do pick or place it is a good idea to
            have the detection add it so there is a defined region of the map in which collision checking for the
            object can be disabled.  If the table is added to the map, the collision_support_surface_id field of
            the pickup goals will be set to its collision id.  Tables are usually added as meshes although
            if only the bounding box for the table is returned they will be added as boxes.
            
            *add_objects_as_mesh ('all', False, or [string]):* This argument controls which objects are added
            to the map as mesh rather than as a bounding box.  'all' will add all recognized objects as mesh while
            False will add no recognized objects as mesh.  For finer control, you can pass a list of labels that
            you want to add as mesh.
            
            *reset_collision_objects (boolean):* If true, this will remove all collision objects from the map before
            adding new ones.  The detection does no correlation with what is currently in the map so if you have
            done a detection on this table before, you will get repeat objects added to the map if you do not
            clear them first.
            
            *table_name (string):* The name of the table on which the objects are sitting.  If passed in, this will be 
            added to the pickup goals to avoid extraneous collisions caused by the object contacting the table
            (and will also be the table_name in the result).  If the table is added to the collision
            map and this is passed in, the table with have this name in the collision map.  If the table is added
            to the collision map and this is not passed in, it will have the name 'current_table', which is
            returned by the TablewareResult.
            
            *point_head_at (geometry_msgs.msg.PointStamped):* A point at which the head should look for the detection.
            If passed in, the robot will first look at this point, but if it cannot find a table or finds a
            vertical table, it will search along the x axis for a table.  For this reason, it is always a good
            idea to pass in this argument.  If left at None, the detection will not move the head.

            *reset_collider (boolean):* True if you want the collider to be reset before detection.  This will not
            wait for repopulate before the detection but will delay the return from the detection until after the
            repopulate is done if the detection is quick.

            *labels_callback (function: string f(pr2_tasks.pickplace_definitions.PickupGoal, 
            tabletop_object_detector.msgs.Table, string)):* After do_detection is finished assigning a label to a 
            cluster, this function (if not None) will be called to do more post-processing on the cluster.  For 
            example, this function is used to identify utensils by checking the height and pose of any 
            unidentified cluster.  It should return the label of the cluster.


        **Returns:**
            A TablewareDetectionResult.  The pickup goals (see pickplace_definitions.py) in this result have all of the 
            detection information, including recognized models and point cloud information, in the GraspableObject
            field.  The pose at which the object was detected and the label are also returned as fields in the 
            pickup goals.
            
         **Raises:**
         
             **exceptions.DetectionError:** if a table is not found or only vertical tables cannot be found.  If only
             vertical tables were found the error code will be OTHER_ERROR.
         '''

        if reset_collision_objects:
            self._wi.reset_collision_objects()
        if reset_collider:
            #this function takes so long that we'll have repopulated by the end of it
            self._wi.reset_collider_node(repopulate=False)
            reset_time = rospy.Time.now()
        collision_objects = self._wi.collision_objects()
        attached_objects = self._wi.attached_collision_objects()
        used_names = []
        for o in collision_objects:
            used_names.append(o.id)
        for ao in attached_objects:
            used_names.append(ao.object.id)
        res = self._do_detection(point_head_at)
        if add_table_to_map:
            table_name = self._add_table_to_map(res.detection.table, table_name)
        #we want to return first the tableware objects we might want to pick up
        tableware_pgs = []
        #recognized means we recognized them but don't think
        #they are anything we should pick up
        recognized_pgs = []
        #graspable means we could pick them up but we don't
        #know what they are
        graspable_pgs = []
        
        for c in range(len(res.detection.clusters)):
            #Compute the bounding box
            try:
                bres = self._bounding_box_srv(res.detection.clusters[c])
            except rospy.ServiceException, e:
                rospy.logerr('Unable to call cluster bounding box service.  Exception was '+str(e)+
                             '.  Ignoring cluster.')
                continue
            if bres.error_code != bres.SUCCESS:
                rospy.logerr('Cluster bounding box service returned error '+str(bres.error_code)+ 
                             '.  Ignoring cluster')
                continue
            go = GraspableObject()
            go.reference_frame_id = res.detection.clusters[c].header.frame_id
            go.cluster = res.detection.clusters[c]
            pg = PickupGoal('', go, object_pose_stamped=bres.pose,
                            collision_support_surface_name=table_name, allow_gripper_support_collision=True)
            co = CollisionObject()
            label = 'graspable'
            if self.get_model_description and self.get_mesh_from_database and\
                    len(res.detection.models[c].model_list) > 0 and\
                    res.detection.models[c].model_list[0].confidence < 0.005:
                #we recognized this - figure out what it is
                model_id = res.detection.models[c].model_list[0].model_id
                go.potential_models = res.detection.models[c].model_list
                rospy.logdebug('Potential models are: '+ str([m.model_id for m in res.detection.models[c].model_list]))
                try:
                    descr = self.get_model_description(model_id)
                    if descr.return_code.code != descr.return_code.SUCCESS:
                        rospy.logwarn('Get model description returned error '+
                                      str(descr.return_code.code))
                    for t in descr.tags:
                        if t in self.tableware_labels:
                            label = t
                            break
                    if label == 'graspable' and descr.tags:
                        label = descr.tags[0]
                    rospy.logdebug('Name of model is '+descr.name)
                except rospy.ServiceException, e:
                    rospy.logerr('Call to get description threw exception '+str(e))
                shape = None
                if add_objects_as_mesh and (add_objects_as_mesh == 'all' or label in add_objects_as_mesh):
                    try:
                        shape = self.get_mesh_from_database(res.detection.models[c].model_list[0].model_id)
                        if label == 'graspable': label = 'recognized'
                        co.header = res.detection.models[c].model_list[0].pose.header
                        co.shapes.append(shape)
                        co.poses.append(res.detection.models[c].model_list[0].pose.pose)
                        pg.object_pose_stamped = tl.transform_pose_stamped(pg.target.reference_frame_id,
                                                                           res.detection.models[c].model_list[0].pose)
                    except DetectionError:
                        shape = None
                if not shape:
                    co = self._get_bounding_box_collision_object(bres)
                    pg.object_pose_stamped = tl.transform_pose_stamped(pg.target.reference_frame_id, bres.pose)
            else:
Esempio n. 25
0
def main():
    obj = sys.argv[1]
    my_world = WorldInterface()
    my_world.attach_object_to_gripper('left_arm', obj)