def place(self, x, y, z): self.base.move_manipulable_pose(x, y, z, try_hard = True, group=self.arm_name) world_pose = Pose() world_pose.position.x = x world_pose.position.y = y world_pose.position.z = z world_pose.orientation.x = 0.0 world_pose.orientation.y = 0.0 world_pose.orientation.z = 0.0 world_pose.orientation.w = 1.0 new_pose = transform_pose('base_footprint', 'map', world_pose) hand_pose = self.planner.get_hand_frame_pose() #print hand_pose #print new_pose hand_pose.pose.position.x = new_pose.position.x hand_pose.pose.position.y = new_pose.position.y hand_pose.pose.position.z = new_pose.position.z + 0.1 self if self.arm_name == 'right_arm': joint_position = [-1.254, -0.325, -1.064, -1.525, 0.109, -1.185, 0.758] else: joint_position = [1.191, -0.295, 0.874, -1.610, 0.048, -1.069, -.988] joint_state = self.mover.get_joint_state() joint_state.position = joint_position self.mover.move_to_goal(joint_state) self.mover.move_to_goal_directly(hand_pose,collision_aware=False) #print "********" #print self.mover.get_exceptions() #print "********" still_in_free_space = True hand_pose = self.planner.get_hand_frame_pose() while still_in_free_space: goal_z = hand_pose.pose.position.z - 0.03 hand_pose.pose.position.z = goal_z self.mover.move_to_goal_directly(hand_pose,collision_aware=False) hand_pose = self.planner.get_hand_frame_pose() if abs(hand_pose.pose.position.z - goal_z) > 0.01: still_in_free_space = False #print self.mover.reached_goal() #if not self.mover.reached_goal(): # still_in_free_space = False self.gripper.open() if self.arm_name == 'right_arm': gripper_name = 'r_gripper_palm_link' else: gripper_name = 'l_gripper_palm_link' gripper_pose = transform_pose_stamped(gripper_name, hand_pose) gripper_pose.pose.position.x -= 0.20 hand_pose = transform_pose_stamped(hand_pose.header.frame_id, gripper_pose) self.mover.move_to_goal_directly(hand_pose, collision_aware=False) self.mover.move_to_goal(joint_state)
def get_joint_goal(arm_name, target, robot_state, seed = None): cart_goal = False try: ps_target, ps_origin = parse_cartesian_parameters(arm_name, target) cart_goal = "link" in ps_target.header.frame_id or "odom_combined" in ps_target.header.frame_id or "map" in ps_target.header.frame_id #todo: use tf? except (KeyError, ValueError, TypeError): cart_goal = False if cart_goal: if "map" in ps_target.header.frame_id: ps_target = transform_listener.transform_pose_stamped('base_link', ps_target, use_most_recent=False) iks = rospy.ServiceProxy("/cob_ik_wrapper/arm/get_ik_extended", GetPositionIKExtended) req = GetPositionIKExtendedRequest() req.ik_pose = ps_origin.pose req.constraint_aware = True req.timeout = rospy.Duration(5.0) req.ik_request.ik_link_name = ps_origin.header.frame_id req.ik_request.pose_stamped = ps_target req.ik_request.ik_seed_state = deepcopy(robot_state) if seed is not None: js, err = get_joint_goal(arm_name, seed, robot_state) if (err is None or err.val == err.SUCCESS) and js is not None: set_joint_state_in_robot_state(js,req.ik_request.ik_seed_state) req.ik_request.robot_state = robot_state res = iks(req) return res.solution.joint_state, res.error_code else: return read_target_state_from_param(arm_name, target), None
def __init__(self, graspable, location): pose = graspable.object_pose_stamped self.mappose = transform_listener.transform_pose_stamped("/map",pose) self.graspable = graspable self.label = graspable.label self.location = location self.arm_name = "right_arm"
def _add_table_to_map(self, table, table_name): co = CollisionObject() co.id = table_name if not co.id: co.id = 'current_table' co.header = table.pose.header #we do NOT want a padded table co.padding = -1 if len(table.convex_hull.vertices) > 0: if self.table_thickness > 0: table_z = range(0, int(1000*self.table_thickness/2.0), int(1000*TABLE_RESOLUTION)) table_z.append(1000.0*self.table_thickness/2.0) sgnrng = [1.0, -1.0] else: table_z = [0] sgnrng = [1.0] for z in table_z: for sgn in sgnrng: co.shapes.append(table.convex_hull) ps = tl.transform_pose_stamped(self._wi.world_frame, table.pose) ps.pose.position.z += sgn*z/1000.0 co.poses.append(tl.transform_pose(table.pose.header.frame_id, ps.header.frame_id, ps.pose)) rospy.logdebug('Adding table as convex hull') else: bbox = Shape() bbox.type = bbox.BOX bbox.dimensions = [abs(table.x_max - table.x_min), abs(table.y_max - table.y_min), self.table_thickness] co.shapes.append(bbox) co.poses.append(table.pose.pose) rospy.logdebug('Adding table as bounding box') self._wi.add_object(co) return co.id
def _do_detection(self, point_head_at): req = TabletopDetectionRequest() req.return_clusters = True req.return_models = True req.num_models = 1 if point_head_at: point_head_at = tl.transform_point_stamped(self._wi.world_frame, point_head_at) zrng = range(0, int(100*self.table_search_max), int(100*self.table_search_resolution)) sgnrng = [-1.0, 1.0] rospy.logdebug('For table finding, trying z offsets for head of:' +str(zrng)) if not point_head_at or not zrng: zrng = [0] sgnrng = [1.0] first_pass=True for z in zrng: for sgn in sgnrng: if point_head_at: new_point = copy.copy(point_head_at.point) new_point.z += sgn*z/100.0 self._head._point_head([new_point.x, new_point.y, new_point.z], point_head_at.header.frame_id) rospy.sleep(0.4) #let the head bobbing settle out res = self._detect_srv(req) if res.detection.result != res.detection.SUCCESS: rospy.logerr('Unable to find table. Error was: '+ str(res.detection.result)) if first_pass: exc = DetectionError('Unable to find table', error_code=res.detection.result) first_pass = False continue if self.allow_vertical_tables: return res table = res.detection.table #is the table somewhat horizontal? table_pose_bl = tl.transform_pose_stamped(self._wi.robot_frame, table.pose) vert = Point() vert.z = 1.0 oripose = Pose() oripose.orientation = table_pose_bl.pose.orientation table_vertical = gt.transform_point(vert, oripose) if table_vertical.z < self.vertical_threshold: rospy.logerr('Table vertical is [%f, %f, %f]. This is not a horizontal table', table_vertical.x, table_vertical.y, table_vertical.z) res.detection.result = res.detection.OTHER_ERROR if first_pass: exc = DetectionError('Unable to find horizontal table', error_code=res.detection.result) first_pass = False continue rospy.loginfo('Found horizontal table. Vertical is [%f, %f, %f]', table_vertical.x, table_vertical.y, table_vertical.z) return res raise exc
def execute(self, userdata): #rospy.logwarn("PEZZ HACKING!!") #return "success" goal = self.world.pickup_next pose = goal.object_pose_stamped mappose = transform_listener.transform_pose_stamped("/map",pose) x = mappose.pose.position.x y = mappose.pose.position.y z = mappose.pose.position.z rospy.loginfo("Trying to go to %s", (x,y,z)) try: self.base.move_manipulable_pose(x, y, z, try_hard = True, group="right_arm") except: raise #return "failure" return "success"
def find_height_above_table(pose_stamped_in, table_pose_stamped): ''' Utility function that, given the pose (presumably of an object) and the pose of a table will return the height of that object above the table. **Args:** **pose_stamped_in (geometry_msgs.msg.PoseStamped):** The pose above the table **table_pose_stamped (geometry_msgs.msg.PoseStamped):** The pose of the table **Returns:** The height of the pose above the table. **Raises:** All exceptions a TransformListener can raise. ''' pose_stamped = tl.transform_pose_stamped(table_pose_stamped.header.frame_id, pose_stamped_in) #take this coordinate and transform it into the table's tp = gt.inverse_transform_point(pose_stamped.pose.position, table_pose_stamped.pose) rospy.logdebug('Height above table is '+str(tp.z)) return tp.z
#!/usr/bin/env python import roslib roslib.load_manifest('sushi_tutorials') import rospy import sys import tf.transformations as trans from pr2_python.transform_listener import transform_pose_stamped from geometry_msgs.msg import PoseStamped def _yaw(q): e = trans.euler_from_quaternion([q.x, q.y, q.z, q.w]) return e[2] rospy.init_node('get_current_pose', anonymous=True) zero_pose = PoseStamped() zero_pose.header.frame_id = "/base_footprint" zero_pose.pose.orientation.w = 1 mappose = transform_pose_stamped("/map", zero_pose) rospy.loginfo("Pose is %s", (mappose.pose.position.x, mappose.pose.position.y, _yaw(mappose.pose.orientation) ))
def execute(self, userdata, overwrite_presets=dict()): presets = self.default_presets presets.update(overwrite_presets) sss.set_light('blue') wi = WorldInterface() wi.reset_attached_objects() graspdata = dict() print userdata.object print "time difference = ", ( rospy.Time.now() - userdata.object.pose.header.stamp).to_sec() # add wall wall_extent = [3.0, 0.1, 2.5] wall_pose = conversions.create_pose_stamped( [0, -0.99 - wall_extent[1], wall_extent[2] / 2.0, 0, 0, 0, 1], '/base_link') # magic numbers are cool wi.add_collision_box(wall_pose, wall_extent, "wall") # add floor floor_extent = [3.0, 3.0, 0.1] floor_pose = conversions.create_pose_stamped( [0, 0, floor_extent[2] / 2.0, 0, 0, 0, 1], '/base_link') # magic numbers are cool wi.add_collision_box(floor_pose, floor_extent, "floor") #transform into base_link grasp_pose = transform_listener.transform_pose_stamped( '/base_link', userdata.object.pose, use_most_recent=False) print grasp_pose # add object bounding box obj_pose = deepcopy(userdata.object.pose) lwh = userdata.object.bounding_box_lwh m1 = pm.toMatrix(pm.fromMsg(obj_pose.pose)) m2 = pm.toMatrix(pm.fromTf(((0, 0, lwh.z / 2.0), (0, 0, 0, 1)))) obj_pose.pose = pm.toMsg(pm.fromMatrix(numpy.dot(m1, m2))) wi.add_collision_box(obj_pose, (lwh.x * 2.0, lwh.y * 2.0, lwh.z), "grasp_object") # compute minimal height of object bounding box edges in base_link min_z = grasp_pose.pose.position.z for k in [(0.5, 0.5, 1.0), (0.5, 0.5, 0.0), (0.5, -0.5, 1.0), (0.5, -0.5, 0.0), (-0.5, 0.5, 1.0), (-0.5, 0.5, 0.0), (-0.5, -0.5, 1.0), (-0.5, -0.5, 0.0)]: m1 = pm.toMatrix(pm.fromMsg(grasp_pose.pose)) # BFromO m2 = pm.toMatrix( pm.fromTf(((k[0] * lwh.x, k[1] * lwh.y, k[2] * lwh.z), (0, 0, 0, 1)))) # inO min_z = min(min_z, pm.toMsg(pm.fromMatrix(numpy.dot( m1, m2))).position.z) #min_z inB # add table table_extent = (2.0, 2.0, min_z) # base_link # x - points towards front of robot # y - follow right hand rule # z - points upwards 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') # center of table, 0.5 meter behind the robot wi.add_collision_box(table_pose, table_extent, "table") # calculate grasp and lift pose grasp_pose.pose.position.x += presets['grasp_offset'][0] grasp_pose.pose.position.y += presets['grasp_offset'][1] grasp_pose.pose.position.z += presets['grasp_offset'][2] grasp_pose.pose.orientation = Quaternion( *quaternion_from_euler(*presets['fixed_orientation_euler']) ) # orientation of sdh_grasp_link in base_link for 'grasp' joint goal graspdata['height'] = grasp_pose.pose.position.z - table_extent[2] pregrasp_pose = deepcopy(grasp_pose) pregrasp_pose.pose.position.x += presets['pregrasp_offset'][0] pregrasp_pose.pose.position.y += presets['pregrasp_offset'][1] pregrasp_pose.pose.position.z += presets['pregrasp_offset'][2] lift_pose = deepcopy(grasp_pose) lift_pose.pose.position.x += presets['lift_offset'][0] lift_pose.pose.position.y += presets['lift_offset'][1] lift_pose.pose.position.z += presets['lift_offset'][2] mp = MotionPlan() # open hand mp += CallFunction(sss.move, 'sdh', 'cylopen', False) mp += MoveArm('arm', [pregrasp_pose, [presets['tcp_link']]], seed=presets['pregrasp_seed']) mp += MoveComponent('sdh', 'cylopen', True) # allow collison hand/object for l in hand_description.HandDescription('arm').touch_links: mp += EnableCollision("grasp_object", l) # goto grasp mp += MoveArm('arm', [grasp_pose, [presets['tcp_link']] ]) # Move sdh_grasp_link to grasp_pose # close hand mp += MoveComponent('sdh', 'cylclosed') # check grasp #mp += CheckService('/sdh_controller/is_cylindric_grasped', Trigger, lambda res: res.success.data) # disable collison mp += ResetCollisions() # attach object mp += AttachObject('arm', "grasp_object") # enable collision mp += EnableCollision("grasp_object", "table") # lift motion mp += MoveArm('arm', [lift_pose, [presets['tcp_link']] ]) # Move sdh_grasp_link to lift_pose # disable collison mp += ResetCollisions() # move away #mp += MoveArm('arm', [pregrasp_pose,[presets['tcp_link']]]) # goto hold mp += MoveArm('arm', 'hold') userdata.graspdata = graspdata if not mp.plan(5).success: sss.set_light('green') return "not_grasped" sss.set_light('yellow') sss.say(["I am grasping " + userdata.object.label + " now."], False) # run, handle errors i = 0 for ex in mp.execute(): if not ex.wait(80.0).success: sss.set_light('red') return 'failed' i += 1 sss.set_light('green') return 'grasped'
def execute(self, userdata): sss.set_light("blue") wi = WorldInterface() wi.reset_attached_objects() graspdata = dict() print userdata.object # add wall wall_extent = [3.0, 0.1, 2.5] wall_pose = conversions.create_pose_stamped( [0, -0.99 - wall_extent[1], wall_extent[2] / 2.0, 0, 0, 0, 1], "base_link" ) # magic numbers are cool wi.add_collision_box(wall_pose, wall_extent, "wall") # add floor floor_extent = [3.0, 3.0, 0.1] floor_pose = conversions.create_pose_stamped( [0, 0, floor_extent[2] / 2.0, 0, 0, 0, 1], "/base_link" ) # magic numbers are cool wi.add_collision_box(floor_pose, floor_extent, "floor") # transform into base_link grasp_pose = transform_listener.transform_pose_stamped("base_link", userdata.object.pose, use_most_recent=False) # add object bounding box obj_pose = deepcopy(grasp_pose) lwh = userdata.object.bounding_box_lwh m1 = pm.toMatrix(pm.fromMsg(obj_pose.pose)) m2 = pm.toMatrix(pm.fromTf(((0, 0, lwh.z / 2.0), (0, 0, 0, 1)))) obj_pose.pose = pm.toMsg(pm.fromMatrix(numpy.dot(m1, m2))) wi.add_collision_box(obj_pose, (lwh.x * 2.0, lwh.y * 2.0, lwh.z), "grasp_object") print grasp_pose # add table table_extent = (2.0, 2.0, grasp_pose.pose.position.z) 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") # calculate grasp and lift pose grasp_pose.pose.position.x += 0.02 grasp_pose.pose.position.y += 0.02 grasp_pose.pose.position.z += 0.1 # 0.03 + 0.05 grasp_pose.pose.orientation = Quaternion( *quaternion_from_euler(-1.706, 0.113, 2.278) ) # orientation of sdh_grasp_link in base_link for 'grasp' joint goal graspdata["height"] = grasp_pose.pose.position.z - table_extent[2] pregrasp_pose = deepcopy(grasp_pose) pregrasp_pose.pose.position.x += 0.20 pregrasp_pose.pose.position.y += 0.11 pregrasp_pose.pose.position.z += 0.1 lift_pose = deepcopy(grasp_pose) lift_pose.pose.position.z += 0.03 mp = MotionPlan() # open hand mp += CallFunction(sss.move, "sdh", "cylopen", False) mp += MoveArm("arm", [pregrasp_pose, ["sdh_grasp_link"]], seed="pregrasp") mp += MoveComponent("sdh", "cylopen", True) # allow collison hand/object # for l in hand_description.HandDescription('arm').touch_links: # mp += EnableCollision("grasp_object", l) # # disable collison # mp += ResetCollisions() # goto grasp mp += MoveArmUnplanned("arm", [grasp_pose, ["sdh_grasp_link"]]) # close hand mp += MoveComponent("sdh", "cylclosed") # check grasp # mp += CheckService('/sdh_controller/is_cylindric_grasped', Trigger, lambda res: res.success.data) # attach object mp += AttachObject("arm", "grasp_object") # enable collision mp += EnableCollision("grasp_object", "table") # lift motion mp += MoveArmUnplanned("arm", [lift_pose, ["sdh_grasp_link"]]) # disable collison mp += ResetCollisions() # move away mp += MoveArm("arm", [pregrasp_pose, ["sdh_grasp_link"]]) # goto hold mp += MoveArm("arm", "hold") userdata.graspdata = graspdata if not mp.plan(5).success: sss.set_light("green") return "not_grasped" sss.set_light("yellow") sss.say(["I am grasping " + userdata.object.label + " now."], False) # run, handle errors i = 0 for ex in mp.execute(): if not ex.wait(80.0).success: sss.set_light("red") return "failed" i += 1 sss.set_light("green") return "grasped"
def execute(self, userdata, overwrite_presets = dict()): presets = self.default_presets presets.update(overwrite_presets) sss.set_light('blue') wi = WorldInterface() wi.reset_attached_objects() graspdata = dict() print userdata.object print "time difference = ", (rospy.Time.now() - userdata.object.pose.header.stamp).to_sec() # add wall wall_extent = [3.0,0.1,2.5] wall_pose = conversions.create_pose_stamped([ 0, -0.99 - wall_extent[1], wall_extent[2]/2.0 ,0,0,0,1], '/base_link') # magic numbers are cool wi.add_collision_box(wall_pose, wall_extent, "wall") # add floor floor_extent = [3.0,3.0,0.1] floor_pose = conversions.create_pose_stamped([ 0, 0, floor_extent[2]/2.0 ,0,0,0,1], '/base_link') # magic numbers are cool wi.add_collision_box(floor_pose, floor_extent, "floor") #transform into base_link grasp_pose = transform_listener.transform_pose_stamped('/base_link', userdata.object.pose, use_most_recent=False) print grasp_pose # add object bounding box obj_pose = deepcopy(userdata.object.pose) lwh = userdata.object.bounding_box_lwh m1 = pm.toMatrix( pm.fromMsg(obj_pose.pose) ) m2 = pm.toMatrix( pm.fromTf( ((0,0, lwh.z/2.0),(0,0,0,1)) ) ) obj_pose.pose = pm.toMsg( pm.fromMatrix(numpy.dot(m1,m2)) ) wi.add_collision_box(obj_pose,(lwh.x*2.0,lwh.y*2.0,lwh.z) , "grasp_object") # compute minimal height of object bounding box edges in base_link min_z = grasp_pose.pose.position.z for k in [(0.5,0.5,1.0),(0.5,0.5,0.0),(0.5,-0.5,1.0),(0.5,-0.5,0.0),(-0.5,0.5,1.0),(-0.5,0.5,0.0),(-0.5,-0.5,1.0),(-0.5,-0.5,0.0)]: m1 = pm.toMatrix( pm.fromMsg(grasp_pose.pose) ) # BFromO m2 = pm.toMatrix( pm.fromTf( ((k[0]*lwh.x,k[1]*lwh.y, k[2]*lwh.z),(0,0,0,1)) ) ) # inO min_z = min(min_z,pm.toMsg( pm.fromMatrix(numpy.dot(m1,m2))).position.z) #min_z inB # add table table_extent = (2.0, 2.0, min_z) # base_link # x - points towards front of robot # y - follow right hand rule # z - points upwards 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') # center of table, 0.5 meter behind the robot wi.add_collision_box(table_pose, table_extent, "table") # calculate grasp and lift pose grasp_pose.pose.position.x += presets['grasp_offset'][0] grasp_pose.pose.position.y += presets['grasp_offset'][1] grasp_pose.pose.position.z += presets['grasp_offset'][2] grasp_pose.pose.orientation = Quaternion(*quaternion_from_euler(*presets['fixed_orientation_euler'])) # orientation of sdh_grasp_link in base_link for 'grasp' joint goal graspdata['height'] = grasp_pose.pose.position.z - table_extent[2] pregrasp_pose = deepcopy(grasp_pose) pregrasp_pose.pose.position.x += presets['pregrasp_offset'][0] pregrasp_pose.pose.position.y += presets['pregrasp_offset'][1] pregrasp_pose.pose.position.z += presets['pregrasp_offset'][2] lift_pose = deepcopy(grasp_pose) lift_pose.pose.position.x += presets['lift_offset'][0] lift_pose.pose.position.y += presets['lift_offset'][1] lift_pose.pose.position.z += presets['lift_offset'][2] mp = MotionPlan() # open hand mp += CallFunction(sss.move, 'sdh','cylopen', False) mp += MoveArm('arm',[pregrasp_pose,[presets['tcp_link']]], seed = presets['pregrasp_seed']) mp += MoveComponent('sdh','cylopen', True) # allow collison hand/object for l in hand_description.HandDescription('arm').touch_links: mp += EnableCollision("grasp_object", l) # goto grasp mp += MoveArm('arm', [grasp_pose,[presets['tcp_link']]]) # Move sdh_grasp_link to grasp_pose # close hand mp += MoveComponent('sdh','cylclosed') # check grasp #mp += CheckService('/sdh_controller/is_cylindric_grasped', Trigger, lambda res: res.success.data) # disable collison mp += ResetCollisions() # attach object mp += AttachObject('arm', "grasp_object") # enable collision mp += EnableCollision("grasp_object", "table") # lift motion mp += MoveArm('arm', [lift_pose,[presets['tcp_link']]]) # Move sdh_grasp_link to lift_pose # disable collison mp += ResetCollisions() # move away #mp += MoveArm('arm', [pregrasp_pose,[presets['tcp_link']]]) # goto hold mp += MoveArm('arm', 'hold') userdata.graspdata = graspdata if not mp.plan(5).success: sss.set_light('green') return "not_grasped" sss.set_light('yellow') sss.say(["I am grasping " + userdata.object.label + " now."],False) # run, handle errors i = 0 for ex in mp.execute(): if not ex.wait(80.0).success: sss.set_light('red') return 'failed' i+=1 sss.set_light('green') return 'grasped'
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)
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: co = self._get_bounding_box_collision_object(bres) pg.object_pose_stamped = tl.transform_pose_stamped(pg.target.reference_frame_id, bres.pose) co.padding = self.object_padding if labels_callback: #more logic for assigning labels if you want it label = labels_callback(pg, res.detection.table, label) rospy.loginfo('Cluster '+str(c)+': '+label) self._object_id += 1 co.id = label+'_'+str(self._object_id) while co.id in used_names: co.id = label+'_'+str(self._object_id) self._object_id += 1 pg.collision_object_name = co.id go.collision_name = co.id pg.label = label if label in self.tableware_labels: tableware_pgs.append(pg)