def get_table():
'''
Uses tabletop detection to find a plane which corresponds to the cabinet door
@rtype: tabletop_object_detector/Table
@returns the detected cabinet door
'''
ppm = PickAndPlaceManager()
(things, table) = ppm.call_tabletop_detection(take_static_collision_map=1,
update_table=1)
print table
return table
def __init__(self):
self.place_viz_pub = rospy.Publisher('place_pose', PoseStamped)
self.manager = PickAndPlaceManager()
self.cursor_proxy = rospy.ServiceProxy('get_cursor_stats',
GetCursorStats)
#~ self.cursor_proxy.wait_for_service()
rospy.loginfo("Waiting for polygon service")
self.polygon_proxy = rospy.ServiceProxy('draw_polygon', DrawPolygon)
self.polygon_proxy.wait_for_service()
rospy.loginfo("Waiting for circle inhibit service")
self.circle_inhibit_proxy = rospy.ServiceProxy('circle_inhibit',
CircleInhibit)
self.circle_inhibit_proxy.wait_for_service()
rospy.loginfo("Drawing trash/recycling")
self.polygon_proxy('Recycling', True, RECYCLING)
self.polygon_proxy('Garbage', True, TRASH)
def __init__(self):
self.place_viz_pub = rospy.Publisher('place_pose', PoseStamped)
self.manager = PickAndPlaceManager()
self.cursor_proxy = rospy.ServiceProxy('get_cursor_stats', GetCursorStats)
#~ self.cursor_proxy.wait_for_service()
rospy.loginfo("Waiting for polygon service")
self.polygon_proxy = rospy.ServiceProxy('draw_polygon', DrawPolygon)
self.polygon_proxy.wait_for_service()
rospy.loginfo("Waiting for circle inhibit service")
self.circle_inhibit_proxy = rospy.ServiceProxy('circle_inhibit', CircleInhibit)
self.circle_inhibit_proxy.wait_for_service()
rospy.loginfo("Drawing trash/recycling")
self.polygon_proxy('Recycling', True, RECYCLING)
self.polygon_proxy('Garbage', True, TRASH)
def __init__(self, name):
'''
Begins the action to open the cupboard
'''
rospy.loginfo("IN ACTION")
self._action_name = name
self._as = actionlib.SimpleActionServer(
self._action_name,
opendoors_executive.msg.OpenCupboardAction,
execute_cb=self.main1)
self._as.start()
self.ppm = PickAndPlaceManager()
self.s = rospy.Service('table_status',
opendoors_executive.srv.table_status,
self.table_status)
print "Ready to check table status"
self.result = opendoors_executive.msg.OpenCupboardResult()
'''
def dude():
ppm = PickAndPlaceManager()
(things, table) = ppm.call_tabletop_detection(take_static_collision_map=1,
update_table=1)
class Manipulator(object):
state = NOTGRASPING
point = None
grasp_arm = -1
INPROGRESS = False
click_lock = RLock()
def __init__(self):
self.place_viz_pub = rospy.Publisher('place_pose', PoseStamped)
self.manager = PickAndPlaceManager()
self.cursor_proxy = rospy.ServiceProxy('get_cursor_stats', GetCursorStats)
#~ self.cursor_proxy.wait_for_service()
rospy.loginfo("Waiting for polygon service")
self.polygon_proxy = rospy.ServiceProxy('draw_polygon', DrawPolygon)
self.polygon_proxy.wait_for_service()
rospy.loginfo("Waiting for circle inhibit service")
self.circle_inhibit_proxy = rospy.ServiceProxy('circle_inhibit', CircleInhibit)
self.circle_inhibit_proxy.wait_for_service()
rospy.loginfo("Drawing trash/recycling")
self.polygon_proxy('Recycling', True, RECYCLING)
self.polygon_proxy('Garbage', True, TRASH)
def click(self, msg):
if self.click_lock.acquire(blocking=False):
#~ self.circle_inhibit_proxy(True)
rospy.logwarn('Calling tabletop detection')
self.manager.call_tabletop_detection()
# self.point = self.cursor_proxy().click_pos
print 'STATE = ',self.state
if self.state == NOTGRASPING:
rospy.logwarn('About to grasp')
self.grasp()
elif self.state == GRASPING:
rospy.logwarn('About to drop')
self.drop()
self.click_lock.release()
#~ self.circle_inhibit_proxy(False)
def point_cb(self, msg):
#self.point = msg
cloud = read_points_np(msg)
point = PointStamped()
point.header = msg.header
if cloud.shape[1] == 0: return
point.point.x, point.point.y, point.point.z = cloud[0][0]
self.point = point
def transformAtLatestCommonTime(self, target_frame, point):
lct = self.manager.tf_listener.getLatestCommonTime(point.header.frame_id, target_frame)
point_lct = deepcopy(point)
point_lct.header.stamp = lct
return self.manager.tf_listener.transformPoint(target_frame, point_lct)
#TODO need to make sure we can't trigger grasp/drop multiple times concurrently
def grasp(self):
print self.state, self.point
if (self.state == NOTGRASPING) and (self.point is not None):
rospy.loginfo('===grasping')
self.manager.move_arm_to_side(0)
ptt = self.transformAtLatestCommonTime('base_link', self.point)
status = self.manager.pick_up_object_near_point(ptt, 0)
self.grasp_arm = 0
# if not success:
# self.manager.move_arm_to_side(1)
# success = self.manager.pick_up_object_near_point(self.point, 1)
# self.grasp_arm = 1
if status == ManipulationResult.SUCCESS:
self.state = GRASPING
rospy.logerr('SUCCESSFUL GRASP')
else:
rospy.logerr('FAILED TO GRASP')
def drop(self):
if self.state == GRASPING:
#success = self.manager.put_down_object(self.grasp_arm, max_place_tries=25, use_place_override=1)
place_pose = PoseStamped()
ptt = self.transformAtLatestCommonTime('table', self.point)
place_pose.header = ptt.header
place_pose.pose.position = ptt.point
place_pose.pose.orientation = euler_to_quaternion(0,0,0)
place_pose.pose.position.z = 0.03
# see if it's in one of our predefined boxes
pw,ph = 0.1,0.1
#boxes = [('TRASH', TRASH), ('RECYCLING', RECYCLING)]
boxes = []
for name, polygon in boxes:
pts_arr = np.array([[(p.x,p.y) for p in polygon.polygon.points]])
print pts_arr
if pnpoly(ptt.point.x, ptt.point.y, pts_arr.squeeze()):
# we're doing this from the top right corner (don't ask)
# place_pose.pose.position = polygon.polygon.points[0]
poly_center = pts_arr.squeeze().mean(0)
place_pose.pose.position.x = poly_center[0]
place_pose.pose.position.y = poly_center[1]
pw,ph = w,h
rospy.loginfo('Placing in box: %s' % name)
rospy.loginfo('Placing at %s' % place_pose)
self.place_viz_pub.publish(place_pose)
#import pdb; pdb.set_trace()
self.manager.set_place_area(place_pose, (pw,ph))
# status = self.manager.place_object(self.grasp_arm, place_pose, padding=0)
status = self.manager.put_down_object(self.grasp_arm, max_place_tries=25, use_place_override=1)
if status == ManipulationResult.SUCCESS:
self.state = NOTGRASPING
self.grasp_arm = -1
def main():
ppm = PickAndPlaceManager()
move_to()
grasp()
open(ppm)
class Manipulator(object):
state = NOTGRASPING
point = None
grasp_arm = -1
INPROGRESS = False
click_lock = RLock()
def __init__(self):
self.place_viz_pub = rospy.Publisher('place_pose', PoseStamped)
self.manager = PickAndPlaceManager()
self.cursor_proxy = rospy.ServiceProxy('get_cursor_stats',
GetCursorStats)
#~ self.cursor_proxy.wait_for_service()
rospy.loginfo("Waiting for polygon service")
self.polygon_proxy = rospy.ServiceProxy('draw_polygon', DrawPolygon)
self.polygon_proxy.wait_for_service()
rospy.loginfo("Waiting for circle inhibit service")
self.circle_inhibit_proxy = rospy.ServiceProxy('circle_inhibit',
CircleInhibit)
self.circle_inhibit_proxy.wait_for_service()
rospy.loginfo("Drawing trash/recycling")
self.polygon_proxy('Recycling', True, RECYCLING)
self.polygon_proxy('Garbage', True, TRASH)
def click(self, msg):
if self.click_lock.acquire(blocking=False):
#~ self.circle_inhibit_proxy(True)
rospy.logwarn('Calling tabletop detection')
self.manager.call_tabletop_detection()
# self.point = self.cursor_proxy().click_pos
print 'STATE = ', self.state
if self.state == NOTGRASPING:
rospy.logwarn('About to grasp')
self.grasp()
elif self.state == GRASPING:
rospy.logwarn('About to drop')
self.drop()
self.click_lock.release()
#~ self.circle_inhibit_proxy(False)
def point_cb(self, msg):
#self.point = msg
cloud = read_points_np(msg)
point = PointStamped()
point.header = msg.header
if cloud.shape[1] == 0: return
point.point.x, point.point.y, point.point.z = cloud[0][0]
self.point = point
def transformAtLatestCommonTime(self, target_frame, point):
lct = self.manager.tf_listener.getLatestCommonTime(
point.header.frame_id, target_frame)
point_lct = deepcopy(point)
point_lct.header.stamp = lct
return self.manager.tf_listener.transformPoint(target_frame, point_lct)
#TODO need to make sure we can't trigger grasp/drop multiple times concurrently
def grasp(self):
print self.state, self.point
if (self.state == NOTGRASPING) and (self.point is not None):
rospy.loginfo('===grasping')
self.manager.move_arm_to_side(0)
ptt = self.transformAtLatestCommonTime('base_link', self.point)
status = self.manager.pick_up_object_near_point(ptt, 0)
self.grasp_arm = 0
# if not success:
# self.manager.move_arm_to_side(1)
# success = self.manager.pick_up_object_near_point(self.point, 1)
# self.grasp_arm = 1
if status == ManipulationResult.SUCCESS:
self.state = GRASPING
rospy.logerr('SUCCESSFUL GRASP')
else:
rospy.logerr('FAILED TO GRASP')
def drop(self):
if self.state == GRASPING:
#success = self.manager.put_down_object(self.grasp_arm, max_place_tries=25, use_place_override=1)
place_pose = PoseStamped()
ptt = self.transformAtLatestCommonTime('table', self.point)
place_pose.header = ptt.header
place_pose.pose.position = ptt.point
place_pose.pose.orientation = euler_to_quaternion(0, 0, 0)
place_pose.pose.position.z = 0.03
# see if it's in one of our predefined boxes
pw, ph = 0.1, 0.1
#boxes = [('TRASH', TRASH), ('RECYCLING', RECYCLING)]
boxes = []
for name, polygon in boxes:
pts_arr = np.array([[(p.x, p.y)
for p in polygon.polygon.points]])
print pts_arr
if pnpoly(ptt.point.x, ptt.point.y, pts_arr.squeeze()):
# we're doing this from the top right corner (don't ask)
# place_pose.pose.position = polygon.polygon.points[0]
poly_center = pts_arr.squeeze().mean(0)
place_pose.pose.position.x = poly_center[0]
place_pose.pose.position.y = poly_center[1]
pw, ph = w, h
rospy.loginfo('Placing in box: %s' % name)
rospy.loginfo('Placing at %s' % place_pose)
self.place_viz_pub.publish(place_pose)
#import pdb; pdb.set_trace()
self.manager.set_place_area(place_pose, (pw, ph))
# status = self.manager.place_object(self.grasp_arm, place_pose, padding=0)
status = self.manager.put_down_object(self.grasp_arm,
max_place_tries=25,
use_place_override=1)
if status == ManipulationResult.SUCCESS:
self.state = NOTGRASPING
self.grasp_arm = -1
