def cal_grasp(msg, cam_pos_):
points_ = pointclouds.pointcloud2_to_xyz_array(msg)
points_ = points_.astype(np.float32)
remove_white = False
if remove_white:
points_ = remove_white_pixel(msg, points_, vis=True)
# begin voxel points
n = n_voxel # parameter related to voxel method
# gpg improvements, highlights: flexible n parameter for voxelizing.
points_voxel_ = get_voxel_fun(points_, n)
if len(points_) < 2000: # should be a parameter
while len(points_voxel_) < len(points_) - 15:
points_voxel_ = get_voxel_fun(points_, n)
n = n + 100
rospy.loginfo(
"the voxel has {} points, we want get {} points".format(
len(points_voxel_), len(points_)))
rospy.loginfo("the voxel has {} points, we want get {} points".format(
len(points_voxel_), len(points_)))
points_ = points_voxel_
remove_points = False
if remove_points:
points_ = remove_table_points(points_, vis=True)
point_cloud = pcl.PointCloud(points_)
norm = point_cloud.make_NormalEstimation()
norm.set_KSearch(30) # critical parameter when calculating the norms
normals = norm.compute()
surface_normal = normals.to_array()
surface_normal = surface_normal[:, 0:3]
vector_p2cam = cam_pos_ - points_
vector_p2cam = vector_p2cam / np.linalg.norm(vector_p2cam, axis=1).reshape(
-1, 1)
tmp = np.dot(vector_p2cam, surface_normal.T).diagonal()
angel = np.arccos(np.clip(tmp, -1.0, 1.0))
wrong_dir_norm = np.where(angel > np.pi * 0.5)[0]
tmp = np.ones([len(angel), 3])
tmp[wrong_dir_norm, :] = -1
surface_normal = surface_normal * tmp
select_point_above_table = 0.010
# modify of gpg: make it as a parameter. avoid select points near the table.
points_for_sample = points_[np.where(
points_[:, 2] > select_point_above_table)[0]]
if len(points_for_sample) == 0:
rospy.loginfo(
"Can not seltect point, maybe the point cloud is too low?")
return [], points_, surface_normal
yaml_config['metrics']['robust_ferrari_canny']['friction_coef'] = value_fc
if not using_mp:
rospy.loginfo("Begin cal grasps using single thread, slow!")
grasps_together_ = ags.sample_grasps(point_cloud,
points_for_sample,
surface_normal,
num_grasps,
max_num_samples=max_num_samples,
show_final_grasp=show_final_grasp)
else:
# begin parallel grasp:
rospy.loginfo("Begin cal grasps using parallel!")
def grasp_task(num_grasps_, ags_, queue_):
ret = ags_.sample_grasps(point_cloud,
points_for_sample,
surface_normal,
num_grasps_,
max_num_samples=max_num_samples,
show_final_grasp=show_final_grasp)
queue_.put(ret)
queue = mp.Queue()
num_grasps_p_worker = int(num_grasps / num_workers)
workers = [
mp.Process(target=grasp_task,
args=(num_grasps_p_worker, ags, queue))
for _ in range(num_workers)
]
[i.start() for i in workers]
grasps_together_ = []
for i in range(num_workers):
grasps_together_ = grasps_together_ + queue.get()
rospy.loginfo("Finish mp processing!")
rospy.loginfo("Grasp sampler finish, generated {} grasps.".format(
len(grasps_together_)))
return grasps_together_, points_, surface_normal
def main():
baxter_test = Baxter_Test()
rate = rospy.Rate(baxter_test._rate)
#retrieve point cloud data from Baxter
point_cloud_msg = ros_topic_get("/camera/depth_registered/points", PointCloud2)
point_cloud_array = pointclouds.pointcloud2_to_xyz_array(point_cloud_msg)
point_cloud_array = transform_matrix(point_cloud_array, "/camera_rgb_optical_frame", "/base")
point_cloud = pcl.PointCloud() #create new pcl PointCloud
point_cloud.from_list(point_cloud_array) #import data to PointCloud from 2d matrix
#Experimental code for the hand mounted camera
print "looking up hand camera"
"""f200_cloud_msg = ros_topic_get("/camera/depth/points", PointCloud2)
print f200_cloud_msg
point_cloud_array = pointclouds.pointcloud2_to_xyz_array(f200_cloud_msg)
print point_cloud_array"""
#filter data
fil = point_cloud.make_passthrough_filter()
fil.set_filter_field_name("x")
fil.set_filter_limits(0.5, 1)
cloud_filtered_x = fil.filter()
fil = cloud_filtered_x.make_passthrough_filter()
fil.set_filter_field_name("z")
fil.set_filter_limits(-0.1, 0.5)
cloud_filtered = fil.filter()
#segment data
seg = cloud_filtered.make_segmenter()
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_distance_threshold(0.03)
indices, model = seg.segment()
#remove table plane based on model
object_cloud = filter_plane(model, cloud_filtered, 0.1)
print object_cloud
X = object_cloud.to_array()
#our PCA only needs to deal with x-y plane, remove z values
X = X[:,[0,1]]
pca = PCA(n_components=2)
X = pca.fit_transform(X)
#calculate object centroid
object_centroid = get_centroid(object_cloud)
#calculate object bounding box
bounding_box = fit_bounding_box(object_cloud, pca.components_, object_centroid)
#baxter_test.move_left_to_coord(object_centroid)
publisher = rospy.Publisher("/baxter_test", MarkerArray, queue_size = 10)
marker_array = MarkerArray()
#PCA vector sanity check (if needed)
"""count = 0
for vector in pca.components_:
vector_marker = Marker()
vector_marker.header.frame_id = "/base"
vector_marker.header.stamp = rospy.Time(0)
vector_marker.type = vector_marker.ARROW
vector_marker.action = vector_marker.ADD
start = Point()
vector_marker.points = [createPoint(object_centroid[0], object_centroid[1], 0),
createPoint(object_centroid[0] + vector[0],
object_centroid[1] + vector[1], 0)]
if count == 0:
vector_marker.color.a = 1.0 #first vector green
vector_marker.color.r = 0.0
vector_marker.color.g = 1.0
vector_marker.color.b = 0.0
else:
vector_marker.color.a = 1.0 #second vector blue
vector_marker.color.r = 0.0
vector_marker.color.g = 0.0
vector_marker.color.b = 1.0
vector_marker.scale.x = 0.01
vector_marker.scale.y = 0.02
vector_marker.id = count
marker_array.markers.append(vector_marker)
count += 1"""
#continually do stuff
while not rospy.is_shutdown():
publisher.publish(marker_array)
#baxter_test.move_left_to_coord(object_centroid)
rate.sleep()
def cal_grasp(msg, cam_pos_):
"""根据在线采集的点云计算候选的抓取姿态
"""
#把pointcloud2类型的消息点云，转换为ndarray points_
points_ = pointclouds.pointcloud2_to_xyz_array(msg)
#复制一份points_ ndarray对象，并将所有的点坐标转换为float32类型
points_ = points_.astype(np.float32)
remove_white = False
if remove_white:
points_ = remove_white_pixel(msg, points_, vis=True)
# begin voxel points
n = n_voxel # parameter related to voxel method
# gpg improvements, highlights: flexible n parameter for voxelizing.
#这一句话执行的时候，不能打开虚拟机，否则容易卡住
points_voxel_ = get_voxel_fun(points_, n)
#当点云点数小于2000时
if len(points_) < 2000: # should be a parameter
while len(points_voxel_) < len(points_) - 15:
points_voxel_ = get_voxel_fun(points_, n)
n = n + 100
rospy.loginfo(
"the voxel has {} points, we want get {} points".format(
len(points_voxel_), len(points_)))
rospy.loginfo("the voxel has {} points, we want get {} points".format(
len(points_voxel_), len(points_)))
#
points_ = points_voxel_
remove_points = False
#是否剔除支撑平面
if remove_points:
points_ = remove_table_points(points_, vis=True)
#重新构造经过“降采样”的点云
point_cloud = pcl.PointCloud(points_)
print(len(points_))
#构造法向量估计对象
norm = point_cloud.make_NormalEstimation()
tree = point_cloud.make_kdtree()
norm.set_SearchMethod(tree)
#以周边30个点作为法向量计算点
norm.set_KSearch(10) # critical parameter when calculating the norms
normals = norm.compute()
#将点云法向量转换为ndarry类型
surface_normal = normals.to_array()
surface_normal = surface_normal[:, 0:3]
#每个点到 相机位置（无姿态）的向量 但是，感觉是相机到点的向量
vector_p2cam = cam_pos_ - points_
#print(vector_p2cam)
#print(cam_pos_)
"""
np.linalg.norm(vector_p2cam, axis=1) 默认求2范数，axis=1 代表按行向量处理，求多个行向量的2范数（求模）
np.linalg.norm(vector_p2cam, axis=1).reshape(-1, 1) 将其调整为m行 1列
整句话的含义是，将vector_p2cam归一化，单位化
"""
vector_p2cam = vector_p2cam / np.linalg.norm(vector_p2cam, axis=1).reshape(
-1, 1)
#将表面法相与表面法相（都是单位向量）点乘，以备后面计算向量夹角
tmp = np.dot(vector_p2cam, surface_normal.T).diagonal()
#print(vector_p2cam)
#print(surface_normal.T)
#print(tmp)
"""
np.clip(tmp, -1.0, 1.0) 截取函数，将tmp中的值，都限制在-1.0到1.0之间，大于1的变成1，小于-1的记为-1
np.arccos() 求解反余弦，求夹角
"""
angel = np.arccos(np.clip(tmp, -1.0, 1.0))
#print(angel)
#找到与视角向量夹角大于90度的角（认为法向量计算错误）
wrong_dir_norm = np.where(angel > np.pi * 0.5)[0]
#print(np.where(angel > np.pi * 0.5))
#print(wrong_dir_norm)
#print(len(wrong_dir_norm))
#创建一个len(angel)行，3列的ndarry对象
tmp = np.ones([len(angel), 3])
#将法向量错误的行的元素都改写为-1
tmp[wrong_dir_norm, :] = -1
#与表面法相元素对元素相乘，作用是将"错误的"法向量的方向 扭转过来
surface_normal = surface_normal * tmp
#选取桌子以上2cm处的点作为检测点
select_point_above_table = 0.020
#modify of gpg: make it as a parameter. avoid select points near the table.
#查看每个点的z方向，如果它们的点z轴方向的值大于select_point_above_table，就把他们抽出来
points_for_sample = points_[np.where(
points_[:, 2] > select_point_above_table)[0]]
print(len(points_for_sample))
if len(points_for_sample) == 0:
rospy.loginfo(
"Can not seltect point, maybe the point cloud is too low?")
return [], points_, surface_normal
yaml_config['metrics']['robust_ferrari_canny']['friction_coef'] = value_fc
grasps_together_ = []
if rospy.get_param("/robot_at_home") == "false":
robot_at_home = False
else:
robot_at_home = True
if not robot_at_home:
rospy.loginfo("Robot is moving, waiting the robot go home.")
elif not using_mp:
rospy.loginfo("Begin cal grasps using single thread, slow!")
grasps_together_ = ags.sample_grasps(point_cloud,
points_for_sample,
surface_normal,
num_grasps_single_worker,
max_num_samples=max_num_samples,
show_final_grasp=show_final_grasp)
else:
# begin parallel grasp:
rospy.loginfo("Begin cal grasps using parallel!")
def grasp_task(num_grasps_, ags_, queue_):
ret = ags_.sample_grasps(point_cloud,
points_for_sample,
surface_normal,
num_grasps_,
max_num_samples=max_num_samples,
show_final_grasp=False)
queue_.put(ret)
queue = mp.Queue()
#num_grasps_p_worker = int(num_grasps/num_workers)
workers = [
mp.Process(target=grasp_task,
args=(num_grasps_p_worker, ags, queue))
for _ in range(num_workers)
]
[i.start() for i in workers]
grasps_together_ = []
for i in range(num_workers):
grasps_together_ = grasps_together_ + queue.get()
rospy.loginfo("Finish mp processing!")
if show_final_grasp and using_mp:
ags.show_all_grasps(points_, grasps_together_)
ags.show_points(points_, scale_factor=0.002)
mlab.show()
rospy.loginfo("Grasp sampler finish, generated {} grasps.".format(
len(grasps_together_)))
#返回抓取 场景的点 以及点云的表面法向量
return grasps_together_, points_, surface_normal
def main_cb(cloud_msg):
#DECLARE GLOBAL VARIABLES
global slot_count
global final_data
global all_hogs
global train_surfaces
global surfacesX
global all_surf
global labels
#CONVERT TO XYZ
rospy.loginfo('converting pointcloud %d to XYZ array ',slot_count)
raw_data = pointclouds.pointcloud2_to_xyz_array(cloud_msg, remove_nans=True)
#
mode=0 # mode=0 -->COLLECT DATA mode=1 --> TRAIN AND TEST
#BUILD CLUSTER
filter_zeros=np.where(raw_data[:, 0] != 0)[0]
clear_data = raw_data[filter_zeros,:] # ignore the zeros on X
filter_zeros=np.where(clear_data[:, 1] != 0)[0]
clear_data = clear_data[filter_zeros,:] # ignore the zeros on Y
cluster_labels =np.zeros((len(clear_data),1),int)
#% TRAINING
eps = 0.5
min_points = 5
rospy.loginfo('call DBscan ')
[core_samples,cluster_labels, n_clusters, human, surfacesX]=scikit_dbscan.dbscan(clear_data, eps, min_points,mode,False)
clear_data=clear_data[core_samples,:]
# SURFACE & HOG Features EXTRACTION
all_surf.append(surfacesX)
rospy.loginfo('Done.')
#EXTRACT AND SAVE HOGS FEATURES
rospy.loginfo('extract hogs for timeslot %d',slot_count)
[hogs,hog_image] = myhog.hog(surfacesX)
#pl.plot(hog_image)
#pl.show(0.5)
final_data.append(clear_data)
labels.append(cluster_labels)
human_detection.append(human)
all_hogs.append(hogs)
rospy.loginfo('all_hogs length %d',len(all_hogs))
rospy.loginfo('final_data length %d',len(final_data))
rospy.loginfo('human_detection length %d',len(human_detection))
if mode==0:
f = open("train_hogs.txt","a")
simplejson.dump(all_hogs,f)
f.close()
f = open("train_classifications.txt","a")
simplejson.dump(all_human_detection,f)
f.close()
if mode==1:
with open("train_hogs.txt") as f:
train_hogs = simplejson.load(f)
with open("train_classifications.txt") as f2:
train_labels = simplejson.load(f2)
X=array(train_hogs)
y=array(train_labels)
clf = svm.SVC()
clf.fit(X, y)
#[p0V,p1V,pAb]= trainNB0(array(train_hogs),array(train_labels))
#print testEntry,'classified as: ',classifyNB(array(hogs),p0V,p1V,pAb)
slot_count = slot_count + 1
