def detect_objects(): ''' TODO: Need to figure out the target frame in which the cluster point co-ordinates are returned. ''' service_name = '/object_detection' rospy.wait_for_service(service_name) try: table_object_detector = rospy.ServiceProxy(service_name, TabletopDetection) req = TabletopDetectionRequest() req.return_clusters = False req.return_models = True req.num_models = 1 resp = TabletopDetectionResponse() resp = table_object_detector(req) if resp.detection.result == resp.detection.SUCCESS: points = resp.detection.clusters[0].points x_points = [point.x for point in points] y_points = [point.y for point in points] z_points = [point.z for point in points] x = (min(x_points) + max(x_points)) / 2.0 y = (min(y_points) + max(y_points)) / 2.0 z = (min(z_points) + max(z_points)) / 2.0 return x, y, z else: print resp.detection.result except rospy.ServiceException, e: print "Service call failed: %s" % e
def detect_objects(self): rospy.loginfo("calling tabletop detection") det_req = TabletopDetectionRequest() det_req.return_clusters = 1 det_req.return_models = 1 #det_req.num_models = num_models #call tabletop detection, get a detection result (attempt 3 times) for try_num in range(3): try: det_res = self.grasper_detect_srv(det_req) except rospy.ServiceException, e: rospy.logerr("error when calling %s: %s" % (self.grasper_detect_name, e)) self.throw_exception() return ([], None) if det_res.detection.result == det_res.detection.SUCCESS: rospy.loginfo("tabletop detection reports success") break else: rospy.logerr( "tabletop detection failed with error %s, trying again" % det_res.detection.result)
def __init__(self, recognize_objects=False): smach.StateMachine.__init__(self, [succeeded, preempted, aborted, 'no_table'], input_keys=[], output_keys=['tabletop_info']) with self: # HeadScan and Snapshot action call scan_table = PointHeadGoal() StateMachine.add('SCAN_TABLE', SimpleActionState('/head_traj_controller/head_scan_snapshot_action', PointHeadAction, goal=scan_table), transitions={succeeded: 'TABLE_DETECTION'}) # TabletopDetection call detect_table = TabletopDetectionRequest() # Return the pointcloud (cluster) for each detected object # if this is false, you have no data to pass to tabletop_collision_map_processing node detect_table.return_clusters = recognize_objects # Return matching object models # Each cluster will have 1 or more models, even if the confidence rating is too low to display a marker in Rviz # If the household objects database is not connected, the model list will be empty detect_table.return_models = recognize_objects # Number of models to return for each object cluster # If there is more than 1 model with high confidence, they will all be displayed in Rviz detect_table.num_models = 1 @smach.cb_interface(input_keys=[], output_keys=['tabletop_info']) def table_response_cb(userdata, response): if response.detection.result == response.detection.SUCCESS: if recognize_objects: userdata.tabletop_info = [response.detection] # Return table and detected objects else: userdata.tabletop_info = [response.detection.table] # Return only the table return succeeded userdata.tabletop_info = [response.detection] return aborted StateMachine.add('TABLE_DETECTION', ServiceState('/object_detection', TabletopDetection, request=detect_table, response_cb=table_response_cb, output_keys=['tabletop_info']), remapping={'tabletop_info': 'tabletop_info'}, transitions={succeeded: succeeded, aborted: 'CHECK_RESPONSE'}) # Check why it failed @smach.cb_interface(input_keys=['tabletop_info'], output_keys=[], outcomes=[succeeded, 'no_table']) def check_tabletop_data(userdata): if userdata.tabletop_info[0].result == userdata.tabletop_info.NO_TABLE: #or userdata.tabletop_info.result == userdata.tabletop_info.NO_CLOUD_RECEIVED: return 'no_table' return aborted StateMachine.add('CHECK_RESPONSE', CBState(check_tabletop_data, input_keys=['in_map', 'in_room_name'], output_keys=['out_route'], outcomes=[aborted, 'no_table']), remapping={'tabletop_info': 'tabletop_info'}, transitions={'no_table': 'no_table', aborted: aborted})
def __detect(self, detector): req = TabletopDetectionRequest() req.return_clusters = True req.return_models = True try: reply = detector(req) except rospy.ServiceException, e: rospy.logerr("error when calling object_detection: %s"%e) return None
def __detect(self, detector): req = TabletopDetectionRequest() req.return_clusters = True req.return_models = True try: reply = detector(req) except rospy.ServiceException, e: rospy.logerr("error when calling object_detection: %s" % e) return None
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 call_object_detector(): req = TabletopDetectionRequest() req.return_clusters = 1 req.return_models = 0 service_name = "/object_detection" rospy.loginfo("waiting for object_detection service") rospy.wait_for_service(service_name) serv = rospy.ServiceProxy(service_name, TabletopDetection) try: res = serv(req) except rospy.ServiceException, e: rospy.logerr("error when calling object_detection: %s"%e) return 0
def call_object_detector(): req = TabletopDetectionRequest() req.return_clusters = 1 req.return_models = 0 service_name = "/object_detection" rospy.loginfo("waiting for object_detection service") rospy.wait_for_service(service_name) serv = rospy.ServiceProxy(service_name, TabletopDetection) try: res = serv(req) except rospy.ServiceException, e: rospy.logerr("error when calling object_detection: %s" % e) return 0
def find_table(self): rospy.loginfo("calling tabletop detection") det_req = TabletopDetectionRequest() det_req.return_clusters = 0 det_req.return_models = 0 det_req.num_models = 0 self.detected_table = None #call tabletop detection, get a detection result try: det_res = self.grasper_detect_srv(det_req) except rospy.ServiceException, e: rospy.logerr("error when calling %s: %s"%(self.grasper_detect_name, e)) rospy.signal_shutdown("no table top detection service") return None
def call_tabletop_object_detection(self): rospy.loginfo("object detection service call.") det_req = TabletopDetectionRequest() det_req.return_clusters = 1 det_req.return_models = 1 det_req.num_models = 0 # call tabletop detection, get a detection result for try_num in range(3): try: det_res = self.detect_srv(det_req) except rospy.ServiceException, e: rospy.logerr("error when calling %s: %s" % ("/object_detection", e)) self.throw_exception() return ([], None) if det_res.detection.result == det_res.detection.SUCCESS: rospy.loginfo("tabletop detection reports success") break else: rospy.logerr("tabletop detection failed with error. Trying again")
def detect_and_pickup(self): rospy.loginfo("calling tabletop detection") det_req = TabletopDetectionRequest() det_req.return_clusters = 1 det_req.return_models = 1 #det_req.num_models = num_models #call tabletop detection, get a detection result (attempt 3 times) for try_num in range(3): try: det_res = self.grasper_detect_srv(det_req) except rospy.ServiceException, e: rospy.logerr("error when calling %s: %s"%(self.grasper_detect_name, e)) self.throw_exception() return ([], None) if det_res.detection.result == det_res.detection.SUCCESS: rospy.loginfo("tabletop detection reports success") break else: rospy.logerr("tabletop detection failed with error %s, trying again"%det_res.detection.result)
def call_tabletop_object_detection(self): rospy.loginfo("object detection service call.") det_req = TabletopDetectionRequest() det_req.return_clusters = 1 det_req.return_models = 1 det_req.num_models = 0 #call tabletop detection, get a detection result for try_num in range(3): try: det_res = self.detect_srv(det_req) except rospy.ServiceException, e: rospy.logerr("error when calling %s: %s" % ("/object_detection", e)) self.throw_exception() return ([], None) if det_res.detection.result == det_res.detection.SUCCESS: rospy.loginfo("tabletop detection reports success") break else: rospy.logerr( "tabletop detection failed with error. Trying again")
rospy.signal_shutdown("") sys.exit() collision_processing_srv = rospy.ServiceProxy(COLLISION_PROCESSING_SERVICE_NAME, TabletopCollisionMapProcessing) #pickup client pickup_client = actionlib.SimpleActionClient(PICKUP_ACTION_NAME, PickupAction) while not pickup_client.wait_for_server(rospy.Duration(2.0)): rospy.loginfo("Witing for action client %s", PICKUP_ACTION_NAME) #place client place_client = actionlib.SimpleActionClient(PLACE_ACTION_NAME, PlaceAction) while not place_client.wait_for_server(rospy.Duration(2.0)): rospy.loginfo("Witing for action client %s", PICKUP_ACTION_NAME) rospy.loginfo("Calling tabletop detector") detection_call = TabletopDetectionRequest() detection_call.return_clusters = True detection_call.return_models = True detection_reply = object_detection_srv.call(detection_call) if len(detection_reply.detection.clusters) == 0: rospy.logerr("No objects found!") rospy.signal_shutdown("") sys.exit() rospy.loginfo("%d objects detected" % len(detection_reply.detection.clusters)) # detection_reply.detection.cluster_model_indices = tuple(xrange(len(detection_reply.detection.clusters))) rospy.loginfo("Calling collision map processing") processing_call = TabletopCollisionMapProcessingRequest() processing_call.detection_result = detection_reply.detection
def pick_up(self, whicharm="l", NUM_TRIES=5, near_pose=None): self.pickup_client.cancel_goal() for i in range(NUM_TRIES): rospy.loginfo("Attempt %d to grasp " % i ) # call the tabletop detection rospy.loginfo("Calling tabletop detector") detection_call = TabletopDetectionRequest() # we want recognized database objects returned # set this to false if you are using the pipeline without the database detection_call.return_clusters = True #we want the individual object point clouds returned as well detection_call.return_models = True detection_call.num_models = 1 detection_call_response = self.object_detection_srv(detection_call).detection if not detection_call_response: rospy.loginfo("Tabletop detection service failed") success=False; break if detection_call_response.result != detection_call_response.SUCCESS: rospy.loginfo("Tabletop detection returned error code %d", detection_call_response.result) success=False; break if len(detection_call_response.clusters)==0\ and len(detection_call_response.models)==0 : rospy.loginfo("The tabletop detector detected the table, " "but found no objects") success=False; break # call collision map processing rospy.loginfo("Calling collision map processing") processing_call = TabletopCollisionMapProcessingRequest() # pass the result of the tabletop detection processing_call.detection_result = detection_call_response print detection_call_response.models import pdb;pdb.set_trace() # ask for the existing map and collision models to be reset processing_call.reset_collision_models = True processing_call.reset_attached_models = True #ask for the results to be returned in base link frame processing_call.desired_frame = "base_link" processing_call_response = self.collision_processing_srv(processing_call) if not processing_call_response: rospy.loginfo("Collision map processing service failed") success=False; break #the collision map processor returns instances of graspable objects if len(processing_call_response.graspable_objects) == 0: rospy.loginfo("Collision map processing returned no graspable objects") success=False; break graspable_objects = processing_call_response.graspable_objects # sort graspable objects import pdb; # sort graspable objects objects_with_offsets = [None]*len(graspable_objects) #near_point = np.array(list(near_pose[:3]) + [1.0]) for i, obj in enumerate(graspable_objects): pdb.set_trace() points = point_cloud_to_mat(obj.cluster) #centroid =np.reshape(np.mean(points, axis=1), 4) - near_point #offset = np.linalg.norm(centroid - near_point) #objects_with_offsets[i] = (offset,obj) #ordering = sorted(objects_with_offsets, key=lambda item:float((item[0]))) #graspable_objects = [items[1] for items in ordering] #print [items[0] for items in ordering] for i, grasp_object in enumerate(graspable_objects): # call object pickup rospy.loginfo("Calling the pickup action"); pickup_goal = PickupGoal() # pass one of the graspable objects returned # by the collision map processor #pickup_goal.target = processing_call_response.graspable_objects[0] pickup_goal.target = grasp_object # pass the name that the object has in the collision environment # this name was also returned by the collision map processor pickup_goal.collision_object_name = \ processing_call_response.collision_object_names[i] # pass the collision name of the table, also returned by the collision # map processor pickup_goal.collision_support_surface_name = \ processing_call_response.collision_support_surface_name # pick up the object with the left arm if whicharm=="l": pickup_goal.arm_name = "left_arm" else: pickup_goal.arm_name = "right_arm" # we will be lifting the object along the "vertical" direction # which is along the z axis in the base_link frame direction = Vector3Stamped() direction.header.stamp = rospy.Time.now() direction.header.frame_id = "base_link" direction.vector.x = 0 direction.vector.y = 0 direction.vector.z = 1 pickup_goal.lift.direction = direction #request a vertical lift of 10cm after grasping the object pickup_goal.lift.desired_distance = 0.1; pickup_goal.lift.min_distance = 0.05; #do not use tactile-based grasping or tactile-based lift pickup_goal.use_reactive_lift = False; pickup_goal.use_reactive_execution = False; #pickup_goal.allow_gripper_support_collision = True #pickup_goal.ignore_collisions = True #send the goal rospy.loginfo("Waiting for the pickup action...") self.pickup_client.send_goal_and_wait(pickup_goal,rospy.Duration(15)) pickup_result =self.pickup_client.get_result() assert isinstance(pickup_result, PickupResult) success = pickup_result.manipulation_result.value ==\ pickup_result.manipulation_result.SUCCESS if success: rospy.loginfo("Success! Grasped Object.") return success if not success: rospy.loginfo("failed to grasp object") return success
def __init__(self, distance_treshold=0.5, recognize_objects=False): smach.StateMachine.__init__(self, [succeeded, preempted, aborted, 'no_table'], input_keys=[], output_keys=['tabletop_info']) with self: @smach.cb_interface(input_keys=[], output_keys=['out_tabletop_info', 'out_n_looks'], outcomes=[succeeded]) def init_udata(userdata): userdata.out_n_looks = 0 # Number of looks done userdata.out_tabletop_info = [] # To be filled in the GATHER_TTOP_DATA state return succeeded StateMachine.add('INIT_USERDATA', CBState(init_udata, output_keys=['out_tabletop_info', 'out_n_looks'], outcomes=[succeeded]), remapping={'out_tabletop_info': 'tabletop_info', 'out_n_looks': 'n_looks'}, transitions={succeeded: 'CONTROL_STATE'}) @smach.cb_interface(input_keys=['in_n_looks', 'in_tabletop_info'], output_keys=['look_goal', 'out_n_looks'], outcomes=[succeeded, 'next', 'no_table']) def control_cb(userdata): if userdata.in_n_looks == 0: # first look userdata.look_goal = look_right().trajectory elif userdata.in_n_looks == 1: # Second look userdata.look_goal = look_straight().trajectory elif userdata.in_n_looks == 2: # Third look userdata.look_goal = look_left().trajectory else: # Finished all the looks if not userdata.in_tabletop_info: return 'no_table' return succeeded userdata.out_n_looks = userdata.in_n_looks + 1 return 'next' StateMachine.add('CONTROL_STATE', CBState(control_cb, input_keys=['n_looks'], output_keys=['look_goal'], outcomes=[succeeded, 'next', 'no_table']), transitions={'next': 'LOOK_STATE', succeeded: succeeded, 'no_table': 'no_table'}, remapping={'in_n_looks': 'n_looks', 'out_n_looks': 'n_looks', 'look_goal': 'look_goal', 'in_tabletop_info': 'tabletop_info'}) # Move the head before detecting table StateMachine.add('LOOK_STATE', SimpleActionState(HEAD_ACTION_NAME, HEAD_ACTION_TYPE, goal_slots=['trajectory']), transitions={succeeded: 'TAKE_SNAPSHOT'}, remapping={'trajectory': 'look_goal'}) StateMachine.add('TAKE_SNAPSHOT', TakeXtionSnapshot(WAIT_BEFORE_SNAPSHOT), transitions={succeeded: 'TABLE_DETECTION'}) # TabletopDetection call detect_table = TabletopDetectionRequest() # Return the pointcloud (cluster) for each detected object # if this is false, you have no data to pass to tabletop_collision_map_processing node detect_table.return_clusters = recognize_objects # Return matching object models # Each cluster will have 1 or more models, even if the confidence rating is too low to display a marker in Rviz # If the household objects database is not connected, the model list will be empty detect_table.return_models = recognize_objects # Number of models to return for each object cluster # If there is more than 1 model with high confidence, they will all be displayed in Rviz detect_table.num_models = 1 @smach.cb_interface(input_keys=[], output_keys=['tabletop_response']) def table_response_cb(userdata, response): if response.detection.result == response.detection.SUCCESS: if recognize_objects: userdata.tabletop_response = response.detection # Return table and detected objects else: userdata.tabletop_response = response.detection.table # Return only the table return succeeded userdata.tabletop_response = response.detection return aborted StateMachine.add('TABLE_DETECTION', ServiceState('/object_detection', TabletopDetection, request=detect_table, response_cb=table_response_cb, output_keys=['tabletop_response']), remapping={'tabletop_response': 'tabletop_response'}, transitions={succeeded: 'GATHER_TTOP_DATA', aborted: 'CHECK_FAILURE'}) # Check why it failed @smach.cb_interface(input_keys=['tabletop_response'], output_keys=[], outcomes=[succeeded, 'no_table']) def check_if_no_table(userdata): if userdata.tabletop_response.result == userdata.tabletop_response.NO_TABLE: #or userdata.tabletop_response.result == userdata.tabletop_response.NO_CLOUD_RECEIVED: return 'no_table' print 'Tabletop detection aborted with status: %s' % userdata.tabletop_response.result return aborted StateMachine.add('CHECK_FAILURE', CBState(check_if_no_table, input_keys=[], output_keys=['out_route'], outcomes=[aborted, 'no_table']), remapping={'tabletop_response': 'tabletop_response'}, transitions={'no_table': 'CONTROL_STATE', aborted: aborted}) @smach.cb_interface(input_keys=['in_tabletop_info', 'in_tabletop_response'], output_keys=['out_tabletop_info'], outcomes=[succeeded]) def check_gather_data(userdata): table_data = userdata.in_tabletop_response table_orient = table_data.pose.pose.orientation if table_orient.w < 0.95 or table_orient.x > 0.05 or table_orient.y > 0.05 or table_orient.z > 0.05: # The surface is not horizontal, so we don't care about it. print 'It was not a table...' return succeeded table_pose = ((table_data.x_min+table_data.x_max)/2, (table_data.y_min+table_data.y_max)/2) for ti in userdata.in_tabletop_info: tp = ((ti.x_min+ti.x_max)/2, (ti.y_min+ti.y_max)/2) if ofb_utils.euclidean_distance(tp, table_pose) <= distance_treshold: break else: # The loop ended without breaking userdata.in_tabletop_info.append(userdata.in_tabletop_response) userdata.out_tabletop_info = userdata.in_tabletop_info return succeeded StateMachine.add('GATHER_TTOP_DATA', CBState(check_gather_data, input_keys=['in_tabletop_info', 'tabletop_response'], output_keys=['out_tabletop_info'], outcomes=[succeeded]), remapping={'in_tabletop_response': 'tabletop_response', 'in_tabletop_info': 'tabletop_info', 'out_tabletop_info': 'tabletop_info'}, transitions={succeeded: 'CONTROL_STATE'})
rospy.loginfo("waiting for object_detection service") rospy.wait_for_service(grasper_detect_name) rospy.loginfo("waiting for collision_map_processing service") rospy.wait_for_service(collision_map_processing_name) # create service proxies grasper_detect_srv = rospy.ServiceProxy(grasper_detect_name, TabletopDetection) collision_map_processing_srv = rospy.ServiceProxy( collision_map_processing_name, TabletopCollisionMapProcessing) ''' object detection ''' rospy.loginfo("detecting objects...") # set up detection request det_req = TabletopDetectionRequest() # we want recognized database objects returned, set this to 0 if using the pipeline without the database det_req.return_models = 1 # we want the individual object point clouds returned as well det_req.return_clusters = 1 # call tabletop detection, get a detection result try: det_res = grasper_detect_srv(det_req) except rospy.ServiceException, e: rospy.logerr("error when calling %s: %s" % (grasper_detect_name, e)) sys.exit() if det_res.detection.result == det_res.detection.SUCCESS: rospy.loginfo("tabletop detection reports success") else: rospy.logerr("tabletop detection failed with error code %d" %
collision_processing_srv = rospy.ServiceProxy( COLLISION_PROCESSING_SERVICE_NAME, TabletopCollisionMapProcessing) #pickup client pickup_client = actionlib.SimpleActionClient(PICKUP_ACTION_NAME, PickupAction) while not pickup_client.wait_for_server(rospy.Duration(2.0)): rospy.loginfo("Witing for action client %s", PICKUP_ACTION_NAME) #place client place_client = actionlib.SimpleActionClient(PLACE_ACTION_NAME, PlaceAction) while not place_client.wait_for_server(rospy.Duration(2.0)): rospy.loginfo("Witing for action client %s", PICKUP_ACTION_NAME) rospy.loginfo("Calling tabletop detector") detection_call = TabletopDetectionRequest() detection_call.return_clusters = True detection_call.return_models = True detection_reply = object_detection_srv.call(detection_call) if len(detection_reply.detection.clusters) == 0: rospy.logerr("No objects found!") rospy.signal_shutdown("") sys.exit() rospy.loginfo("%d objects detected" % len(detection_reply.detection.clusters)) # detection_reply.detection.cluster_model_indices = tuple(xrange(len(detection_reply.detection.clusters))) rospy.loginfo("Calling collision map processing") processing_call = TabletopCollisionMapProcessingRequest()