def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data # TODO: Voxel Grid Downsampling # TODO: PassThrough Filter # TODO: RANSAC Plane Segmentation # TODO: Extract inliers and outliers # TODO: Euclidean Clustering # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately # TODO: Convert PCL data to ROS messages # TODO: Publish ROS messages # Exercise-3 TODOs: for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = cloud_objects.extract(pts_list) # TODO: convert the cluster from pcl to ROS using helper function # Extract histogram features # TODO: complete this step just as is covered in capture_features.py # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects)
def CB_msgPCL(msgPCL): """ ROS "/sensor_stick/point_cloud" subscription Callback handler Handle the PointCloud ROS msg received by the "/sensor_stick/point_cloud" This function is almost entirely unpacking/packing ROS messages and publishing. The the unpacked input pcl is processed by Process_rawPCL(pclpcRawIn) which returns the values that need to be packed and published :param msgPCL: :return: """ global g_callBackCount g_callBackCount += 1 if (g_callBackCount % g_callBackSkip != 0): return; print "\rCBCount= {:05d}".format(g_callBackCount), sys.stdout.flush() DebugDumpMsgPCL(msgPCL) # Extract pcl Raw from Ros msgPCL pclpcRawIn = pcl_helper.ros_to_pcl(msgPCL) #------- PROCESS RAW PCL------------------------- labelRecs, pclpcObjects, pclpcTable, pclpcClusters = Process_rawPCL(pclpcRawIn) detected_objects_labels = [] # For ros loginfo only detected_objects = [] # For publish - for PROJ3! for (labelText, labelPos, labelIndex) in labelRecs: detected_objects_labels.append(labelText) g_object_markers_pub.publish(marker_tools.make_label(labelText, labelPos, labelIndex )) # Add detected object to the list of detected objects. do = DetectedObject() do.label = labelText do.cloud = pclpcClusters detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) # Package Processed pcls into Ros msgPCL msgPCLObjects = pcl_helper.pcl_to_ros(pclpcObjects) msgPCLTable = pcl_helper.pcl_to_ros(pclpcTable) msgPCLClusters = pcl_helper.pcl_to_ros(pclpcClusters) # Publish everything # This is the output you'll need to complete the upcoming project! g_detected_objects_pub.publish(detected_objects) # THIS IS THE CRUCIAL STEP FOR PROJ3 g_pcl_objects_pub.publish(msgPCLObjects) g_pcl_table_pub.publish(msgPCLTable) g_pcl_cluster_pub.publish(msgPCLClusters)
def detect_objects(self, cloud, cluster_indices): #take in cloud and array of point indices within clusters #return array of detected objects within cloud along with #dict of the object labels and their corresponding centroids # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] #detected_objects_list = {'labels':[], 'centroids':[]} positions = [] centroids = [] for j, indices in enumerate(cluster_indices): # Grab the points for the cluster cluster = cloud.extract(indices) centroid = self.get_centroid(cluster) #set the starting position of the label #based on first point in cluster #offset later during label publishing positions.append(list(cluster[0][:3])) cluster = pcl_to_ros(cluster) # Compute the associated feature vector chists = compute_color_histograms(cluster) #, using_hsv=True) normals = self.get_normals(cluster) nhists = compute_normal_histograms(normals) feature_vector = np.concatenate((chists, nhists)) # Make the prediction prediction = self.clf.predict( self.scaler.transform(feature_vector.reshape(1, -1))) label = self.encoder.inverse_transform(prediction)[0] # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = cluster detected_objects.append(do) detected_objects_labels.append(label) centroids.append(centroid) detected_objects_dict = dict(zip(detected_objects_labels, centroids)) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) return detected_objects, detected_objects_dict
def detect_objects(cloud_objects, cluster_indices): # TODO: Classify the clusters! detected_objects_labels = [] detected_objects = [] labeled_features = [] for index, pts_list in enumerate(cluster_indices): # TODO: Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = cloud_objects.extract(pts_list) # TODO: Convert the cluster from pcl to ROS using helper function ros_cluster = pclh.pcl_to_ros(pcl_cluster) # TODO: Extract histogram features hists_color = ft.compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) hists_normals = ft.compute_normal_histograms(normals) feature = np.concatenate((hists_color, hists_normals)) #labeled_features.append([feature, model_name]) # TODO: Make the prediction prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) # TODO: Retrieve the label for the result label = encoder.inverse_transform(prediction)[0] # TODO: Add it to detected_objects_labels list detected_objects_labels.append(label) # TODO: Publish a label into RViz white_cloud = pclh.XYZRGB_to_XYZ(cloud_objects) label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .2 object_markers_pub.publish(make_label(label, label_pos, index)) # TODO: Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) return detected_objects, detected_objects_labels
def classify(white_cloud, cloud_objects, cluster_indices): # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] for idx, pts_list in enumerate(cluster_indices): # Grab the points for the cluster pcl_cluster = cloud_objects.extract(pts_list) # TODO: convert the cluster from pcl to ROS using helper function ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features # TODO: complete this step just as you did before in capture_features.py chists = compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) if np.isnan(nhists).any(): continue feature = np.concatenate((chists, nhists)) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1,-1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label,label_pos, idx)) # Add the detected object to the list of detected objects. an_object = DetectedObject() an_object.label = label an_object.cloud = ros_cluster detected_objects.append(an_object) rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) return detected_objects
def classification(pcl_msg): objects, cluster_indices, white_cloud = segmentation(pcl_msg) #--------------------------------------- # Classify the clusters! (loop through each detected cluster one at a time) #--------------------------------------- # Store the detected objects and labels in thesevc lists detected_objects_labels = [] detected_objects_list = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = objects.extract(pts_list) # <type 'pcl._pcl.PointCloud_PointXYZRGB'> # TODO: convert the cluster from pcl to ROS using helper function ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features histogram_bins = 128 chists = compute_color_histograms(ros_cluster, nbins=histogram_bins, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals, nbins=histogram_bins) feature = np.concatenate((chists, nhists)) # Make the prediction, retrieve the label for the result and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1,-1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 pcl_object_markers_pub.publish(make_label(label,label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects_list.append(do) # Publish the list of detected objects rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) return detected_objects_list, detected_objects_labels
def detect_objects(cluster_indices, cloud_objects, white_cloud, clf, object_markers_pub, scaler, encoder): # Create some empty lists detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab points for the cluster from the extracted outliers(cloud_objects) pcl_cluster = cloud_objects.extract(pts_list) # Convert pcl_cluster to ros_msg data type ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features chists = compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # labeled_features.append([feature, model_name]) # Make prediction, retrieve the label for the result # then add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish label to RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) return detected_objects
def detect_objects(object_cloud, cluster_indices): """ Classify object with an SVM classifier. Parameters: object_cloud: pcl point cloud of objects to classify cluster_indices: array of cluster indices Returns: detected_objects: Array of objects of type DetectedObject detected_objects_labels: Array of object labels """ detected_objects = [] detected_objects_labels = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = object_cloud.extract(pts_list) ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features features = extract_features(ros_cluster) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(features.reshape(1,-1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(object_cloud[pts_list[0]])[:-1] label_pos[2] += .4 object_markers_pub.publish(make_label(label,label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) return detected_objects, detected_objects_labels
def classifyClusters(point_cloud, white_cloud, cluster_indices): detected_objects_labels = [] detected_objects = [] # Classify each detected cluster for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = point_cloud.extract(pts_list) # convert the cluster from pcl to ROS using helper function ros_cluster = pcl_to_ros(pcl_cluster) # Extract the cluster colour feature histogram chists = compute_color_histograms(ros_cluster, using_hsv=True, nbins=44) normals = get_normals(ros_cluster) # Extract the cluster surface normals histogram nhists = compute_normal_histograms(normals, nbins=20) feature = np.concatenate((chists, nhists)) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # store the label to be published into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) return [detected_objects, detected_objects_labels]
def pcl_callback(pcl_msg): cloud = ros_to_pcl(pcl_msg) cloud = pipeline.passth(cloud) cloud = pipeline.passth(cloud, axis='y', amin=-0.45, amax=0.45) cloud = pipeline.denoise(cloud) filtered = pipeline.voxel(cloud) table_points = pipeline.plane_points(filtered) table_cloud = filtered.extract(table_points, negative=False) obj_cloud = filtered.extract(table_points, negative=True) pcl_objects_pub.publish(pcl_to_ros(obj_cloud)) pcl_table_pub.publish(pcl_to_ros(table_cloud)) cluster_ix = pipeline.cluster_ix(obj_cloud) pcl_cluster_pub.publish( pcl_to_ros(pipeline.color_clusters(obj_cloud, cluster_ix))) detected_objects = [] for i, ixs in enumerate(cluster_ix): pcl_data = obj_cloud.extract(ixs) ros_data = pcl_to_ros(pcl_data) feature = np.concatenate( (features.compute_color_histograms(ros_data), )) prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] label_pos = list(next(pc2.read_points(ros_data))[:3]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, i)) do = DetectedObject() do.label = label do.cloud = ros_data detected_objects.append(do) detected_objects_pub.publish(detected_objects)
def do_object_detection(cluster_indices, cloud_objects, white_cloud): detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = cloud_objects.extract(pts_list) # convert the cluster from pcl to ROS using helper function ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features 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) # Publish the list of detected objects # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects) return detected_objects_labels, detected_objects
def classify_clusters(cluster_indices, white_cloud, extracted_inliers_objects): # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = extracted_inliers_objects.extract(pts_list) # Compute the associated feature vector # convert the cluster from pcl to ROS using helper function ros_pcl_objects = pcl_to_ros(pcl_cluster) # Extract histogram features chists = compute_color_histograms(ros_pcl_objects, using_hsv=True) normals = get_normals(ros_pcl_objects) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_pcl_objects detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) return detected_objects
def recognise_object(sample_cloud): """ Attempts to recognise the given sample cloud as an object. :sample_cloud: The sample cloud we are trying to recognise. :returns: The detected object and label. """ chists = compute_color_histograms(sample_cloud, using_hsv=True) normals = get_normals(sample_cloud) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] do = DetectedObject() do.label = label do.cloud = sample_cloud return do, label
def detect_objects(self, cloud, cluster_indices): # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] positions = [] for j, indices in enumerate(cluster_indices): # Grab the points for the cluster cluster = cloud.extract(indices) positions.append(list(cluster[0][:3])) cluster = pcl_to_ros(cluster) # Compute the associated feature vector chists = compute_color_histograms(cluster, using_hsv=True) normals = self.get_normals(cluster) nhists = compute_normal_histograms(normals) feature_vector = np.concatenate((chists, nhists)) # Make the prediction prediction = self.clf.predict( self.scaler.transform(feature_vector.reshape(1, -1))) label = self.encoder.inverse_transform(prediction)[0] # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = cluster detected_objects.append(do) detected_objects_labels.append(label) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) return detected_objects, detected_objects_labels, positions
def pcl_callback(pcl_msg): # Exercise-2 TODOs: rospy.loginfo('Ex-2') # TODO: Convert ROS msg to PCL data pcl_data = ros_to_pcl(pcl_msg) # XYZRGB #print('size1: ', pcl_data.size) pcl_data.to_file('pcl_data.pcd') pcl_data = outlier_filter(pcl_data) #print('size2: ', pcl_data.size) #cloud_filtered = pcl_data # TODO: Voxel Grid Downsampling vox = pcl_data.make_voxel_grid_filter() # try a voxel size and set to cloud LEAF_SIZE = 0.005 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) # call filter function to obtain the resultant downsampled PC cloud_filtered = vox.filter() # save file ##filename = 'voxel_sampled1.pcd' ##pcl.save(cloud_filtered, filename) #print('size3: ', cloud_filtered.size) # TODO: PassThrough Filter to isolate the table and objects passthrough_z = cloud_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object filter_axis = "z" passthrough_z.set_filter_field_name(filter_axis) axis_min = 0.6 # 0.6, 1.1 axis_max = 0.85 # 1.5 1.1 passthrough_z.set_filter_limits(axis_min, axis_max) #passthrough.set_filter_limits(0, 1.5) # use filter function to obtain only table and objects cloud_filtered = passthrough_z.filter() passthrough_y = cloud_filtered.make_passthrough_filter() passthrough_y.set_filter_field_name("y") axis_min = -0.42 axis_max = 0.42 passthrough_y.set_filter_limits(axis_min, axis_max) # use filter function to obtain only table and objects cloud_filtered = passthrough_y.filter() # save result filename = 'pass_through_filtered1.pcd' pcl.save(cloud_filtered, filename) #print('size4: ', cloud_filtered.size) # TODO: RANSAC Plane Segmentation, will table and objects # seg = cloud_filtered.make_segmenter_normals(ksearch=50) seg = cloud_filtered.make_segmenter() seg.set_optimize_coefficients(True) # TODO: Extract inliers and outliers # set the model we wish to fit # seg.set_model_type(pcl.SACMODEL_NORMAL_PLANE) seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) # seg.set_max_iterations(100) max_distance = 0.018 # 0.01 # set max distance for a point to be considered fitting the model seg.set_distance_threshold(max_distance) # call the segment function to obtain set of inlier indices and model coefficients inliers, coefficients = seg.segment() #print(coefficients) # extracted inliers - table pcl_table = cloud_filtered.extract(inliers, negative=False) # save file table filename = 'pcl_table1.pcd' pcl.save(pcl_table, filename) # extracted outliers - objects pcl_objects = cloud_filtered.extract(inliers, negative=True) # save file objects filename = 'pcl_objects1.pcd' pcl.save(pcl_objects, filename) # Outlier Removal Filter # pcl_objects = outlier_filter(pcl_objects) # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ(pcl_objects) tree = white_cloud.make_kdtree() # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately # create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) ec.set_ClusterTolerance(0.04) # .08 ec.set_MinClusterSize(30) ec.set_MaxClusterSize(100000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() # Assign a color corresponding to each segmented object in scene cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([ white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j]) ]) # Create new cloud containing all clusters, each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # TODO: Convert PCL data to ROS messages ros_cloud_objects = pcl_to_ros(pcl_objects) ros_cloud_table = pcl_to_ros(pcl_table) # now publish ros_cluster_cloud ros_cluster_cloud = pcl_to_ros(cluster_cloud) # TODO: Publish ROS messages pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cluster_cloud) # Exercise-3 TODOs: rospy.loginfo('Ex-3') # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_list = [] detected_labels = [] for index, pts_list in enumerate(cluster_indices): features = [] #print(index, len(cluster_indices)) # Grab the points for the cluster pcl_cluster = pcl_objects.extract(pts_list) pcl_cluster.to_file('pcl_cluster1-{0}.pcd'.format(index)) # Convert the cluster from pcl to ROS using helper function ros_cluster_cloud = pcl_to_ros(pcl_cluster) # Compute the associated feature vector # Extract histogram features chists = compute_color_histograms(ros_cluster_cloud, using_hsv=True) normals = get_normals(ros_cluster_cloud) nhists = compute_normal_histograms(normals) features = np.concatenate((chists, nhists)) ###labeled_features.append([feature, model_name]) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list sc = scaler.transform(features.reshape(1, -1)) prediction = clf.predict(sc) label = encoder.inverse_transform(prediction)[0] detected_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster_cloud detected_objects_list.append(do) # Objects detected rospy.loginfo('Detected {} objects: {}'.format(len(detected_labels), detected_labels)) # Publish the list of detected objects # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects_list) # Suggested location for where to invoke your pr2_mover() function within pcl_callback() # Could add some logic to determine whether or not your object detections are robust # before calling pr2_mover() try: pr2_mover(detected_objects_list) except rospy.ROSInterruptException: pass
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # Convert ROS msg to PCL data cloud_objects = ros_to_pcl(pcl_msg) # Statistical Outlier Filtering outlier_filter = cloud_objects.make_statistical_outlier_filter() outlier_filter.set_mean_k(20) outlier_filter.set_std_dev_mul_thresh(0.1) cloud_objects = outlier_filter.filter() # Voxel Grid Downsampling LEAF_SIZE = 0.002 vox = cloud_objects.make_voxel_grid_filter() vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_objects = vox.filter() # PassThrough Filter axis_min = 0.6 axis_max = 1.0 filter_axis = 'z' passthrough = cloud_objects.make_passthrough_filter() passthrough.set_filter_field_name(filter_axis) passthrough.set_filter_limits(axis_min, axis_max) cloud_objects = passthrough.filter() axis_min = -0.4 axis_max = 0.4 filter_axis = 'y' passthrough2 = cloud_objects.make_passthrough_filter() passthrough2.set_filter_field_name(filter_axis) passthrough2.set_filter_limits(axis_min, axis_max) cloud_objects = passthrough2.filter() # RANSAC Plane Segmentation seg = cloud_objects.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) max_distance = 0.01 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # Extract inliers and outliers cloud_objects = cloud_objects.extract(inliers, negative=True) # Euclidean Clustering white_cloud = XYZRGB_to_XYZ(cloud_objects) tree = white_cloud.make_kdtree() ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.05) ec.set_MinClusterSize(20) ec.set_MaxClusterSize(50000) ec.set_SearchMethod(tree) cluster_indicies = ec.Extract() # Create Cluster-Mask Point Cloud to visualize each cluster separately colors = get_color_list(len(cluster_indicies)) cloud_objects_cluster_list = [] for i, pts_list in enumerate(cluster_indicies): pcl_cluster = cloud_objects.extract(pts_list) color = colors[i] cluster = XYZ_to_XYZRGB(pcl_cluster, color) cloud_objects_cluster_list.extend(cluster) cloud_objects_cluster = pcl.PointCloud_PointXYZRGB() cloud_objects_cluster.from_list(cloud_objects_cluster_list) # Convert PCL data to ROS messages ros_cluster = pcl_to_ros(cloud_objects_cluster) # Publish ROS messages p_cluster_pub.publish(ros_cluster) # Exercise-3 TODOs: detected_objects_labels = [] detected_objects = [] # Classify the clusters! (loop through each detected cluster one at a time) for index, pts_list in enumerate(cluster_indicies): # Grab the points for the cluster pcl_cluster = cloud_objects.extract(pts_list) ros_cluster = pcl_to_ros(pcl_cluster) # Compute the associated feature vector chists = compute_color_histograms(ros_cluster, bins=128, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals, bins=128) feature = np.concatenate((chists, nhists)) # Make the prediction prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) # Publish the list of detected objects rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) detected_objects_pub.publish(detected_objects) # Suggested location for where to invoke your pr2_mover() function within pcl_callback() # Could add some logic to determine whether or not your object detections are robust # before calling pr2_mover() ground_truth = [] object_list_param = rospy.get_param('/object_list') for object_param in object_list_param: ground_truth.append(object_param['name']) ground_truth_set = set(ground_truth) detected_objects_set = set(detected_objects_labels) n1 = len(ground_truth_set) n2 = len(detected_objects_set) # run pr2_mover only when requirments are met: # 100% (3/3) objects in test1.world # 80% (4/5) objects in test2.world # 75% (6/8) objects in test3.world print(ground_truth) print(n1, n2) if (n1 == 3 and n2 == 3) or (n1 == 5 and n2 >= 4) or (n1 == 8 and n2 >= 6): try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass
def pcl_callback(pcl_msg): # TODO: Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) # TODO: Voxel Grid Downsampling vox = cloud.make_voxel_grid_filter() LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # TODO: PassThrough Filter passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.6 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # TODO: RANSAC Plane Segmentation seg = cloud_filtered.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) max_distance = 0.01 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # TODO: Extract inliers and outliers cloud_table = cloud_filtered.extract(inliers, negative=False) cloud_objects = cloud_filtered.extract(inliers, negative=True) # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ(cloud_objects) tree = white_cloud.make_kdtree() # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.05) ec.set_MinClusterSize(100) ec.set_MaxClusterSize(10000) ec.set_SearchMethod(tree) cluster_indices = ec.Extract() #Assign a color corresponding to each segmented object in scene cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j])]) #Create new cloud containing all clusters, each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) ros_cluster_cloud = pcl_to_ros(cluster_cloud) # TODO: Publish ROS messages pcl_cluster_pub.publish(ros_cluster_cloud) # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = cloud_objects.extract(pts_list) # TODO: convert the cluster from pcl to ROS using helper function ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features # TODO: complete this step just as is covered in capture_features.py chists = compute_color_histograms(ros_cluster, using_hsv=False) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1,-1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label,label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg): cloud = ros_to_pcl(pcl_msg) vox = cloud.make_voxel_grid_filter() # Choose a voxel (also known as leaf) size # Note: this (1) is a poor choice of leaf size # Experiment and find the appropriate size! LEAF_SIZE = 0.005 # Set the voxel (or leaf) size vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) # Call the filter function to obtain the resultant downsampled point cloud cloud_filtered = vox.filter() # PassThrough filter # Create a PassThrough filter object. passthroughZ = cloud_filtered.make_passthrough_filter() passthroughZ.set_filter_field_name('z') passthroughZ.set_filter_limits(0.61, 1.1) cloud_filtered = passthroughZ.filter() passthroughY = cloud_filtered.make_passthrough_filter() passthroughY.set_filter_field_name('y') passthroughY.set_filter_limits(-0.5, 0.5) cloud_filtered = passthroughY.filter() outlier_filter = cloud_filtered.make_statistical_outlier_filter() # Set the number of neighboring points to analyze for any given point outlier_filter.set_mean_k(50) # Set threshold scale factor x = 0.4 # Any point with a mean distance larger than global (mean distance+x*std_dev) will be considered outlier outlier_filter.set_std_dev_mul_thresh(x) # Finally call the filter function for magic cloud_filtered = outlier_filter.filter() # RANSAC plane segmentation seg = cloud_filtered.make_segmenter() # Set the model you wish to fit seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) # Max distance for a point to be considered fitting the model # Experiment with different values for max_distance # for segmenting the table max_distance = 0.01 seg.set_distance_threshold(max_distance) # Call the segment function to obtain set of inlier indices and model coefficients inliers, coefficients = seg.segment() # Extract inliers cloud_table = cloud_filtered.extract(inliers, negative=False) # Extract outliers cloud_objects = cloud_filtered.extract(inliers, negative=True) ros_cloud_objects = pcl_to_ros(cloud_objects) ros_cloud_table = pcl_to_ros(cloud_table) pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) # Euclidean Clustering white_cloud = XYZRGB_to_XYZ( cloud_objects) # Apply function to convert XYZRGB to XYZ tree = white_cloud.make_kdtree() # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) # NOTE: These are poor choices of clustering parameters # Your task is to experiment and find values that work for segmenting objects. ec.set_ClusterTolerance(0.03) ec.set_MinClusterSize(10) ec.set_MaxClusterSize(5000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([ white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j]) ]) # Create new cloud containing all clusters, each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) ros_cluster_cloud = pcl_to_ros(cluster_cloud) pcl_cluster_pub.publish(ros_cluster_cloud) # Exercise-3 TODOs: detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = cloud_objects.extract(pts_list) ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features chists = compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects detected_objects_pub.publish(detected_objects) # Suggested location for where to invoke your pr2_mover() function within pcl_callback() # Could add some logic to determine whether or not your object detections are robust # before calling pr2_mover() try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass
def pcl_callback(ros_pcl_msg): #---------------------------------------------------------------------------------- # Convert a ROS PointCloud2 message to a pcl PointXYZRGB #---------------------------------------------------------------------------------- cloud_filtered = ros_to_pcl(ros_pcl_msg) #---------------------------------------------------------------------------------- # Statistical Outlier Filtering #---------------------------------------------------------------------------------- # Create a statistical filter object: outlier_filter = cloud_filtered.make_statistical_outlier_filter() # Set the number of neighboring points to analyze for any given point outlier_filter.set_mean_k(3) # Set threshold scale factor x = 0.00001 # Any point with a mean distance larger than global (mean distance+x*std_dev) # will be considered outlier outlier_filter.set_std_dev_mul_thresh(x) # Call the filter function cloud_filtered = outlier_filter.filter() #---------------------------------------------------------------------------------- # Voxel Grid Downsampling filter #---------------------------------------------------------------------------------- # Create a VoxelGrid filter object for our input point cloud vox = cloud_filtered.make_voxel_grid_filter() # Choose a voxel (also known as leaf) size # 1 means 1mx1mx1m leaf size # Experiment and find the appropriate size! LEAF_SIZE = 0.005 # Set the voxel (or leaf) size vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) # Call the filter function to obtain the resultant downsampled point cloud cloud_filtered = vox.filter() #---------------------------------------------------------------------------------- # PassThrough filter (Z Axis) #---------------------------------------------------------------------------------- # Create a PassThrough filter object. passthrough = cloud_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object. filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.6095 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) # Use the filter function to obtain the resultant point cloud. cloud_filtered = passthrough.filter() #---------------------------------------------------------------------------------- # PassThrough filter (Y Axis) #---------------------------------------------------------------------------------- # Create a PassThrough filter object. passthrough = cloud_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object. filter_axis = 'y' passthrough.set_filter_field_name(filter_axis) axis_min = -0.456 axis_max = 0.456 passthrough.set_filter_limits(axis_min, axis_max) # Use the filter function to obtain the resultant point cloud. cloud_filtered = passthrough.filter() #---------------------------------------------------------------------------------- # RANSAC plane segmentation #---------------------------------------------------------------------------------- # Create the segmentation object seg = cloud_filtered.make_segmenter() # Set the model you wish to fit seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) # Max distance for a point to be considered fitting the model # Experiment with different values for max_distance # for segmenting the table max_distance = 0.006 seg.set_distance_threshold(max_distance) # Call the segment function to obtain set of inlier indices and model coefficients inliers, coefficients = seg.segment() # Extract inliers (Table) extracted_table = cloud_filtered.extract(inliers, negative=False) # Extract outliers (Tabletop Objects) extracted_objects = cloud_filtered.extract(inliers, negative=True) #---------------------------------------------------------------------------------- # Euclidean Clustering #---------------------------------------------------------------------------------- white_cloud = XYZRGB_to_XYZ(extracted_objects) tree = white_cloud.make_kdtree() # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) ec.set_ClusterTolerance(0.03) ec.set_MinClusterSize(10) ec.set_MaxClusterSize(9000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() #---------------------------------------------------------------------------------- # Create Cluster-Mask Point Cloud to visualize each cluster separately #---------------------------------------------------------------------------------- # Assign a color corresponding to each segmented object in scene cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([ white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j]) ]) # Create new cloud containing all clusters, each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) #---------------------------------------------------------------------------------- # Converts a pcl PointXYZRGB to a ROS PointCloud2 message #---------------------------------------------------------------------------------- ros_cloud_objects = pcl_to_ros(extracted_objects) ros_cloud_table = pcl_to_ros(extracted_table) ros_cluster_cloud = pcl_to_ros(cluster_cloud) #---------------------------------------------------------------------------------- # Publish ROS messages #---------------------------------------------------------------------------------- pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cluster_cloud) #---------------------------------------------------------------------------------- # Classify the clusters! #---------------------------------------------------------------------------------- detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): #---------------------------------------------------------------------------------- # Grab the points for the cluster from the extracted_objects #---------------------------------------------------------------------------------- pcl_cluster = extracted_objects.extract(pts_list) # Convert the cluster from pcl to ROS using helper function ros_cluster = pcl_to_ros(pcl_cluster) #---------------------------------------------------------------------------------- # Generate Histograms #---------------------------------------------------------------------------------- # Color Histogram c_hists = compute_color_histograms(ros_cluster, using_hsv=True) # Normals Histogram normals = get_normals(ros_cluster) n_hists = compute_normal_histograms(normals) #---------------------------------------------------------------------------------- # Generate feature by concatenate of color and normals. #---------------------------------------------------------------------------------- feature = np.concatenate((c_hists, n_hists)) #---------------------------------------------------------------------------------- # Make the prediction #---------------------------------------------------------------------------------- # Retrieve the label for the result and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) #---------------------------------------------------------------------------------- # Publish a label into RViz #---------------------------------------------------------------------------------- label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .2 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. #---------------------------------------------------------------------------------- do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) #---------------------------------------------------------------------------------- # Publish the list of detected objects #---------------------------------------------------------------------------------- detected_objects_pub.publish(detected_objects) #---------------------------------------------------------------------------------- # Invoke pr2_mover() function #---------------------------------------------------------------------------------- if len(detected_objects) > 0: try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass else: rospy.logwarn('No detected objects !!!') return
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) # TODO: Voxel Grid Downsampling vox = cloud.make_voxel_grid_filter() LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # TODO: PassThrough Filter passthrough = cloud_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object. filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.6 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # TODO: RANSAC Plane Segmentation seg = cloud_filtered.make_segmenter() # Set the model you wish to fit seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) # Max distance for a point to be considered fitting the model # Experiment with different values for max_distance # for segmenting the table max_distance = 0.01 seg.set_distance_threshold(max_distance) # TODO: Extract inliers and outliers # Call the segment function to obtain set of inlier indices and model coefficients inliers, coefficients = seg.segment() ros_cloud_table = cloud_filtered.extract(inliers, negative=False) ros_cloud_objects = cloud_filtered.extract(inliers, negative=True) # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ(ros_cloud_objects) tree = white_cloud.make_kdtree() # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) # NOTE: These are poor choices of clustering parameters # Your task is to experiment and find values that work for segmenting objects. ec.set_ClusterTolerance(0.05) ec.set_MinClusterSize(10) ec.set_MaxClusterSize(2000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately #Assign a color corresponding to each segmented object in scene cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([ white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j]) ]) #Create new cloud containing all clusters, each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = ros_cloud_objects.extract(pts_list) # TODO: convert the cluster from pcl to ROS using helper function ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features # TODO: complete this step just as is covered in capture_features.py chists = compute_color_histograms(ros_cluster, using_hsv=False) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1,-1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label,label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects) # TODO: Convert PCL data to ROS messages ros_cloud_table = pcl_to_ros(ros_cloud_table) ros_cloud_objects = pcl_to_ros(ros_cloud_objects) ros_cluster_cloud = pcl_to_ros(cluster_cloud) # TODO: Publish ROS messages pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cluster_cloud)
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data pcl_data = ros_to_pcl(pcl_msg) # TODO: Statistical Outlier Filtering cloud_filtered = filter_noise(pcl_data) pcl_noise_filtered_pub.publish(pcl_to_ros(cloud_filtered)) # TODO: Voxel Grid Downsampling cloud_filtered = voxel_grid_downsample(cloud_filtered, leaf_size=0.01) pcl_downsampled_pub.publish(pcl_to_ros(cloud_filtered)) # TODO: PassThrough Filter USE_PASSTHROUGH = True if USE_PASSTHROUGH: # about z-axis cloud_filtered = filter_passthrough(cloud_filtered, 'z', 0.6, 1.1) # about y-axis cloud_filtered = filter_passthrough(cloud_filtered, 'y', -0.42, 0.42) pcl_passthrough_filtered_pub.publish(pcl_to_ros(cloud_filtered)) # TODO: RANSAC Plane Segmentation # TODO: Extract inliers and outliers cloud_table, cloud_objects = ransac_plane(cloud_filtered, max_distance=0.02) pcl_table_pub.publish(pcl_to_ros(cloud_table)) pcl_objects_pub.publish(pcl_to_ros(cloud_objects)) # Filter more planes # for i in range(2): # seg = cloud_objects.make_segmenter() # seg.set_model_type(pcl.SACMODEL_PLANE) # seg.set_method_type(pcl.SAC_RANSAC) # max_distance = 0.02 # seg.set_distance_threshold(max_distance) # inliers, coefficients = seg.segment() # print("inliers: {}".format(len(inliers))) # if len(inliers) < 500: # break # else: # cloud_objects = cloud_objects.extract(inliers, negative=True) pcl_objects_2_pub.publish(pcl_to_ros(cloud_objects)) # TODO: Statistical Outlier Filtering again after getting objects cloud_objects = filter_noise(cloud_objects, 20, 0.9) pcl_noise_filtered_objects_pub.publish(pcl_to_ros(cloud_objects)) # 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(30) ec.set_MaxClusterSize(5000) 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, index in enumerate(indices): color_cluster_point_list.append([ white_cloud[index][0], white_cloud[index][1], white_cloud[index][2], rgb_to_float(cluster_color[j]) ]) cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # TODO: Convert PCL data to ROS messages # TODO: Publish ROS messages pcl_cluster_pub.publish(pcl_to_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 pcl_cluster = cloud_objects.extract(pts_list) ros_cluster = pcl_to_ros(pcl_cluster) # Compute the associated feature vector chists = compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # Make the prediction prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += 0.4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects_list.append(do) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) # Assembles points that make up obstacles obstacles_table = filter_noise(pcl_data) obstacles_table = voxel_grid_downsample(obstacles_table, leaf_size=0.01) obstacles_table = filter_passthrough(obstacles_table, 'z', 0.1, 5) obstacles_table, _ = ransac_plane(obstacles_table, max_distance=0.02) # Assemble table and objects together (tried, but prevented the robot from reaching to objects) # obstacles = pcl.PointCloud_PointXYZRGB() # obstacles_table_array = obstacles_table.to_array() # objects_cloud_array = cloud_objects.to_array() # obstacles.from_array(np.concatenate((obstacles_table_array, objects_cloud_array), axis=0)) obstacles_pub.publish(pcl_to_ros(obstacles_table)) # Publish the list of detected objects detected_objects_pub.publish(detected_objects_list) # Suggested location for where to invoke your pr2_mover() function within pcl_callback() # Could add some logic to determine whether or not your object detections are robust # before calling pr2_mover() global pick_and_place_prepared print('prepared: {}'.format(pick_and_place_prepared)) if pick_and_place_prepared: try: pr2_mover(detected_objects_list) except rospy.ROSInterruptException: pass
def pcl_callback(pcl_msg): # Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) # Outlier Removal Filter outlier_filter = cloud.make_statistical_outlier_filter() outlier_filter.set_mean_k(50) outlier_filter.set_std_dev_mul_thresh(0.5) cloud_filtered = outlier_filter.filter() # Voxel Grid Downsampling vox = cloud_filtered.make_voxel_grid_filter() LEAF_SIZE = 0.005 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # PassThrough Filter z_passthrough = cloud_filtered.make_passthrough_filter() z_passthrough.set_filter_field_name("z") z_passthrough.set_filter_limits(0.6, 0.85) cloud_filtered = z_passthrough.filter() y_passthrough = cloud_filtered.make_passthrough_filter() y_passthrough.set_filter_field_name("y") y_passthrough.set_filter_limits(-0.45, 0.45) cloud_filtered = y_passthrough.filter() # RANSAC Plane Segmentation seg = cloud_filtered.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) max_distance = 0.025 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # Extract inliers and outliers cloud_table = cloud_filtered.extract(inliers, negative=False) cloud_objects = cloud_filtered.extract(inliers, negative=True) # Euclidean Clustering white_cloud = XYZRGB_to_XYZ(cloud_objects) tree = white_cloud.make_kdtree() ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.05) ec.set_MinClusterSize(100) ec.set_MaxClusterSize(100000) ec.set_SearchMethod(tree) cluster_indices = ec.Extract() # Create Cluster-Mask Point Cloud to visualize each cluster separately cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = list() for j, indices in enumerate(cluster_indices): for i, index in enumerate(indices): color_cluster_point_list.append([white_cloud[index][0], white_cloud[index][1], white_cloud[index][2], rgb_to_float(cluster_color[j])]) cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # Convert PCL data to ROS messages ros_cloud = pcl_to_ros(cluster_cloud) # Publish ROS messages pcl_pub.publish(ros_cloud) detected_objects_labels = list() detected_objects_list = list() # Classify the clusters! (loop through each detected cluster one at a time) for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster pcl_cluster = cloud_objects.extract(pts_list) ros_cluster = pcl_to_ros(pcl_cluster) # Compute the associated feature vector chists = compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # Make the prediction prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += 0.4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects_list.append(do) # Publish the list of detected objects rospy.loginfo("Detected {} objects: {}".format(len(detected_objects_labels), detected_objects_labels)) detected_objects_pub.publish(detected_objects_list) # Suggested location for where to invoke your pr2_mover() function within pcl_callback() # Could add some logic to determine whether or not your object detections are robust # before calling pr2_mover() try: pr2_mover(detected_objects_list, cloud_table) except rospy.ROSInterruptException: pass
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data pclMsg = ros_to_pcl(pcl_msg) # TODO: Voxel Grid Downsampling vox = pclMsg.make_voxel_grid_filter() LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # TODO: PassThrough Filter passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.77 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # TODO: RANSAC Plane Segmentation seg = cloud_filtered.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) max_distance = 0.01 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # TODO: Extract inliers and outliers pcl_cloud_table = cloud_filtered.extract(inliers, negative=False) pcl_cloud_objects = cloud_filtered.extract(inliers, negative=True) # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ( pcl_cloud_objects) # Apply function to convert XYZRGB to XYZ tree = white_cloud.make_kdtree() # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) ec.set_ClusterTolerance(0.05) ec.set_MinClusterSize(50) ec.set_MaxClusterSize(2000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() # Exercise-3 TODOs: # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = pcl_cloud_objects.extract(pts_list) # TODO: convert the cluster from pcl to ROS using helper function ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features # TODO: complete this step just as is covered in capture_features.py chists = compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg): print("pcl Callback invoked") cloud = ros_to_pcl(pcl_msg) cloud_filtered = vox_downsample(cloud) # PassThrough filter passthrough_filtered = passthrough_filter(cloud_filtered) #Filter noise outlier_filtered = filter_outliers(passthrough_filtered) # RANSAC plane segmentation cloud_objects, cloud_table = ransac_filter(outlier_filtered) #ros_cloud_objects = pcl_to_ros(cloud_objects) #pcl_objects_pub.publish(ros_cloud_objects) cluster_indices = find_cluster_indices(cloud_objects) print("Cluster indices count = {0}".format(len(cluster_indices))) # Exercise-3 TODOs: # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): print("Index: {0}, pts_list size: {1}".format(index, len(pts_list))) # Grab the points for the cluster pcl_cluster = cloud_objects.extract(pts_list) ros_pcl_cluster = pcl_to_ros(pcl_cluster) # Compute the associated feature vector chists = compute_color_histograms(ros_pcl_cluster, using_hsv=True) normals = get_normals(ros_pcl_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # Make the prediction print("Making prediction") prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) label_pos = list(cloud_objects[pts_list[0]]) label_pos[2] += .4 # Publish a label into RViz print("Publising markers") object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_pcl_cluster detected_objects.append(do) # Publish the list of detected objects detected_objects_pub.publish(detected_objects) ros_cloud_objects = pcl_to_ros(cloud_objects) pcl_objects_pub.publish(ros_cloud_objects) # Suggested location for where to invoke your pr2_mover() function within pcl_callback() # Could add some logic to determine whether or not your object detections are robust # before calling pr2_mover() try: pr2_mover(detected_objects, cloud_table) except rospy.ROSInterruptException: pass
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) # TODO: Voxel Grid Downsampling vox = cloud.make_voxel_grid_filter() # Select a leaf/voxel size - note that 1.0 is very large. # The leaf size is measured in meters. Therefore a size of 1 is a cubic meter. LEAF_SIZE = 0.01 # experiment with this # Set the voxel/leaf size. vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) # Call the filter function to obtain the resultant downsampled point cloud cloud_filtered = vox.filter() # TODO: PassThrough Filter # Create a passthrough filter object for the z-axis passthrough_z = cloud_filtered.make_passthrough_filter() # Assign axis and range of passthrough filter object. filter_axis = 'z' passthrough_z.set_filter_field_name(filter_axis) axis_min = 0.735 # experiment with this axis_max = 1.1 # experiment with this passthrough_z.set_filter_limits(axis_min, axis_max) # Filter the downsampled point cloud with the passthrough object to get resultant cloud. cloud_filtered = passthrough_z.filter() # Create a passthrough filter for the y-axis to remove the front table edge passthrough_y = cloud_filtered.make_passthrough_filter() filter_axis = 'y' passthrough_y.set_filter_field_name(filter_axis) axis_min = -2.26 axis_max = -1.35 passthrough_y.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough_y.filter() # TODO: RANSAC Plane Segmentation # Create segmentation model seg = cloud_filtered.make_segmenter() # Set the model to fit the objects to seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) # Set the max distance for a point to be considered to be fitting the model max_distance = 0.001 # experiement with this seg.set_distance_threshold(max_distance) # TODO: Extract inliers and outliers # Call the segment function to obtain the set of inlier indices and model coefficients inliers, coefficients = seg.segment() pcl_cloud_objects = cloud_filtered.extract(inliers, negative=True) pcl_cloud_table = cloud_filtered.extract(inliers, negative=False) # TODO: Euclidean Clustering # Convert XYZRGB point cloud to XYZ as Euclidean Clustering cannot use colour information white_cloud = XYZRGB_to_XYZ(pcl_cloud_objects) # Construct the k-d tree tree = white_cloud.make_kdtree() # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as a minimum and maximum cluster size (in points) ec.set_ClusterTolerance(0.05) #experiment ec.set_MinClusterSize(200) #experiment ec.set_MaxClusterSize(2000) #experiment # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered indices cluster_indices = ec.Extract() # cluster_indices now contains a list of indices for each cluster (a list of lists) # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately # This cloud will contain points for each of the segmented objects, with each set of points having a unique colour. # Assign a colour corresponding to each segmented object in the camera view cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j])]) # Create new cloud object containing all clusters, each with a unique colour cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # TODO: Convert PCL data to ROS messages ros_cloud_objects = pcl_to_ros(pcl_cloud_objects) ros_cloud_table = pcl_to_ros(pcl_cloud_table) ros_cloud_clusters = pcl_to_ros(cluster_cloud) # TODO: Publish ROS messages pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_cloud.publish(ros_cloud_clusters) # Exercise-3 TODOs: # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster sample_cloud = pcl_cloud_objects.extract(pts_list) sample_cloud = pcl_to_ros(sample_cloud) # Compute the associated feature vector chists = compute_color_histograms(sample_cloud, using_hsv=True) normals = get_normals(sample_cloud) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1,-1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label,label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = sample_cloud detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) # TODO: Voxel Grid Downsampling vox = cloud.make_voxel_grid_filter() LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # TODO: PassThrough Filter passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.6 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # TODO: RANSAC Plane Segmentation seg = cloud_filtered.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) max_distance = 0.01 seg.set_distance_threshold(max_distance) # TODO: Extract inliers and outliers inliers, coefficients = seg.segment() extracted_inliers = cloud_filtered.extract(inliers, negative=False) extracted_outliers = cloud_filtered.extract(inliers, negative=True) # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ(extracted_outliers) # <type 'pcl._pcl.PointCloud'> tree = white_cloud.make_kdtree() # <type 'pcl._pcl.KdTree'> ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.05) ec.set_MinClusterSize(50) ec.set_MaxClusterSize(1000) ec.set_SearchMethod(tree) cluster_indices = ec.Extract() # list of list print len(cluster_indices) # how many clusters # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j])]) # Create new cloud containing all clusters, each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # TODO: Convert PCL data to ROS messages ros_cluster_cloud = pcl_to_ros(cluster_cloud) ros_cloud_table = pcl_to_ros(extracted_inliers) # TODO: Publish ROS messages pcl_objects_pub.publish(ros_cluster_cloud) pcl_table_pub.publish(ros_cloud_table) # Exercise-3 TODOs: # Classify the clusters! detected_objects_labels = [] detected_objects = [] # Classify the clusters! (loop through each detected cluster one at a time) for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = extracted_outliers.extract(pts_list) # <type 'pcl._pcl.PointCloud_PointXYZRGB'> # TODO: convert the cluster from pcl to ROS using helper function ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features # TODO: complete this step just as is covered in capture_features.py chists = compute_color_histograms(ros_cluster, using_hsv=False) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) #labeled_features.append([feature, model_name]) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1,-1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label,label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data pcl_cloud = ros_to_pcl(pcl_msg) # TODO: Statistical Outlier Filtering #create filter object outlier_filtered = pcl_cloud.make_statistical_outlier_filter() # Set number of neighboring points to analyse for a given points outlier_filtered.set_mean_k(5) # Set threshold scale factor x = 1.0 # Consider any point with a mean distance than global mean (mean + x * std_dev) outlier_filtered.set_std_dev_mul_thresh(x) # Finally, call the filter function cloud_filtered = outlier_filtered.filter() filename = 'outlier_filtered.pcd' pcl.save(cloud_filtered,filename ) # TODO: Voxel Grid Downsampling # Create a voxelGrid filter object the point cloud vox = cloud_filtered.make_voxel_grid_filter() # Choose a voxel or leaf size LEAF_SIZE = 0.01 # Set the leaf size vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) # call the filter function to obtain the resultant downsampled point cloud cloud_downsampled = vox.filter() filename= 'voxel_downsampled.pcd' pcl.save(cloud_downsampled,filename) # TODO: PassThrough Filter # Create passthrough filter object passthrough = cloud_downsampled.make_passthrough_filter() # Assign axis and range to the passthrough filter object for z axis filter_axis='z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.61 axis_max = 0.9 passthrough.set_filter_limits(axis_min,axis_max) cloud_passed = passthrough.filter() # Assign axis and range to the passthrough filter object for y axis passthrough = cloud_passed.make_passthrough_filter() filter_axis='y' passthrough.set_filter_field_name(filter_axis) axis_min = -0.4 axis_max = +0.4 passthrough.set_filter_limits(axis_min,axis_max ) cloud_passed = passthrough.filter() filename ='pass_through_cloud_filtered.pcd' pcl.save(cloud_passed, filename) # TODO: RANSAC Plane Segmentation # Create the segmentation object seg = cloud_passed.make_segmenter() # Set model you wish to fit seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) # Set maximum distant to be considerred for fitiing max_distance = 0.01 seg.set_distance_threshold(max_distance) # Call the segment function to obtain set of inliers indices and model coefficients inliers, coefficients = seg.segment() # Extract inliers cloud_table = cloud_passed.extract(inliers, negative=False) filename = 'extracted_inliers.pcd' # Save pcd for table pcl.save(cloud_table, filename) # Extract outliers cloud_objects = cloud_passed.extract(inliers, negative=True) filename = 'extracted_outliers.pcd' # Save pcd for tabletop objects pcl.save(cloud_objects, filename) # TODO: Euclidean Clustering # Apply function to convert XYZRGB to XYZ white_cloud = XYZRGB_to_XYZ(cloud_objects) # Contruct k-d tree tree = white_cloud.make_kdtree() # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # set tolearance for diistance threshold as well as maximum cluster size ec.set_ClusterTolerance(0.02) ec.set_MinClusterSize(10) ec.set_MaxClusterSize(2800) # Search the k-d tree for cluster ec.set_SearchMethod(tree) # Extract indices for each of the discovered cluster cluster_indices = ec.Extract() # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately # Assign a color to each segmented object in the scene cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2],rgb_to_float(cluster_color[j])]) # Create new cluster to contain all cluters each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) filename ='cluster_cloud.pcd' pcl.save(cluster_cloud, filename) # TODO: Convert PCL data to ROS messages ros_cloud_filtered = pcl_to_ros(cloud_passed) ros_cloud_table = pcl_to_ros(cloud_table) ros_cloud_objects = pcl_to_ros(cloud_objects) ros_cluster_cloud = pcl_to_ros(cluster_cloud) # TODO: Publish ROS messages pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cloud_filtered) pcl_cluster_cloud_pub.publish(ros_cluster_cloud) # Exercise-3 TODOs: # Classify the clusters! (loop through each detected cluster one at a time) detected_object_labels = [] detected_objects = [] for index, point_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outlier (cloud_objects) pcl_cluster = cloud_objects.extract(point_list) # convert cluster from pcl to ROS ros_cluster = pcl_to_ros(pcl_cluster) # Compute the associated feature vector color_hists = compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) normals_hists = compute_normal_histograms(normals) hists_feature = np.concatenate((color_hists, normals_hists)) # Make the prediction # Retrieve the label for the result and add it to detection_objects_label prediction = clf.predict(scaler.transform(hists_feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_object_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[point_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) # Publish the list of detected objectss detected_objects_pub.publish(detected_objects) # Suggested location for where to invoke your pr2_mover() function within pcl_callback() # Could add some logic to determine whether or not your object detections are robust # before calling pr2_mover() try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass
def pcl_callback(pcl_msg): rospy.loginfo('Begin pcl_callback...') # Exercise-2 TODOs: # DONE: Convert ROS msg to PCL data(PointXYZRGB) cloud = ros_to_pcl(pcl_msg) # DONE: Voxel Grid Downsampling cloud_filtered = voxel_filter(cloud) # DONE: PassThrough Filter # ざっくり z軸でテーブルの上だけを切り取る cloud_filtered = pass_through_filter(cloud_filtered, filter_axis='z', axis_limit=(0.6, 1.1)) # テーブルの手前もオブジェクトに見えるので、切り取る cloud_filtered = pass_through_filter(cloud_filtered, filter_axis='y', axis_limit=(-10.0, -1.39)) # DONE: RANSAC Plane Segmentation inliers, _ = ransac_plane_segmentation(cloud_filtered) # DONE: Extract inliers and outliers # フィルタでオブジェクト群とテーブルを分離する(結果は ransanc の distance に依存する) cloud_objects = cloud_filtered.extract(inliers, negative=True) rospy.loginfo(' cloud_objects: %d' % (cloud_objects.size)) # DONE: Euclidean Clustering # オブジェクト毎に分割する cluster_indices, white_cloud = euclidean_clustering(cloud_objects, tolerance=0.02, cluster_range=(100, 15000)) rospy.loginfo(' cluster_indices: %d, white_cloud: %d ' % (len(cluster_indices), white_cloud.size)) rospy.loginfo('DONE Exercise-2') # Exercise-3 TODOs: # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] # 各オブジェクトを認識する for index, pts_list in enumerate(cluster_indices): #rospy.loginfo(' Detecting... : %d' % (index)) # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = cloud_objects.extract(pts_list) # DONE: convert the cluster from pcl to ROS using helper function ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features # DONE: complete this step just as is covered in capture_features.py # Compute the associated feature vector #rospy.loginfo(' type : %s' % (type(ros_cluster))) chists = compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz # ラベルを publish する label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects # This is the output you'll need to complete the upcoming project! # # 認識したオブジェクトを publish する detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg): # Convert ROS msg to PCL data pcl_msg = ros_to_pcl(pcl_msg) # Voxel Grid Downsampling vox = pcl_msg.make_voxel_grid_filter() LEAF_SIZE = .005 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # Two PassThrough Filter one over Z one over X passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.3 axis_max = 5.0 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'x' passthrough.set_filter_field_name(filter_axis) axis_min = 0.34 axis_max = 1.0 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # Outlier filter outlier_filter = cloud_filtered.make_statistical_outlier_filter() outlier_filter.set_mean_k(50) x = 0.5 outlier_filter.set_std_dev_mul_thresh(x) cloud_filtered = outlier_filter.filter() # RANSAC Plane Segmentation seg = cloud_filtered.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) max_distance = 0.015 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # Extract inliers and outliers extracted_inliers = cloud_filtered.extract(inliers, negative=False) extracted_outliers = cloud_filtered.extract(inliers, negative=True) # Euclidean Clustering white_cloud = XYZRGB_to_XYZ( extracted_outliers) # Apply function to convert XYZRGB to XYZ tree = white_cloud.make_kdtree() #Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) ec.set_ClusterTolerance(0.01) ec.set_MinClusterSize(50) ec.set_MaxClusterSize(15000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() # Create Cluster-Mask Point Cloud to visualize each cluster separately # Assign a color corresponding to each segmented object in scene cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([ white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j]) ]) # Create new cloud containing all clusters, each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # Convert PCL data to ROS messages table_pcl_msg = pcl_to_ros(extracted_inliers) objects_pcl_msg = pcl_to_ros(extracted_outliers) ros_cluster_cloud = pcl_to_ros(cluster_cloud) # Publish ROS messages pcl_objects_pub.publish(objects_pcl_msg) pcl_table_pub.publish(table_pcl_msg) pcl_clusters_pub.publish(ros_cluster_cloud) # Exercise-3 TODOs: # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] lastone = 0 for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster pcl_cluster = extracted_outliers.extract(pts_list) ros_cluster = pcl_to_ros(pcl_cluster) # Compute the associated feature vector chists = compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # Make the prediction prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .25 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) # Publish the list of detected objects # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects) # Suggested location for where to invoke your pr2_mover() function within pcl_callback() # Could add some logic to determine whether or not your object detections are robust # before calling pr2_mover() try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass
def pcl_callback(pcl_msg): # 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(50) outlier_filter.set_std_dev_mul_thresh(0.9) cloud_no_outliers = outlier_filter.filter() # TODO: Voxel Grid Downsampling vox = cloud_no_outliers.make_voxel_grid_filter() # voxel size (also called leafs) LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_downsampled = vox.filter() # TODO: PassThrough Filter passthrough_z = cloud_downsampled.make_passthrough_filter() filter_axis = 'z' passthrough_z.set_filter_field_name(filter_axis) axis_min = 0.60 axis_max = 1.1 passthrough_z.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough_z.filter() passthrough_y = cloud_filtered.make_passthrough_filter() 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) cloud_filtered = passthrough_y.filter() if not os.path.isfile('test.pcd'): pcl.save(cloud_filtered, 'test.pcd') # 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 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # TODO: Extract inliers and outliers cloud_table = cloud_filtered.extract(inliers, negative=False) cloud_objects = cloud_filtered.extract(inliers, negative=True) # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ(cloud_objects) kd_tree = white_cloud.make_kdtree() ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.03) ec.set_MinClusterSize(5) ec.set_MaxClusterSize(2200) ec.set_SearchMethod(kd_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]) ]) #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) rospy.loginfo("Publishing results") # TODO: Publish ROS messages pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cluster_cloud) # Exercise-3 TODOs: # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = cloud_objects.extract(pts_list) 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 pcl_data = ros_to_pcl(pcl_msg) # TODO: Voxel Grid Downsampling # Create a VoxelGrid filter object for our input point cloud vox = pcl_data.make_voxel_grid_filter() # Choose a voxel (also known as leaf) size # Note: this (1) is a poor choice of leaf size # Experiment and find the appropriate size! LEAF_SIZE = 0.01 # Set the voxel (or leaf) size vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) # Call the filter function to obtain the resultant downsampled point cloud cloud_filtered = vox.filter() # filename = 'voxel_downsampled.pcd' # pcl.save(cloud_filtered, filename) # TODO: PassThrough Filter # Create a PassThrough filter object. passthrough = cloud_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object in z axis. filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.6 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # Assign axis and range to the passthrough filter object in y axis. passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'y' passthrough.set_filter_field_name(filter_axis) axis_min = -0.45 axis_max = 0.45 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # filename = 'pass_through_filtered.pcd' # pcl.save(cloud_filtered, filename) # TODO: RANSAC Plane Segmentation # Create the segmentation object seg = cloud_filtered.make_segmenter() # Set the model you wish to fit seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) # Max distance for a point to be considered fitting the model # Experiment with different values for max_distance # for segmenting the table max_distance = 0.01 seg.set_distance_threshold(max_distance) # Call the segment function to obtain set of inlier indices and model coefficients inliers, coefficients = seg.segment() # TODO: Extract inliers and outliers cloud_table = cloud_filtered.extract(inliers, negative=False) # filename = 'extracted_inliers.pcd' # pcl.save(extracted_inliers, filename) cloud_objects = cloud_filtered.extract(inliers, negative=True) # filename = 'extracted_outliers.pcd' # pcl.save(extracted_outliers, filename) # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ( cloud_objects) # Apply function to convert XYZRGB to XYZ tree = white_cloud.make_kdtree() # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) # NOTE: These are poor choices of clustering parameters # Your task is to experiment and find values that work for segmenting objects. ec.set_ClusterTolerance(0.02) ec.set_MinClusterSize(20) ec.set_MaxClusterSize(1500) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately # Assign a color corresponding to each segmented object in scene cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([ white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j]) ]) # Create new cloud containing all clusters, each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # TODO: Convert PCL data to ROS messages ros_cloud_objects = pcl_to_ros(cloud_objects) ros_cloud_table = pcl_to_ros(cloud_table) ros_cluster_cloud = pcl_to_ros(cluster_cloud) # TODO: Publish ROS messages pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cluster_cloud) # Exercise-3 TODOs: # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = cloud_objects.extract(pts_list) # TODO: convert the cluster from pcl to ROS using helper function ros_pcl_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features # TODO: complete this step just as is covered in capture_features.py chists = compute_color_histograms(ros_pcl_cluster, using_hsv=True) normals = get_normals(ros_pcl_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # labeled_features.append([feature, model_name]) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_pcl_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects) # Suggested location for where to invoke your pr2_mover() function within pcl_callback() # Could add some logic to determine whether or not your object detections are robust # before calling pr2_mover() try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass
def pcl_callback(pcl_msg): # TODO: Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) # TODO: Statistical Outlier Filtering cloud_fliter = Statistical_Outlier_Filtering(cloud) # TODO: Voxel Grid Downsampling #create a Voxel filter object forourinput point cloud cloud_filtered_v = voxel_filter(cloud_fliter) # TODO: PassThrough Filter #Create a PassThrough filter object #cloud_filtered_pass=passthrough_filter(cloud_filtered_v,'x',0.08,0.6) #for test2 (0.08,0.6) cloud_filtered_pass = passthrough_filter(cloud_filtered_v, 'y', -0.4, 0.4) #for all cloud_filtered_pass = passthrough_filter(cloud_filtered_v, 'z', 0.6, 0.91) #for test2 (0.6,0.8) # 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, nbins=32) #False normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals, nbins=16) 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) # 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): # Exercise-2 TODOs: #### Convert ROS msg to PCL data # cloud = ros_to_pcl(pcl_msg) #### Voxel Grid Downsampling # vox = cloud.make_voxel_grid_filter() # Set the voxel (or leaf) size LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) # call the filter function to obtain the resultant downsampled point cloud cloud_filtered = vox.filter() #### PassThrough Filter # # Create a PassThrough filter object. passthrough = cloud_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object. filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.76 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() #### RANSAC Plane Segmentation # # Create the segmentation object seg = cloud_filtered.make_segmenter() # Set the model you with to fit seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) # Max distance for a point to be considered fitting the model # Experiment with different values for max_distance # for segmenting the table max_distance = 0.01 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() #### Extract inliers and outliers # cloud_table = cloud_filtered.extract(inliers, negative=False) cloud_objects = cloud_filtered.extract(inliers, negative=True) #### Euclidean Clustering # white_cloud = XYZRGB_to_XYZ(cloud_objects) # Apply function to convert XYZRGB to XYZ tree = white_cloud.make_kdtree() # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) # NOTE: These are poor choices of clustering parameters # Your task is to experiment and find values that work for segmenting objects. ec.set_ClusterTolerance(0.02) ec.set_MinClusterSize(10) ec.set_MaxClusterSize(5000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() #### Create Cluster-Mask Point Cloud to visualize each cluster separately # # Assign a color correspoding to each segmented object in scene cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j])]) #Create new cloud containing all clusters, each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) #### Convert PCL data to ROS messages ros_cloud_objects = pcl_to_ros(cloud_objects) ros_cloud_table = pcl_to_ros(cloud_table) ros_cloud_cluster = pcl_to_ros(cluster_cloud) #### Publish ROS messages pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cloud_cluster) # Exercise-3 TODOs: #### Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): #### Grab the points for the cluster pcl_cluster_i = cloud_objects.extract(pts_list) #### Compute the associated feature vector # ros_cluster_i = pcl_to_ros(pcl_cluster_i) # # Extract histogram features chists = compute_color_histograms(ros_cluster_i, using_hsv=True) normals = get_normals(ros_cluster_i) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) #### Make the prediction # # Make the prediction, retrieve the label for the result # and add it to detected_objects_label list prediction = clf.predict(scaler.transform(feature.reshape(1,-1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += 0.4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster_i detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg): # Exercise-2 TODOs: ### Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) ### Voxel Grid Downsampling # Create a VoxelGrid filter object for our input point cloud vox = cloud.make_voxel_grid_filter() # Choose a voxel (also known as leaf) size # Note: this (1) is a poor choice of leaf size # Experiment and find the appropriate size! LEAF_SIZE = 0.003 # Set the voxel (or leaf) size vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) # Call the filter function to obtain the resultant downsampled point cloud cloud_filtered = vox.filter() ### Statistical Outlier Removal Filter # Much like the previous filters, we start by creating a filter object: outlier_filter = cloud_filtered.make_statistical_outlier_filter() # Set the number of neighboring points to analyze for any given point outlier_filter.set_mean_k(50) # Set threshold scale factor x = 1.0 # Any point with a mean distance larger than global (mean distance+x*std_dev) will be considered outlier outlier_filter.set_std_dev_mul_thresh(x) # Finally call the filter function for magic cloud_filtered = outlier_filter.filter() ### PassThrough Filter # Create a PassThrough filter object first across the Z axis passThroughZ = cloud_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object. filter_axis = 'z' passThroughZ.set_filter_field_name(filter_axis) axis_min = 0.6 axis_max = 1.1 passThroughZ.set_filter_limits(axis_min, axis_max) # Finally use the filter function to obtain the resultant point cloud. cloud_filtered = passThroughZ.filter() ## Now, Create a PassThrough filter object across the Y axis passThroughY = cloud_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object. filter_axis = 'y' passThroughY.set_filter_field_name(filter_axis) axis_min = -0.5 axis_max = 0.5 passThroughY.set_filter_limits(axis_min, axis_max) # Finally use the filter function to obtain the resultant point cloud. cloud_filtered = passThroughY.filter() ### RANSAC Plane Segmentation # Create the segmentation object seg = cloud_filtered.make_segmenter() # Set the model you wish to fit seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) # Max distance for a point to be considered fitting the model # Experiment with different values for max_distance # for segmenting the table max_distance = 0.01 seg.set_distance_threshold(max_distance) # Call the segment function to obtain set of inlier indices and model coefficients inliers, coefficients = seg.segment() ### Extract inliers and outliers extracted_inliers = cloud_filtered.extract(inliers, negative=False) extracted_outliers = cloud_filtered.extract(inliers, negative=True) ### Euclidean Clustering # Go from XYZRGB to RGB since to build the k-d tree we only needs spatial data white_cloud = XYZRGB_to_XYZ(extracted_outliers) # Apply function to convert XYZRGB to XYZ tree = white_cloud.make_kdtree() ### Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) # Your task is to experiment and find values that work for segmenting objects. ec.set_ClusterTolerance(0.01) ec.set_MinClusterSize(100) # Refering to the minimum and maximum number of points that make up an object's cluster ec.set_MaxClusterSize(50000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() ### Create Cluster-Mask Point Cloud to visualize each cluster separately #Assign a color corresponding to each segmented object in scene cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j])]) #Create new cloud containing all clusters, each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # TODO - set proper names ### Convert PCL data to ROS messages ros_cloud_objects = pcl_to_ros(extracted_outliers) ros_cloud_table = pcl_to_ros(extracted_inliers) ros_cluster_cloud = pcl_to_ros(cluster_cloud) # TODO set names ### Publish ROS messages pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cluster_cloud) # Exercise-3 TODOs: # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = extracted_outliers.extract(pts_list) ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features # Complete this step just as is covered in capture_features.py chists = compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # labeled_features.append([feature, model_name]) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1,-1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label,label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) print "Trying to match the pick list with the objects detected..." print "\n" # Publish the list of detected objects rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects) object_list_param = rospy.get_param('/object_list') pick_list_objects = [] for i in range(len(object_list_param)): pick_list_objects.append(object_list_param[i]['name']) print "\n" print "Pick List includes: " print pick_list_objects print "\n" pick_set_objects = set(pick_list_objects) detected_set_objects = set(detected_objects_labels) if detected_set_objects == pick_set_objects: try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass