def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# Convert ROS msg to PCL data
pcl_data = ros_to_pcl(pcl_msg)
# Steps:
# 1. Downsample your point cloud by applying the Voxel Grid Filter
# 2. Apply a Pass Through Filter to isolate the table and objects
# 3. Perform RANSAC plane fitting to identify the table
# 4. Use the ExtractIndices Filter to create new point clouds containg
# the table and objects separately (called cloud_table and cloud_objects)
# Voxel Grid Downsampling
LEAF_SIZE = 0.01
vox = pcl_data.make_voxel_grid_filter()
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_filtered = vox.filter()
# PassThrough Filter
filter_axis = 'z'
axis_min = 0.759
axis_max = 1.1
passthrough = cloud_filtered.make_passthrough_filter()
passthrough.set_filter_field_name(filter_axis)
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()
# RANSAC Plane Segmentation
max_distance = 0.01
ransac = cloud_filtered.make_segmenter()
ransac.set_model_type(pcl.SACMODEL_PLANE)
ransac.set_method_type(pcl.SAC_RANSAC)
ransac.set_distance_threshold(max_distance)
# Extract inliers and outliers
inliers, coefficients = ransac.segment()
cloud_table = cloud_filtered.extract(inliers, negative=False)
cloud_objects = cloud_filtered.extract(inliers, negative=True)
# Euclidean Clustering
# white_cloud would be a spatial information array data type
white_cloud = XYZRGB_to_XYZ(cloud_objects)
kd_tree = white_cloud.make_kdtree()
# Create Cluster-Mask Point Cloud to visualize each cluster separately
# Cluster extraction
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(200)
ec.set_MaxClusterSize(2000)
# Search the k-d tree for clusters
ec.set_SearchMethod(kd_tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
# print cluster_indices
# Visualization of Euclidean Segmentation step
# 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, 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])
])
# 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:
# Array containers to get the pint cloud data and the labels
detected_objects_labels = []
detected_objects = []
# Classify the clusters! (loop through each detected cluster one at a time)
# cycle through each segmented clusters
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)
sample_cloud = ros_cluster
sample_cloud_arr = ros_to_pcl(sample_cloud).to_array()
# Compute the associated feature vector
# Extract histogram features
# complete this step just as is covered in capture_features.py
# Check for invalid clouds.
# TODO: note used
if sample_cloud_arr.shape[0] == 0:
print('Invalid cloud detected')
try_count += 1
else:
sample_was_good = True
# Using hsv instead of RGB
chists = compute_color_histograms(sample_cloud, using_hsv=True)
normals = get_normals(sample_cloud)
nhists = compute_normal_histograms(normals)
feature = np.concatenate((chists, nhists))
# labeled_features.append([feature, model_name]) # Not needed, we're not saving the labels as training or model data
# Make the prediction
# retrieve the label for the result and add
# it to the 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] += 0.4 # TODO: What is this?
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)
# Do some logging
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:
# TODO: Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
cloud = statistical_outlier_filtering(cloud, 10, 0.001)
# TODO: Voxel Grid Downsampling
LEAF_SIZE = 0.01
cloud = voxel_grid_downssampling(cloud, LEAF_SIZE)
# TODO: PassThrough Filter
filter_axis = 'z'
axis_min = 0.50
axis_max = 0.90
cloud = passthrough(cloud, filter_axis, axis_min, axis_max)
filter_axis = 'x'
axis_min = 0.30
axis_max = 1.0
cloud = passthrough(cloud, filter_axis, axis_min, axis_max)
# TODO: RANSAC Plane Segmentation
ransac_segmentation = ransac_plane_segmentation(cloud, pcl.SACMODEL_PLANE,
pcl.SAC_RANSAC, 0.01)
# TODO: Extract inliers and outliers
cloud_table, cloud_objects = extract_cloud_objects_and_cloud_table(
cloud, ransac_segmentation)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
cluster_cloud, cluster_indices = euclidean_clustering(white_cloud)
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
# TODO: Convert PCL data to ROS messages
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_table = pcl_to_ros(cloud_table)
# 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 = []
# write a for loop to cycle through each of the segmented clusters
for index, 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)
# Compute the associated feature vector
# 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
# 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:
# 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:
#display raw input
pcl_raw.publish(pcl_msg)
# TODO: Convert ROS msg to PCL data
pcl_data = ros_to_pcl(pcl_msg)
# TODO: Statistical Outlier Filtering
outlier_filter = pcl_data.make_statistical_outlier_filter()
#trial and error values
mean_k = 25
thresh_scale = 0.001
outlier_filter.set_mean_k(mean_k)
outlier_filter.set_std_dev_mul_thresh(thresh_scale)
pcl_o_f = outlier_filter.filter()
# TODO: Voxel Grid Downsampling
vox = pcl_o_f.make_voxel_grid_filter()
LEAF_SIZE = 0.005 #value from testing in class lab
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
pcl_vox = vox.filter()
# TODO: PassThrough Filter
#X axis passthrough
passthrough_x = pcl_vox.make_passthrough_filter()
#all max_min values determined trial and error, should have used measure function in Rviz/Gazebo
x_min = 0.33
x_max = 0.88
passthrough_x.set_filter_field_name('x')
passthrough_x.set_filter_limits(x_min, x_max)
passthrough = passthrough_x.filter()
#Y axis passthrough
passthrough_y = passthrough.make_passthrough_filter()
y_min = -0.46
y_max = 0.46
passthrough_y.set_filter_field_name('y')
passthrough_y.set_filter_limits(y_min, y_max)
passthrough = passthrough_y.filter()
#Z axis passthrough
passthrough_z = passthrough.make_passthrough_filter()
z_min = 0.605
z_max = 0.9
passthrough_z.set_filter_field_name('z')
passthrough_z.set_filter_limits(z_min, z_max)
passthrough = passthrough_z.filter()
# TODO: RANSAC Plane Segmentation
seg = passthrough.make_segmenter()
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
#value determined through trial and error
max_distance_table = 0.008
seg.set_distance_threshold(max_distance_table)
inliers, outliers = seg.segment()
# TODO: Extract inliers and outliers
cloud_objects = passthrough.extract(inliers, negative=True)
cloud_table = passthrough.extract(inliers, negative=False)
# TODO: Euclidean Clustering
#color information is not required
euclidean_objects = XYZRGB_to_XYZ(cloud_objects)
#initialize KDtree
tree = euclidean_objects.make_kdtree()
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
ec = euclidean_objects.make_EuclideanClusterExtraction()
#clustering values determined through trial and error
ec.set_ClusterTolerance(0.01)
ec.set_MinClusterSize(25)
ec.set_MaxClusterSize(5000)
#set euclidian cluster search method
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
#variable for coloring the clusters
cluster_color = get_color_list(len(cluster_indices))
color_cluster_point_list = []
#assign color to each cluster
for j, indices in enumerate(cluster_indices):
for i, indice in enumerate(indices):
color_cluster_point_list.append([
euclidean_objects[indice][0], euclidean_objects[indice][1],
euclidean_objects[indice][2],
rgb_to_float(cluster_color[j])
])
#Convert cluster back to
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
# TODO: Convert PCL data to ROS messages
#use pcl_to_ros() function
ros_filtered = pcl_to_ros(pcl_o_f)
ros_vox = pcl_to_ros(pcl_vox)
ros_passthrough = pcl_to_ros(passthrough)
ros_objects = pcl_to_ros(cloud_objects)
ros_table = pcl_to_ros(cloud_table)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
stat_filter.publish(ros_filtered)
pub_vox.publish(ros_vox)
pub_pass.publish(ros_passthrough)
pub_objects.publish(ros_objects)
pub_table.publish(ros_table)
pub_cloud.publish(ros_cluster_cloud)
# Exercise-3 TODOs:
#intialize detected objects variables.
detected_objects_labels = []
detected_objects = []
#Looping through the cluster list:
for index, pts_list in enumerate(cluster_indices):
pcl_cluster = cloud_objects.extract(pts_list)
pcl_to_ros_cluster = pcl_to_ros(pcl_cluster)
#Construct the color histogram uses HSV instead of RGB
chists = compute_color_histograms(pcl_to_ros_cluster, using_hsv=True)
#collect the surface normals
normals = get_normals(pcl_to_ros_cluster)
nhists = compute_normal_histograms(normals)
#concatenate the HSV and surface normals
feature = np.concatenate((chists, nhists))
#make the prediction based on the combined HSV and surface normal information
prediction = clf.predict(scaler.transform(feature.reshape(1, -1)))
#match the label
label = encoder.inverse_transform(prediction)[0]
detected_objects_labels.append(label)
label_pos = list(euclidean_objects[pts_list[0]])
label_pos[2] += .4
object_markers_pub.publish(make_label(label, label_pos, index))
#fill detected objects and label variables
det_ob = DetectedObject()
det_ob.label = label
det_ob.cloud = pcl_to_ros_cluster
detected_objects.append(det_ob)
rospy.loginfo('Detected {} objects: {}'.format(
len(detected_objects_labels), detected_objects_labels))
#publish detected objects
detected_objects_pub.publish(detected_objects)
try:
pr2_mover(detected_objects)
except rospy.ROSInterruptException:
pass
def pcl_callback(pcl_msg):
# Exercise-2:
# Convert ROS msg to PCL data
pcl_data = ros_to_pcl(pcl_msg)
# Voxel Grid Downsampling
vox = pcl_data.make_voxel_grid_filter()
LEAF_SIZE = 0.005
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_filtered = vox.filter()
# PassThrough Filter in z axis
passthrough = cloud_filtered.make_passthrough_filter()
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_filtered = passthrough.filter()
# PassThrough Filter in y axis
passthrough_2 = cloud_filtered.make_passthrough_filter()
filter_axis_2 = 'y'
passthrough_2.set_filter_field_name(filter_axis_2)
axis_min_2 = -0.425
axis_max_2 = 0.425
passthrough_2.set_filter_limits(axis_min_2, axis_max_2)
cloud_filtered = passthrough_2.filter()
# RANSAC Plane Segmentation
max_distance = 0.005
seg = cloud_filtered.make_segmenter()
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
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 Cluster-Mask Point Cloud to visualize each cluster separately
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold
# as well as minimum and maximum cluster size (in points)
ec.set_ClusterTolerance(0.010)
ec.set_MinClusterSize(250)
ec.set_MaxClusterSize(3550)
# 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)
# 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)
# 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:
# 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 = cloud_out.extract(pts_list)
pcl_cluster = extracted_outliers.extract(pts_list)
# convert the cluster from pcl to ROS using helper function
sample_cloud = pcl_to_ros(pcl_cluster)
# Extract histogram features
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
# This is the output you'll need to complete the upcoming project!
detected_objects_pub.publish(detected_objects)
# Invoking the 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
pcl_unfiltered = ros_to_pcl(pcl_msg)
# TODO: Voxel Grid Downsampling
vox = pcl_unfiltered.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)
outlier_filter = cloud_filtered.make_statistical_outlier_filter()
outlier_filter.set_mean_k(50)
x = 1.0
outlier_filter.set_std_dev_mul_thresh(x)
cloud_filtered = outlier_filter.filter()
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_filtered)
tree = white_cloud.make_kdtree()
# Create a cluster extraction object
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold
ec.set_ClusterTolerance(0.001)
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(250)
# 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])])
# TODO: Convert PCL data to ROS messages
ros_cloud_object = pcl_to_array(extracted_outliers)
ros_table_object = pcl_to_array(extracted_inliers)
# Convert cluster cloud to pcl2 for ROS
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
pcl_objects_pub.publish(ros_cloud_object)
pcl_table_pub.publish(ros_table_object)
ros_cluster_cloud_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 = []
object_list_param = rospy.get_param('/object_list')
# Grab the points for the cluster
for index, pts_lists in enumerate(cluster_indices)
pcl_cluster = cloud_objects.extract(pts_lists)
pcl_cluster = pcl_to_ros(pcl_cluster)
# Extract Histogram Features
chists = compute_color_histograms(pcl_cluster, using_hsv=False)
normals = get_normals(pcl_cluster)
nhists = compute_normal_histograms(normals)
feature = np.concatenate((chists, nhists))
detected_objects_labels.append([feature, model_name])
# Compute the associated feature vector
# Make the prediction
prediction = clf.predict(scalar.transform(feature.reshape(1,-1)))
label = encoder.inverse_transform(prediction)[0]
detected_objects.append(label)
# Publish a label into RViz
label_pos = list(white_cloud[pts_list[0]])
label_pos[2] += .4
object_markers_pub.publish(make_label(label, label_pos, index))
# Add the detected object to the list of detected objects.
do = DetectedObject()
do.label = label
do.cloud = ros_cluster_cloud
detected_objects.append(do)
lables = []
centroids = []
for object in objects:
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
dict_list = []
for i in range(0, len(object_list_param)):
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose)
dict_list.append(yaml_dict)
yaml_filename = "output_yaml.txt"
send_to_yaml(yaml_filename, dict_list)
# Publish the list of detected objects
rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))
detected_objects_pub.publish(detected_objects)
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
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)
cloud_filtered = outlier_filter.filter()
### PassThrough Filter
# Create a PassThrough filter object first across the Z axis
passThroughZ = cloud_filtered.make_passthrough_filter()
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)
cloud_filtered = passThroughZ.filter()
## Now, Create a PassThrough filter object across the Y axis
passThroughY = cloud_filtered.make_passthrough_filter()
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)
cloud_filtered = passThroughY.filter()
### RANSAC Plane Segmentation
# Create the segmentation object
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()
### Extract inliers and outliers
pcl_table_cloud = cloud_filtered.extract(inliers, negative=False)
pcl_object_cloud = 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(pcl_object_cloud)
# Apply function to convert XYZRGB to XYZ
tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.005)
ec.set_MinClusterSize(150)
ec.set_MaxClusterSize(50000)
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 = []
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_cluster_cloud = pcl_to_ros(cluster_cloud)
ros_object_cloud = pcl_to_ros(pcl_object_cloud)
ros_table_cloud = pcl_to_ros(pcl_table_cloud)
#Publish ROS messages
pcl_objects_pub.publish(ros_object_cloud)
pcl_table_pub.publish(ros_table_cloud)
pcl_cluster_pub.publish(ros_cluster_cloud)
det_obj_label = []
det_obj = []
# Grab the points for the cluster
for index, pts_list in enumerate(cluster_indices):
pcl_cluster = pcl_object_cloud.extract(pts_list)
ros_cluster = pcl_to_ros(pcl_cluster)
# Compute associated feature vector
color_hist = compute_color_histograms(ros_cluster, using_hsv=True)
norms = get_normals(ros_cluster)
norm_hist = compute_normal_histograms(norms)
# Make prediction
features = np.concatenate((color_hist, norm_hist))
predict = clf.predict(scaler.transform(features.reshape(1, -1)))
label = encoder.inverse_transform(predict)[0]
det_obj_label.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))
do = DetectedObject()
do.label = label
do.cloud = ros_cluster
det_obj.append(do)
rospy.loginfo('Detected {} objects: {}'.format(len(det_obj_label),
det_obj_label))
# Publish the list of detected objects
detected_objects_pub.publish(det_obj)
# Suggested location for where to invoke your pr2_mover() function within pcl_callback()
true_object_list = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
true_list = []
for i in range(len(true_object_list)):
true_list.append(true_object_list[i]['name'])
print "\n"
print "Need to find: "
print true_list
print "\n"
trueset = set(true_list)
detset = set(det_obj_label)
# Could add some logic to determine whether or not your object detections are robust
# before calling pr2_mover()
if detset == trueset:
try:
pr2_mover(det_obj)
except rospy.ROSInterruptException:
pass
else:
rospy.loginfo("Wrong object(s) detected")
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
##############################################################################
# TODO: Convert ROS msg to PCL data
##############################################################################
cloud = ros_to_pcl(pcl_msg)
##############################################################################
# TODO: Statistical Outlier Filtering
##############################################################################
outlier_filter = cloud.make_statistical_outlier_filter()
outlier_filter.set_mean_k(2)
outlier_filter.set_std_dev_mul_thresh(0.5)
cloud_filtered = outlier_filter.filter()
##############################################################################
# TODO: Voxel Grid Downsampling
##############################################################################
# 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
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()
##############################################################################
# TODO: PassThrough Filter
##############################################################################
# Create a PassThrough filter object.
passthrough_z = cloud_filtered.make_passthrough_filter()
# ZAXIS Assign axis and range to the passthrough filter object.
filter_axis = 'z'
passthrough_z.set_filter_field_name(filter_axis)
axis_min = 0.60
axis_max = 1.8
passthrough_z.set_filter_limits(axis_min, axis_max)
# Use the filter function to obtain the filtered z-axis.
cloud_filtered = passthrough_z.filter()
# Create PassThrough filter object
passthrough_y = cloud_filtered.make_passthrough_filter()
# YAXIS Assign axis and range to the passthrough filter object
filter_axis = 'y'
passthrough_y.set_filter_field_name(filter_axis)
axis_min = -0.5
axis_max = 0.5
passthrough_y.set_filter_limits(axis_min, axis_max)
# Use the filter function to obtain the filtered y-axis
cloud_filtered = passthrough_y.filter()
##############################################################################
# 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
##############################################################################
# Extract inliers
cloud_table = cloud_filtered.extract(inliers, negative=False)
# Extract outliers
cloud_objects = cloud_filtered.extract(inliers, negative=True)
##############################################################################
# TODO: Euclidean Clustering
##############################################################################
white_cloud = XYZRGB_to_XYZ(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(50)
ec.set_MaxClusterSize(3000)
# 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_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_table_pub.publish(ros_cloud_table)
pcl_objects_pub.publish(ros_cloud_objects)
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 = []
# Grab the points for the cluster
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 functions
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)
# Compute the associated feature vector
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
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_list)
except rospy.ROSInterruptException:
pass
def pcl_callback(pcl_msg):
## Convert ROS msg to PCL data
pcl_data = ros_to_pcl(pcl_msg)
## Voxel Grid Downsampling
voxel_grid_filter = pcl_data.make_voxel_grid_filter()
voxel_grid_filter.set_leaf_size(0.01, 0.01, 0.01)
pcl_data = voxel_grid_filter.filter()
# PassThrough Filter Z
passthrough_filter = pcl_data.make_passthrough_filter()
filter_axis = 'z'
passthrough_filter.set_filter_field_name(filter_axis)
axis_min = 0.6
axis_max = 1.1
passthrough_filter.set_filter_limits(axis_min, axis_max)
pcl_data = passthrough_filter.filter()
# PassThrough Filter X
passthrough_filter = pcl_data.make_passthrough_filter()
filter_axis = 'y'
passthrough_filter.set_filter_field_name(filter_axis)
axis_min = -5.0
axis_max = -1.4
passthrough_filter.set_filter_limits(axis_min, axis_max)
pcl_data = passthrough_filter.filter()
# RANSAC Plane Segmentation
segmenter = pcl_data.make_segmenter()
segmenter.set_model_type(pcl.SACMODEL_PLANE)
segmenter.set_method_type(pcl.SAC_RANSAC)
segmenter.set_distance_threshold(0.01)
inliers, coefficients = segmenter.segment()
# Extract inliers and outliers
pcl_data_objects = pcl_data.extract(inliers, negative=True)
pcl_data_table = pcl_data.extract(inliers, negative=False)
# Euclidean Clustering
pcl_data_objects_xyz = XYZRGB_to_XYZ(pcl_data_objects)
tree = pcl_data_objects_xyz.make_kdtree()
EuclideanClusterExtraction = pcl_data_objects_xyz.make_EuclideanClusterExtraction(
)
EuclideanClusterExtraction.set_ClusterTolerance(0.05)
EuclideanClusterExtraction.set_MinClusterSize(100)
EuclideanClusterExtraction.set_MaxClusterSize(2000)
# Search the k-d tree for clusters
EuclideanClusterExtraction.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = EuclideanClusterExtraction.Extract()
# 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([
pcl_data_objects_xyz[indice][0],
pcl_data_objects_xyz[indice][1],
pcl_data_objects_xyz[indice][2],
rgb_to_float(cluster_color[j])
])
pcl_data_objects_clustered = pcl.PointCloud_PointXYZRGB()
pcl_data_objects_clustered.from_list(color_cluster_point_list)
# Convert PCL data to ROS messages
ros_objects_msg = pcl_to_ros(pcl_data_objects_clustered)
ros_table_msg = pcl_to_ros(pcl_data_table)
# Publish ROS messages
pcl_objects_pub.publish(ros_objects_msg)
pcl_table_pub.publish(ros_table_msg)
detected_objects = []
detected_objects_labels = []
# 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_data_single_object_clustered = pcl_data_objects.extract(pts_list)
ros_data_single_object_clustered = pcl_to_ros(
pcl_data_single_object_clustered)
# Compute the associated feature vector
chists = compute_color_histograms(ros_data_single_object_clustered,
using_hsv=True)
normals = get_normals(ros_data_single_object_clustered)
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(pcl_data_objects_xyz[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_data_single_object_clustered
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)
def pcl_callback(pcl_msg):
# Convert ROS msg to PCL data
PCL_data = ros_to_pcl(pcl_msg)
# Voxel Grid Downsampling filter
# 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) means 1mx1mx1m 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()
# 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.77
axis_max = 1.1
passthrough.set_filter_limits(axis_min, axis_max)
# Finally 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.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
# pcd for table
cloud_table = cloud_filtered.extract(inliers, negative=False)
# Extract outliers
# pcd for tabletop objects
cloud_objects = cloud_filtered.extract(inliers, negative=True)
# Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(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.02)
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(30000)
# 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
################################
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# Publish ROS messages
################################
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cluster_cloud)
# Exercise-3:
# Classify the clusters!
################################
detected_objects_labels = []
detected_objects = []
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster
pcl_cluster = cloud_objects.extract(pts_list)
ros_cluster = pcl_to_ros(pcl_cluster)
# Compute the associated feature vector
# Extract histogram features
# Generate Color hist
c_hists = compute_color_histograms(ros_cluster, using_hsv=True)
# Generate normals and notmal histograms
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
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.
det_obj = DetectedObject()
det_obj.label = label
det_obj.cloud = ros_cluster
detected_objects.append(det_obj)
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 = ros_to_pcl(pcl_msg)
# 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()
#filename = 'vox_downsampled.pcd'
#pcl.save(cloud_filtered, filename)
# PassThrough filter
passthrough = cloud_filtered.make_passthrough_filter()
filter_axis = 'z'
axis_min = 0.6
axis_max = 1.1
passthrough.set_filter_field_name(filter_axis)
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = 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.01
seg.set_distance_threshold(max_distance)
inliers, coefficients = seg.segment()
# TODO: Extract inliers and outliers
pcl_data_table = cloud.extract(inliers, negative=False)
pcl_data_objects = cloud.extract(inliers, negative=True)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(pcl_data_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.015)
ec.set_MinClusterSize(120)
ec.set_MaxClusterSize(2000)
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])
])
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
# TODO: Convert PCL data to ROS messages
ros_cloud_cluster = pcl_to_ros(cluster_cloud)
ros_cloud_objects = pcl_to_ros(pcl_data_objects)
ros_cloud_table = pcl_to_ros(pcl_data_table)
# TODO: Publish ROS messages
pcl_cluster_pub.publish(ros_cloud_cluster)
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
# Exercise-3 TODOs:
detected_objects_labels = []
detected_objects = []
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster
pcl_cluster = cloud_objects.extract(pts_list)
pcl_cluster = pcl_to_ros(pcl_cluster)
# Compute the associated feature vector
chists = compute_color_histograms(pcl_cluster, using_hsv=False)
normals = get_normals(pcl_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)
# Publish the list of detected objects
detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg):
global map_ready
global mapping_stage
global start_time
# Exercise-2 TODOs:
# TODO: Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# TODO: Statistical Outlier Filtering
outlier_filter = cloud.make_statistical_outlier_filter()
x = 0.005
outlier_filter.set_std_dev_mul_thresh(x)
cloud = outlier_filter.filter()
# TODO: Voxel Grid Downsampling
LEAF_SIZE = 0.01
vox = cloud.make_voxel_grid_filter()
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
obs_cloud = cloud_filtered = vox.filter()
# TODO: PassThrough Filter
# passthrough filter of obs
passthrough = obs_cloud.make_passthrough_filter()
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.6
axis_max = 0.62
passthrough.set_filter_limits(axis_min, axis_max)
obs_cloud = passthrough.filter()
obvoidence_pub.publish(pcl_to_ros(obs_cloud))
if map_ready:
passthrough = cloud_filtered.make_passthrough_filter()
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.62
axis_max = 1.1
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()
passthrough = cloud_filtered.make_passthrough_filter()
filter_axis = 'y'
passthrough.set_filter_field_name(filter_axis)
axis_min = -0.5
axis_max = 0.5
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)
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) # bigger makes more point as a p
ec.set_MinClusterSize(0)
ec.set_MaxClusterSize(1000)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
miss_list = []
for i in range(len(cluster_indices)):
if len(cluster_indices[i]) < 30:
miss_list.append(i)
# 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)
# print(cluster_indices)
# TODO: Convert PCL data to ROS messages
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
# Exercise-3 TODOs:
# Classify the clusters! (loop through each detected cluster one at a time)
detected_objects_labels = []
detected_objects = []
# needed to be alternate when change the world
for index, pts_list in enumerate(cluster_indices):
if index in miss_list:
continue
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = extracted_outliers.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)
cloud_puber.publish(pcl_to_ros(extracted_outliers))
# cloud_puber.publish(ros_cluster_cloud)
print('point cloud published')
detected_objects_list = detected_objects
try:
pr2_mover(detected_objects_list)
except rospy.ROSInterruptException:
pass
# rotate PR2 to capture the collision map
if not map_ready and mapping_stage == 0:
start_time = rospy.get_time()
pr2_world_joint_pub.publish(angle1)
mapping_stage += 1
if not map_ready and mapping_stage == 1:
if rospy.get_time() - start_time >= 15:
mapping_stage += 1
pr2_world_joint_pub.publish(angle2)
if not map_ready and mapping_stage == 2:
if rospy.get_time() - start_time >= 45:
mapping_stage += 1
pr2_world_joint_pub.publish(0)
if not map_ready and mapping_stage == 3:
if rospy.get_time() - start_time >= 65:
mapping_stage += 1
map_ready = True
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# Convert ROS msg to PCL data
msg_to_pcl = ros_to_pcl(pcl_msg)
# Statistical Outlier Filtering
# We start by creating a filter object:
outlier_filter = msg_to_pcl.make_statistical_outlier_filter()
# Set the number of neighboring points to analyze for any given point
outlier_filter.set_mean_k(5)
# Set threshold scale factor
x = 0.00000001
# 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
msg_to_pcl = outlier_filter.filter()
# Voxel Grid Downsampling
vox = msg_to_pcl.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
passthroughZ = cloud_filtered.make_passthrough_filter()
filter_axis = 'z'
passthroughZ.set_filter_field_name(filter_axis)
axis_min = 0.6
axis_max = 1.1
# axis_min = 0.1
# axis_max = 0.7
passthroughZ.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthroughZ.filter()
# Passthrough filter X to remove appearance of bin edges
passthroughX = cloud_filtered.make_passthrough_filter()
filter_axis = 'x'
passthroughX.set_filter_field_name(filter_axis)
axis_min = 0.4
axis_max = 3.5
#axis_min = 0.3
#axis_max = 3.6
passthroughX.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthroughX.filter()
# RANSAC Plane Segmentation
# Max distance for a point to be considered fitting the model
# Experiment with different values for max_distance
# for segmenting the table
seg = cloud_filtered.make_segmenter()
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
max_distance = 0.008
seg.set_distance_threshold(max_distance)
# Extract inliers and outliers
inliers, coefficients = seg.segment()
extracted_inliers = cloud_filtered.extract(inliers, negative=False)
extracted_outliers = cloud_filtered.extract(inliers, negative=True)
cloud_table = extracted_inliers
cloud_objects = extracted_outliers
# Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
tree = white_cloud.make_kdtree()
# 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)
# Your task is to experiment and find values that work for segmenting objects.
ec.set_ClusterTolerance(0.0500)
ec.set_MinClusterSize(50)
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 a Cluster Visualization using PointCloud_PointXYZRGB
# 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)
#Convert PCL data to ROS messages
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# Publish ROS messages
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cluster_cloud)
# 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 = 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)#ros_pcl_cluster, using_hsv=False)
normals = get_normals(ros_cluster)#ros_pcl_cluster) as changed above
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. Uses DetectedObject.msg, a
# two line file that defines the msg fields
do = DetectedObject()
#Populate the msg fields
do.label = label
#Populate with cluster array
do.cloud = ros_cluster
#this is the detected objects list that we later compare the pick list to, outputs
# format e.g. ['biscuits', 'soap'. 'soap2']
detected_objects.append(do)
#print('test = ', do.label)
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:
# 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):
pcl_msg = ros_to_pcl(pcl_msg)
# Voxel grid downsampling
vox = pcl_msg.make_voxel_grid_filter()
LEAF_SIZE = 0.005
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_filtered = vox.filter()
# PassThrough Filter
passthrough = cloud_filtered.make_passthrough_filter()
filter_axis = 'z'
passthrough.set_filter_field_name (filter_axis)
axis_min = 0.75
axis_max = 1.1
passthrough.set_filter_limits (axis_min, axis_max)
cloud_filtered = passthrough.filter()
passthrough = cloud_filtered.make_passthrough_filter()
filter_axis = 'y'
passthrough.set_filter_field_name (filter_axis)
axis_min = -3
axis_max = -1.35
passthrough.set_filter_limits (axis_min, axis_max)
cloud_filtered = 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.01
seg.set_distance_threshold(max_distance)
# Extract inliers and outliers
inliers, coefficients = seg.segment()
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.01)
ec.set_MinClusterSize(600)
ec.set_MaxClusterSize(3000)
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 = []
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])])
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
# Convert PCL data to ROS messages
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# Classify the clusters! (loop through each detected cluster one at a time)
detected_objects_labels = []
detected_objects = []
# Grab the points for the cluster
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster
cloud_objects_cluster = cloud_objects.extract(pts_list)
ros_cluster = pcl_to_ros(cloud_objects_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)
# Publish the list of detected objects
rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))
# Publish ROS messages
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cluster_cloud)
detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg):
# Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# Since the raw Point Cloud contains unnecessary data making publishing intensive, its a good idea to trim it first.
# PassThrough Filters
passthrough = cloud.make_passthrough_filter()
passthrough.set_filter_field_name('y')
passthrough.set_filter_limits(-0.5, 0.5)
cloud_passthrough = passthrough.filter()
passthrough = cloud_passthrough.make_passthrough_filter()
passthrough.set_filter_field_name('z')
passthrough.set_filter_limits(0.6, 1.2)
cloud_passthrough = passthrough.filter()
# Statistical Outlier Filtering
stats = cloud_passthrough.make_statistical_outlier_filter()
stats.set_mean_k(1)
stats.set_std_dev_mul_thresh(1.0)
cloud_stats = stats.filter()
# Voxel Grid Downsampling
vox = cloud_stats.make_voxel_grid_filter()
LEAF_SIZE = 0.01
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_vox = vox.filter()
# RANSAC Plane Segmentation
seg = cloud_vox.make_segmenter()
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
max_distance = 0.01
seg.set_distance_threshold(max_distance)
inliers, coefficients = seg.segment()
# Extract inliers and outliers
cloud_table = cloud_vox.extract(inliers, negative=False)
cloud_objects = cloud_vox.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(50)
ec.set_MaxClusterSize(2500)
ec.set_SearchMethod(tree)
# cluster_indices contains a list of indices for each cluster (a list of list)
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 = []
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])
])
cloud_cluster = pcl.PointCloud_PointXYZRGB()
cloud_cluster.from_list(color_cluster_point_list)
# Convert PCL data to ROS messages
ros_cloud_passthrough = pcl_to_ros(cloud_passthrough)
ros_cloud_stats = pcl_to_ros(cloud_stats)
ros_cloud_vox = pcl_to_ros(cloud_vox)
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_cluster = pcl_to_ros(cloud_cluster)
# Publish ROS messages
pcl_pub_passthrough.publish(ros_cloud_passthrough)
pcl_pub_stats.publish(ros_cloud_stats)
pcl_pub_vox.publish(ros_cloud_vox)
pcl_pub_table.publish(ros_cloud_table)
pcl_pub_objects.publish(ros_cloud_objects)
pcl_pub_cluster.publish(ros_cloud_cluster)
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
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] += .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:
# 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.75
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()
cloud_objects = cloud_filtered.extract(inliers, negative=True)
table_top = cloud_filtered.extract(inliers, negative=False)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
objects_tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.02)
ec.set_MinClusterSize(190)
ec.set_MaxClusterSize(2000)
# Search the k-d tree for clusters
ec.set_SearchMethod(objects_tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
cluster_color = get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
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
pcl_msg_objects = pcl_to_ros(cloud_objects)
pcl_msg_table = pcl_to_ros(table_top)
pcl_msg_cluster = pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
pcl_objects_pub.publish(pcl_msg_objects)
pcl_table_pub.publish(pcl_msg_table)
pcl_cluster.publish(pcl_msg_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 from the extracted outliers (cloud_objects)
pcl_clus = cloud_objects.extract(pts_list)
# TODO: convert the cluster from pcl to ROS using helper function
ros_cluster = pcl_to_ros(pcl_clus)
# 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):
PCL_Counter = 0
# 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.005
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_filtered = vox.filter()
if PCL_Counter < 3:
filename = 'voxel_downsampled' + str(PCL_Counter) + '.pcd'
pcl.save(cloud_filtered, filename)
# TODO: PassThrough Filter
# Assign axis and range to the passthrough filter object.
passthrough = cloud_filtered.make_passthrough_filter()
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.6
axis_max = 0.85
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()
# Y direction
passthrough_2 = cloud_filtered.make_passthrough_filter()
filter_axis_2 = 'y'
passthrough_2.set_filter_field_name(filter_axis_2)
axis_2_min = -0.35
axis_2_max = 0.30
passthrough_2.set_filter_limits(axis_2_min, axis_2_max)
cloud_filtered = passthrough_2.filter()
# Y direction
passthrough_3 = cloud_filtered.make_passthrough_filter()
filter_axis_3 = 'x'
passthrough_3.set_filter_field_name(filter_axis_3)
axis_3_min = 0.4
axis_3_max = 0.7
passthrough_3.set_filter_limits(axis_3_min, axis_3_max)
cloud_filtered = passthrough_3.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()
# 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.02 #0.01
seg.set_distance_threshold(max_distance)
# TODO: Extract inliers and outliers
inliers, cofficients = seg.segment()
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) # Apply function to convert XYZRGB to XYZ
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()
ec.set_ClusterTolerance(0.05) #0.05
ec.set_MinClusterSize(50) #10
ec.set_MaxClusterSize(3000)
# 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(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)v
detected_objects_labels = []
detected_objects = []
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster
pcl_cluster = cloud_objects.extract(pts_list)
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 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)
normals = get_normals(ros_cloud)
nhists = compute_normal_histograms(normals, nbins=histogram_bins)
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
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_cloud
detected_objects.append(do)
#---------------------------------------
# Publish the list of detected objects
#---------------------------------------
rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))
pcl_detected_objects_pub.publish(detected_objects)
if __name__ == '__main__':
#---------------------------------------
# ROS node initialization
#---------------------------------------
rospy.init_node('recognition')
def detect_objects(self, pcl_msg):
"""
This method takes in the scene in front of the robot, carries out
object detection, and generates the front part of the collision map
"""
# Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# Statistical Outlier Filtering
outlier_filter = cloud.make_statistical_outlier_filter()
outlier_filter.set_mean_k(20)
x = 0.05
outlier_filter.set_std_dev_mul_thresh(x)
cloud = outlier_filter.filter()
# Voxel Grid Downsampling
vox = cloud.make_voxel_grid_filter()
LEAF_SIZE = 0.007
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud = vox.filter()
# 1st PassThrough filter in z-axis
passthrough = cloud.make_passthrough_filter()
filter_axis = 'z'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.59
axis_max = 5.0
passthrough.set_filter_limits(axis_min, axis_max)
cloud = passthrough.filter()
# 2nd PassThrough filter in y-axis
passthrough2 = cloud.make_passthrough_filter()
filter_axis = 'y'
passthrough2.set_filter_field_name(filter_axis)
axis_min = -0.55
axis_max = 0.55
passthrough2.set_filter_limits(axis_min, axis_max)
cloud = passthrough2.filter()
# RANSAC Plane Segmentation
seg = cloud.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 as subset point clouds
self.collision_map = cloud.extract(inliers, negative=False)
extracted_outliers = cloud.extract(inliers, negative=True)
outlier_filter = extracted_outliers.make_statistical_outlier_filter()
outlier_filter.set_mean_k(20)
x = 0.05
outlier_filter.set_std_dev_mul_thresh(x)
extracted_outliers = outlier_filter.filter()
# Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(extracted_outliers)
tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.02)
ec.set_MinClusterSize(30)
ec.set_MaxClusterSize(25000)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
# Classify the clusters! (loop through each detected cluster one at a time)
detected_objects_labels = []
# Grab the indices of extracted_outliers/white_cloud for each cluster
for index, pts_list in enumerate(cluster_indices):
# Extract the points for the current cluster using indices (pts_list)
pcl_cluster = extracted_outliers.extract(pts_list)
# Compute the associated feature vector
ros_cluster = pcl_to_ros(pcl_cluster)
chists = compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
nhists = compute_normal_histograms(normals)
feature = np.concatenate((chists, nhists))
# Make the prediction
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, place above 1st cluster point
label_pos = list(white_cloud[pts_list[0]])
label_pos[2] += 0.4
self.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
self.detected_objects.append(do)
print 'Detected {} objects: {}'.format(len(detected_objects_labels),
detected_objects_labels)
# Publish the list of detected objects
self.detected_objects_pub.publish(self.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
vox = cloud.make_voxel_grid_filter()
LEAF_SIZE = .01
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_filtered = vox.filter()
filename = 'voxel_downsampled.pcd'
pcl.save(cloud_filtered, filename)
# Statistical Outlier Removal 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 = 1.0
# Points with 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()
# PassThrough Filter
passthrough = cloud_filtered.make_passthrough_filter()
# Assign axis and range to the passthrough filter object.
filter_axis = 'x'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.4
axis_max = 3.
passthrough.set_filter_limits(axis_min, axis_max)
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)
# Finally use the filter function to obtain the resultant point cloud.
cloud_filtered = passthrough.filter()
filename = 'pass_through_filtered.pcd'
pcl.save(cloud_filtered, filename)
# 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
max_distance = 0.01
seg.set_distance_threshold(max_distance)
# Segment Function to obtain set of inlier indices/model coeffs
inliers, coefficients = seg.segment()
outliers, coefficients = seg.segment()
# Extract inliers and outliers
extracted_inliers = cloud_filtered.extract(inliers, negative=False)
filename = 'extracted_inliers.pcd'
pcl.save(extracted_inliers, filename)
# Extract outliers
extracted_outliers = cloud_filtered.extract(outliers, negative=True)
filename = 'extracted_outliers.pcd'
pcl.save(extracted_outliers, filename)
cloud_table = extracted_inliers
cloud_objects = extracted_outliers
# Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
tree = white_cloud.make_kdtree()
# 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)
# 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(30)
ec.set_MaxClusterSize(1200)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
# Create a Cluster Visualization using PointCloud_PointXYZRGB
# 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)
# Convert PCL data to ROS messages
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# Publish ROS messages
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cluster_cloud)
# 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 = cloud_objects.extract(pts_list)
pcl_cluster = pcl_to_ros(pcl_cluster)
chists = compute_color_histograms(pcl_cluster, using_hsv=True)
normals = get_normals(pcl_cluster)
nhists = compute_normal_histograms(normals)
feature = np.concatenate((chists, nhists))
detected_objects.append([feature])
# Compute the associated feature vector
# 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_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(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):
cloud = ros_to_pcl(pcl_msg) #Convert ROS msg to PCL data
# Statistical Outlier Filtering
outlier_filter = cloud.make_statistical_outlier_filter()
neighboring_pts_to_analyze = 8
threshold_scale_factor = 0.5
outlier_filter.set_mean_k(neighboring_pts_to_analyze)
outlier_filter.set_std_dev_mul_thresh(threshold_scale_factor)
cloud_filtered = outlier_filter.filter() # resulting filtered point cloud
# Voxel Grid Downsampling
vox = cloud_filtered.make_voxel_grid_filter()
LEAF_SIZE = 0.01 # voxel size
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_filtered = vox.filter() #resulting downsampled point cloud
# PassThrough Filter
passthrough = cloud_filtered.make_passthrough_filter()
# z-axis passthrough filter for table elimination
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() # resulting filtered point cloud
# x-axis passthrough filter for dropboxes elimination
filter_axis = 'x'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.4
axis_max = 1.0
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter() # resulting filtered point cloud
# 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(
) # set of inlier indices and model coefficients
cloud_table = cloud_filtered.extract(inliers,
negative=False) # Extract inliers
ros_cloud_table = pcl_to_ros(
cloud_table) # convert PCL data to ROS messages
cloud_objects = cloud_filtered.extract(inliers,
negative=True) # Extract outliers
ros_cloud_objects = pcl_to_ros(
cloud_objects) # convert PCL data to ROS messages
# Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.02)
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(800)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract(
) # extract indices for each discovered cluster
# Visualize Clusters
cluster_color = get_color_list(
len(cluster_indices)) # assign a color for each segmented object
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])
])
cluster_cloud = pcl.PointCloud_PointXYZRGB() # create new cloud object
cluster_cloud.from_list(
color_cluster_point_list
) # assign containing all clusters, each with unique color
ros_cloud_cluster = pcl_to_ros(
cluster_cloud) # convert PCL data to ROS messages
# Classify Clusters
detected_objects_labels = []
detected_objects_list = []
for index, pts_list in enumerate(cluster_indices):
pcl_cluster = cloud_objects.extract(pts_list)
ros_cluster = pcl_to_ros(
pcl_cluster) # convert PCL data to ROS messages
# 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))
prediction = clf.predict(scaler.transform(feature.reshape(
1, -1))) # Make the prediction using support vector machines model
label = encoder.inverse_transform(prediction)[
0] # get labels from predictions
detected_objects_labels.append(label) #append labels list
# 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 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))
rospy.loginfo('Detected {} objects: {}'.format(
len(detected_objects_labels), detected_objects_labels))
# Publish ROS messages
detected_objects_pub.publish(detected_objects_list)
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cloud_cluster)
try:
pr2_mover(detected_objects_list)
except rospy.ROSInterruptException:
pass
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# TODO: Convert ROS msg to PCL data
pcl_datacloud=ros_to_pcl(pcl_msg)
# TODO: Statistical Outlier Filtering
outlier_filter = pcl_datacloud.make_statistical_outlier_filter() #change 1, cloud_filtered to pcl_datacloud
# 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() #change 1, plc_datacloud to cloud_filtered
# TODO: Voxel Grid Downsampling
vox = cloud_filtered.make_voxel_grid_filter()
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()
# 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()
passthrough = cloud_filtered.make_passthrough_filter() # second filter for passing out drop boxes
# Assign axis and range to the passthrough filter object.
filter_axis = 'x'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.3
axis_max = 1.0
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 = 0.01
seg.set_distance_threshold(max_distance)
inliers, coefficients = seg.segment()
# TODO: Extract inliers and outliers
extracted_inliers = cloud_filtered.extract(inliers, negative=False)
extracted_outliers = cloud_filtered.extract(inliers, negative=True)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(extracted_outliers)
tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction()
# 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.01)
ec.set_MinClusterSize(100)
ec.set_MaxClusterSize(6000)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
cluster_color = get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
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(extracted_outliers)
ros_cloud_table = pcl_to_ros(extracted_inliers)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cluster_cloud)
# 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_indices):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = extracted_outliers.extract(pts_list) ##change 1
# TODO: convert the cluster from pcl to ROS using helper function
rospcl_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(rospcl_cluster, using_hsv=True)
normals = get_normals(rospcl_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 = rospcl_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
# 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
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# TODO: Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# TODO: Statistical Outlier Filtering
outlier_filter = cloud.make_statistical_outlier_filter()
# Set the number of neighboring points to analyze for any given point
outlier_filter.set_mean_k(50)
# Set threshold scale factor
x = 0.001
# 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()
# TODO: Voxel Grid Downsampling
vox = cloud_filtered.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
axis_max = 2
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()
# filter_axis = 'x'
# axis_min = 0.33
# axis_max = 1.0
# cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max)
# 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()
table_cloud = cloud_filtered.extract(inliers, negative=False)
objects_cloud = cloud_filtered.extract(inliers, negative=True)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(objects_cloud)
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()
ec.set_ClusterTolerance(0.020)
ec.set_MinClusterSize(20)
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()
#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_objects_cloud = pcl_to_ros(objects_cloud)
ros_table_cloud = pcl_to_ros(table_cloud)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
pcl_objects_pub.publish(ros_objects_cloud)
pcl_table_pub.publish(ros_table_cloud)
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
pcl_cluster = objects_cloud.extract(pts_list)
# convert the cluster from pcl to ROS using helper function
ros_cluster = pcl_to_ros(pcl_cluster)
# Extract histogram features
hist_features = compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
norm_hist_features = compute_normal_histograms(normals)
feature = np.concatenate((hist_features, norm_hist_features))
# 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)
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:
# TODO: Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
print(cloud)
# TODO: Statistical Outlier Filtering
# we start by creating a filter object:
vox = cloud.make_voxel_grid_filter()
outlier_filter = cloud.make_statistical_outlier_filter()
outlier_filter.set_mean_k(
3
) # Set the number of neighboring points to analyze for any given point
# Any point with a mean distance larger than global (mean distance+1*std_dev) will be considered outlier
outlier_filter.set_std_dev_mul_thresh(0.)
cloud_filtered = outlier_filter.filter(
) # Finally call the filter function for magic
pub_noise.publish(pcl_to_ros(cloud_filtered))
# TODO: Voxel Grid Downsampling
vox = cloud_filtered.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.61
axis_max = 0.9
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()
passthrough = cloud_filtered.make_passthrough_filter()
filter_axis = 'y'
passthrough.set_filter_field_name(filter_axis)
axis_min = -0.5
axis_max = 0.5
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()
pub_passthrough.publish(pcl_to_ros(cloud_filtered))
# 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.005
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)
pub_table.publish(pcl_to_ros(cloud_table))
pub_table_outlier.publish(pcl_to_ros(cloud_objects))
# 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)
ec.set_ClusterTolerance(0.05)
ec.set_MinClusterSize(100)
ec.set_MaxClusterSize(10000)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
cluster_color = get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
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])
])
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
# TODO: Convert PCL data to ROS messages and publish
ros_cloud = pcl_to_ros(cluster_cloud)
points_pub.publish(ros_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)
# convert the cluster from pcl to ROS using helper function
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)
# 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
# Bring camaera image (ROS) to PCL to do some CV methods
pcl_data = ros_to_pcl(pcl_msg)
# TODO: Voxel Grid Downsampling
vox = pcl_data.make_voxel_grid_filter()
LEAF_SIZE = 0.01
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_filtered = vox.filter()
# Statistical filter
stat_filter = cloud_filtered.make_statistical_outlier_filter()
stat_filter.set_mean_k(5)
x = 0.1
stat_filter.set_std_dev_mul_thresh(x)
cloud_stat_filtered = stat_filter.filter()
# TODO: PassThrough Filter
passthrough = cloud_stat_filtered.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_filtered = passthrough.filter()
# Need to apply in Y direction as well since Robot try to guess its arms as book ...
passthrough2 = cloud_filtered.make_passthrough_filter()
filter_axis = 'z'
passthrough2.set_filter_field_name(filter_axis)
axis_min = 0.6
axis_max = 0.8
passthrough2.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough2.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)
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)
# 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()
#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_cluster = pcl_to_ros(cluster_cloud)
cloud_objects = extracted_outliers
cloud_table = extracted_inliers
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_table = pcl_to_ros(cloud_table)
# TODO: Publish ROS messages
pcl_points_pub.publish(pcl_msg)
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cloud_cluster)
# Exercise-3 TODOs:
EX3 = 1
if (EX3 == 1):
# 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
sample_cloud = ros_cluster
chists = compute_color_histograms(sample_cloud, using_hsv=True)
normals = get_normals(sample_cloud)
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] += 0
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)
# Publish the list of 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)
############## ###############
############## FILTERING & RANSAC ###############
############## ###############
#################################################################
############### Statistical Outlier Filtering ###################
outlier_filter = cloud.make_statistical_outlier_filter()
outlier_filter.set_mean_k(1) # Set the number of neighboring points to analyze for any given point
outlier_filter.set_std_dev_mul_thresh(0.3) # Any point with a mean distance larger than global (mean distance+x*std_dev) will be considered outlier
cloud_filtered = outlier_filter.filter()
#################################################################
############### Voxel Grid Downsampling #########################
vox = cloud_filtered.make_voxel_grid_filter()
LEAF_SIZE = 0.01 #voxel size
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) #set voxel (leaf) size.
cloud_filtered = vox.filter()
#################################################################
############## PassThrough Filter ###############################
passthrough_z = cloud_filtered.make_passthrough_filter()
filter_axis = 'z'
passthrough_z.set_filter_field_name(filter_axis)
axis_min = 0.32
axis_max = 0.86
passthrough_z.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough_z.filter()
passthrough_x = cloud_filtered.make_passthrough_filter()
filter_axis = 'x'
passthrough_x.set_filter_field_name(filter_axis)
axis_min = 0.35
axis_max = 0.79
passthrough_x.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough_x.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 #####################
cloud_table = cloud_filtered.extract(inliers, negative=False)
cloud_objects = cloud_filtered.extract(inliers, negative=True)
################# ###################
################# Euclidean Clusting ###################
################# ###################
white_cloud = XYZRGB_to_XYZ(cloud_objects) # Apply function to convert XYZRGB to XYZ
tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction() # Create a cluster extraction object
ec.set_ClusterTolerance(0.026) # Set tolerances for distance threshold
ec.set_MinClusterSize(10) # Minimum cluster size (in points)
ec.set_MaxClusterSize(1000) # Maximum cluster size (in points)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
####################################################################################
############ 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 #############
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
###########################################################
############ Publish ROS messages #########################
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cluster_cloud)
################# Object Recognition ###################
################# & ###################
################# Laveling ###################
# 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
pcl_cluster = cloud_objects.extract(pts_list)
####### Convert the cluster from pcl to ROS
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))
####### 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)
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()
try:
pr2_mover(detected_objects)
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: Statistical Outlier Filtering
outlier_filter = cloud.make_statistical_outlier_filter()
outlier_filter.set_mean_k(7)
outlier_filter.set_std_dev_mul_thresh(-0.6)
cloud_filtered = outlier_filter.filter()
# TODO: Voxel Grid Downsampling
vox = cloud_filtered.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.605
axis_max = 1.0
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()
# TODO: Rotate PR2 in place to capture side tables for the collision map
joint_angle = Float64()
global task_state
global collision_base
collision_map_pub.publish(pcl_to_ros(collision_base))
if task_state == 'start':
joint_angle.data = np.pi / 2
joint_command_pub.publish(joint_angle)
current_joint_angle = get_joint_properties('world_joint').position[0]
if abs(current_joint_angle - np.pi / 2) < 0.0001:
task_state = 'left_sector'
passthrough = cloud_filtered.make_passthrough_filter()
filter_axis = 'y'
passthrough.set_filter_field_name(filter_axis)
axis_min = 0.43
axis_max = 1.0
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()
collision_base = cloud_filtered
collision_map_pub.publish(pcl_to_ros(collision_base))
return
else:
return
elif task_state == 'left_sector':
joint_angle.data = -np.pi / 2
joint_command_pub.publish(joint_angle)
current_joint_angle = get_joint_properties('world_joint').position[0]
if abs(current_joint_angle - -np.pi / 2) < 0.0001:
task_state = 'right_sector'
passthrough = cloud_filtered.make_passthrough_filter()
filter_axis = 'y'
passthrough.set_filter_field_name(filter_axis)
axis_min = -1.0
axis_max = -0.43
passthrough.set_filter_limits(axis_min, axis_max)
cloud_filtered = passthrough.filter()
collision_base.from_list(collision_base.to_list() +
cloud_filtered.to_list())
collision_map_pub.publish(pcl_to_ros(collision_base))
return
else:
return
elif task_state == 'right_sector':
joint_angle.data = 0
joint_command_pub.publish(joint_angle)
current_joint_angle = get_joint_properties('world_joint').position[0]
if abs(current_joint_angle) < 0.0001:
task_state = 'finish'
return
else:
return
else:
pass
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()
# 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.006
seg.set_distance_threshold(max_distance)
plane_inliers, coefficients = seg.segment()
# TODO: Extract inliers and outliers
cloud_table = cloud_filtered.extract(plane_inliers, negative=False)
collision_base.from_list(collision_base.to_list() + cloud_table.to_list())
collision_map_pub.publish(pcl_to_ros(collision_base))
cloud_objects = cloud_filtered.extract(plane_inliers, negative=True)
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.02)
ec.set_MinClusterSize(10)
ec.set_MaxClusterSize(1500)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
cluster_color = get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
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])
])
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
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] += .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
if detected_objects:
pr2_mover(detected_objects)
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# Convert ROS msg to PCL data
pcl_data = ros_to_pcl(pcl_msg)
# Voxel Grid Downsampling
pcl_downsampled = voxel_downsampling(pcl_data)
# PassThrough Filter
pcl_passed = passthrough_filtering(pcl_downsampled)
# RANSAC Plane Segmentation
inliers = plane_fitting(pcl_passed)
# Extract inliers and outliers
# inliers: table
# outliers: objects
cloud_table = extract_inliers(inliers, pcl_passed)
cloud_objects = extract_outliers(inliers, pcl_passed)
# Euclidean Clustering
white_cloud, tree, cluster_indices = clustering(cloud_objects)
# Create Cluster-Mask Point Cloud to visualize each cluster separately
cluster_cloud_colored = color_clusters(white_cloud, cluster_indices)
# Convert PCL data to ROS messages
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cluster_cloud_colored = pcl_to_ros(cluster_cloud_colored)
# Publish ROS messages
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cluster_cloud_colored)
# 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 = 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
chists = compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
nhists = compute_normal_histograms(normals)
# Compute the associated feature vector
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)
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):
# TODO: Convert ROS msg to PCL data
cloud=ros_to_pcl(pcl_msg)
# TODO: Voxel Grid Downsampling
#create a Voxel filter object forourinput point cloud
cloud_filtered_v=voxel_filter(cloud)
# TODO: PassThrough Filter
#Create a PassThrough filter object
#cloud_filtered_pass=passthrough_filter(cloud_filtered_v,'x',0.6,1.1)
cloud_filtered_pass=passthrough_filter(cloud_filtered_v,'y',-0.4,0.47)
cloud_filtered_pass=passthrough_filter(cloud_filtered_v,'z',0.6,0.91)
# TODO: RANSAC Plane Segmentation
extracted_inliers,extracted_outliers=RANSCAN_filter(cloud_filtered_pass)
# TODO: Convert PCL data to ROS msg
ros_cloud_objects = pcl_to_ros(extracted_outliers)
ros_cloud_table = pcl_to_ros(extracted_inliers)
# TODO: Euclidean Clustering
cluster_indices,white_cloud=Euclidean_Clustering(extracted_inliers,extracted_outliers)
##cluster_indices now contains a list of indices for each cluster (a list of lists).
#In the next step, you'll create a new point cloud to visualize the clusters by assigning a color to each of them.
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
#Assign a color corresponding to each segmented object in scene
cluster_cloud=Cluster_Mask_Point(cluster_indices,white_cloud)
# TODO: Convert PCL data to ROS messages
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# TODO: Publish ROS msg
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)
# 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 outliers (cloud_objects)
pcl_cluster = extracted_outliers.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
# Extract histogram features
chists = compute_color_histograms(ros_cluster, using_hsv=True)#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))
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
