def BinBBDims(binNum):
lW = 0.035
wW = 0.03
wH = 0.075
lH = 0.04
bin_cnstr = get_bin_cnstr()
bin_cnstr_offset = [[0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0],
[0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0],
[0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0],
[0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0]]
new_bin_cnstr = (np.array(bin_cnstr) + np.array(bin_cnstr_offset)).tolist()
nrows = len(new_bin_cnstr)
minHeight = new_bin_cnstr[binNum][4] + wH+lH
maxHeight = new_bin_cnstr[binNum][5] - wH
leftWall = new_bin_cnstr[binNum][1] - wW
rightWall = new_bin_cnstr[binNum][0] + wW
return( minHeight, maxHeight, leftWall, rightWall)
def getSides(objX,listener):
bin_cnstr = get_bin_cnstr()
#X_list=[-0.42926,-0.1494,0.1554,0.42926]
X_list=[bin_cnstr[2][0],bin_cnstr[2][1],bin_cnstr[1][1],bin_cnstr[0][1]]
print X_list
#AdjustedX_list=[-0.42926+.045,-0.1494,0.1554,0.42926-.045] #adjust for the lips on far right/left sides
AdjustedX_list=[bin_cnstr[2][0]+.045,bin_cnstr[2][1],bin_cnstr[1][1],bin_cnstr[0][1]-.045]
print AdjustedX_list
objXVec = [0,objX,0]
newObjXVec = coordinateFrameTransform(objXVec, 'map', 'shelf', listener)
newObjX = newObjXVec.pose.position.x
for i in range(0, len(X_list)-1):
if (X_list[i]<=newObjX) and (newObjX<=X_list[i+1]):
shelf_gap=AdjustedX_list[i+1]-AdjustedX_list[i]
shelfXVec=[AdjustedX_list[i+1],0,0]
newShelfXVec=coordinateFrameTransform(shelfXVec, 'shelf', 'map', listener)
return (newShelfXVec.pose.position.y-shelf_gap,newShelfXVec.pose.position.y)
return 'failed'
def BinBBDims(binNum):
lW = 0.035 # side lip width
wW = 0.045 # wrist width
wH = 0.075 # wrist height
lH = 0.04 # bottom lip height
bin_cnstr = get_bin_cnstr()
bin_cnstr_offset = [[0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0],
[0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0],
[0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0],
[0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0]]
new_bin_cnstr = (np.array(bin_cnstr) + np.array(bin_cnstr_offset)).tolist()
nrows = len(new_bin_cnstr)
minHeight = new_bin_cnstr[binNum][4] + wH+lH
maxHeight = new_bin_cnstr[binNum][5] - wH
leftWall = new_bin_cnstr[binNum][1] - wW
rightWall = new_bin_cnstr[binNum][0] + wW
return( minHeight, maxHeight, leftWall, rightWall)
def getCeilHeight(objHeight, listener):
bin_cnstr = get_bin_cnstr()
#height_list=[0.800027,1.06673,1.29533,1.52393,1.79063]
height_list = [
bin_cnstr[11][4], bin_cnstr[8][5], bin_cnstr[5][5], bin_cnstr[2][5],
bin_cnstr[11][6]
]
print '[SucSide] height_list:', height_list
objHeightVec = [0, 0,
objHeight] # define over the lip distance in shelf frame
newObjHeightVec = coordinateFrameTransform(objHeightVec, 'map', 'shelf',
listener)
newObjHeight = newObjHeightVec.pose.position.z
print '[SucSide] objHeight:', objHeight
for i in range(0, len(height_list) - 1):
if (height_list[i] <= newObjHeight) and (newObjHeight <=
height_list[i + 1]):
shelf_gap = height_list[i + 1] - height_list[i]
shelfHeightVec = [0, 0, height_list[i + 1]]
newShelfHeightVec = coordinateFrameTransform(
shelfHeightVec, 'shelf', 'map', listener)
return (newShelfHeightVec.pose.position.z - shelf_gap,
newShelfHeightVec.pose.position.z)
return '[SucSide] getCeilHeight failed'
def getSides(binNum,listener):
bin_cnstr = get_bin_cnstr()
AdjustedX_list=[bin_cnstr[2][0]+.045,bin_cnstr[2][1],bin_cnstr[1][1],bin_cnstr[0][1]-.045]
print 'AdjustedX_list:', AdjustedX_list
i=2-(binNum%3)
shelf_gap=AdjustedX_list[i+1]-AdjustedX_list[i]
shelfXVec=[AdjustedX_list[i+1],0,0]
newShelfXVec=coordinateFrameTransform(shelfXVec, 'shelf', 'map', listener)
return (newShelfXVec.pose.position.y-shelf_gap,newShelfXVec.pose.position.y)
def getBinSideWalls(FirstBin):
bin_cnstr = get_bin_cnstr()
lW = 0
bin_cnstr_offset = [[0, -lW, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0], [0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [+lW, 0, 0, 0, 0, 0],
[0, -lW, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0], [0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [+lW, 0, 0, 0, 0, 0]]
new_bin_cnstr = (np.array(bin_cnstr) + np.array(bin_cnstr_offset)).tolist()
leftWall = new_bin_cnstr[FirstBin][1]
rightWall = new_bin_cnstr[FirstBin][0]
return leftWall, rightWall
def getSides(binNum, listener):
bin_cnstr = get_bin_cnstr()
AdjustedX_list = [
bin_cnstr[2][0] + .045, bin_cnstr[2][1], bin_cnstr[1][1],
bin_cnstr[0][1] - .045
]
print '[SucDown] AdjustedX_list:', AdjustedX_list
i = 2 - (binNum % 3)
shelf_gap = AdjustedX_list[i + 1] - AdjustedX_list[i]
shelfXVec = [AdjustedX_list[i + 1], 0, 0]
newShelfXVec = coordinateFrameTransform(shelfXVec, 'shelf', 'map',
listener)
return (newShelfXVec.pose.position.y - shelf_gap,
newShelfXVec.pose.position.y)
def getBinSideWalls(FirstBin):
bin_cnstr = get_bin_cnstr()
lW = 0
bin_cnstr_offset = [[0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0],
[0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0],
[0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0],
[0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0]]
new_bin_cnstr = (np.array(bin_cnstr) + np.array(bin_cnstr_offset)).tolist()
leftWall = new_bin_cnstr[FirstBin][1]
rightWall = new_bin_cnstr[FirstBin][0]
return leftWall, rightWall
def getCeilHeight(objHeight,listener):
bin_cnstr = get_bin_cnstr()
#height_list=[0.800027,1.06673,1.29533,1.52393,1.79063]
height_list=[bin_cnstr[11][4],bin_cnstr[8][5],bin_cnstr[5][5],bin_cnstr[2][5],bin_cnstr[11][6]]
print height_list
objHeightVec = [0,0,objHeight] # define over the lip distance in shelf frame
newObjHeightVec = coordinateFrameTransform(objHeightVec, 'map', 'shelf', listener)
newObjHeight = newObjHeightVec.pose.position.z
print objHeight
for i in range(0, len(height_list)-1):
if (height_list[i]<=newObjHeight) and (newObjHeight<=height_list[i+1]):
shelf_gap=height_list[i+1]-height_list[i]
shelfHeightVec=[0,0,height_list[i+1]]
newShelfHeightVec=coordinateFrameTransform(shelfHeightVec, 'shelf', 'map', listener)
return (newShelfHeightVec.pose.position.z-shelf_gap,newShelfHeightVec.pose.position.z)
return 'failed'
def BinBBDims(binNum):
lW = 0.035 # side lip width
wW = 0.045 # wrist width
wH = 0.075 # wrist height
lH = 0.04 # bottom lip height
bin_cnstr = get_bin_cnstr()
bin_cnstr_offset = [[0, -lW, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0], [0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [+lW, 0, 0, 0, 0, 0],
[0, -lW, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[+lW, 0, 0, 0, 0, 0], [0, -lW, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [+lW, 0, 0, 0, 0, 0]]
new_bin_cnstr = (np.array(bin_cnstr) + np.array(bin_cnstr_offset)).tolist()
nrows = len(new_bin_cnstr)
minHeight = new_bin_cnstr[binNum][4] + wH + lH
maxHeight = new_bin_cnstr[binNum][5] - wH
leftWall = new_bin_cnstr[binNum][1] - wW
rightWall = new_bin_cnstr[binNum][0] + wW
return (minHeight, maxHeight, leftWall, rightWall)
def _detectOneObject(obj_id, bin_num, kinect_num):
global _detect_one_object_srv
global br
print 'In', bcolors.WARNING, '_detectOneObject', bcolors.ENDC, 'obj_ids:', obj_id, 'bin_num:', bin_num
# filter the point cloud
pc, foreground_mask = get_filtered_pointcloud([obj_id], bin_num, kinect_num)
if pc is None:
return (None, None)
# string[] model_names
# sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480)
# bool[] foreground_mask
# bool find_exact_object_list # Set to true if you only want to consider scene hypotheses that contain exactly the objects in 'model_names'
# ObjectConstraint[] constraints # These apply to all objects
# ---
# SceneHypothesis[] detections
# 2. prepare constraints
bin_cnstr = get_bin_cnstr()[bin_num] # a list of right \ # left \ # back \ # front \ # bottom \ # top
ccnstr = []
tol = 0.9 # larger is more strict
(trans,rot) = lookupTransform(pc.header.frame_id, '/shelf', _tflistener)
# 2.1 right
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([1,0,0], [bin_cnstr[0],0,0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.2 left
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([-1,0,0], [bin_cnstr[1],0,0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.3 back
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,1,0], [0,bin_cnstr[2],0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.4 front
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,-1,0], [0,bin_cnstr[3],0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.5 floor
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,0,1], [0,0,bin_cnstr[4]], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.6 top
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,0,-1], [0,0,bin_cnstr[5]], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.7 on floor
floor_thick = 0.03
ccnstr.append( createCapsenConstraint(ObjectConstraint.SUPPORTING_PLANE, transformPlane([0,0,1], [0,0, bin_cnstr[4]-floor_thick/2], trans,rot, pc.header.frame_id), tol, bin_num) )
# string model_name
# sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480)
# bool[] foreground_mask
# ObjectConstraint[] constraints
# geometry_msgs/Pose true_pose # for testing
# ---
#
# ObjectHypothesis[] detections
with Timer('detect_one_object'):
# detect using capsen
service_name = '/detection_service/detect_one_object'
req = DetectOneObjectRequest()
req.model_name = obj_id
req.cloud = pc
req.constraints = ccnstr
req.foreground_mask = foreground_mask
#req.foreground_mask = [True for i in xrange(req.cloud.height*req.cloud.width)]
print 'Waiting for service up: ', service_name
rospy.wait_for_service(service_name)
try:
print 'Calling service:', service_name
ret = _detect_one_object_srv(req)
# ret.detections is a list of capsen_vision/ObjectHypothesis
# string name
# geometry_msgs/Pose pose
# float32 score
# float32[] score_components
if len(ret.detections)>0:
print len(ret.detections), 'ObjectHypothesis returned, max score', ret.detections[0].score
for i in range(len(ret.detections)):
poselist_capsen_world = poseTransform(pose2list(ret.detections[i].pose), pc.header.frame_id, 'map', _tflistener)
cap_T_our = get_obj_capsentf(obj_id) # x,y,z,qx,qy,qz,qw
poselist_world = transformBack(cap_T_our, poselist_capsen_world) # transform to our desired pose
# check whether inside bin
poselist_shelf = poseTransform(poselist_world, 'map', 'shelf', _tflistener)
if inside_bin(poselist_shelf[0:3], bin_num):
#pubFrame(br, poselist_world, 'obj', 'map')
return (poselist_world, ret.detections[i].score)
else:
print 'reject hypo', i, 'because it is outside the target bin'
print 'No ObjectHypothesis satisfy hard bin constraint'
return (None, None)
else:
print 'No ObjectHypothesis returned'
return (None, None)
except:
print 'Calling service:', service_name, 'failed'
print 'encounters errors:', traceback.format_exc()
return (None, None)
def _detectObjects(obj_ids, bin_num, kinect_num):
# return pose, retScore
global _detect_objects_srv
global br
global _tflistener
print 'In', '_detectObjects', 'obj_ids:', obj_ids, 'bin_num:', bin_num
# 1. filter the point cloud
pc, foreground_mask = get_filtered_pointcloud(obj_ids, bin_num, kinect_num) # need to pass in list
if pc is None or foreground_mask is None:
return (None, None)
# 2. prepare constraints
bin_cnstr = get_bin_cnstr()[bin_num] # a list of right \ # left \ # back \ # front \ # bottom \ # top
ccnstr = []
tol = 0.9 # larger is more strict
(trans,rot) = lookupTransform(pc.header.frame_id, '/shelf', _tflistener)
# 2.1 right
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([1,0,0], [bin_cnstr[0],0,0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.2 left
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([-1,0,0], [bin_cnstr[1],0,0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.3 back
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,1,0], [0,bin_cnstr[2],0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.4 front
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,-1,0], [0,bin_cnstr[3],0], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.5 floor
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,0,1], [0,0,bin_cnstr[4]], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.6 top
ccnstr.append( createCapsenConstraint(ObjectConstraint.HALF_SPACE, transformPlane([0,0,-1], [0,0,bin_cnstr[5]], trans,rot, pc.header.frame_id), tol, bin_num) )
# 2.7 on floor
floor_thick = 0.03
ccnstr.append( createCapsenConstraint(ObjectConstraint.SUPPORTING_PLANE, transformPlane([0,0,1], [0,0, bin_cnstr[4]+floor_thick*2], trans,rot, pc.header.frame_id), tol, bin_num) )
#visualizeConstraint(ccnstr[6], pc.header.frame_id)
#pause()
# 3. detect using capsen
with Timer('detect_objects'):
service_name = '/detection_service/detect_objects'
req = DetectObjectsRequest()
req.model_names = obj_ids
req.constraints = ccnstr
req.cloud = pc
req.foreground_mask = foreground_mask
sum_pt = sum(foreground_mask)
if allFalse(foreground_mask, sum_pt):
return (None, None)
foreground_mask = subsample(foreground_mask, sum_pt)
# outputfile = '/tmp/foreground_mask'
# with open(outputfile, 'w') as outfile:
# json.dump(foreground_mask, outfile)
# pause()
#req.foreground_mask = [True for i in xrange(req.cloud.height*req.cloud.width)] # hack
req.find_exact_object_list = True
print '\tWaiting for service up: ', service_name
rospy.wait_for_service(service_name)
#pdb.set_trace()
try:
print '\tCalling service:', service_name
ret = _detect_objects_srv(req)
# ret.detections is a list of capsen_vision/SceneHypothesis
# [capsen_vision/SceneHypothesis]:
# std_msgs/Header header
# uint32 seq
# time stamp
# string frame_id
# capsen_vision/ObjectHypothesis[] objects
# string name
# geometry_msgs/Pose pose
# geometry_msgs/Point position
# float64 x
# float64 y
# float64 z
# geometry_msgs/Quaternion orientation
# float64 x
# float64 y
# float64 z
# float64 w
# float32 score
# float32[] score_components
# float32[2] errors
# float32 score
if len(ret.detections)>0:
print '\t', len(ret.detections), 'SceneHypothesis returned, max score', ret.detections[0].score
#print ret.detections
for i in range(len(ret.detections)):
scene = ret.detections[i]
nobj = len(scene.objects)
scene_desired = transformObjectsFromCapsenToDesiredFrame(scene, pc.header.frame_id)
if allObjectsInsideBin(scene_desired, bin_num):
return (scene_desired.objects, scene_desired.score)
#else:
#print 'reject scene hypo', i, 'because one object of it is outside the target bin'
print '\tNo SceneHypothesis satisfy hard bin constraint'
return (None, None)
else:
print '\tNo SceneHypothesis returned'
return (None, None)
except:
print '\tCalling service:', service_name, 'failed'
print '\tencounters errors:', traceback.format_exc()
return (None, None)
req = GetObjectPointCloud2Request()
req.bin_num = bin_num
req.obj_id = obj_ids[0] # peterkty: need to pass in a list
print '\tWaiting for service up: ', service_name
rospy.wait_for_service(service_name)
try:
print '\tCalling service:', service_name
response = _pointcloud2_service_srv[kinect_num-1](req)
return response.pc2, response.foreground_mask
except:
print '\tCalling service:', service_name, 'failed'
print '\tencounters errors:', traceback.format_exc()
print '\tDid you call capsen.capsen.init()? Is camera connection good?'
return None, None
bin_cnstr = get_bin_cnstr()
def inside_bin(point, bin_num):
cnstr = bin_cnstr[bin_num]
# return (
# x > cnstr[0]+0.02 && x < cnstr[1]-0.02 &&
# y > cnstr[2]+0.1 && y < cnstr[3]-0.01 &&
# z > cnstr[4]+0.00 && z < cnstr[5]-0.02/* && z < cnstr[4]+obj_max_height*/);
if point[0] > cnstr[0]+0.015 and point[0] < cnstr[1]-0.015 and \
point[1] > cnstr[2]+0.1 and point[1] < cnstr[3]-0.01 and \
point[2] > cnstr[4]-0.02 and point[2] < cnstr[5]-0.02:
return True
# #todo: make the numbers out of python code
def _detectOneObject(obj_id, bin_num, kinect_num):
global _detect_one_object_srv
global br
print 'In', bcolors.WARNING, '_detectOneObject', bcolors.ENDC, 'obj_ids:', obj_id, 'bin_num:', bin_num
# filter the point cloud
pc, foreground_mask = get_filtered_pointcloud([obj_id], bin_num,
kinect_num)
if pc is None:
return (None, None)
# string[] model_names
# sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480)
# bool[] foreground_mask
# bool find_exact_object_list # Set to true if you only want to consider scene hypotheses that contain exactly the objects in 'model_names'
# ObjectConstraint[] constraints # These apply to all objects
# ---
# SceneHypothesis[] detections
# 2. prepare constraints
bin_cnstr = get_bin_cnstr(
)[bin_num] # a list of right \ # left \ # back \ # front \ # bottom \ # top
ccnstr = []
tol = 0.9 # larger is more strict
(trans, rot) = lookupTransform(pc.header.frame_id, '/shelf', _tflistener)
# 2.1 right
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([1, 0, 0], [bin_cnstr[0], 0, 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.2 left
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([-1, 0, 0], [bin_cnstr[1], 0, 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.3 back
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, 1, 0], [0, bin_cnstr[2], 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.4 front
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, -1, 0], [0, bin_cnstr[3], 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.5 floor
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, 0, 1], [0, 0, bin_cnstr[4]], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.6 top
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, 0, -1], [0, 0, bin_cnstr[5]], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.7 on floor
floor_thick = 0.03
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.SUPPORTING_PLANE,
transformPlane([0, 0, 1], [0, 0, bin_cnstr[4] - floor_thick / 2],
trans, rot, pc.header.frame_id), tol, bin_num))
# string model_name
# sensor_msgs/PointCloud2 cloud # Note: this must be an organized point cloud (e.g., 640x480)
# bool[] foreground_mask
# ObjectConstraint[] constraints
# geometry_msgs/Pose true_pose # for testing
# ---
#
# ObjectHypothesis[] detections
with Timer('detect_one_object'):
# detect using capsen
service_name = '/detection_service/detect_one_object'
req = DetectOneObjectRequest()
req.model_name = obj_id
req.cloud = pc
req.constraints = ccnstr
req.foreground_mask = foreground_mask
#req.foreground_mask = [True for i in xrange(req.cloud.height*req.cloud.width)]
print 'Waiting for service up: ', service_name
rospy.wait_for_service(service_name)
try:
print 'Calling service:', service_name
ret = _detect_one_object_srv(req)
# ret.detections is a list of capsen_vision/ObjectHypothesis
# string name
# geometry_msgs/Pose pose
# float32 score
# float32[] score_components
if len(ret.detections) > 0:
print len(
ret.detections
), 'ObjectHypothesis returned, max score', ret.detections[
0].score
for i in range(len(ret.detections)):
poselist_capsen_world = poseTransform(
pose2list(ret.detections[i].pose), pc.header.frame_id,
'map', _tflistener)
cap_T_our = get_obj_capsentf(obj_id) # x,y,z,qx,qy,qz,qw
poselist_world = transformBack(
cap_T_our,
poselist_capsen_world) # transform to our desired pose
# check whether inside bin
poselist_shelf = poseTransform(poselist_world, 'map',
'shelf', _tflistener)
if inside_bin(poselist_shelf[0:3], bin_num):
#pubFrame(br, poselist_world, 'obj', 'map')
return (poselist_world, ret.detections[i].score)
else:
print 'reject hypo', i, 'because it is outside the target bin'
print 'No ObjectHypothesis satisfy hard bin constraint'
return (None, None)
else:
print 'No ObjectHypothesis returned'
return (None, None)
except:
print 'Calling service:', service_name, 'failed'
print 'encounters errors:', traceback.format_exc()
return (None, None)
def _detectObjects(obj_ids, bin_num, kinect_num):
# return pose, retScore
global _detect_objects_srv
global br
global _tflistener
print 'In', '_detectObjects', 'obj_ids:', obj_ids, 'bin_num:', bin_num
# 1. filter the point cloud
pc, foreground_mask = get_filtered_pointcloud(
obj_ids, bin_num, kinect_num) # need to pass in list
if pc is None or foreground_mask is None:
return (None, None)
# 2. prepare constraints
bin_cnstr = get_bin_cnstr(
)[bin_num] # a list of right \ # left \ # back \ # front \ # bottom \ # top
ccnstr = []
tol = 0.9 # larger is more strict
(trans, rot) = lookupTransform(pc.header.frame_id, '/shelf', _tflistener)
# 2.1 right
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([1, 0, 0], [bin_cnstr[0], 0, 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.2 left
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([-1, 0, 0], [bin_cnstr[1], 0, 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.3 back
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, 1, 0], [0, bin_cnstr[2], 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.4 front
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, -1, 0], [0, bin_cnstr[3], 0], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.5 floor
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, 0, 1], [0, 0, bin_cnstr[4]], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.6 top
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.HALF_SPACE,
transformPlane([0, 0, -1], [0, 0, bin_cnstr[5]], trans, rot,
pc.header.frame_id), tol, bin_num))
# 2.7 on floor
floor_thick = 0.03
ccnstr.append(
createCapsenConstraint(
ObjectConstraint.SUPPORTING_PLANE,
transformPlane([0, 0, 1], [0, 0, bin_cnstr[4] + floor_thick * 2],
trans, rot, pc.header.frame_id), tol, bin_num))
#visualizeConstraint(ccnstr[6], pc.header.frame_id)
#pause()
# 3. detect using capsen
with Timer('detect_objects'):
service_name = '/detection_service/detect_objects'
req = DetectObjectsRequest()
req.model_names = obj_ids
req.constraints = ccnstr
req.cloud = pc
req.foreground_mask = foreground_mask
sum_pt = sum(foreground_mask)
if allFalse(foreground_mask, sum_pt):
return (None, None)
foreground_mask = subsample(foreground_mask, sum_pt)
# outputfile = '/tmp/foreground_mask'
# with open(outputfile, 'w') as outfile:
# json.dump(foreground_mask, outfile)
# pause()
#req.foreground_mask = [True for i in xrange(req.cloud.height*req.cloud.width)] # hack
req.find_exact_object_list = True
print '\tWaiting for service up: ', service_name
rospy.wait_for_service(service_name)
#pdb.set_trace()
try:
print '\tCalling service:', service_name
ret = _detect_objects_srv(req)
# ret.detections is a list of capsen_vision/SceneHypothesis
# [capsen_vision/SceneHypothesis]:
# std_msgs/Header header
# uint32 seq
# time stamp
# string frame_id
# capsen_vision/ObjectHypothesis[] objects
# string name
# geometry_msgs/Pose pose
# geometry_msgs/Point position
# float64 x
# float64 y
# float64 z
# geometry_msgs/Quaternion orientation
# float64 x
# float64 y
# float64 z
# float64 w
# float32 score
# float32[] score_components
# float32[2] errors
# float32 score
if len(ret.detections) > 0:
print '\t', len(
ret.detections
), 'SceneHypothesis returned, max score', ret.detections[
0].score
#print ret.detections
for i in range(len(ret.detections)):
scene = ret.detections[i]
nobj = len(scene.objects)
scene_desired = transformObjectsFromCapsenToDesiredFrame(
scene, pc.header.frame_id)
if allObjectsInsideBin(scene_desired, bin_num):
return (scene_desired.objects, scene_desired.score)
#else:
#print 'reject scene hypo', i, 'because one object of it is outside the target bin'
print '\tNo SceneHypothesis satisfy hard bin constraint'
return (None, None)
else:
print '\tNo SceneHypothesis returned'
return (None, None)
except:
print '\tCalling service:', service_name, 'failed'
print '\tencounters errors:', traceback.format_exc()
return (None, None)
req.bin_num = bin_num
req.obj_id = obj_ids[0] # peterkty: need to pass in a list
print '\tWaiting for service up: ', service_name
rospy.wait_for_service(service_name)
try:
print '\tCalling service:', service_name
response = _pointcloud2_service_srv[kinect_num - 1](req)
return response.pc2, response.foreground_mask
except:
print '\tCalling service:', service_name, 'failed'
print '\tencounters errors:', traceback.format_exc()
print '\tDid you call capsen.capsen.init()? Is camera connection good?'
return None, None
bin_cnstr = get_bin_cnstr()
def inside_bin(point, bin_num):
cnstr = bin_cnstr[bin_num]
# return (
# x > cnstr[0]+0.02 && x < cnstr[1]-0.02 &&
# y > cnstr[2]+0.1 && y < cnstr[3]-0.01 &&
# z > cnstr[4]+0.00 && z < cnstr[5]-0.02/* && z < cnstr[4]+obj_max_height*/);
if point[0] > cnstr[0]+0.015 and point[0] < cnstr[1]-0.015 and \
point[1] > cnstr[2]+0.1 and point[1] < cnstr[3]-0.01 and \
point[2] > cnstr[4]-0.02 and point[2] < cnstr[5]-0.02:
return True
