def gt_cb(self, msg):
labels = []
for i, (n,p) in enumerate(zip(msg.name, msg.pose)):
if not (n in self._models):
continue
pos = [p.position.x, p.position.y, p.position.z + 0.3]
label = make_label(n, pos, i, color=[0.0,1.0,0.0])
labels.append(label)
for l in labels:
self._gt_pub.publish(l)
def CB_msgPCL(msgPCL):
"""
ROS "/sensor_stick/point_cloud" subscription Callback handler
Handle the PointCloud ROS msg received by the "/sensor_stick/point_cloud"
This function is almost entirely unpacking/packing ROS messages and publishing.
The the unpacked input pcl is processed by Process_rawPCL(pclpcRawIn)
which returns the values that need to be packed and published
:param msgPCL:
:return:
"""
global g_callBackCount
g_callBackCount += 1
if (g_callBackCount % g_callBackSkip != 0):
return;
print "\rCBCount= {:05d}".format(g_callBackCount),
sys.stdout.flush()
DebugDumpMsgPCL(msgPCL)
# Extract pcl Raw from Ros msgPCL
pclpcRawIn = pcl_helper.ros_to_pcl(msgPCL)
#------- PROCESS RAW PCL-------------------------
labelRecs, pclpcObjects, pclpcTable, pclpcClusters = Process_rawPCL(pclpcRawIn)
detected_objects_labels = [] # For ros loginfo only
detected_objects = [] # For publish - for PROJ3!
for (labelText, labelPos, labelIndex) in labelRecs:
detected_objects_labels.append(labelText)
g_object_markers_pub.publish(marker_tools.make_label(labelText, labelPos, labelIndex ))
# Add detected object to the list of detected objects.
do = DetectedObject()
do.label = labelText
do.cloud = pclpcClusters
detected_objects.append(do)
rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))
# Package Processed pcls into Ros msgPCL
msgPCLObjects = pcl_helper.pcl_to_ros(pclpcObjects)
msgPCLTable = pcl_helper.pcl_to_ros(pclpcTable)
msgPCLClusters = pcl_helper.pcl_to_ros(pclpcClusters)
# Publish everything
# This is the output you'll need to complete the upcoming project!
g_detected_objects_pub.publish(detected_objects) # THIS IS THE CRUCIAL STEP FOR PROJ3
g_pcl_objects_pub.publish(msgPCLObjects)
g_pcl_table_pub.publish(msgPCLTable)
g_pcl_cluster_pub.publish(msgPCLClusters)
def pcl_callback(pcl_msg):
cloud = ros_to_pcl(pcl_msg)
cloud = pipeline.passth(cloud)
cloud = pipeline.passth(cloud, axis='y', amin=-0.45, amax=0.45)
cloud = pipeline.denoise(cloud)
filtered = pipeline.voxel(cloud)
table_points = pipeline.plane_points(filtered)
table_cloud = filtered.extract(table_points, negative=False)
obj_cloud = filtered.extract(table_points, negative=True)
pcl_objects_pub.publish(pcl_to_ros(obj_cloud))
pcl_table_pub.publish(pcl_to_ros(table_cloud))
cluster_ix = pipeline.cluster_ix(obj_cloud)
pcl_cluster_pub.publish(
pcl_to_ros(pipeline.color_clusters(obj_cloud, cluster_ix)))
detected_objects = []
for i, ixs in enumerate(cluster_ix):
pcl_data = obj_cloud.extract(ixs)
ros_data = pcl_to_ros(pcl_data)
feature = np.concatenate(
(features.compute_color_histograms(ros_data), ))
prediction = clf.predict(scaler.transform(feature.reshape(1, -1)))
label = encoder.inverse_transform(prediction)[0]
label_pos = list(next(pc2.read_points(ros_data))[:3])
label_pos[2] += .4
object_markers_pub.publish(make_label(label, label_pos, i))
do = DetectedObject()
do.label = label
do.cloud = ros_data
detected_objects.append(do)
detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg):
rospy.loginfo('Begin pcl_callback...')
# Exercise-2 TODOs:
# DONE: Convert ROS msg to PCL data(PointXYZRGB)
cloud = ros_to_pcl(pcl_msg)
# DONE: Voxel Grid Downsampling
cloud_filtered = voxel_filter(cloud)
# DONE: PassThrough Filter
# ざっくり z軸でテーブルの上だけを切り取る
cloud_filtered = pass_through_filter(cloud_filtered,
filter_axis='z',
axis_limit=(0.6, 1.1))
# テーブルの手前もオブジェクトに見えるので、切り取る
cloud_filtered = pass_through_filter(cloud_filtered,
filter_axis='y',
axis_limit=(-10.0, -1.39))
# DONE: RANSAC Plane Segmentation
inliers, _ = ransac_plane_segmentation(cloud_filtered)
# DONE: Extract inliers and outliers
# フィルタでオブジェクト群とテーブルを分離する(結果は ransanc の distance に依存する)
cloud_objects = cloud_filtered.extract(inliers, negative=True)
rospy.loginfo(' cloud_objects: %d' % (cloud_objects.size))
# DONE: Euclidean Clustering
# オブジェクト毎に分割する
cluster_indices, white_cloud = euclidean_clustering(cloud_objects,
tolerance=0.02,
cluster_range=(100,
15000))
rospy.loginfo(' cluster_indices: %d, white_cloud: %d ' %
(len(cluster_indices), white_cloud.size))
rospy.loginfo('DONE Exercise-2')
# Exercise-3 TODOs:
# Classify the clusters! (loop through each detected cluster one at a time)
detected_objects_labels = []
detected_objects = []
# 各オブジェクトを認識する
for index, pts_list in enumerate(cluster_indices):
#rospy.loginfo(' Detecting... : %d' % (index))
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = cloud_objects.extract(pts_list)
# DONE: convert the cluster from pcl to ROS using helper function
ros_cluster = pcl_to_ros(pcl_cluster)
# Extract histogram features
# DONE: complete this step just as is covered in capture_features.py
# Compute the associated feature vector
#rospy.loginfo(' type : %s' % (type(ros_cluster)))
chists = compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
nhists = compute_normal_histograms(normals)
feature = np.concatenate((chists, nhists))
# Make the prediction, retrieve the label for the result
# and add it to detected_objects_labels list
prediction = clf.predict(scaler.transform(feature.reshape(1, -1)))
label = encoder.inverse_transform(prediction)[0]
detected_objects_labels.append(label)
# Publish a label into RViz
# ラベルを publish する
label_pos = list(white_cloud[pts_list[0]])
label_pos[2] += .4
object_markers_pub.publish(make_label(label, label_pos, index))
# Add the detected object to the list of detected objects.
do = DetectedObject()
do.label = label
do.cloud = ros_cluster
detected_objects.append(do)
rospy.loginfo('Detected {} objects: {}'.format(
len(detected_objects_labels), detected_objects_labels))
# Publish the list of detected objects
# This is the output you'll need to complete the upcoming project!
#
# 認識したオブジェクトを publish する
detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
detected_objects_labels = []
detected_objects = []
# TODO: Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# TODO: Statistical Outlier Filtering
outlier_filter = cloud.make_statistical_outlier_filter()
# Set the number of neighboring points to analyze for any given point
outlier_filter.set_mean_k(8)
# Set threshold scale factor
x = 0.3
# Any point with a mean distance larger than global (mean distance+x*std_dev) will be considered outlier
outlier_filter.set_std_dev_mul_thresh(x)
cloud_filtered = outlier_filter.filter()
vox = cloud_filtered.make_voxel_grid_filter()
LEAF_SIZE = 0.005
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_filtered = vox.filter()
# TODO: PassThrough Filter
passthrough = cloud_filtered.make_passthrough_filter()
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.6
axis_max = 1.1
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()
# PassThrough filter 0.34 < x < 1.0
px = cloud_filtered.make_passthrough_filter()
px.set_filter_field_name('x')
px.set_filter_limits(0.34, 1.0)
cloud_filtered = px.filter()
# TODO: RANSAC Plane Segmentation
seg = cloud_filtered.make_segmenter()
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
max_distance = 0.01
seg.set_distance_threshold(max_distance)
inliers, coefficients = seg.segment()
# TODO: Extract inliers and outliers
extracted_inliers = cloud_filtered.extract(inliers, negative=False)
extracted_outliers = cloud_filtered.extract(inliers, negative=True)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(extracted_outliers)
tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.02)
ec.set_MinClusterSize(40)
ec.set_MaxClusterSize(4000)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
cluster_color = get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = extracted_outliers.extract(indices)
# TODO: convert the cluster from pcl to ROS using helper function
ros_cluster = pcl_to_ros(pcl_cluster)
# Extract histogram features
# TODO: complete this step just as is covered in capture_features.py
chists = compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
nhists = compute_normal_histograms(normals)
feature = np.concatenate((chists, nhists))
# Make the prediction, retrieve the label for the result
# and add it to detected_objects_labels list
prediction = clf.predict(scaler.transform(feature.reshape(1, -1)))
label = encoder.inverse_transform(prediction)[0]
detected_objects_labels.append(label)
# Publish a label into RViz
label_pos = list(white_cloud[indices[0]])
label_pos[2] += .4
object_markers_pub.publish(make_label(label, label_pos, j))
# Add the detected object to the list of detected objects.
do = DetectedObject()
do.label = label
do.cloud = ros_cluster
detected_objects.append(do)
rospy.loginfo('Detected {} objects: {}'.format(
len(detected_objects_labels), detected_objects_labels))
# Publish the list of detected objects
# This is the output you'll need to complete the upcoming project!
detected_objects_pub.publish(detected_objects)
#Create new cloud containing all clusters, each with unique color
#cluster_cloud = pcl.PointCloud_PointXYZRGB()
#cluster_cloud.from_list(color_cluster_point_list)
#print color_cluster_point_list
# TODO: Convert PCL data to ROS messages
ros_cloud_objects = pcl_to_ros(extracted_outliers)
ros_cloud_table = pcl_to_ros(extracted_inliers)
#ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
#pcl_cluster_pub.publish(ros_cluster_cloud)
# Suggested location for where to invoke your pr2_mover() function within pcl_callback()
# Could add some logic to determine whether or not your object detections are robust
# before calling pr2_mover()
try:
pr2_mover(detected_objects)
except rospy.ROSInterruptException:
pass
def callback(pcl_msg):
"""Callback function for your Point Cloud Subscriber"""
# Convert ROS msg to PCL data
pcl_data = ros_to_pcl(pcl_msg)
# Voxel Grid Downsampling
vox = pcl_data.make_voxel_grid_filter()
leaf_size = 0.005
vox.set_leaf_size(leaf_size, leaf_size, leaf_size)
pcl_data_filtered = vox.filter()
# PassThrough Filter
passthrough = pcl_data_filtered.make_passthrough_filter()
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.6
axis_max = 1.1
passthrough.set_filter_limits(axis_min, axis_max)
pcl_data_filtered = passthrough.filter()
passthrough = pcl_data_filtered.make_passthrough_filter()
filter_axis = 'y'
passthrough.set_filter_field_name(filter_axis)
axis_min = -0.55
axis_max = 0.55
passthrough.set_filter_limits(axis_min, axis_max)
pcl_data_filtered = passthrough.filter()
# RANSAC Plane Segmentation
seg = pcl_data_filtered.make_segmenter()
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
max_distance = 0.01
seg.set_distance_threshold(max_distance)
inliers, coefficients = seg.segment()
# Extract inliers and outliers
cloud_table = pcl_data_filtered.extract(inliers, negative=False)
cloud_objects = pcl_data_filtered.extract(inliers, negative=True)
# Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
tree = white_cloud.make_kdtree()
# Create Cluster-Mask Point Cloud to visualize each cluster separately
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.02)
ec.set_MinClusterSize(100)
ec.set_MaxClusterSize(25000)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
# Assign a color corresponding to each segmented object in scene
cluster_color = get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
for i, indice in enumerate(indices):
color_cluster_point_list.append([
white_cloud[indice][0], white_cloud[indice][1],
white_cloud[indice][2],
rgb_to_float(cluster_color[j])
])
# Create new cloud containing all clusters, each with unique color
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
# Convert PCL data to ROS messages
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# Publish ROS messages
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cluster_cloud)
collision_pub.publish(ros_cloud_table)
# classification
detected_objects_labels = []
detected_objects = []
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster
pcl_cluster = cloud_objects.extract(pts_list)
# convert the cluster from pcl to ROS using helper function
ros_cluster_cloud = pcl_to_ros(pcl_cluster)
# Extract histogram features
color_hists = compute_color_histograms(ros_cluster_cloud,
using_hsv=True)
normal_hists = compute_normal_histograms(
get_normals(ros_cluster_cloud))
feature = np.concatenate((color_hists, normal_hists))
# Make the prediction, retrieve the label for the result
# and add it to detected_objects_labels list
prediction = clf.predict(scaler.transform(feature.reshape(1, -1)))
label = encoder.inverse_transform(prediction)[0]
detected_objects_labels.append(label)
# Publish a label into RViz
label_pos = list(white_cloud[pts_list[0]])
label_pos[2] += .4
object_markers_pub.publish(make_label(label, label_pos, index))
# Add the detected object to the list of detected objects.
do = DetectedObject()
do.label = label
do.cloud = ros_cluster_cloud
detected_objects.append(do)
detected_objects_pub.publish(detected_objects)
rospy.loginfo('Detected {} objects: {}'.format(
len(detected_objects_labels), detected_objects_labels))
def pcl_callback(pcl_msg):
rospy.loginfo('Begin pcl_callback...')
# Exercise-2 TODOs:
# DONE: Convert ROS msg to PCL data(PointXYZRGB)
cloud = ros_to_pcl(pcl_msg)
# DONE: Voxel Grid Downsampling
cloud_filtered = voxel_filter(cloud)
# DONE: PassThrough Filter
# Assign axis and range to the passthrough filter object.
cloud_filtered = pass_through_filter(cloud_filtered,
filter_axis='z',
axis_limit=(0.6, 1.1))
# Cut off the near side of the table too.
cloud_filtered = pass_through_filter(cloud_filtered,
filter_axis='y',
axis_limit=(-10.0, -1.39))
# DONE: RANSAC Plane Segmentation
inliers, _ = ransac_plane_segmentation(cloud_filtered)
# DONE: Extract inliers and outliers
cloud_table = cloud_filtered.extract(inliers, negative=False)
cloud_objects = cloud_filtered.extract(inliers, negative=True)
rospy.loginfo(' cloud_objects: %d' % (cloud_objects.size))
# DONE: Euclidean Clustering
cluster_indices, white_cloud = euclidean_clustering(cloud_objects,
tolerance=0.02,
cluster_range=(100,
15000))
rospy.loginfo(' cluster_indices: %d, white_cloud: %d ' %
(len(cluster_indices), white_cloud.size))
# DONE: Create Cluster-Mask Point Cloud to visualize each cluster separately
cluster_cloud = create_colored_cluster_cloud(cluster_indices, white_cloud)
rospy.loginfo(' colored cluster: %d' % (cluster_cloud.size))
# DONE: Convert PCL data to ROS messages
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# DONE: Publish ROS messages
rospy.loginfo(' Publish to objects')
pcl_objects_pub.publish(ros_cloud_objects)
rospy.loginfo(' Publish to table')
pcl_table_pub.publish(ros_cloud_table)
rospy.loginfo(' Publish to cluster_cloud')
pcl_cluster_pub.publish(ros_cluster_cloud)
rospy.loginfo('DONE Exercise-2')
# Exercise-3 TODOs:
# Classify the clusters! (loop through each detected cluster one at a time)
detected_objects_labels = []
detected_objects = []
# Object recognition
for index, pts_list in enumerate(cluster_indices):
#rospy.loginfo(' Detecting... : %d' % (index))
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = cloud_objects.extract(pts_list)
# DONE: convert the cluster from pcl to ROS using helper function
ros_cluster = pcl_to_ros(pcl_cluster)
# Extract histogram features
# DONE: complete this step just as is covered in capture_features.py
# Compute the associated feature vector
#rospy.loginfo(' type : %s' % (type(ros_cluster)))
chists = compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
nhists = compute_normal_histograms(normals)
feature = np.concatenate((chists, nhists))
# Make the prediction, retrieve the label for the result
# and add it to detected_objects_labels list
prediction = clf.predict(scaler.transform(feature.reshape(1, -1)))
label = encoder.inverse_transform(prediction)[0]
detected_objects_labels.append(label)
# Publish a label into RViz
label_pos = list(white_cloud[pts_list[0]])
label_pos[2] += .4
object_markers_pub.publish(make_label(label, label_pos, index))
# Add the detected object to the list of detected objects.
do = DetectedObject()
do.label = label
do.cloud = ros_cluster
detected_objects.append(do)
rospy.loginfo('Detected {} objects: {}'.format(
len(detected_objects_labels), detected_objects_labels))
# Publish the list of detected objects
# This is the output you'll need to complete the upcoming project!
detected_objects_pub.publish(detected_objects)