示例#1
0
def main():
	# find objects on the table    
    rospy.loginfo("Waiting for find_cluster_bounding_box2 service")
    find_box = rospy.ServiceProxy("/find_cluster_bounding_box", FindClusterBoundingBox)
    detector = TablewareDetection(table_thickness=0.1)
    det = detector.detect_objects(point_head_at=None)

    table = det.table
    table_dims = (table.x_max - table.x_min, table.y_max - table.y_min)
    rospy.loginfo("Table dims: %s", table_dims)
        
    objs = []
    for goal in det.pickup_goals:
        pointcloud = goal.target.cluster
        box = find_box(pointcloud)
        rospy.loginfo("----- Found label %s", goal.label)
        dims = box.box_dims
        pos = (box.pose.pose.position.x, box.pose.pose.position.y)
        obj = ( (pos[0] - dims.x/2 - table.x_min, pos[1] - dims.y/2 - table.y_min), (dims.x, dims.y))
        rospy.loginfo("Obj tuple: %s", obj)
        objs.append(obj)
    
    obj_dim = [(0.1, 0.10)]
    spots = free_spots(table_dims, obj_dim, 0.01, objs)
    rospy.loginfo("Spots: %s", spots)
示例#2
0
def main():
	# find objects on the table
	head_point = PointStamped()
	head_point.header.frame_id = '/torso_lift_link'
	head_point.point.x = 0.5
        head_point = None
	detector = TablewareDetection(table_thickness=0.1)
	det = detector.detect_objects(point_head_at=head_point)
	if not det.pickup_goals:
		rospy.loginfo('Nothing to pick up!')
		return

	# pick up the first object that we found
	if len(sys.argv) > 1:
		arm = sys.argv[1]
	else:
		arm = "right_arm"
	rospy.loginfo("Trying arm: %s", arm)
	pickplace = PickPlace()
	pg = det.pickup_goals[0]
	pg.arm_name = arm
	try:
		pickplace.pickup(pg)
	except Exception as e:
		rospy.logerr("No grasping, err: %s", e)
		return
	
	gripper = Gripper(arm)
	gripper.open()
示例#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')
示例#4
0
def main():
    client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
    pub = rospy.Publisher('darrt_trajectory', MarkerArray)
    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
    client.wait_for_server()
    
    goal = DARRTGoal()
    
    goal.pickup_goal = det.pickup_goals[0]

    goal.pickup_goal.arm_name = 'right_arm'
    
    place_pose_stamped = PoseStamped()
    place_pose_stamped.header.frame_id = 'map'
    place_pose_stamped.pose.position.x -= 1.3
    place_pose_stamped.pose.position.y -= 0.3
    place_pose_stamped.pose.position.z  = 0.98
    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.min_grasp_distance_from_surface = 0.17
    goal.object_sampling_fraction = 0.9
    goal.retreat_distance = 0.3
    goal.debug_level = 2
    goal.do_pause = 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()
    if result.error_code == result.SUCCESS:
        #pickle everything!! great excitement
        filename = 'pickplace_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'))

    marray = MarkerArray()
    if result.error_code == result.SUCCESS:
        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)
示例#5
0
def main():
    det = TablewareDetection()
    head_point = PointStamped()
    head_point.header.frame_id = '/torso_lift_link'
    head_point.point.x = 0.6
    res = det.detect_objects(point_head_at=head_point)
    rospy.loginfo('Detected objects with the following labels and poses:')
    for pg in res.pickup_goals:
        rospy.loginfo('Label: '+str(pg.label)+', Pose:\n'+str(pg.object_pose_stamped))
    return
示例#6
0
def main():
    # find objects on the table
    rospy.loginfo("Waiting for find_cluster_bounding_box2 service")
    find_box = rospy.ServiceProxy("/find_cluster_bounding_box", FindClusterBoundingBox)
    detector = TablewareDetection(table_thickness=0.1)
    det = detector.detect_objects(point_head_at=None)

    if not det.pickup_goals:
        rospy.loginfo("Nothing to pick up!")
        return

    for goal in det.pickup_goals:
        pointcloud = goal.target.cluster
        box = find_box(pointcloud)
        rospy.loginfo("----- Found label %s", goal.label)
        dims = box.box_dims
        rospy.loginfo("Bounding box dims: %.2f, %.2f, %.2f", dims.x, dims.y, dims.z)
        area = dims.x * dims.y * dims.z
        squareness = min(dims.x, dims.y) / max(dims.x, dims.y)
        rospy.loginfo("Area: %f", area)
        rospy.loginfo("Squareness: %f", squareness)
示例#7
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)
def main():

    #Find an object to pick up
    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

    #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'
    
    #place goal (move left 20 cm)
    place_pose_stamped = copy.deepcopy(goal.pickup_goal.object_pose_stamped)
    place_pose_stamped.pose.position.y += 0.2
    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)

    #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)
示例#9
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!')