def callback(self, data):
'''
realsenseのパッケージから点群のtopicを受け取って、
オブジェクトのカメラ位置をpublishする.
'''
# 現在時刻に近いデータなら実行.
if rospy.Time.now().secs == data.header.stamp.secs:
# Set pcl data. ----->>>
input_pcl = util.convert_pcl(data)
#input_pcl = read_point_cloud(self.package_path + "/etc/dataset001.ply")
model = read_point_cloud(self.package_path +
"/etc/sandwich002.ply")
#model = read_point_cloud("../samples/TestData/ICP/cloud_bin_0.pcd"),
#model = read_point_cloud(self.package_path + "/etc/sandwich001.ply"),
# Process. ----->>>
#draw_geometries([input_pcl])
# 点群データを編集したい時使うやつ.
#draw_geometries_with_editing([input_pcl])
#draw_geometries_with_editing([read_point_cloud(self.package_path + "/etc/sandwich003.ply")])
# 点群データの保存
#print write_point_cloud(self.package_path + "/etc/dataset001.ply", input_pcl, write_ascii=False, compressed=False)
print
print input_pcl
print "pcdtree 0"
# nanを含むものがあれば終了.
#print np.any(np.isnan(np.asarray(input_pcl.points)))
#if np.any(np.isnan(np.asarray(input_pcl.points))) == True:
# return
input_pcl = self.preprocess(input_pcl)
# 確認用にGreenに塗る
#np.asarray(input_pcl.colors)[idx[1:], :] = [0, 1, 0]
#np.asarray(input_pcl.points)[:, :] = [0, 1, 0]
#np.asarray(input_pcl.points)[idx[1:], :] = [0, 1, 0]
#draw_geometries([input_pcl])
if len(input_pcl.points) > 0:
#self.colorIcp(
#result_a = self.calcPointCloud(
# source = input_pcl,
# target = model
# )
result = self.calcPointCloud(source=model, target=input_pcl)
# 複数の認識結果から一番良い結果を使う.
#eval_a = (result_a.fitness * result_a.inlier_rmse)
#eval_b = (result_b.fitness * result_b.inlier_rmse)
#if eval_a > eval_b:
# print "a is better *************************************************"
# result = result_a
#else:
# print "b is better *************************************************"
# result = result_b
# Publish.
if result.inlier_rmse == 0 or result.fitness == 0:
logging.debug('Detect fail.')
else:
logging.debug('Detect success.')
print result
# Publish model coordinate in robot system.
# 姿勢情報をカメラから見た現実座標に変換する.
if 1:
'''
# 自作手法その1 --->
# 点群が密であるところは重みが高く設定されると同義なので、
# vexelによって重み付けを消して、その点群重心から座標を求める.
'''
t_down = voxel_down_sample(model, self.VOXEL_SIZE)
x, y, z = self.returnMovedCenterGravityFromTransformation(
t_down, result.transformation)
x, y, z = self.returnObjectCenterFromSurfaceCenter(
x, y, z)
x, y, z = self.adjustOffset(x, y, z)
print "x", x
print "y", y
print "z", z
if DEBUG == True:
draw_registration_result(model, input_pcl,
result.transformation)
if 1:
'''
# 自作手法その2 --->
# オブジェクトの姿勢によってはmodelは完全にマッチングすることはないという前提.
# ならば、modelの重心座標付近にあるinput点群の点を複数とって、
# その重心がオブジェクトの表面重心だとする方法.
# これを使う場合、modelは面の点群、ICPにはColorICPを用いることが望ましいはず.
'''
try:
t_down = voxel_down_sample(model, self.VOXEL_SIZE)
x, y, z = self.returnMovedCenterGravityFromTransformation(
t_down, result.transformation)
print "x", x
print "y", y
print "z", z
pcd_tree = KDTreeFlann(input_pcl)
[k, idx, _] = pcd_tree.search_radius_vector_3d(
[x, y, z], self.TARGET_OBJECT_RADIUS * 1.5)
sand_and_noise = select_down_sample(input_pcl, idx)
x, y, z = np.average(sand_and_noise.points,
axis=0) # cameraのx,y,zを出す.
x, y, z = self.adjustOffset(x, y, z)
print "x", x
print "y", y
print "z", z
sand_and_noise.paint_uniform_color([1, 0, 0])
if DEBUG == True:
draw_geometries([sand_and_noise, input_pcl])
except:
# publishせずに返す
return
# Publish ----->>>
msg = Point()
msg.x, msg.y, msg.z = x, y, z
print msg
self.object_point_sub.publish(msg)
logging.debug('Publish point data')
return
def callback(data):
pcl_data = util.convert_pcl(data)
result_pcl = numpy_pcl(pcl_data)
util.publish_pointcloud(result_pcl, data)
def callback(data):
pcl_data = util.convert_pcl(data)
result_pcl = down_sampling(pcl_data)
util.publish_pointcloud(result_pcl, data)
def callback(data):
pcl_data = util.convert_pcl(data)
view_pcl(pcl_data)