def make_detectedObject(label, cluster_msg): detectedObject = DetectedObject() detectedObject.label = label detectedObject.cloud = cluster_msg return detectedObject
def classify_objects_clusters(cloud_objects, cluster_idx):
# Classify the clusters! (loop through each detected cluster one at a time)
detected_objects_labels = []
detected_objects = []
for index, pts_list in enumerate(cluster_idx):
# Grab the points for the cluster
pcl_cluster = cloud_objects.extract(pts_list)
ros_cluster = pcl_to_ros(pcl_cluster)
# Compute the associated feature vector
color_h = compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
normal_h = compute_normal_histograms(normals)
feature_vector = np.concatenate((color_h, normal_h))
# Make the prediction
prediction = clf.predict(
scaler.transform(feature_vector.reshape(1, -1)))
label = encoder.inverse_transform(prediction)[0]
detected_objects_labels.append(label)
# Publish a label into RViz
label_pos = list(cloud_objects[pts_list[0]])
label_pos[2] += 0.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)
return detected_objects, detected_objects_labels
def classify_clusters(cluster_indices, cloud_objects, white_cloud):
detected_objects_labels = []
detected_objects = []
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = cloud_objects.extract(pts_list)
# convert the cluster from pcl to ROS using helper function
ros_cluster = pcl_to_ros(pcl_cluster)
# Extract histogram features
# 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[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)
return detected_objects, detected_objects_labels
def detect_objects(object_cloud, cluster_indices):
detected_objects = []
detected_objects_labels = []
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = object_cloud.extract(pts_list)
ros_cluster = pcl_to_ros(pcl_cluster)
# Extract histogram features
features = extract_features(ros_cluster)
# Make the prediction, retrieve the label for the result
# and add it to detected_objects_labels list
prediction = clf.predict(scaler.transform(features.reshape(1, -1)))
label = encoder.inverse_transform(prediction)[0]
detected_objects_labels.append(label)
# Publish a label into RViz
label_pos = list(object_cloud[pts_list[0]])[:-1]
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)
return detected_objects, detected_objects_labels
def cluster(point_cloud, white_cloud, cluster_indices):
# Exercise-3
# 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):
# Grab the points for the cluster
pcl_cluster = point_cloud.extract(pts_list)
# convert the cluster from pcl to ROS using helper function
ros_cluster = pcl_to_ros(pcl_cluster)
# Compute the associated feature vector
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
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)
return [detected_objects, detected_objects_labels]
def __object_detection(self, cluster_indices, cloud_objects):
"""Detect objects given PCL cloud objects and the cluster indices."""
do_labels = []
del self.detected_objects_list[:]
# Classify the clusters! (loop through each detected cluster one at a time)
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster
pcl_cluster = cloud_objects.extract(pts_list)
ros_cluster = pcl_to_ros(pcl_cluster)
# Compute the associated feature vector
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 do_labels list
prediction = self.clf.predict(
self.scaler.transform(feature.reshape(1, -1)))
rospy.loginfo('Prediction: {} {}'.format(
prediction, self.encoder.inverse_transform(prediction)))
label = self.encoder.inverse_transform(prediction)[0]
do_labels.append(label)
if DEBUG_SVM:
# NOTE: The SVC should be created during training with probability=True
prediction_proba = self.clf.predict_proba(
self.scaler.transform(feature.reshape(1, -1)))
rospy.loginfo(
'Prediction probabilities: {} for classes {}'.format(
prediction_proba, self.encoder.classes_))
probability = np.max(prediction_proba)
rospy.loginfo('Detected {} with probability {}'.format(
label, probability))
# The object marker label in RViz will have the object detected
# name and its probability
marker_label = '{} {:0.2f}'.format(label,
np.asscalar(probability))
else:
marker_label = label
# Publish the detected object label to RViz
label_pos = get_pcl_centroid(pcl_cluster)
label_pos[2] += .2
self.object_markers_pub.publish(
make_label(marker_label, label_pos, index))
# Add the detected object to the list of detected objects so it can
# be retrieved later by the PR2 logic componenets
do = DetectedObject()
do.label = '{}'.format(label)
do.cloud = ros_cluster
self.detected_objects_list.append(do)
# Publish the list of detected objects for debugging purposes.
rospy.loginfo('Detected {} objects: {}'.format(len(do_labels),
do_labels))
self.detected_objects_pub.publish(self.detected_objects_list)
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# TODO: Convert ROS msg to PCL data
# TODO: Voxel Grid Downsampling
# TODO: PassThrough Filter
# TODO: RANSAC Plane Segmentation
# TODO: Extract inliers and outliers
# TODO: Euclidean Clustering
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
# TODO: Convert PCL data to ROS messages
# TODO: Publish ROS messages
# Exercise-3 TODOs:
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = cloud_objects.extract(pts_list)
# TODO: convert the cluster from pcl to ROS using helper function
# Extract histogram features
# TODO: complete this step just as is covered in capture_features.py
# 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)
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 detect_objects(self, cloud, cluster_indices):
#take in cloud and array of point indices within clusters
#return array of detected objects within cloud along with
#dict of the object labels and their corresponding centroids
# Classify the clusters! (loop through each detected cluster one at a time)
detected_objects_labels = []
detected_objects = []
#detected_objects_list = {'labels':[], 'centroids':[]}
positions = []
centroids = []
for j, indices in enumerate(cluster_indices):
# Grab the points for the cluster
cluster = cloud.extract(indices)
centroid = self.get_centroid(cluster)
#set the starting position of the label
#based on first point in cluster
#offset later during label publishing
positions.append(list(cluster[0][:3]))
cluster = pcl_to_ros(cluster)
# Compute the associated feature vector
chists = compute_color_histograms(cluster) #, using_hsv=True)
normals = self.get_normals(cluster)
nhists = compute_normal_histograms(normals)
feature_vector = np.concatenate((chists, nhists))
# Make the prediction
prediction = self.clf.predict(
self.scaler.transform(feature_vector.reshape(1, -1)))
label = self.encoder.inverse_transform(prediction)[0]
# Add the detected object to the list of detected objects.
do = DetectedObject()
do.label = label
do.cloud = cluster
detected_objects.append(do)
detected_objects_labels.append(label)
centroids.append(centroid)
detected_objects_dict = dict(zip(detected_objects_labels, centroids))
rospy.loginfo('Detected {} objects: {}'.format(
len(detected_objects_labels), detected_objects_labels))
return detected_objects, detected_objects_dict
def detect_objects(cloud_objects, cluster_indices):
# TODO: Classify the clusters!
detected_objects_labels = []
detected_objects = []
labeled_features = []
for index, pts_list in enumerate(cluster_indices):
# TODO: Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = cloud_objects.extract(pts_list)
# TODO: Convert the cluster from pcl to ROS using helper function
ros_cluster = pclh.pcl_to_ros(pcl_cluster)
# TODO: Extract histogram features
hists_color = ft.compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
hists_normals = ft.compute_normal_histograms(normals)
feature = np.concatenate((hists_color, hists_normals))
#labeled_features.append([feature, model_name])
# TODO: Make the prediction
prediction = clf.predict(scaler.transform(feature.reshape(1, -1)))
# TODO: Retrieve the label for the result
label = encoder.inverse_transform(prediction)[0]
# TODO: Add it to detected_objects_labels list
detected_objects_labels.append(label)
# TODO: Publish a label into RViz
white_cloud = pclh.XYZRGB_to_XYZ(cloud_objects)
label_pos = list(white_cloud[pts_list[0]])
label_pos[2] += .2
object_markers_pub.publish(make_label(label, label_pos, index))
# TODO: 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))
return detected_objects, detected_objects_labels
def classify(white_cloud, cloud_objects, cluster_indices):
# Classify the clusters! (loop through each detected cluster one at a time)
detected_objects_labels = []
detected_objects = []
for idx, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster
pcl_cluster = cloud_objects.extract(pts_list)
# 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 you did before in capture_features.py
chists = compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
nhists = compute_normal_histograms(normals)
if np.isnan(nhists).any():
continue
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, idx))
# Add the detected object to the list of detected objects.
an_object = DetectedObject()
an_object.label = label
an_object.cloud = ros_cluster
detected_objects.append(an_object)
rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))
return detected_objects
def classification(pcl_msg):
objects, cluster_indices, white_cloud = segmentation(pcl_msg)
#---------------------------------------
# Classify the clusters! (loop through each detected cluster one at a time)
#---------------------------------------
# Store the detected objects and labels in thesevc lists
detected_objects_labels = []
detected_objects_list = []
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = objects.extract(pts_list) # <type 'pcl._pcl.PointCloud_PointXYZRGB'>
# TODO: convert the cluster from pcl to ROS using helper function
ros_cluster = pcl_to_ros(pcl_cluster)
# Extract histogram features
histogram_bins = 128
chists = compute_color_histograms(ros_cluster, nbins=histogram_bins, using_hsv=True)
normals = get_normals(ros_cluster)
nhists = compute_normal_histograms(normals, nbins=histogram_bins)
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
pcl_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_list.append(do)
# Publish the list of detected objects
rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))
return detected_objects_list, detected_objects_labels
def detect_objects(cluster_indices, cloud_objects, white_cloud, clf,
object_markers_pub, scaler, encoder):
# Create some empty lists
detected_objects_labels = []
detected_objects = []
for index, pts_list in enumerate(cluster_indices):
# Grab points for the cluster from the extracted outliers(cloud_objects)
pcl_cluster = cloud_objects.extract(pts_list)
# Convert pcl_cluster to ros_msg data type
ros_cluster = pcl_to_ros(pcl_cluster)
# Extract histogram features
chists = compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
nhists = compute_normal_histograms(normals)
feature = np.concatenate((chists, nhists))
# labeled_features.append([feature, model_name])
# Make prediction, retrieve the label for the result
# then 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 label to 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))
return detected_objects
def classify_clusters(cluster_indices, white_cloud, extracted_inliers_objects):
# 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):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = extracted_inliers_objects.extract(pts_list)
# Compute the associated feature vector
# convert the cluster from pcl to ROS using helper function
ros_pcl_objects = pcl_to_ros(pcl_cluster)
# Extract histogram features
chists = compute_color_histograms(ros_pcl_objects, using_hsv=True)
normals = get_normals(ros_pcl_objects)
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_pcl_objects
detected_objects.append(do)
rospy.loginfo('Detected {} objects: {}'.format(
len(detected_objects_labels), detected_objects_labels))
return detected_objects
def classifyClusters(point_cloud, white_cloud, cluster_indices):
detected_objects_labels = []
detected_objects = []
# Classify each detected cluster
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = point_cloud.extract(pts_list)
# convert the cluster from pcl to ROS using helper function
ros_cluster = pcl_to_ros(pcl_cluster)
# Extract the cluster colour feature histogram
chists = compute_color_histograms(ros_cluster,
using_hsv=True,
nbins=44)
normals = get_normals(ros_cluster)
# Extract the cluster surface normals histogram
nhists = compute_normal_histograms(normals, nbins=20)
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)
# store the label to be published 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)
return [detected_objects, detected_objects_labels]
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 recognise_object(sample_cloud):
"""
Attempts to recognise the given sample cloud as an object.
:sample_cloud: The sample cloud we are trying to recognise.
:returns: The detected object and label.
"""
chists = compute_color_histograms(sample_cloud, using_hsv=True)
normals = get_normals(sample_cloud)
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]
do = DetectedObject()
do.label = label
do.cloud = sample_cloud
return do, label
def detect_objects(self, cloud, cluster_indices):
# Classify the clusters! (loop through each detected cluster one at a time)
detected_objects_labels = []
detected_objects = []
positions = []
for j, indices in enumerate(cluster_indices):
# Grab the points for the cluster
cluster = cloud.extract(indices)
positions.append(list(cluster[0][:3]))
cluster = pcl_to_ros(cluster)
# Compute the associated feature vector
chists = compute_color_histograms(cluster, using_hsv=True)
normals = self.get_normals(cluster)
nhists = compute_normal_histograms(normals)
feature_vector = np.concatenate((chists, nhists))
# Make the prediction
prediction = self.clf.predict(
self.scaler.transform(feature_vector.reshape(1, -1)))
label = self.encoder.inverse_transform(prediction)[0]
# Add the detected object to the list of detected objects.
do = DetectedObject()
do.label = label
do.cloud = cluster
detected_objects.append(do)
detected_objects_labels.append(label)
rospy.loginfo('Detected {} objects: {}'.format(
len(detected_objects_labels), detected_objects_labels))
return detected_objects, detected_objects_labels, positions
def pcl_callback(pcl_msg):
# Convert ROS msg to PCL data
pcl_msg = ros_to_pcl(pcl_msg)
# Voxel Grid Downsampling
vox = pcl_msg.make_voxel_grid_filter()
LEAF_SIZE = .005
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_filtered = vox.filter()
# Two PassThrough Filter one over Z one over X
passthrough = cloud_filtered.make_passthrough_filter()
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.3
axis_max = 5.0
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()
passthrough = cloud_filtered.make_passthrough_filter()
filter_axis = 'x'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.34
axis_max = 1.0
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()
# Outlier filter
outlier_filter = cloud_filtered.make_statistical_outlier_filter()
outlier_filter.set_mean_k(50)
x = 0.5
outlier_filter.set_std_dev_mul_thresh(x)
cloud_filtered = outlier_filter.filter()
# 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.015
seg.set_distance_threshold(max_distance)
inliers, coefficients = seg.segment()
# Extract inliers and outliers
extracted_inliers = cloud_filtered.extract(inliers, negative=False)
extracted_outliers = cloud_filtered.extract(inliers, negative=True)
# Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(
extracted_outliers) # Apply function to convert XYZRGB to XYZ
tree = white_cloud.make_kdtree()
#Create a cluster extraction object
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold
# as well as minimum and maximum cluster size (in points)
ec.set_ClusterTolerance(0.01)
ec.set_MinClusterSize(50)
ec.set_MaxClusterSize(15000)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
# Create Cluster-Mask Point Cloud to visualize each cluster separately
# 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
table_pcl_msg = pcl_to_ros(extracted_inliers)
objects_pcl_msg = pcl_to_ros(extracted_outliers)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# Publish ROS messages
pcl_objects_pub.publish(objects_pcl_msg)
pcl_table_pub.publish(table_pcl_msg)
pcl_clusters_pub.publish(ros_cluster_cloud)
# Exercise-3 TODOs:
# Classify the clusters! (loop through each detected cluster one at a time)
detected_objects_labels = []
detected_objects = []
lastone = 0
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster
pcl_cluster = extracted_outliers.extract(pts_list)
ros_cluster = pcl_to_ros(pcl_cluster)
# Compute the associated feature vector
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
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] += .25
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)
# Publish the list of detected objects
# This is the output you'll need to complete the upcoming project!
detected_objects_pub.publish(detected_objects)
# 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 pcl_callback(pcl_msg):
# Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# Outlier Removal Filter
outlier_filter = cloud.make_statistical_outlier_filter()
outlier_filter.set_mean_k(50)
outlier_filter.set_std_dev_mul_thresh(0.5)
cloud_filtered = outlier_filter.filter()
# Voxel Grid Downsampling
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()
# PassThrough Filter
z_passthrough = cloud_filtered.make_passthrough_filter()
z_passthrough.set_filter_field_name("z")
z_passthrough.set_filter_limits(0.6, 0.85)
cloud_filtered = z_passthrough.filter()
y_passthrough = cloud_filtered.make_passthrough_filter()
y_passthrough.set_filter_field_name("y")
y_passthrough.set_filter_limits(-0.45, 0.45)
cloud_filtered = y_passthrough.filter()
# 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.025
seg.set_distance_threshold(max_distance)
inliers, coefficients = seg.segment()
# Extract inliers and outliers
cloud_table = cloud_filtered.extract(inliers, negative=False)
cloud_objects = cloud_filtered.extract(inliers, negative=True)
# Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.05)
ec.set_MinClusterSize(100)
ec.set_MaxClusterSize(100000)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
# Create Cluster-Mask Point Cloud to visualize each cluster separately
cluster_color = get_color_list(len(cluster_indices))
color_cluster_point_list = list()
for j, indices in enumerate(cluster_indices):
for i, index in enumerate(indices):
color_cluster_point_list.append([white_cloud[index][0],
white_cloud[index][1],
white_cloud[index][2],
rgb_to_float(cluster_color[j])])
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
# Convert PCL data to ROS messages
ros_cloud = pcl_to_ros(cluster_cloud)
# Publish ROS messages
pcl_pub.publish(ros_cloud)
detected_objects_labels = list()
detected_objects_list = list()
# Classify the clusters! (loop through each detected cluster one at a time)
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster
pcl_cluster = cloud_objects.extract(pts_list)
ros_cluster = pcl_to_ros(pcl_cluster)
# Compute the associated feature vector
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
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] += 0.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_list.append(do)
# Publish the list of detected objects
rospy.loginfo("Detected {} objects: {}".format(len(detected_objects_labels), detected_objects_labels))
detected_objects_pub.publish(detected_objects_list)
# 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_list, cloud_table)
except rospy.ROSInterruptException:
pass
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# TODO: Convert ROS msg to PCL data
pcl_cloud = ros_to_pcl(pcl_msg)
# TODO: Statistical Outlier Filtering
#create filter object
outlier_filtered = pcl_cloud.make_statistical_outlier_filter()
# Set number of neighboring points to analyse for a given points
outlier_filtered.set_mean_k(5)
# Set threshold scale factor
x = 1.0
# Consider any point with a mean distance than global mean (mean + x * std_dev)
outlier_filtered.set_std_dev_mul_thresh(x)
# Finally, call the filter function
cloud_filtered = outlier_filtered.filter()
filename = 'outlier_filtered.pcd'
pcl.save(cloud_filtered,filename )
# TODO: Voxel Grid Downsampling
# Create a voxelGrid filter object the point cloud
vox = cloud_filtered.make_voxel_grid_filter()
# Choose a voxel or leaf size
LEAF_SIZE = 0.01
# Set the leaf size
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
# call the filter function to obtain the resultant downsampled point cloud
cloud_downsampled = vox.filter()
filename= 'voxel_downsampled.pcd'
pcl.save(cloud_downsampled,filename)
# TODO: PassThrough Filter
# Create passthrough filter object
passthrough = cloud_downsampled.make_passthrough_filter()
# Assign axis and range to the passthrough filter object for z axis
filter_axis='z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.61
axis_max = 0.9
passthrough.set_filter_limits(axis_min,axis_max)
cloud_passed = passthrough.filter()
# Assign axis and range to the passthrough filter object for y axis
passthrough = cloud_passed.make_passthrough_filter()
filter_axis='y'
passthrough.set_filter_field_name(filter_axis)
axis_min = -0.4
axis_max = +0.4
passthrough.set_filter_limits(axis_min,axis_max )
cloud_passed = passthrough.filter()
filename ='pass_through_cloud_filtered.pcd'
pcl.save(cloud_passed, filename)
# TODO: RANSAC Plane Segmentation
# Create the segmentation object
seg = cloud_passed.make_segmenter()
# Set model you wish to fit
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
# Set maximum distant to be considerred for fitiing
max_distance = 0.01
seg.set_distance_threshold(max_distance)
# Call the segment function to obtain set of inliers indices and model coefficients
inliers, coefficients = seg.segment()
# Extract inliers
cloud_table = cloud_passed.extract(inliers, negative=False)
filename = 'extracted_inliers.pcd'
# Save pcd for table
pcl.save(cloud_table, filename)
# Extract outliers
cloud_objects = cloud_passed.extract(inliers, negative=True)
filename = 'extracted_outliers.pcd'
# Save pcd for tabletop objects
pcl.save(cloud_objects, filename)
# TODO: Euclidean Clustering
# Apply function to convert XYZRGB to XYZ
white_cloud = XYZRGB_to_XYZ(cloud_objects)
# Contruct k-d tree
tree = white_cloud.make_kdtree()
# Create a cluster extraction object
ec = white_cloud.make_EuclideanClusterExtraction()
# set tolearance for diistance threshold as well as maximum cluster size
ec.set_ClusterTolerance(0.02)
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(2800)
# Search the k-d tree for cluster
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered cluster
cluster_indices = ec.Extract()
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
# Assign a color to each segmented object in the 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 cluster to contain all cluters each with unique color
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
filename ='cluster_cloud.pcd'
pcl.save(cluster_cloud, filename)
# TODO: Convert PCL data to ROS messages
ros_cloud_filtered = pcl_to_ros(cloud_passed)
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cloud_objects = pcl_to_ros(cloud_objects)
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_cloud_filtered)
pcl_cluster_cloud_pub.publish(ros_cluster_cloud)
# Exercise-3 TODOs:
# Classify the clusters! (loop through each detected cluster one at a time)
detected_object_labels = []
detected_objects = []
for index, point_list in enumerate(cluster_indices):
# Grab the points for the cluster from the extracted outlier (cloud_objects)
pcl_cluster = cloud_objects.extract(point_list)
# convert cluster from pcl to ROS
ros_cluster = pcl_to_ros(pcl_cluster)
# Compute the associated feature vector
color_hists = compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
normals_hists = compute_normal_histograms(normals)
hists_feature = np.concatenate((color_hists, normals_hists))
# Make the prediction
# Retrieve the label for the result and add it to detection_objects_label
prediction = clf.predict(scaler.transform(hists_feature.reshape(1, -1)))
label = encoder.inverse_transform(prediction)[0]
detected_object_labels.append(label)
# Publish a label into RViz
label_pos = list(white_cloud[point_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)
# Publish the list of detected objectss
detected_objects_pub.publish(detected_objects)
# 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 pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# TODO: Convert ROS msg to PCL data
pclMsg = ros_to_pcl(pcl_msg)
# TODO: Voxel Grid Downsampling
vox = pclMsg.make_voxel_grid_filter()
LEAF_SIZE = 0.01
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.77
axis_max = 1.1
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.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
pcl_cloud_table = cloud_filtered.extract(inliers, negative=False)
pcl_cloud_objects = cloud_filtered.extract(inliers, negative=True)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(
pcl_cloud_objects) # Apply function to convert XYZRGB to XYZ
tree = white_cloud.make_kdtree()
# Create a cluster extraction object
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold
# as well as minimum and maximum cluster size (in points)
ec.set_ClusterTolerance(0.05)
ec.set_MinClusterSize(50)
ec.set_MaxClusterSize(2000)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
# 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):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = pcl_cloud_objects.extract(pts_list)
# 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[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)
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
rospy.loginfo('Ex-2')
# TODO: Convert ROS msg to PCL data
pcl_data = ros_to_pcl(pcl_msg) # XYZRGB
#print('size1: ', pcl_data.size)
pcl_data.to_file('pcl_data.pcd')
pcl_data = outlier_filter(pcl_data)
#print('size2: ', pcl_data.size)
#cloud_filtered = pcl_data
# TODO: Voxel Grid Downsampling
vox = pcl_data.make_voxel_grid_filter()
# try a voxel size and set to cloud
LEAF_SIZE = 0.005
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
# call filter function to obtain the resultant downsampled PC
cloud_filtered = vox.filter()
# save file
##filename = 'voxel_sampled1.pcd'
##pcl.save(cloud_filtered, filename)
#print('size3: ', cloud_filtered.size)
# TODO: PassThrough Filter to isolate the table and objects
passthrough_z = cloud_filtered.make_passthrough_filter()
# Assign axis and range to the passthrough filter object
filter_axis = "z"
passthrough_z.set_filter_field_name(filter_axis)
axis_min = 0.6 # 0.6, 1.1
axis_max = 0.85 # 1.5 1.1
passthrough_z.set_filter_limits(axis_min, axis_max)
#passthrough.set_filter_limits(0, 1.5)
# use filter function to obtain only table and objects
cloud_filtered = passthrough_z.filter()
passthrough_y = cloud_filtered.make_passthrough_filter()
passthrough_y.set_filter_field_name("y")
axis_min = -0.42
axis_max = 0.42
passthrough_y.set_filter_limits(axis_min, axis_max)
# use filter function to obtain only table and objects
cloud_filtered = passthrough_y.filter()
# save result
filename = 'pass_through_filtered1.pcd'
pcl.save(cloud_filtered, filename)
#print('size4: ', cloud_filtered.size)
# TODO: RANSAC Plane Segmentation, will table and objects
# seg = cloud_filtered.make_segmenter_normals(ksearch=50)
seg = cloud_filtered.make_segmenter()
seg.set_optimize_coefficients(True)
# TODO: Extract inliers and outliers
# set the model we wish to fit
# seg.set_model_type(pcl.SACMODEL_NORMAL_PLANE)
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
# seg.set_max_iterations(100)
max_distance = 0.018 # 0.01
# set max distance for a point to be considered fitting the model
seg.set_distance_threshold(max_distance)
# call the segment function to obtain set of inlier indices and model coefficients
inliers, coefficients = seg.segment()
#print(coefficients)
# extracted inliers - table
pcl_table = cloud_filtered.extract(inliers, negative=False)
# save file table
filename = 'pcl_table1.pcd'
pcl.save(pcl_table, filename)
# extracted outliers - objects
pcl_objects = cloud_filtered.extract(inliers, negative=True)
# save file objects
filename = 'pcl_objects1.pcd'
pcl.save(pcl_objects, filename)
# Outlier Removal Filter
# pcl_objects = outlier_filter(pcl_objects)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(pcl_objects)
tree = white_cloud.make_kdtree()
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
# create a cluster extraction object
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold
# as well as minimum and maximum cluster size (in points)
ec.set_ClusterTolerance(0.04) # .08
ec.set_MinClusterSize(30)
ec.set_MaxClusterSize(100000)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
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)
# TODO: Convert PCL data to ROS messages
ros_cloud_objects = pcl_to_ros(pcl_objects)
ros_cloud_table = pcl_to_ros(pcl_table)
# now publish ros_cluster_cloud
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)
# Exercise-3 TODOs:
rospy.loginfo('Ex-3')
# Classify the clusters! (loop through each detected cluster one at a time)
detected_objects_list = []
detected_labels = []
for index, pts_list in enumerate(cluster_indices):
features = []
#print(index, len(cluster_indices))
# Grab the points for the cluster
pcl_cluster = pcl_objects.extract(pts_list)
pcl_cluster.to_file('pcl_cluster1-{0}.pcd'.format(index))
# Convert the cluster from pcl to ROS using helper function
ros_cluster_cloud = pcl_to_ros(pcl_cluster)
# Compute the associated feature vector
# Extract histogram features
chists = compute_color_histograms(ros_cluster_cloud, using_hsv=True)
normals = get_normals(ros_cluster_cloud)
nhists = compute_normal_histograms(normals)
features = np.concatenate((chists, nhists))
###labeled_features.append([feature, model_name])
# Make the prediction, retrieve the label for the result
# and add it to detected_objects_labels list
sc = scaler.transform(features.reshape(1, -1))
prediction = clf.predict(sc)
label = encoder.inverse_transform(prediction)[0]
detected_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_list.append(do)
# Objects detected
rospy.loginfo('Detected {} objects: {}'.format(len(detected_labels),
detected_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_list)
# 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_list)
except rospy.ROSInterruptException:
pass
def pcl_callback(pcl_msg):
# TODO: Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# TODO: Voxel Grid Downsampling
vox = cloud.make_voxel_grid_filter()
LEAF_SIZE = 0.01
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()
# 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
cloud_table = cloud_filtered.extract(inliers, negative=False)
cloud_objects = cloud_filtered.extract(inliers, negative=True)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
tree = white_cloud.make_kdtree()
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.05)
ec.set_MinClusterSize(100)
ec.set_MaxClusterSize(10000)
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)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
pcl_cluster_pub.publish(ros_cluster_cloud)
# 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):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = cloud_objects.extract(pts_list)
# 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=False)
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)
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# Convert ROS msg to PCL data
cloud_objects = ros_to_pcl(pcl_msg)
# Statistical Outlier Filtering
outlier_filter = cloud_objects.make_statistical_outlier_filter()
outlier_filter.set_mean_k(20)
outlier_filter.set_std_dev_mul_thresh(0.1)
cloud_objects = outlier_filter.filter()
# Voxel Grid Downsampling
LEAF_SIZE = 0.002
vox = cloud_objects.make_voxel_grid_filter()
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_objects = vox.filter()
# PassThrough Filter
axis_min = 0.6
axis_max = 1.0
filter_axis = 'z'
passthrough = cloud_objects.make_passthrough_filter()
passthrough.set_filter_field_name(filter_axis)
passthrough.set_filter_limits(axis_min, axis_max)
cloud_objects = passthrough.filter()
axis_min = -0.4
axis_max = 0.4
filter_axis = 'y'
passthrough2 = cloud_objects.make_passthrough_filter()
passthrough2.set_filter_field_name(filter_axis)
passthrough2.set_filter_limits(axis_min, axis_max)
cloud_objects = passthrough2.filter()
# RANSAC Plane Segmentation
seg = cloud_objects.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_objects = cloud_objects.extract(inliers, negative=True)
# Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.05)
ec.set_MinClusterSize(20)
ec.set_MaxClusterSize(50000)
ec.set_SearchMethod(tree)
cluster_indicies = ec.Extract()
# Create Cluster-Mask Point Cloud to visualize each cluster separately
colors = get_color_list(len(cluster_indicies))
cloud_objects_cluster_list = []
for i, pts_list in enumerate(cluster_indicies):
pcl_cluster = cloud_objects.extract(pts_list)
color = colors[i]
cluster = XYZ_to_XYZRGB(pcl_cluster, color)
cloud_objects_cluster_list.extend(cluster)
cloud_objects_cluster = pcl.PointCloud_PointXYZRGB()
cloud_objects_cluster.from_list(cloud_objects_cluster_list)
# Convert PCL data to ROS messages
ros_cluster = pcl_to_ros(cloud_objects_cluster)
# Publish ROS messages
p_cluster_pub.publish(ros_cluster)
# Exercise-3 TODOs:
detected_objects_labels = []
detected_objects = []
# Classify the clusters! (loop through each detected cluster one at a time)
for index, pts_list in enumerate(cluster_indicies):
# Grab the points for the cluster
pcl_cluster = cloud_objects.extract(pts_list)
ros_cluster = pcl_to_ros(pcl_cluster)
# Compute the associated feature vector
chists = compute_color_histograms(ros_cluster,
bins=128,
using_hsv=True)
normals = get_normals(ros_cluster)
nhists = compute_normal_histograms(normals, bins=128)
feature = np.concatenate((chists, nhists))
# Make the prediction
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)
# Publish the list of detected objects
rospy.loginfo('Detected {} objects: {}'.format(
len(detected_objects_labels), detected_objects_labels))
detected_objects_pub.publish(detected_objects)
# 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()
ground_truth = []
object_list_param = rospy.get_param('/object_list')
for object_param in object_list_param:
ground_truth.append(object_param['name'])
ground_truth_set = set(ground_truth)
detected_objects_set = set(detected_objects_labels)
n1 = len(ground_truth_set)
n2 = len(detected_objects_set)
# run pr2_mover only when requirments are met:
# 100% (3/3) objects in test1.world
# 80% (4/5) objects in test2.world
# 75% (6/8) objects in test3.world
print(ground_truth)
print(n1, n2)
if (n1 == 3 and n2 == 3) or (n1 == 5 and n2 >= 4) or (n1 == 8 and n2 >= 6):
try:
pr2_mover(detected_objects)
except rospy.ROSInterruptException:
pass
def pcl_callback(ros_pcl_msg):
#----------------------------------------------------------------------------------
# Convert a ROS PointCloud2 message to a pcl PointXYZRGB
#----------------------------------------------------------------------------------
cloud_filtered = ros_to_pcl(ros_pcl_msg)
#----------------------------------------------------------------------------------
# Statistical Outlier Filtering
#----------------------------------------------------------------------------------
# Create a statistical filter object:
outlier_filter = cloud_filtered.make_statistical_outlier_filter()
# Set the number of neighboring points to analyze for any given point
outlier_filter.set_mean_k(3)
# Set threshold scale factor
x = 0.00001
# 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)
# Call the filter function
cloud_filtered = outlier_filter.filter()
#----------------------------------------------------------------------------------
# Voxel Grid Downsampling filter
#----------------------------------------------------------------------------------
# Create a VoxelGrid filter object for our input point cloud
vox = cloud_filtered.make_voxel_grid_filter()
# Choose a voxel (also known as leaf) size
# 1 means 1mx1mx1m leaf size
# Experiment and find the appropriate size!
LEAF_SIZE = 0.005
# Set the voxel (or leaf) size
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
# Call the filter function to obtain the resultant downsampled point cloud
cloud_filtered = vox.filter()
#----------------------------------------------------------------------------------
# PassThrough filter (Z Axis)
#----------------------------------------------------------------------------------
# Create a PassThrough filter object.
passthrough = cloud_filtered.make_passthrough_filter()
# Assign axis and range to the passthrough filter object.
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.6095
axis_max = 1.1
passthrough.set_filter_limits(axis_min, axis_max)
# Use the filter function to obtain the resultant point cloud.
cloud_filtered = passthrough.filter()
#----------------------------------------------------------------------------------
# PassThrough filter (Y Axis)
#----------------------------------------------------------------------------------
# Create a PassThrough filter object.
passthrough = cloud_filtered.make_passthrough_filter()
# Assign axis and range to the passthrough filter object.
filter_axis = 'y'
passthrough.set_filter_field_name(filter_axis)
axis_min = -0.456
axis_max = 0.456
passthrough.set_filter_limits(axis_min, axis_max)
# Use the filter function to obtain the resultant point cloud.
cloud_filtered = passthrough.filter()
#----------------------------------------------------------------------------------
# RANSAC plane segmentation
#----------------------------------------------------------------------------------
# Create the segmentation object
seg = cloud_filtered.make_segmenter()
# Set the model you wish to fit
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
# Max distance for a point to be considered fitting the model
# Experiment with different values for max_distance
# for segmenting the table
max_distance = 0.006
seg.set_distance_threshold(max_distance)
# Call the segment function to obtain set of inlier indices and model coefficients
inliers, coefficients = seg.segment()
# Extract inliers (Table)
extracted_table = cloud_filtered.extract(inliers, negative=False)
# Extract outliers (Tabletop Objects)
extracted_objects = cloud_filtered.extract(inliers, negative=True)
#----------------------------------------------------------------------------------
# Euclidean Clustering
#----------------------------------------------------------------------------------
white_cloud = XYZRGB_to_XYZ(extracted_objects)
tree = white_cloud.make_kdtree()
# Create a cluster extraction object
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold
# as well as minimum and maximum cluster size (in points)
ec.set_ClusterTolerance(0.03)
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(9000)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
#----------------------------------------------------------------------------------
# Create Cluster-Mask Point Cloud to visualize each cluster separately
#----------------------------------------------------------------------------------
# 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)
#----------------------------------------------------------------------------------
# Converts a pcl PointXYZRGB to a ROS PointCloud2 message
#----------------------------------------------------------------------------------
ros_cloud_objects = pcl_to_ros(extracted_objects)
ros_cloud_table = pcl_to_ros(extracted_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)
#----------------------------------------------------------------------------------
# Classify the clusters!
#----------------------------------------------------------------------------------
detected_objects_labels = []
detected_objects = []
for index, pts_list in enumerate(cluster_indices):
#----------------------------------------------------------------------------------
# Grab the points for the cluster from the extracted_objects
#----------------------------------------------------------------------------------
pcl_cluster = extracted_objects.extract(pts_list)
# Convert the cluster from pcl to ROS using helper function
ros_cluster = pcl_to_ros(pcl_cluster)
#----------------------------------------------------------------------------------
# Generate Histograms
#----------------------------------------------------------------------------------
# Color Histogram
c_hists = compute_color_histograms(ros_cluster, using_hsv=True)
# Normals Histogram
normals = get_normals(ros_cluster)
n_hists = compute_normal_histograms(normals)
#----------------------------------------------------------------------------------
# Generate feature by concatenate of color and normals.
#----------------------------------------------------------------------------------
feature = np.concatenate((c_hists, n_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] += .2
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
#----------------------------------------------------------------------------------
detected_objects_pub.publish(detected_objects)
#----------------------------------------------------------------------------------
# Invoke pr2_mover() function
#----------------------------------------------------------------------------------
if len(detected_objects) > 0:
try:
pr2_mover(detected_objects)
except rospy.ROSInterruptException:
pass
else:
rospy.logwarn('No detected objects !!!')
return
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# TODO: Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# TODO: Voxel Grid Downsampling
vox = cloud.make_voxel_grid_filter()
# Select a leaf/voxel size - note that 1.0 is very large.
# The leaf size is measured in meters. Therefore a size of 1 is a cubic meter.
LEAF_SIZE = 0.01 # experiment with this
# Set the voxel/leaf size.
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
# Call the filter function to obtain the resultant downsampled point cloud
cloud_filtered = vox.filter()
# TODO: PassThrough Filter
# Create a passthrough filter object for the z-axis
passthrough_z = cloud_filtered.make_passthrough_filter()
# Assign axis and range of passthrough filter object.
filter_axis = 'z'
passthrough_z.set_filter_field_name(filter_axis)
axis_min = 0.735 # experiment with this
axis_max = 1.1 # experiment with this
passthrough_z.set_filter_limits(axis_min, axis_max)
# Filter the downsampled point cloud with the passthrough object to get resultant cloud.
cloud_filtered = passthrough_z.filter()
# Create a passthrough filter for the y-axis to remove the front table edge
passthrough_y = cloud_filtered.make_passthrough_filter()
filter_axis = 'y'
passthrough_y.set_filter_field_name(filter_axis)
axis_min = -2.26
axis_max = -1.35
passthrough_y.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough_y.filter()
# TODO: RANSAC Plane Segmentation
# Create segmentation model
seg = cloud_filtered.make_segmenter()
# Set the model to fit the objects to
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
# Set the max distance for a point to be considered to be fitting the model
max_distance = 0.001 # experiement with this
seg.set_distance_threshold(max_distance)
# TODO: Extract inliers and outliers
# Call the segment function to obtain the set of inlier indices and model coefficients
inliers, coefficients = seg.segment()
pcl_cloud_objects = cloud_filtered.extract(inliers, negative=True)
pcl_cloud_table = cloud_filtered.extract(inliers, negative=False)
# TODO: Euclidean Clustering
# Convert XYZRGB point cloud to XYZ as Euclidean Clustering cannot use colour information
white_cloud = XYZRGB_to_XYZ(pcl_cloud_objects)
# Construct the k-d tree
tree = white_cloud.make_kdtree()
# Create a cluster extraction object
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold
# as well as a minimum and maximum cluster size (in points)
ec.set_ClusterTolerance(0.05) #experiment
ec.set_MinClusterSize(200) #experiment
ec.set_MaxClusterSize(2000) #experiment
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered indices
cluster_indices = ec.Extract()
# cluster_indices now contains a list of indices for each cluster (a list of lists)
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
# This cloud will contain points for each of the segmented objects, with each set of points having a unique colour.
# Assign a colour corresponding to each segmented object in the camera view
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 object containing all clusters, each with a unique colour
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
# TODO: Convert PCL data to ROS messages
ros_cloud_objects = pcl_to_ros(pcl_cloud_objects)
ros_cloud_table = pcl_to_ros(pcl_cloud_table)
ros_cloud_clusters = 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_cloud.publish(ros_cloud_clusters)
# 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):
# Grab the points for the cluster
sample_cloud = pcl_cloud_objects.extract(pts_list)
sample_cloud = pcl_to_ros(sample_cloud)
# Compute the associated feature vector
chists = compute_color_histograms(sample_cloud, using_hsv=True)
normals = get_normals(sample_cloud)
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 = sample_cloud
detected_objects.append(do)
rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))
# Publish the list of detected objects
detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# TODO: Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# TODO: Voxel Grid Downsampling
vox = cloud.make_voxel_grid_filter()
LEAF_SIZE = 0.01
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()
# 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)
# TODO: Extract inliers and outliers
inliers, coefficients = seg.segment()
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) # <type 'pcl._pcl.PointCloud'>
tree = white_cloud.make_kdtree() # <type 'pcl._pcl.KdTree'>
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.05)
ec.set_MinClusterSize(50)
ec.set_MaxClusterSize(1000)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract() # list of list
print len(cluster_indices) # how many clusters
# 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):
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)
# TODO: Convert PCL data to ROS messages
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
ros_cloud_table = pcl_to_ros(extracted_inliers)
# TODO: Publish ROS messages
pcl_objects_pub.publish(ros_cluster_cloud)
pcl_table_pub.publish(ros_cloud_table)
# Exercise-3 TODOs:
# Classify the clusters!
detected_objects_labels = []
detected_objects = []
# Classify the clusters! (loop through each detected cluster one at a time)
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = extracted_outliers.extract(pts_list) # <type 'pcl._pcl.PointCloud_PointXYZRGB'>
# 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=False)
normals = get_normals(ros_cluster)
nhists = compute_normal_histograms(normals)
feature = np.concatenate((chists, nhists))
#labeled_features.append([feature, model_name])
# 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)
def pcl_callback(pcl_msg):
print("pcl Callback invoked")
cloud = ros_to_pcl(pcl_msg)
cloud_filtered = vox_downsample(cloud)
# PassThrough filter
passthrough_filtered = passthrough_filter(cloud_filtered)
#Filter noise
outlier_filtered = filter_outliers(passthrough_filtered)
# RANSAC plane segmentation
cloud_objects, cloud_table = ransac_filter(outlier_filtered)
#ros_cloud_objects = pcl_to_ros(cloud_objects)
#pcl_objects_pub.publish(ros_cloud_objects)
cluster_indices = find_cluster_indices(cloud_objects)
print("Cluster indices count = {0}".format(len(cluster_indices)))
# 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):
print("Index: {0}, pts_list size: {1}".format(index, len(pts_list)))
# Grab the points for the cluster
pcl_cluster = cloud_objects.extract(pts_list)
ros_pcl_cluster = pcl_to_ros(pcl_cluster)
# Compute the associated feature vector
chists = compute_color_histograms(ros_pcl_cluster, using_hsv=True)
normals = get_normals(ros_pcl_cluster)
nhists = compute_normal_histograms(normals)
feature = np.concatenate((chists, nhists))
# Make the prediction
print("Making prediction")
prediction = clf.predict(scaler.transform(feature.reshape(1, -1)))
label = encoder.inverse_transform(prediction)[0]
detected_objects_labels.append(label)
label_pos = list(cloud_objects[pts_list[0]])
label_pos[2] += .4
# Publish a label into RViz
print("Publising markers")
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_pcl_cluster
detected_objects.append(do)
# Publish the list of detected objects
detected_objects_pub.publish(detected_objects)
ros_cloud_objects = pcl_to_ros(cloud_objects)
pcl_objects_pub.publish(ros_cloud_objects)
# 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, cloud_table)
except rospy.ROSInterruptException:
pass
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# TODO: Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# TODO: Voxel Grid Downsampling
vox = cloud.make_voxel_grid_filter()
LEAF_SIZE = 0.01
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_filtered = vox.filter()
# TODO: PassThrough Filter
passthrough = cloud_filtered.make_passthrough_filter()
# Assign axis and range to the passthrough filter object.
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()
# TODO: RANSAC Plane Segmentation
seg = cloud_filtered.make_segmenter()
# Set the model you wish to fit
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
# Max distance for a point to be considered fitting the model
# Experiment with different values for max_distance
# for segmenting the table
max_distance = 0.01
seg.set_distance_threshold(max_distance)
# TODO: Extract inliers and outliers
# Call the segment function to obtain set of inlier indices and model coefficients
inliers, coefficients = seg.segment()
ros_cloud_table = cloud_filtered.extract(inliers, negative=False)
ros_cloud_objects = cloud_filtered.extract(inliers, negative=True)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(ros_cloud_objects)
tree = white_cloud.make_kdtree()
# Create a cluster extraction object
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold
# as well as minimum and maximum cluster size (in points)
# NOTE: These are poor choices of clustering parameters
# Your task is to experiment and find values that work for segmenting objects.
ec.set_ClusterTolerance(0.05)
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(2000)
# 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
#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)
detected_objects_labels = []
detected_objects = []
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = ros_cloud_objects.extract(pts_list)
# 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=False)
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)
# TODO: Convert PCL data to ROS messages
ros_cloud_table = pcl_to_ros(ros_cloud_table)
ros_cloud_objects = pcl_to_ros(ros_cloud_objects)
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)
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# TODO: Convert ROS msg to PCL data
pcl_data = ros_to_pcl(pcl_msg)
# TODO: Voxel Grid Downsampling
# Create a VoxelGrid filter object for our input point cloud
vox = pcl_data.make_voxel_grid_filter()
# Choose a voxel (also known as leaf) size
# Note: this (1) is a poor choice of leaf size
# Experiment and find the appropriate size!
LEAF_SIZE = 0.01
# Set the voxel (or leaf) size
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
# Call the filter function to obtain the resultant downsampled point cloud
cloud_filtered = vox.filter()
# filename = 'voxel_downsampled.pcd'
# pcl.save(cloud_filtered, filename)
# TODO: PassThrough Filter
# Create a PassThrough filter object.
passthrough = cloud_filtered.make_passthrough_filter()
# Assign axis and range to the passthrough filter object in z axis.
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()
# Assign axis and range to the passthrough filter object in y axis.
passthrough = cloud_filtered.make_passthrough_filter()
filter_axis = 'y'
passthrough.set_filter_field_name(filter_axis)
axis_min = -0.45
axis_max = 0.45
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()
# filename = 'pass_through_filtered.pcd'
# pcl.save(cloud_filtered, filename)
# TODO: RANSAC Plane Segmentation
# Create the segmentation object
seg = cloud_filtered.make_segmenter()
# Set the model you wish to fit
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
# Max distance for a point to be considered fitting the model
# Experiment with different values for max_distance
# for segmenting the table
max_distance = 0.01
seg.set_distance_threshold(max_distance)
# Call the segment function to obtain set of inlier indices and model coefficients
inliers, coefficients = seg.segment()
# TODO: Extract inliers and outliers
cloud_table = cloud_filtered.extract(inliers, negative=False)
# filename = 'extracted_inliers.pcd'
# pcl.save(extracted_inliers, filename)
cloud_objects = cloud_filtered.extract(inliers, negative=True)
# filename = 'extracted_outliers.pcd'
# pcl.save(extracted_outliers, filename)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(
cloud_objects) # Apply function to convert XYZRGB to XYZ
tree = white_cloud.make_kdtree()
# Create a cluster extraction object
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold
# as well as minimum and maximum cluster size (in points)
# NOTE: These are poor choices of clustering parameters
# Your task is to experiment and find values that work for segmenting objects.
ec.set_ClusterTolerance(0.02)
ec.set_MinClusterSize(20)
ec.set_MaxClusterSize(1500)
# 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
# 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)
# TODO: 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)
# 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)
# 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):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = cloud_objects.extract(pts_list)
# TODO: convert the cluster from pcl to ROS using helper function
ros_pcl_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_pcl_cluster, using_hsv=True)
normals = get_normals(ros_pcl_cluster)
nhists = compute_normal_histograms(normals)
feature = np.concatenate((chists, nhists))
# labeled_features.append([feature, model_name])
# 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_pcl_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)
# 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 pcl_callback(pcl_msg):
cloud = ros_to_pcl(pcl_msg)
vox = cloud.make_voxel_grid_filter()
# Choose a voxel (also known as leaf) size
# Note: this (1) is a poor choice of leaf size
# Experiment and find the appropriate size!
LEAF_SIZE = 0.005
# Set the voxel (or leaf) size
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
# Call the filter function to obtain the resultant downsampled point cloud
cloud_filtered = vox.filter()
# PassThrough filter
# Create a PassThrough filter object.
passthroughZ = cloud_filtered.make_passthrough_filter()
passthroughZ.set_filter_field_name('z')
passthroughZ.set_filter_limits(0.61, 1.1)
cloud_filtered = passthroughZ.filter()
passthroughY = cloud_filtered.make_passthrough_filter()
passthroughY.set_filter_field_name('y')
passthroughY.set_filter_limits(-0.5, 0.5)
cloud_filtered = passthroughY.filter()
outlier_filter = cloud_filtered.make_statistical_outlier_filter()
# Set the number of neighboring points to analyze for any given point
outlier_filter.set_mean_k(50)
# Set threshold scale factor
x = 0.4
# 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)
# Finally call the filter function for magic
cloud_filtered = outlier_filter.filter()
# RANSAC plane segmentation
seg = cloud_filtered.make_segmenter()
# Set the model you wish to fit
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
# Max distance for a point to be considered fitting the model
# Experiment with different values for max_distance
# for segmenting the table
max_distance = 0.01
seg.set_distance_threshold(max_distance)
# Call the segment function to obtain set of inlier indices and model coefficients
inliers, coefficients = seg.segment()
# Extract inliers
cloud_table = cloud_filtered.extract(inliers, negative=False)
# Extract outliers
cloud_objects = cloud_filtered.extract(inliers, negative=True)
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_table = pcl_to_ros(cloud_table)
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
# Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(
cloud_objects) # Apply function to convert XYZRGB to XYZ
tree = white_cloud.make_kdtree()
# Create a cluster extraction object
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold
# as well as minimum and maximum cluster size (in points)
# NOTE: These are poor choices of clustering parameters
# Your task is to experiment and find values that work for segmenting objects.
ec.set_ClusterTolerance(0.03)
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(5000)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
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)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
pcl_cluster_pub.publish(ros_cluster_cloud)
# Exercise-3 TODOs:
detected_objects_labels = []
detected_objects = []
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = cloud_objects.extract(pts_list)
ros_cluster = pcl_to_ros(pcl_cluster)
# Extract histogram features
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
detected_objects_pub.publish(detected_objects)
# 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 pcl_callback(pcl_msg):
# Exercise-2 TODOs:
#### Convert ROS msg to PCL data
#
cloud = ros_to_pcl(pcl_msg)
#### Voxel Grid Downsampling
#
vox = cloud.make_voxel_grid_filter()
# Set the voxel (or leaf) size
LEAF_SIZE = 0.01
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
# call the filter function to obtain the resultant downsampled point cloud
cloud_filtered = vox.filter()
#### PassThrough Filter
#
# Create a PassThrough filter object.
passthrough = cloud_filtered.make_passthrough_filter()
# Assign axis and range to the passthrough filter object.
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.76
axis_max = 1.1
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()
#### RANSAC Plane Segmentation
#
# Create the segmentation object
seg = cloud_filtered.make_segmenter()
# Set the model you with to fit
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
# Max distance for a point to be considered fitting the model
# Experiment with different values for max_distance
# for segmenting the table
max_distance = 0.01
seg.set_distance_threshold(max_distance)
inliers, coefficients = seg.segment()
#### Extract inliers and outliers
#
cloud_table = cloud_filtered.extract(inliers, negative=False)
cloud_objects = cloud_filtered.extract(inliers, negative=True)
#### Euclidean Clustering
#
white_cloud = XYZRGB_to_XYZ(cloud_objects) # Apply function to convert XYZRGB to XYZ
tree = white_cloud.make_kdtree()
# Create a cluster extraction object
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold
# as well as minimum and maximum cluster size (in points)
# NOTE: These are poor choices of clustering parameters
# Your task is to experiment and find values that work for segmenting objects.
ec.set_ClusterTolerance(0.02)
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(5000)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
#### Create Cluster-Mask Point Cloud to visualize each cluster separately
#
# Assign a color correspoding 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_cloud_cluster = 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_cloud_cluster)
# 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):
#### Grab the points for the cluster
pcl_cluster_i = cloud_objects.extract(pts_list)
#### Compute the associated feature vector
#
ros_cluster_i = pcl_to_ros(pcl_cluster_i)
#
# Extract histogram features
chists = compute_color_histograms(ros_cluster_i, using_hsv=True)
normals = get_normals(ros_cluster_i)
nhists = compute_normal_histograms(normals)
feature = np.concatenate((chists, nhists))
#### Make the prediction
#
# Make the prediction, retrieve the label for the result
# and add it to detected_objects_label 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] += 0.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_i
detected_objects.append(do)
rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))
# Publish the list of detected objects
detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
### Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
### Voxel Grid Downsampling
# Create a VoxelGrid filter object for our input point cloud
vox = cloud.make_voxel_grid_filter()
# Choose a voxel (also known as leaf) size
# Note: this (1) is a poor choice of leaf size
# Experiment and find the appropriate size!
LEAF_SIZE = 0.003
# Set the voxel (or leaf) size
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
# Call the filter function to obtain the resultant downsampled point cloud
cloud_filtered = vox.filter()
### Statistical Outlier Removal Filter
# Much like the previous filters, we start by creating a filter object:
outlier_filter = cloud_filtered.make_statistical_outlier_filter()
# Set the number of neighboring points to analyze for any given point
outlier_filter.set_mean_k(50)
# Set threshold scale factor
x = 1.0
# 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)
# Finally call the filter function for magic
cloud_filtered = outlier_filter.filter()
### PassThrough Filter
# Create a PassThrough filter object first across the Z axis
passThroughZ = cloud_filtered.make_passthrough_filter()
# Assign axis and range to the passthrough filter object.
filter_axis = 'z'
passThroughZ.set_filter_field_name(filter_axis)
axis_min = 0.6
axis_max = 1.1
passThroughZ.set_filter_limits(axis_min, axis_max)
# Finally use the filter function to obtain the resultant point cloud.
cloud_filtered = passThroughZ.filter()
## Now, Create a PassThrough filter object across the Y axis
passThroughY = cloud_filtered.make_passthrough_filter()
# Assign axis and range to the passthrough filter object.
filter_axis = 'y'
passThroughY.set_filter_field_name(filter_axis)
axis_min = -0.5
axis_max = 0.5
passThroughY.set_filter_limits(axis_min, axis_max)
# Finally use the filter function to obtain the resultant point cloud.
cloud_filtered = passThroughY.filter()
### RANSAC Plane Segmentation
# Create the segmentation object
seg = cloud_filtered.make_segmenter()
# Set the model you wish to fit
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
# Max distance for a point to be considered fitting the model
# Experiment with different values for max_distance
# for segmenting the table
max_distance = 0.01
seg.set_distance_threshold(max_distance)
# Call the segment function to obtain set of inlier indices and model coefficients
inliers, coefficients = seg.segment()
### Extract inliers and outliers
extracted_inliers = cloud_filtered.extract(inliers, negative=False)
extracted_outliers = cloud_filtered.extract(inliers, negative=True)
### Euclidean Clustering
# Go from XYZRGB to RGB since to build the k-d tree we only needs spatial data
white_cloud = XYZRGB_to_XYZ(extracted_outliers)
# Apply function to convert XYZRGB to XYZ
tree = white_cloud.make_kdtree()
### Create a cluster extraction object
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold
# as well as minimum and maximum cluster size (in points)
# Your task is to experiment and find values that work for segmenting objects.
ec.set_ClusterTolerance(0.01)
ec.set_MinClusterSize(100)
# Refering to the minimum and maximum number of points that make up an object's cluster
ec.set_MaxClusterSize(50000)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
### Create Cluster-Mask Point Cloud to visualize each cluster separately
#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)
# TODO - set proper names ### 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 set names ### Publish ROS messages
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cluster_cloud)
# 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):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = extracted_outliers.extract(pts_list)
ros_cluster = pcl_to_ros(pcl_cluster)
# Extract histogram features
# 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))
# labeled_features.append([feature, model_name])
# 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)
print "Trying to match the pick list with the objects detected..."
print "\n"
# Publish the list of detected objects
rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))
# This is the output you'll need to complete the upcoming project!
detected_objects_pub.publish(detected_objects)
object_list_param = rospy.get_param('/object_list')
pick_list_objects = []
for i in range(len(object_list_param)):
pick_list_objects.append(object_list_param[i]['name'])
print "\n"
print "Pick List includes: "
print pick_list_objects
print "\n"
pick_set_objects = set(pick_list_objects)
detected_set_objects = set(detected_objects_labels)
if detected_set_objects == pick_set_objects:
try:
pr2_mover(detected_objects)
except rospy.ROSInterruptException:
pass
