def process_cloud(cloud_transformed, detection):
"""Process the transformed point cloud with respect to the Mask RCNN
detection.
Parameters
----------
- cloud_transformed: sensor_msgs.msg.PointCloud2
Point cloud in the robot frame.
- detection: mask_rcnn_ros.msg.Detection
Detection result of the image.
Returns
----------
- cloud_indexed: gpd.msg.CloudIndexed
The transformed point cloud and a list of indices into it at which to
sample grasp candidates.
"""
cloud_indexed = CloudIndexed()
if len(detection.masks) > 0:
for class_name, mask in zip(detection.class_names, detection.masks):
if class_name == 'bottle':
mask_data = np.fromstring(mask.data, dtype=np.uint8)
one_index = np.nonzero(mask_data)[0]
cloud_indexed.indices = [Int64(idx) for idx in one_index]
break
else:
rospy.logfatal("No bottle found!")
raise SystemExit()
else:
rospy.logfatal("Detect nothing!")
raise SystemExit()
cloud_indexed.cloud_sources.cloud = cloud_transformed
cloud_indexed.cloud_sources.view_points.append(Point(0, 0, 0))
cloud_indexed.cloud_sources.camera_source.append(Int64(0))
return cloud_indexed
def generate_cloud_indexed_msg(self):
np_cloud, idx = self.extract_indices()
msg = CloudIndexed()
header = Header()
# header.frame_id = "xtion_rgb_optical_frame"
header.frame_id = "camera_depth_optical_frame"
header.stamp = rospy.Time.now()
msg.cloud_sources.cloud = point_cloud2.create_cloud_xyz32(header, np_cloud.tolist())
msg.cloud_sources.view_points.append(Point(0, 0, 0))
for i in xrange(np_cloud.shape[0]):
msg.cloud_sources.camera_source.append(Int64(0))
for i in idx[0]:
msg.indices.append(Int64(i))
return msg
def cloudCallback(msg):
global T_camera_world
# read the cloud
cloud_array = []
for p in point_cloud2.read_points(msg):
cloud_array.append([p[0], p[1], p[2]])
cloud_array = np.array(cloud_array).T
# form a point cloud
point_cloud_camera = PointCloud(cloud_array, frame='camera')
# segment the point cloud
point_cloud_world = T_camera_world * point_cloud_camera
seg_point_cloud_world, valid_indices = point_cloud_world.box_mask(
workspace)
"""
point_cloud_world.remove_infinite_points()
vis3d.figure()
vis3d.points(point_cloud_world, random=True, subsample=10, scale=0.001, color=(0,0,1))
vis3d.points(seg_point_cloud_world, random=True, subsample=10, scale=0.0015, color=(0,1,0))
vis3d.pose(T_camera_world)
vis3d.show()
IPython.embed()
"""
# convert to indexed cloud frame
frame_id = msg.header.frame_id
msg = CloudIndexed()
header = Header()
header.frame_id = frame_id
header.stamp = rospy.Time.now()
msg.cloud_sources.cloud = point_cloud2.create_cloud_xyz32(
header, cloud_array.T.tolist())
msg.cloud_sources.view_points.append(Point(0, 0, 0))
for i in xrange(cloud_array.shape[1]):
msg.cloud_sources.camera_source.append(Int64(0))
for i in valid_indices:
msg.indices.append(Int64(i))
# publish
global pub
pub.publish(msg)
print 'Published cloud with', len(msg.indices), 'indices'
while len(cloud) == 0:
rospy.sleep(0.01)
cloud = np.asarray(cloud)
A = np.c_[cloud[:, 0], cloud[:, 1], np.ones(cloud.shape[0])]
b = cloud[:, 2]
C, _, _, _ = lstsq(A, b) # coefficients of the form: a*x + b*y + c*z + d = 0.
a, b, c, d = C[0], C[1], -1., C[2]
dist = ((a*cloud[:, 0] + b*cloud[:, 1] + d) - cloud[:, 2])**2
err = dist.sum()
idx = np.where(dist > 0.01)
# Publish point cloud and nonplanar indices.
pub = rospy.Publisher('/cloud_indexed', CloudIndexed, queue_size=1)
msg = CloudIndexed()
header = Header()
header.frame_id = "/kinect2_link"
header.stamp = rospy.Time.now()
msg.cloud_sources.cloud = point_cloud2.create_cloud_xyz32(header, cloud.tolist())
msg.cloud_sources.view_points.append(Point(0, 0, 0))
for i in xrange(cloud.shape[0]):
msg.cloud_sources.camera_source.append(Int64(0))
for i in idx[0]:
msg.indices.append(Int64(i))
print '-----------------------------------'
print 'Press hit key to publish... (Y/N)'
in_content = raw_input('Input:')
if __name__ == "__main__":
rospy.init_node("test_service")
print("getting image...")
image = rospy.wait_for_message("/camera/rgb/image_raw", Image)
pc2 = rospy.wait_for_message("/camera/depth/points", PointCloud2)
print("waiting for service...")
rospy.wait_for_service("mask_rcnn_srv")
try:
mask_srv = rospy.ServiceProxy("mask_rcnn_srv", MaskDetect)
response = mask_srv(image)
except rospy.ServiceException, e:
print "service call failed: %s"%e
cloudindexed = CloudIndexed()
if len(response.result.masks) > 0:
mask = response.result.masks[0]
for i in range(mask.step * mask.height):
if mask.data[i] == '\xff':
cloudindexed.indices.append(Int64(i))
cloudindexed.cloud_sources.cloud = pc2
cloudindexed.cloud_sources.view_points.append(Point(0,0,0))
cloudindexed.cloud_sources.camera_source.append(Int64(0))
print("generating grasp...")
rospy.wait_for_service("/detect_grasps_server/detect_grasps")
detect_grasp = rospy.ServiceProxy("/detect_grasps_server/detect_grasps", detect_grasps)
graspConfigList = detect_grasp(cloudindexed)
IPython.embed()
rospy.loginfo("Getting image...")
image = rospy.wait_for_message("/camera/rgb/image_raw", Image)
pc2 = rospy.wait_for_message("/camera/depth/points", PointCloud2)
rospy.loginfo("Waiting for Mask RCNN service...")
rospy.wait_for_service('mask_rcnn_srv')
response = None
try:
rospy.loginfo("Calling Mask RCNN service...")
mask_srv = rospy.ServiceProxy('mask_rcnn_srv', MaskDetect)
response = mask_srv(image)
except rospy.ServiceException, e:
rospy.loginfo("Mask RCNN service call failed: %s" % e)
cloud_indexed = CloudIndexed()
if response is not None and len(response.detection.masks) > 0:
for class_name, mask in zip(response.detection.class_names,
response.detection.masks):
if class_name == 'bottle':
one_idx = np.nonzero(mask)
one_idx = np.ravel_multi_index(one_idx)
cloud_indexed.indices = one_idx.tolist()
indices = []
for i in range(mask.step * mask.height):
if mask.data[i] == '\xff':
indices.append(Int64(i))
break
else:
rospy.loginfo("No bottle found!")
def getGraspFromPointcloud(self, entity):
Logger.loginfo("Selected entity : " + str(entity.ID))
Logger.loginfo("Current position : (" + str(entity.position.x) + ", " +
str(entity.position.y) + ", " + str(entity.position.x) +
")")
# Convert to Pointcloud and change frame of reference to base)link
pointCloud = PointCloud()
pointCloud.header = entity.pointcloud.header
for p in point_cloud2.read_points(entity.pointcloud):
point = Point32()
point.x, point.y, point.z = [p[0], p[1], p[2]]
pointCloud.points.append(point)
pointCloud.header.stamp = rospy.Time.now() - rospy.Duration(1)
self.listener.waitForTransform(pointCloud.header.frame_id,
"/base_link", rospy.Time(0),
rospy.Duration(10))
pointCloud = self.listener.transformPointCloud("/base_link",
pointCloud)
cloud = []
for p in pointCloud.points:
cloud.append([p.x, p.y, p.z])
Logger.loginfo("Cloud size : " + str(len(cloud)))
# if len(cloud) > 0:
cloud = np.asarray(cloud)
X = cloud
A = np.c_[X[:, 0], X[:, 1], np.ones(X.shape[0])]
C, _, _, _ = lstsq(A, X[:, 2])
a, b, c, d = C[0], C[1], -1., C[
2] # coefficients of the form: a*x + b*y + c*z + d = 0.
dist = ((a * X[:, 0] + b * X[:, 1] + d) - X[:, 2])**2
err = dist.sum()
idx = np.where(dist > 0.01)
msg = CloudIndexed()
header = Header()
header.frame_id = "/base_link"
header.stamp = rospy.Time.now()
msg.cloud_sources.cloud = point_cloud2.create_cloud_xyz32(
header, cloud.tolist())
msg.cloud_sources.view_points.append(Point(0, -0.5, 1.5))
for i in xrange(cloud.shape[0]):
msg.cloud_sources.camera_source.append(Int64(0))
for i in idx[0]:
msg.indices.append(Int64(i))
# s = raw_input('Hit [ENTER] to publish')
self.pub.publish(msg)
i = 0
################################
# Temporary setting a timeout
while self.graspList == None:
i = i + 1
rospy.sleep(1)
if i > 20:
return self.getGraspWithoutPointcloud(entity)
bestScore = 0
bestGrasp = None
# Normalisation des scores de grasp
for grasp in self.graspList:
if grasp.score.data > self.maxgraspScore:
self.maxgraspScore = grasp.score.data
for grasp in self.graspList:
# Poses with a negative approach gets a negative multiplier
if grasp.approach.z < 0: # Approche par le haut
# poseScore = self.calculateGraspScore(pose)
ref = [0.577350269, 0.577350269, -0.577350269]
app = [grasp.approach.x, grasp.approach.y, grasp.approach.z]
poseScore = np.dot(app, ref)
rospy.loginfo("Total pose score (Positive approach): %s",
str(poseScore))
if bestScore < poseScore:
bestScore = poseScore
bestGrasp = grasp
if bestGrasp is not None:
pose = self.graspToPose(bestGrasp)
# Generate approach pose
approach_pose = Pose()
applength = np.linalg.norm([
bestGrasp.approach.x, bestGrasp.approach.y,
bestGrasp.approach.z
])
approach_pose.position.x = pose.position.x - bestGrasp.approach.x / applength * self.approachDistance
approach_pose.position.y = pose.position.y - bestGrasp.approach.y / applength * self.approachDistance
approach_pose.position.z = pose.position.z - bestGrasp.approach.z / applength * self.approachDistance
approach_pose.orientation = pose.orientation
return pose, approach_pose
return self.getGraspWithoutPointcloud(entity)