def detect_objects(cloud_objects, cluster_indices):
# TODO: Classify the clusters!
detected_objects_labels = []
detected_objects = []
labeled_features = []
for index, pts_list in enumerate(cluster_indices):
# TODO: Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = cloud_objects.extract(pts_list)
# TODO: Convert the cluster from pcl to ROS using helper function
ros_cluster = pclh.pcl_to_ros(pcl_cluster)
# TODO: Extract histogram features
hists_color = ft.compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
hists_normals = ft.compute_normal_histograms(normals)
feature = np.concatenate((hists_color, hists_normals))
#labeled_features.append([feature, model_name])
# TODO: Make the prediction
prediction = clf.predict(scaler.transform(feature.reshape(1, -1)))
# TODO: Retrieve the label for the result
label = encoder.inverse_transform(prediction)[0]
# TODO: Add it to detected_objects_labels list
detected_objects_labels.append(label)
# TODO: Publish a label into RViz
white_cloud = pclh.XYZRGB_to_XYZ(cloud_objects)
label_pos = list(white_cloud[pts_list[0]])
label_pos[2] += .2
object_markers_pub.publish(make_label(label, label_pos, index))
# TODO: Add the detected object to the list of detected objects.
do = DetectedObject()
do.label = label
do.cloud = ros_cluster
detected_objects.append(do)
rospy.loginfo('Detected {} objects: {}'.format(
len(detected_objects_labels), detected_objects_labels))
return detected_objects, detected_objects_labels
labeled_features = []
for model_name in models:
spawn_model(model_name)
for i in range(20):
# make five attempts to get a valid a point cloud then give up
sample_was_good = False
try_count = 0
while not sample_was_good and try_count < 5:
sample_cloud = capture_sample()
sample_cloud_arr = ros_to_pcl(sample_cloud).to_array()
# Check for invalid clouds.
if sample_cloud_arr.shape[0] == 0:
print('Invalid cloud detected')
try_count += 1
else:
sample_was_good = True
# 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))
labeled_features.append([feature, model_name])
delete_model()
pickle.dump(labeled_features, open('training_set.sav', 'wb'))
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# TODO: Convert ROS msg to PCL data
pcl_data = ros_to_pcl(pcl_msg)
# TODO: Statistical Outlier Filtering
# creating a filter object
outlier_filter = pcl_data.make_statistical_outlier_filter()
#Set the number of neighboring points to analyze for any given point
outlier_filter.set_mean_k(20)
#Set threshold scale factor
x = 0.3
#Any point with a mean distance larger than global (mean distance+x*std_dev) will be considered outlier
outlier_filter.set_std_dev_mul_thresh(x)
#then call the filter for it to work on the cloud_filtered
outlier_filtered_cloud = outlier_filter.filter()
# TODO: Voxel Grid Downsampling
vox = outlier_filtered_cloud.make_voxel_grid_filter()
LEAF_SIZE = 0.005
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_vox = vox.filter()
# TODO: PassThrough Filter
# PassThrough filter in the z-axis
passthrough_z = cloud_vox.make_passthrough_filter()
filter_axis_z = 'z'
passthrough_z.set_filter_field_name(filter_axis_z)
passthrough_z.set_filter_limits(0.6, 1.3)
cloud_passthrough = passthrough_z.filter()
# PassThrough Filter in the y axis
passthrough_y = cloud_passthrough.make_passthrough_filter()
filter_axis_y = 'y'
passthrough_y.set_filter_field_name(filter_axis_y)
passthrough_y.set_filter_limits(-0.5, 0.5)
cloud_passthrough = passthrough_y.filter()
# TODO: RANSAC Plane Segmentation
max_distance = 0.01
seg = cloud_passthrough.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()
# TODO: Extract inliers and outliers
cloud_table = cloud_passthrough.extract(inliers, negative=False) #table
cloud_objects = cloud_passthrough.extract(inliers,
negative=True) #objects on table
# 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
ec = white_cloud.make_EuclideanClusterExtraction()
# Set tolerances for distance threshold
# as well as minimum and maximum cluster size (in points)
ec.set_ClusterTolerance(0.015)
ec.set_MinClusterSize(20)
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)
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
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
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):
first_exec = not os.path.isfile("downsampled.pcd")
# TODO: Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# Much like the previous filters, we start by creating a filter object:
outlier_filter = cloud.make_statistical_outlier_filter()
# Set the number of neighboring points to analyze for any given point
outlier_filter.set_mean_k(100)
# Set threshold scale factor - orig 1.0
x = 0.5
# 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 = outlier_filter.filter()
# TODO: Voxel Grid Downsampling
LEAF_SIZE = 0.005
#vox = cloud_filtered.make_voxel_grid_filter()
vox = cloud.make_voxel_grid_filter()
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()
if first_exec:
pcl.save(cloud_filtered, "downsampled.pcd")
# 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 = 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()
if first_exec:
pcl.save(cloud_filtered, "passthrough.pcd")
passthrough_cloud = 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.01
seg.set_distance_threshold(max_distance)
# TODO: Extract inliers and outliers
inliers, coefficients = seg.segment()
cloud_table = cloud_filtered.extract(inliers, negative=False)
if first_exec:
pcl.save(cloud_table, "inliers.pcd")
cloud_objects = cloud_filtered.extract(inliers, negative=True)
if first_exec:
pcl.save(cloud_objects, "outliers.pcd")
# TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
tree = white_cloud.make_kdtree()
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.015)
ec.set_MinClusterSize(50)
ec.set_MaxClusterSize(2500)
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_passthrough_cloud = pcl_to_ros(passthrough_cloud)
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_passthrough_pub.publish(ros_passthrough_cloud)
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_list = []
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 in capture_features.py
chists = compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
nhists = compute_normal_histograms(normals)
#print "%s" % nhists
feature = np.concatenate((chists, nhists))
#detected_objects_labels.append([feature, model_name])
# Make the prediction, retrieve the label for the result
# and add it to detected_objects_labels list
input_features = scaler.transform(feature.reshape(1,-1))
prediction = clf.predict(input_features)
#prediction = clf.predict(scaler.transform(feature.reshape(1,-1)))
print "prediction: %s" % str(prediction)
probabilities = clf.predict_proba(input_features)
print "probability predictions: %s" % str(probabilities)
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_list.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_list)
# Suggested location for where to invoke your pr2_m.ver() 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)
pass
except rospy.ROSInterruptException:
pass
def pcl_callback(pcl_msg):
# Convert ROS msg to PCL data
pcl_data = ros_to_pcl(pcl_msg)
noisy_data_pub.publish(pcl_msg)
# Statistical Outlier Filtering
# Much like the previous filters, we start by creating a filter object:
outlier_filter = pcl_data.make_statistical_outlier_filter()
# Set the number of neighboring points to analyze for any given point
outlier_filter.set_mean_k(25)
# Set threshold scale factor
x = .1
# 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()
filename = 'outlier_filter_output.pcd'
pcl.save(cloud_filtered, filename)
ros_filter_cloud = pcl_to_ros(cloud_filtered)
filtered_data_pub.publish(ros_filter_cloud)
# Voxel Grid Downsampling
vox = cloud_filtered.make_voxel_grid_filter()
LEAF_SIZE = 0.005
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_filtered = vox.filter()
filename = 'voxel_downsampled.pcd'
pcl.save(cloud_filtered, filename)
# PassThrough Filter along Z direction
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)
# 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)
# PassThrough Filter along Y direction to remove table edges
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.5
axis_max = 0.5
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)
ros_passthrough_cloud = pcl_to_ros(cloud_filtered)
passthrough_data_pub.publish(ros_passthrough_cloud)
# RANSAC Plane Segmentation to separate table and its objects
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
cloud_table = cloud_filtered.extract(inliers, negative=False)
filename = 'extracted_inliers.pcd'
pcl.save(cloud_table, filename)
cloud_objects = cloud_filtered.extract(inliers, negative=True)
filename = 'extracted_outliers.pcd'
pcl.save(cloud_objects, filename)
# Publish these objects
ros_msg_objects = pcl_to_ros(cloud_objects)
objects_pub.publish(ros_msg_objects)
# Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(
cloud_objects) # Apply function to convert XYZRGB to XYZ
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.05)
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()
# Assign a color corresponding to each segmented object in scene for Rviz visualization
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)
pcl_cluster_pub.publish(ros_cluster_cloud)
# Classify the clusters! (loop through each detected cluster one at a time)
detected_objects_labels = []
detected_objects = []
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = cloud_objects.extract(pts_list)
# 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
#print(feature.reshape(1, -1).shape)
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)
# 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:
# 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
for model_name in models:
spawn_model(model_name)
for i in range(100):
# make five attempts to get a valid point cloud then give up
sample_was_good = False
try_count = 0
while not sample_was_good and try_count < 5:
sample_cloud = capture_sample()
sample_cloud_arr = ros_to_pcl(sample_cloud).to_array()
# Check for invalid clouds.
if sample_cloud_arr.shape[0] == 0:
print('Invalid cloud detected')
try_count += 1
else:
sample_was_good = True
# Extract histogram features
color_hists = ft.compute_color_histograms(sample_cloud, using_hsv=True)
normals = get_normals(sample_cloud)
normal_hists = ft.compute_normal_histograms(normals)
feature = np.concatenate((color_hists, normal_hists))
labeled_features.append([feature, model_name])
delete_model()
pickle.dump(labeled_features, open('training_set.sav', 'wb'))