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():
'''
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()