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() # 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): # Convert ROS msg to PCL data using helper function cloud = ros_to_pcl(pcl_msg) # Statistical outlier filtering : Since we are dealing with noisy environment outlier_filter = cloud.make_statistical_outlier_filter() outlier_filter.set_mean_k(50) x = 1.0 outlier_filter.set_std_dev_mul_thresh(x) cloud_filtered = outlier_filter.filter() # Voxel Grid Downsampling : Reducing number of volume elements in the picture vox = cloud_filtered.make_voxel_grid_filter() LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # Pass through filter for boxes on the side of the table passthrough_box = cloud_filtered.make_passthrough_filter() passthrough_box.set_filter_field_name('y') axis_min = -0.5 axis_max = 0.5 passthrough_box.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough_box.filter() # PassThrough Filter : Extracting everything except for the table as outliers passthrough = cloud_filtered.make_passthrough_filter() passthrough.set_filter_field_name('z') axis_min = 0.62 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # RANSAC Plane Segmentation seg = cloud_filtered.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) max_distance = 0.01 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # Extract inliers and outliers : Extract indices filter 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() #Creating a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.01) ec.set_MinClusterSize(60) ec.set_MaxClusterSize(10000) ec.set_SearchMethod(tree) cluster_indices = ec.Extract() # Create Cluster-Mask Point Cloud to visualize each cluster separately cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([ white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j]) ]) cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) ros_cluster_cloud = pcl_to_ros(cluster_cloud) # TODO: Convert PCL data to ROS messages ros_cloud_objects = pcl_to_ros(cloud_objects) ros_cloud_table = pcl_to_ros(cloud_table) # Publish ROS messages using helper function pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cluster_cloud) # Classify the clusters! (loop through each detected cluster one at a time) detected_objects = [] detected_objects_labels = [] # Grab the points for the cluster for index, pts_list in enumerate(cluster_indices): pcl_cluster = cloud_objects.extract(pts_list) cloud_ros = pcl_to_ros(pcl_cluster) chists = compute_color_histograms(cloud_ros, using_hsv=True) normals = get_normals(cloud_ros) nhists = compute_normal_histograms(normals) # Compute the associated feature vector feature = np.concatenate((chists, nhists)) # Make the prediction prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = cloud_ros 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() try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass
def pcl_callback(pcl_msg): """Callback function for the Point Cloud Subscriber :param pcl_msg: ROS point cloud message """ # Convert ROS msg to PCL data (XYZRGB) cloud = ros_to_pcl(pcl_msg) #pcl.save(cloud, './misc/pipeline_0_raw_cloud.pcd') # Statistical outlier filtering outlier_filter = cloud.make_statistical_outlier_filter() # Set the number of neighboring points to analyze for any given point outlier_filter.set_mean_k(20) # Any point with a mean distance larger than global will be considered out outlier_filter.set_std_dev_mul_thresh(0.1) cloud_filtered = outlier_filter.filter() #pcl.save(cloud_filtered, './misc/pipeline_1_outlier_removal_filter.pcd') # Voxel Grid Downsampling vox = cloud_filtered.make_voxel_grid_filter() LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() #pcl.save(cloud_filtered, './misc/pipeline_2_voxel_grid_filter.pcd') # PassThrough Filter to remove the areas on the side of the table passthrough_y = cloud_filtered.make_passthrough_filter() passthrough_y.set_filter_field_name('y') y_axis_min = -0.4 y_axis_max = 0.4 passthrough_y.set_filter_limits(y_axis_min, y_axis_max) cloud_filtered = passthrough_y.filter() # PassThrough Filter to isolate only the objects on the table surface passthrough_z = cloud_filtered.make_passthrough_filter() passthrough_z.set_filter_field_name('z') z_axis_min = 0.61 z_axis_max = 0.9 passthrough_z.set_filter_limits(z_axis_min, z_axis_max) cloud_filtered = passthrough_z.filter() #pcl.save(cloud_filtered, './misc/pipeline_3_passthrough_filter.pcd') # RANSAC Plane Segmentation to identify the table from the objects seg = cloud_filtered.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) max_distance = 0.01 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # Extract inliers (table surface) and outliers (objects) cloud_table = cloud_filtered.extract(inliers, negative=False) cloud_objects = cloud_filtered.extract(inliers, negative=True) #pcl.save(cloud_table, './misc/pipeline_4_extracted_inliers.pcd') #pcl.save(cloud_objects, './misc/pipeline_5_extracted_outliers.pcd') # Convert the xyzrgb cloud to only xyz since k-d tree only needs xyz white_cloud = XYZRGB_to_XYZ(cloud_objects) # k-d tree decreases the computation of searching for neighboring points. # PCL's euclidian clustering algo only supports k-d trees tree = white_cloud.make_kdtree() # Euclidean clustering used to build individual point clouds for each # object. The Cluster-Mask point cloud allows each cluster to be # visualized separately. ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.05) ec.set_MinClusterSize(100) ec.set_MaxClusterSize(3000) ec.set_SearchMethod(tree) # Search the k-d tree for clusters # Extract indices for each found cluster. This is a list of indices for # each cluster (list of lists) cluster_indices = ec.Extract() # Assign a random color to each isolated object in the scene cluster_color = get_color_list(len(cluster_indices)) # Store the detected objects and labels in these lists detected_objects_labels = [] detected_objects = [] color_cluster_point_list = [] # Iterate through each detected object cluster for object recognition for index, pts_list in enumerate(cluster_indices): # Store the object's cloud in this list object_cluster = [] # Create an individual cluster just for the object being processed for i, pts in enumerate(pts_list): # Retrieve cloud values for the x, y, z, rgb object object_cluster.append([ cloud_objects[pts][0], cloud_objects[pts][1], cloud_objects[pts][2], cloud_objects[pts][3] ]) # Retrieve cloud values for the x, y, z object, assigning a # preidentified color to all cloud values color_cluster_point_list.append([ white_cloud[pts][0], white_cloud[pts][1], white_cloud[pts][2], rgb_to_float(cluster_color[index]) ]) # Convert list of point cloud features (x,y,z,rgb) into a point cloud pcl_cluster = pcl.PointCloud_PointXYZRGB() pcl_cluster.from_list(object_cluster) # Convert the cluster from pcl to ROS using helper function ros_cloud = pcl_to_ros(pcl_cluster) #pcl_objects_pub.publish(ros_cloud) # Extract histogram features (similar to capture_features.py) histogram_bins = 128 chists = compute_color_histograms(ros_cloud, nbins=histogram_bins, using_hsv=True) normals = get_normals(ros_cloud) 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 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cloud detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) # Create new cloud containing all clusters, each with a unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # Convert PCL data to ROS messages ros_cloud_object_cluster = pcl_to_ros(cluster_cloud) ros_cloud_objects = pcl_to_ros(cloud_objects) ros_cloud_table = pcl_to_ros(cloud_table) # Publish ROS messages of the point clouds and detected objects pcl_objects_cloud_pub.publish( ros_cloud_object_cluster) # solid color objects pcl_objects_pub.publish(ros_cloud_objects) # original color objects pcl_table_pub.publish(ros_cloud_table) # table cloud detected_objects_pub.publish(detected_objects) # detected object labels try: # Add some logic to determine whether or not your object detections # are robust enough before calling pr2_mover() 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: Statistical Outlier Filtering #sof = pcl_data.make_statistical_outlier_filter() #sof.set_mean_k(20) #sof.set_std_dev_mul_thresh(0.3) #filtered_pcl = fil.filter() # Voxel Grid filter vox = pcl_data.make_voxel_grid_filter() LEAF_SIZE = 0.005 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # PassThrough filter pt = cloud_filtered.make_passthrough_filter() filter_axis = 'z' pt.set_filter_field_name(filter_axis) axis_min = 0.6 axis_max = 1.1 pt.set_filter_limits(axis_min, axis_max) cloud_filtered = pt.filter() #added second y-axis filter pt = cloud_filtered.make_passthrough_filter() filter_axis = 'y' pt.set_filter_field_name(filter_axis) axis_min = -1 axis_max = 1 pt.set_filter_limits(axis_min, axis_max) cloud_filtered = pt.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.034 #increased from 0.01 because I was recognizing the front of the table as an object 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) # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ(cloud_objects) tree = white_cloud.make_kdtree() ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.01) #0.01 ec.set_MinClusterSize(100) #25 ec.set_MaxClusterSize(25000) #10000 ec.set_SearchMethod(tree) cluster_indices = ec.Extract() #print(cluster_indices) #checked my parameters above for clustering # 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_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_table) pcl_cluster_pub.publish(ros_cluster_cloud) # Exercise-3 TODOs: print("here") # Classify the clusters! (loop through each detected cluster one at a time) detected_objects = [] detected_objects_labels = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster pcl_cluster = cloud_objects.extract(pts_list) # Convert Cluster to ROS from PCL ros_pcl_array = pcl_to_ros(pcl_cluster) # Compute the associated feature vector chists = compute_color_histograms(ros_pcl_array, using_hsv=True) normals = get_normals(ros_pcl_array) 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_pcl_array detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) #print(detected_objects) if detected_objects: # Publish the list of detected objects detected_objects_pub.publish(detected_objects) try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass else: ros.loginfo("No objects detected")
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # Convert ROS msg to PCL data pcl_data = ros_to_pcl(pcl_msg) # Statistical outlier filtering # Much like the previous filters, # we start by creating a filter object: outlier_filter = pcl_data.make_statistical_outlier_filter() mean = 7 # mean number of neighboring points thresh = 0.3 # Set threshold scale factor # Set the number of neighboring points to analyze for any given point outlier_filter.set_mean_k(mean) # 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(thresh) # filtered point cloud data cloud_filtered = outlier_filter.filter() # Voxel Grid Downsampling # TODO: comments LEAF_SIZE = 0.008 vox = cloud_filtered.make_voxel_grid_filter() # vox = pcl_data.make_voxel_grid_filter() # leaf size for x, y and z dimensions vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # produce a downsampled point cloud data # PassThrough Filter # Set z-axis pass through filter filter_axis = 'z' # set the axis to 'pass through' axis_min = 0.6 # minimum z axis_max = 0.85 # max z passthrough = cloud_filtered.make_passthrough_filter() passthrough.set_filter_field_name(filter_axis) passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # Set x-axis pass through filter # to Filter out the drop boxes being mistakenly clustered filter_x_axis = 'x' # set the axis to 'pass through' axis_x_min = 0.4 axis_x_max = 2 passthrough = cloud_filtered.make_passthrough_filter() passthrough.set_filter_field_name(filter_x_axis) passthrough.set_filter_limits(axis_x_min, axis_x_max) cloud_filtered = passthrough.filter() # RANSAC Plane Segmentation # TODO: Comments max_distance = 0.009 ransac = cloud_filtered.make_segmenter() ransac.set_model_type(pcl.SACMODEL_PLANE) ransac.set_method_type(pcl.SAC_RANSAC) ransac.set_distance_threshold(max_distance) # Extract inliers and outliers # TODO: Comments inliers, coefficients = ransac.segment() cloud_table = cloud_filtered.extract(inliers, negative=False) cloud_objects = cloud_filtered.extract(inliers, negative=True) # Euclidean Clustering # convert object point cloud data to xyz for segmentation white_cloud = XYZRGB_to_XYZ(cloud_objects) kd_tree = white_cloud.make_kdtree() # Create Cluster-Mask Point Cloud to visualize each cluster separately tolerance = 0.03 min_cluster_size = 150 max_cluster_size = 1100 ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(tolerance) ec.set_MinClusterSize(min_cluster_size) ec.set_MaxClusterSize(max_cluster_size) # Search the k-dimension tree for clusters according to set parameters ec.set_SearchMethod(kd_tree) cluster_indices = ec.Extract() # Get the cluster groups # Visualization of the Eucledian Segmentation Step 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], # Red white_cloud[index][1], # Green white_cloud[index][2], # Blue rgb_to_float(cluster_color[j]) ]) # new cloud containing all clusters, each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # Convert PCL data to ROS messages ros_cloud_objects = pcl_to_ros(cloud_objects) ros_cloud_table = pcl_to_ros(cloud_table) ros_cloud_cluster = pcl_to_ros(cluster_cloud) # Publish ROS messages pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cloud_cluster) # Exercise-3: # Array containers to get the pint cloud data and the labels detected_objects_labels = [] detected_objects = [] # Classify the clusters! (loop through each detected cluster one at a time) 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) sample_cloud = pcl_to_ros(pcl_cluster) # Compute the associated feature vector # Using hsv instead of RGB chists = compute_color_histograms(sample_cloud, using_hsv=True) normals = get_normals(sample_cloud) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # Make the prediction # retrieve the label for the result and add # it to the detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += 0.3 # TODO: What is this? offset? 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) # Do some logging rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects print 'Publishing 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: # Add code to handle when no objects are detected.. pass
def pcl_callback(pcl_msg): # Exercise-2 code: # Convert ROS msg to PCL data cloud_filtered = ros_to_pcl(pcl_msg) # Voxel Grid Downsampling vox = cloud_filtered.make_voxel_grid_filter() LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # Statistical Outlier Filtering 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(5) # orig 50 # Set threshold scale factor x = -0.5 # orig 1.0 # Any point with a mean distance larger than (global_mean_distance + x * global_std_dev) is considered an outlier outlier_filter.set_std_dev_mul_thresh(x) # Perform filtering (outliers are filtered out) cloud_filtered = outlier_filter.filter() # PassThrough Filter 1 passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.6 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # PassThrough Filter 2 passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'y' passthrough.set_filter_field_name(filter_axis) axis_min = -0.43 axis_max = 0.43 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # RANSAC Plane Segmentation seg = cloud_filtered.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) max_distance = 0.01 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # Extract inliers and outliers extracted_inliers = cloud_filtered.extract(inliers, negative=False) # table extracted_outliers = cloud_filtered.extract( inliers, negative=True) # tabletop objects # Statistical Outlier Filtering outlier_filter = extracted_outliers.make_statistical_outlier_filter() # Set the number of neighboring points to analyze for any given point outlier_filter.set_mean_k(90) # orig 50 # Set threshold scale factor x = 0.5 # orig 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) # Perform filtering (outliers are filtered out) cloud_filtered = outlier_filter.filter() # Create Cluster-Mask Point Cloud to visualize each cluster separately white_cloud = XYZRGB_to_XYZ(extracted_outliers) tree = white_cloud.make_kdtree() ec = white_cloud.make_EuclideanClusterExtraction() # Tune these parameters ec.set_ClusterTolerance(0.05) # 0.001 ec.set_MinClusterSize(10) # 10 ec.set_MaxClusterSize(3000) # 250 ec.set_SearchMethod(tree) cluster_indices = ec.Extract() # a list of lists, one for each object # Create final point cloud where points of different lists have different colors cluster_colors = 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_colors[j])]) cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # Convert PCL data to ROS messages ros_cloud_table = pcl_to_ros(extracted_inliers) # table ros_cloud_objects = pcl_to_ros(extracted_outliers) # objects ros_cluster_cloud = pcl_to_ros(cluster_cloud) # colored objects # 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 code: # Classify the clusters! detected_objects_labels = [] detected_objects = [] # 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) # Convert the cluster from pcl to ROS using helper function ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features # See src/sensor_stick/src/sensor_stick/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 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
for i in range(capture_attempts): # make five attempts to get a valid a point cloud then give up sample_was_good = False try_count = 0 while not sample_was_good and try_count < 5: # Capture the point cloud using the sensor stock RGBD camera sample_cloud = capture_sample() # Convert the ros cloud to a pcl cloud sample_cloud_arr = ros_to_pcl(sample_cloud).to_array() # Check for invalid clouds. if sample_cloud_arr.shape[0] == 0: print('Invalid cloud detected') try_count += 1 else: sample_was_good = True # Extract histogram features chists = compute_color_histograms(sample_cloud, nbins=histogram_bins, using_hsv=True) normals = get_normals(sample_cloud) nhists = compute_normal_histograms(normals, nbins=histogram_bins) feature = np.concatenate((chists, nhists)) labeled_features.append([feature, model_name]) delete_model() pickle.dump(labeled_features, open('training_set.sav', 'wb'))
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data pointCloud = ros_to_pcl(pcl_msg) # Creating a filter object: outlier_filter = pointCloud.make_statistical_outlier_filter() outlier_filter.set_mean_k(3) # The number of neighboring points x = 0.00001 # Set threshold scale factor outlier_filter.set_std_dev_mul_thresh(x) cloud_filtered = outlier_filter.filter() # TODO: Voxel Grid Downsampling vox = cloud_filtered.make_voxel_grid_filter() LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # TODO: PassThrough Filter passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.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() ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.05) # Default: 0.001 ec.set_MinClusterSize(10) # Default: 10 ec.set_MaxClusterSize(2000) # Default: 250 ec.set_SearchMethod(tree) cluster_indices = ec.Extract() # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j])]) cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # TODO: Convert PCL data to ROS messages ros_cloud_objects = pcl_to_ros(cloud_objects) ros_cloud_table = pcl_to_ros(cloud_table) ros_cluster_cloud = pcl_to_ros(cluster_cloud) # TODO: Publish ROS messages pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cluster_cloud) # Exercise-3 TODOs: # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster pcl_cluster = cloud_objects.extract(pts_list) # Compute the associated feature vector # Conver 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) 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): # Convert ROS msg to PCL data cloud_msg = ros_to_pcl(pcl_msg) pcl.save(cloud_msg, "1.pcd") # Statistical Outlier Filtering outlier_filter = cloud_msg.make_statistical_outlier_filter() outlier_filter.set_mean_k(50) x = 0.1 outlier_filter.set_std_dev_mul_thresh(x) cloud_filter_out = outlier_filter.filter() pcl.save(cloud_filter_out, "2.pcd") # Voxel Grid Downsampling vox = cloud_filter_out.make_voxel_grid_filter() LEAF_SIZE = 0.005 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_vox = vox.filter() pcl.save(cloud_vox, "3.pcd") # PassThrough Filter passthrough = cloud_vox.make_passthrough_filter() filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.6 axis_max = 1.5 passthrough.set_filter_limits(axis_min, axis_max) cloud_filter_pass = passthrough.filter() passthrough2 = cloud_filter_pass.make_passthrough_filter() filter_axis = 'x' passthrough2.set_filter_field_name(filter_axis) axis_min = 0.4 axis_max = 2.5 passthrough2.set_filter_limits(axis_min, axis_max) cloud_filter2_pass = passthrough2.filter() pcl.save(cloud_filter2_pass, "4.pcd") # RANSAC Plane Segmentation max_distance = 0.04 seg = cloud_filter2_pass.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # Extract inliers and outliers cloud_objects = cloud_filter2_pass.extract(inliers, negative=True) pcl.save(cloud_objects, "file5.pcd") # Euclidean Clustering white_cloud =XYZRGB_to_XYZ(cloud_objects) # Apply function to convert XYZRGB to XYZ tree = white_cloud.make_kdtree() ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.027) ec.set_MinClusterSize(50) ec.set_MaxClusterSize(2000) ec.set_SearchMethod(tree) cluster_indices = ec.Extract() cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j])]) cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) pcl.save(cluster_cloud, "cloud.pcd") # Convert PCL data to ROS messages ros_cloud_objects = pcl_to_ros(cloud_objects) ros_cluster_cloud = pcl_to_ros(cluster_cloud) # Publish ROS messages pcl_objects_pub.publish(ros_cloud_objects) pcl_ros_seg.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 pcl_cluster = cloud_objects.extract(pts_list) # Convert pcl to ROS message 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] += .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) # Suggested location for where to invoke your pr2_mover() function within pcl_callback() # Could add some logic to determine whether or not your object detections are robust # before calling pr2_mover() try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass
def pcl_callback(pcl_msg): # Exercise-2 TODOs: ### Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) ### Voxel Grid Downsampling # Create a VoxelGrid filter object for our input point cloud vox = cloud.make_voxel_grid_filter() # Choose a voxel (also known as leaf) size # Note: this (1) is a poor choice of leaf size # Experiment and find the appropriate size! LEAF_SIZE = 0.003 # Set the voxel (or leaf) size vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) # Call the filter function to obtain the resultant downsampled point cloud cloud_filtered = vox.filter() ### Statistical Outlier Removal Filter # Much like the previous filters, we start by creating a filter object: outlier_filter = cloud_filtered.make_statistical_outlier_filter() # Set the number of neighboring points to analyze for any given point outlier_filter.set_mean_k(50) # Set threshold scale factor x = 1.0 # Any point with a mean distance larger than global (mean distance+x*std_dev) will be considered outlier outlier_filter.set_std_dev_mul_thresh(x) # Finally call the filter function for magic cloud_filtered = outlier_filter.filter() ### PassThrough Filter # Create a PassThrough filter object first across the Z axis passThroughZ = cloud_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object. filter_axis = 'z' passThroughZ.set_filter_field_name(filter_axis) axis_min = 0.6 axis_max = 1.1 passThroughZ.set_filter_limits(axis_min, axis_max) # Finally use the filter function to obtain the resultant point cloud. cloud_filtered = passThroughZ.filter() ## Now, Create a PassThrough filter object across the Y axis passThroughY = cloud_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object. filter_axis = 'y' passThroughY.set_filter_field_name(filter_axis) axis_min = -0.5 axis_max = 0.5 passThroughY.set_filter_limits(axis_min, axis_max) # Finally use the filter function to obtain the resultant point cloud. cloud_filtered = passThroughY.filter() ### RANSAC Plane Segmentation # Create the segmentation object seg = cloud_filtered.make_segmenter() # Set the model you wish to fit seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) # Max distance for a point to be considered fitting the model # Experiment with different values for max_distance # for segmenting the table max_distance = 0.01 seg.set_distance_threshold(max_distance) # Call the segment function to obtain set of inlier indices and model coefficients inliers, coefficients = seg.segment() ### Extract inliers and outliers extracted_inliers = cloud_filtered.extract(inliers, negative=False) extracted_outliers = cloud_filtered.extract(inliers, negative=True) ### Euclidean Clustering # Go from XYZRGB to RGB since to build the k-d tree we only needs spatial data white_cloud = XYZRGB_to_XYZ(extracted_outliers) # Apply function to convert XYZRGB to XYZ tree = white_cloud.make_kdtree() ### Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) # Your task is to experiment and find values that work for segmenting objects. ec.set_ClusterTolerance(0.01) ec.set_MinClusterSize(100) # Refering to the minimum and maximum number of points that make up an object's cluster ec.set_MaxClusterSize(50000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() ### Create Cluster-Mask Point Cloud to visualize each cluster separately #Assign a color corresponding to each segmented object in scene cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j])]) #Create new cloud containing all clusters, each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # TODO - set proper names ### Convert PCL data to ROS messages ros_cloud_objects = pcl_to_ros(extracted_outliers) ros_cloud_table = pcl_to_ros(extracted_inliers) ros_cluster_cloud = pcl_to_ros(cluster_cloud) # TODO set names ### Publish ROS messages pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cluster_cloud) # Exercise-3 TODOs: # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = extracted_outliers.extract(pts_list) ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features # Complete this step just as is covered in capture_features.py chists = compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # labeled_features.append([feature, model_name]) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1,-1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label,label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) print "Trying to match the pick list with the objects detected..." print "\n" # Publish the list of detected objects rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects) object_list_param = rospy.get_param('/object_list') pick_list_objects = [] for i in range(len(object_list_param)): pick_list_objects.append(object_list_param[i]['name']) print "\n" print "Pick List includes: " print pick_list_objects print "\n" pick_set_objects = set(pick_list_objects) detected_set_objects = set(detected_objects_labels) if detected_set_objects == pick_set_objects: try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass
def pcl_callback(ros_msg): # Exercise-2 TODOs: pcl_cloud = ros_to_pcl(ros_msg) vox = pcl_cloud.make_voxel_grid_filter() LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.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() 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, _ = seg.segment() pcl_table = cloud_filtered.extract(inliers, negative=False) pcl_objects = cloud_filtered.extract(inliers, negative=True) white_cloud = XYZRGB_to_XYZ(pcl_objects) tree = white_cloud.make_kdtree() # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) # NOTE: These are poor choices of clustering parameters # Your task is to experiment and find values that work for segmenting objects. ec.set_ClusterTolerance(0.02) ec.set_MinClusterSize(5) # ec.set_MaxClusterSize(250) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() # 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_msg_cluster_cloud = pcl_to_ros(cluster_cloud) ros_msg_table = pcl_to_ros(pcl_table) ros_msg_objects = pcl_to_ros(pcl_objects) pcl_cluster_cloud_pub.publish(ros_msg_cluster_cloud) pcl_objects_pub.publish(ros_msg_objects) pcl_table_pub.publish(ros_msg_table) # 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 = pcl_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 # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) # STATISTICAL OUTLIER FILTERING # much like the previous filters, we sart by creating a filter object: outlier_filter = cloud.make_statistical_outlier_filter() # Set the number of neighboring points to analyze for any given point outlier_filter.set_mean_k(3) # Set threshold scale factor x = 0.0001 # Any point with a mean distance larger than global (mean distance+x*std_dev) will be considered outlier outlier_filter.set_std_dev_mul_thresh(x) # Finally call the filter function for magic cloud = outlier_filter.filter() # 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.61 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # FILTER 2 passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'y' passthrough.set_filter_field_name(filter_axis) axis_min = -0.5 axis_max = 0.5 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # TODO: RANSAC Plane Segmentation seg = cloud_filtered.make_segmenter() # 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 extracted_inliers = cloud_filtered.extract(inliers, negative=False) extracted_outliers = cloud_filtered.extract(inliers, negative=True) # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ(extracted_outliers) tree = white_cloud.make_kdtree() # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) # NOTE: These are poor choices of clustering parameters # Your tak sis to experiment and find values tha work for segmenting objects. ec.set_ClusterTolerance(0.05) ec.set_MinClusterSize(10) ec.set_MaxClusterSize(2500) # 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 pcl_cloud_objects = extracted_outliers pcl_cloud_table = extracted_inliers ros_test = pcl_to_ros(cluster_cloud) # ros_cloud_objects = pcl_to_ros(pcl_cloud_objects) # ros_cloud_table = pcl_to_ros(pcl_cloud_table) # ros_cluster_cloud = pcl_to_ros(cluster_cloud) # # TODO: Publish ROS messages pcl_test_pub.publish(ros_test) # pcl_objects_pub.publish(ros_cloud_objects) # pcl_table_pub.publish(ros_cloud_table) # pcl_cluster_pub.publish(ros_cluster_cloud) # pcl_objects_pub.publish(pcl_msg) # 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)) # 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) # 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
nspawn = 50 for i in range(nspawn): print('{} of {}').format(i+1, nspawn) # make five attempts to get a valid a point cloud then give up sample_was_good = False try_count = 0 while not sample_was_good and try_count < 5: sample_cloud = capture_sample() sample_cloud_arr = ros_to_pcl(sample_cloud).to_array() # Check for invalid clouds. if sample_cloud_arr.shape[0] == 0: print('Invalid cloud detected on try count',try_count) try_count += 1 else: sample_was_good = True # Extract histogram features chists = compute_color_histograms(sample_cloud, nbins=64, bins_range = (0,1024), using_hsv=True) normals = get_normals(sample_cloud) nhists = compute_normal_histograms(normals,nbins=64, bins_range = (0,1024)) feature = np.concatenate((chists, nhists)) labeled_features.append([feature, model_name]) delete_model() print(time.time()-tj) print('Total Elapsed time: ',time.time()-t0) pickle.dump(labeled_features, open('training_set_pr2.sav', 'wb'))
def pcl_callback(pcl_msg): print("\nStart pcl_callback,...") # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) # Statistical Outlier Filtering fil = cloud.make_statistical_outlier_filter() fil.set_mean_k(10) fil.set_std_dev_mul_thresh(0.5) cloud_filtered = fil.filter() # TODO: Voxel Grid Downsampling # Create a VoxelGrid filter object for our input point cloud vox = cloud_filtered.make_voxel_grid_filter() # Choose a voxel (also known as leaf) size LEAF_SIZE = 0.01 # Set the voxel (or leaf) size vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) # Call the filter function to obtain the resultant downsampled point cloud cloud_filtered = vox.filter() # TODO: PassThrough Filter # Create a PassThrough filter object. # Assign axis and range to the passthrough filter object. filter_axis = 'z' passthrough = cloud_filtered.make_passthrough_filter() 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. filter_axis = 'y' passthrough = cloud_filtered.make_passthrough_filter() passthrough.set_filter_field_name(filter_axis) axis_min = -0.5 axis_max = 0.5 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # TODO: RANSAC Plane Segmentation # 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 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) cloud_objects = cloud_filtered.extract(inliers, negative=True) # TODO: Euclidean Clustering # Use only spatial information: white_cloud = XYZRGB_to_XYZ(cloud_objects) tree = white_cloud.make_kdtree() # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold as well as minimum and maximum cluster size (in points) ec.set_ClusterTolerance(0.05) ec.set_MinClusterSize(50) ec.set_MaxClusterSize(5000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() # 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 cloud_cluster = pcl.PointCloud_PointXYZRGB() cloud_cluster.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_cloud_cluster = pcl_to_ros(cloud_cluster) # TODO: 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 = 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 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(cloud_objects[pts_list[0]])[0:3] 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) # Invoke your pr2_mover() function within pcl_callback() try: print('\nCalling pr2_mover') pr2_mover(detected_objects) except rospy.ROSInterruptException: pass print('\nShutting down') rospy.signal_shutdown("Completed")
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() # choose voxel (aka leaf) size LEAF_SIZE = .01 # set the voxel/leaf size vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) # call .filter() to obtain the downsampled point cloud cloud_filtered = vox.filter() filename = 'voxel_downsampled.pcd' pcl.save(cloud_filtered, filename) # TODO: PassThrough Filter passthrough = cloud_filtered.make_passthrough_filter() # assign axis and range filter_axis = 'z' passthrough.set_fiter_field_name(filter_axis) axis_min = 0.6 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) # call .filter() to obtain filtered point cloud cloud_filtered = passthrough.filter() filename = 'pass_through_filtered.pcd' pcl.save(cloud_filtered, filename) # TODO: RANSAC Plane Segmentation seg = cloud_filtered.make_segmenter() # set model seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) # max distance threshold for consideration max_distance = .01 seg.set_distance_threshold(max_distance) # call .segment() to obtain set of inlier indices on d model coefficients inliers, coefficients = seg.segment() # TODO: Extract plane inliers (table) and outliers (objects) extracted_inliers = cloud_filtered.extract(inliers, negative=False) filename = 'extracted_inliers.pcd' pcl.save(extracted_inliers, filename) # Object Extraction extracted_outliers = cloud_filtered.extract(inliers, negative=True) filename = 'extracted_outliers.pcd' pcl.save(extracted_outliers, filename) outlier_filter = cloud_filtered.make_statistical_outlier_filter() # set number of neighboring points to analyze outlier_filter.set_mean_k(50) # set threshold scale factor x = 1.0 # outlier is any point with a mean distance larger than global (mean distance + x*std_dev) outlier_filter.set_std_dev_mul_thresh(x) # call .filter() cloud_filtered = outlier_filter.filter() # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ(cloud_filtered) tree = white_cloud.make_kdtree() # Create a cluster extraction object euclidean_cluster = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold and minimum and maximum cluster size (in points) euclidean_cluster.set_ClusterTolerance(0.001) euclidean_cluster.set_MinClusterSize(10) euclidean_cluster.set_MaxClusterSize(250) # Search the k-d tree for clusters euclidean_cluster.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = euclidean_cluster.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_cluster_cloud = pcl_to_ros(cluster_cloud) ros_table_cloud = pcl_to_ros(extracted_inliers) # TODO: Publish ROS messages pcl_objects_pub.publish(ros_cluster_cloud) pcl_table_pub.publish(ros_table_cloud) # Exercise-3 TODOs: # Classify the clusters! (loop through each detected cluster one at a time) for index, pts_list in enumerate(cluster_cloud): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = cloud_objects.extract(pts_list) # Compute the associated feature vector # TODO: convert the cluster from pcl to ROS using helper function sample_cloud = pcl_to_ros(pcl_cluster) # Extract histogram features # TODO: complete this step just as is covered in capture_features.py chists = compute_color_histograms(sample_cloud, using_hsv=False) normals = get_normals(sample_cloud) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) labeled_features.append([feature, model_name]) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1,-1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label,label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects) # Suggested location for where to invoke your pr2_mover() function within pcl_callback() # Could add some logic to determine whether or not your object detections are robust # before calling pr2_mover() try: pr2_mover(detected_objects_list) 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() # Choose a voxel (leaf) size LEAF_SIZE = 0.01 # Set the voxel (or leaf) size vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) # Call the filter function to obtain the resultant downsampled point cloud cloud_filtered = vox.filter() ### TODO: PassThrough Filter passthrough = cloud_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object. filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.6 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) # Finally use the filter function to obtain the resultant point cloud. cloud_filtered = passthrough.filter() ### 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 max_distance = 0.01 seg.set_distance_threshold(max_distance) ### TODO: Extract inliers and outliers inliers, coefficients = seg.segment() # Extract inliers extracted_table = cloud_filtered.extract(inliers, negative=False) extracted_objects = cloud_filtered.extract(inliers, negative=True) # Much like the previous filters, we start by creating a filter object: table_filter = extracted_table.make_statistical_outlier_filter() objects_filter = extracted_objects.make_statistical_outlier_filter() # Set the number of neighboring points to analyze for any given point table_filter.set_mean_k(50) objects_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 #table_filter.set_std_dev_mul_thresh(x) objects_filter.set_std_dev_mul_thresh(x) # Finally call the filter function for magic cloud_table = table_filter.filter() cloud_objects = objects_filter.filter() ### TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ( cloud_objects) # Apply function to convert XYZRGB to XYZ tree = white_cloud.make_kdtree() # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold as well as minimum and maximum cluster size (in points) ec.set_ClusterTolerance(0.01) ec.set_MinClusterSize(10) ec.set_MaxClusterSize(250) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() ### TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately # Assign a color corresponding to each segmented object in scene cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([ white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j]) ]) # 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_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]) # Compute the associated feature vector # 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)
print(str(pct) +"%% - [ " + model_name +" : model " + str(ind+1) +" of 8 ] orientation ( " + str(i) +" of " + str(ORIENTATIONS_PER_OBJECT) + " )") # make five attempts to get a valid a point cloud then give up sample_was_good = False try_count = 0 while not sample_was_good and try_count < 5: sample_cloud = capture_sample() sample_cloud_arr = ros_to_pcl(sample_cloud).to_array() # Check for invalid clouds. if sample_cloud_arr.shape[0] == 0: try_count += 1 else: sample_was_good = True # Extract histogram features hsv_hists = compute_color_histograms(sample_cloud, nbins=N_HIST_BINS, using_hsv=True) rgb_hists = compute_color_histograms(sample_cloud, nbins=N_HIST_BINS, using_hsv=False) normals = get_normals(sample_cloud) nhists = compute_normal_histograms(normals, nbins=N_HIST_BINS) feature = np.concatenate((rgb_hists, hsv_hists, nhists)) labeled_features.append([feature, model_name]) delete_model() features_filename = 'o{}_h{}_training_set.sav'.format(ORIENTATIONS_PER_OBJECT, N_HIST_BINS) pickle.dump(labeled_features, open(SAVE_DIR + features_filename, 'wb'))
def pcl_callback(pcl_msg): # Define object array variables detected_objects_labels = [] detected_objects_centroids = {} detected_objects_list = [] # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) # TODO: Statistical Outlier Filtering outlier_filter = cloud.make_statistical_outlier_filter() # Set the number of neighboring points to analyze for any given point outlier_filter.set_mean_k(50) # Set threshold scale factor x = 0.05 outlier_filter.set_std_dev_mul_thresh(x) cloud_filtered = outlier_filter.filter() # TODO: Voxel Grid Downsampling vox = cloud_filtered.make_voxel_grid_filter() LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # TODO: PassThrough Filter # Passthrough Z passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.6 axis_max = 1.5 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # Passthrough Y passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'y' passthrough.set_filter_field_name(filter_axis) axis_min = -0.5 axis_max = 0.5 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # TODO: RANSAC Plane Segmentation seg = cloud_filtered.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) max_distance = 0.015 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() ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.02) ec.set_MinClusterSize(40) ec.set_MaxClusterSize(800) 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 = [] print "Starting coloring process" for j, indices in enumerate(cluster_indices): print "object ", j + 1 print "indices length: ", len(indices) for i, indice in enumerate(indices): color_cluster_point_list.append([ white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j]) ]) cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # TODO: Convert PCL data to ROS messages ros_cloud_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 print "Publishing data" 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) for index, pts_list in enumerate(cluster_indices): pcl_cluster = cloud_objects.extract(pts_list) # Grab the points for the cluster ros_cluster = pcl_to_ros(pcl_cluster) # Compute the associated feature vector color_hists = compute_color_histograms(ros_cluster) normals = get_normals(ros_cluster) normal_hists = compute_normal_histograms(normals) feature = np.concatenate((color_hists, normal_hists)) # Make the prediction prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Obtain centroid of detected objects points_arr = pcl_cluster.to_array() if label in detected_objects_centroids: print "Found duplicated object! Overwriting object " + label detected_objects_centroids[label] = np.mean(points_arr, axis=0)[:3] # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects_list.append(do) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects detected_objects_pub.publish(detected_objects_list) # Get object list params object_list_param = rospy.get_param('/object_list') # Get dropbox list params dropbox = rospy.get_param('/dropbox') dict_list = [] print object_list_param for i in range(0, len(object_list_param)): # Populate various ROS messages # test_scene_num test_scene_num = Int32() test_scene_num.data = int(rospy.get_param('test_scene_num')) # arm_name if object_list_param[i]['group'] == 'red': dropbox_index = 0 elif object_list_param[i]['group'] == 'green': dropbox_index = 1 arm_name = String() arm_name.data = dropbox[dropbox_index]['name'] # object_name object_name = String() object_name.data = object_list_param[i]['name'] # pick_pose pick_pose = Pose() pick_pose.orientation.x = 0 pick_pose.orientation.y = 0 pick_pose.orientation.z = 0 pick_pose.orientation.w = 0 # TODO: check if object_name is there if object_name.data in detected_objects_centroids: pick_pose.position.x = np.asscalar( detected_objects_centroids[object_name.data][0]) pick_pose.position.y = np.asscalar( detected_objects_centroids[object_name.data][1]) pick_pose.position.z = np.asscalar( detected_objects_centroids[object_name.data][2]) else: pick_pose.position.x = 0 pick_pose.position.y = 0 pick_pose.position.z = 0 # place_pose place_pose = Pose() place_pose.orientation.x = 0 place_pose.orientation.y = 0 place_pose.orientation.z = 0 place_pose.orientation.w = 0 place_pose.position.x = dropbox[dropbox_index]['position'][0] place_pose.position.y = dropbox[dropbox_index]['position'][1] place_pose.position.z = dropbox[dropbox_index]['position'][2] yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose) dict_list.append(yaml_dict) # Output location of objects to YAML yaml_filename = 'output_' + str( rospy.get_param('test_scene_num')) + '.yaml' send_to_yaml(yaml_filename, dict_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
labeled_features = [] for model_name in models: spawn_model(model_name) for i in range(50): # make fifty attempts to get a valid a point cloud then give up sample_was_good = False try_count = 0 while not sample_was_good and try_count < 10: sample_cloud = capture_sample() sample_cloud_arr = ros_to_pcl(sample_cloud).to_array() # Check for invalid clouds. if sample_cloud_arr.shape[0] == 0: print('Invalid cloud detected') try_count += 1 else: sample_was_good = True # Extract histogram features chists = compute_color_histograms(sample_cloud, using_hsv=True) normals = get_normals(sample_cloud) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) labeled_features.append([feature, model_name]) delete_model() pickle.dump(labeled_features, open('training_set.sav', 'wb'))
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data pcldata = ros_to_pcl(pcl_msg) # TODO: Statistical Outlier Filtering # Much like the previous filters, we start by creating a filter object: outlier_filter = pcldata.make_statistical_outlier_filter() # Set the number of neighboring points to analyze for any given point outlier_filter.set_mean_k(100) # Set threshold scale factor x = 1 # Any point with a mean distance larger than global (mean distance+x*std_dev) will be considered outlier outlier_filter.set_std_dev_mul_thresh(x) # Finally call the filter function for magic pcldata_s_filtered = outlier_filter.filter() # TODO: Voxel Grid Downsampling # Create a VoxelGrid filter object for our input point cloud vox = pcldata_s_filtered.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 pcldata_h_filtered = vox.filter() # TODO: PassThrough Filter # PassThrough filter # Create a PassThrough filter object. passthrough = pcldata_h_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 axis_max = 1.5 passthrough_z.set_filter_limits(axis_min, axis_max) pcldata_z_filtered = passthrough_z.filter() passthrough_y = pcldata_z_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) # TODO: RANSAC Plane Segmentation seg = pcldata_filtered.make_segmenter() # set the model to fit seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) # max distance for a point to be considered fitting the model max_distance = 0.01 seg.set_distance_threshold(max_distance) #call the segment function to obtain set of inlier indices and model coefficients inliers, coefficients = seg.segment() # TODO: Extract inliers and outliers extracted_inliers = pcldata_filtered.extract(inliers,negative=False) extracted_outliers = pcldata_filtered.extract(inliers,negative=True) # TODO: 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.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() # 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_object = pcl_to_ros(extracted_outliers) ros_cloud_table = pcl_to_ros(extracted_inliers) ros_cloud_cluster = pcl_to_ros(cluster_cloud) ros_s_filtered_cloud = pcl_to_ros(pcldata_s_filtered) ros_h_filtered_cloud = pcl_to_ros(pcldata_h_filtered) ros_l_filtered_cloud = pcl_to_ros(pcldata_filtered) # TODO: Publish ROS messages pcl_objects_pub.publish(ros_cloud_object) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cloud_cluster) pcl_s_filtered_pub.publish(ros_s_filtered_cloud) pcl_h_filtered_pub.publish(ros_h_filtered_cloud) pcl_l_filtered_pub.publish(ros_l_filtered_cloud) # to do 3 # Exercise-3 TODOs: # 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 rosdata = pcl_to_ros(pcl_cluster) # Extract histogram features # TODO: complete this step just as is covered in capture_features.py chists = compute_color_histograms(rosdata, using_hsv=True) normals = get_normals(rosdata) 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 = 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): # 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() filename = 'voxel_downsampled.pcd' pcl.save(cloud_filtered, filename) # TODO: PassThrough Filter passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.75 axis_max = 1.4 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 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 extracted_inliers = cloud_filtered.extract(inliers, negative=False) filename = 'cloud_table.pcd' pcl.save(extracted_inliers, filename) extracted_outliers = cloud_filtered.extract(inliers, negative=True) filename = 'cloud_objects.pcd' pcl.save(extracted_outliers, filename) # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ(extracted_outliers) tree = white_cloud.make_kdtree() # Create a cluster extraction object ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) # NOTE: These are poor choices of clustering parameters # Your task is to experiment and find values that work for segmenting objects. ec.set_ClusterTolerance(0.05) ec.set_MinClusterSize(100) 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() print(cluster_indices) # 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) ros_cluster_cloud = pcl_to_ros(cluster_cloud) # TODO: Convert PCL data to ROS messages ros_cloud_table = pcl_to_ros(extracted_inliers) ros_cloud_objects = pcl_to_ros(extracted_outliers) # 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 = [] detected_objects_list = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster pcl_cluster = extracted_outliers.extract(pts_list) # TODO: convert the cluster from pcl to ROS using helper function ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features # TODO: complete this step just as is covered in capture_features.py chists = compute_color_histograms(ros_cluster, using_hsv=False) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) # Compute the associated feature vector feature = np.concatenate((chists, nhists)) # Make the prediction prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) # filename = 'basic.pcd' # pcl.save(cloud, filename) # TODO: Statistical Outlier Filtering # Much like the previous filters, we start by creating a filter object: outlier_filter = cloud.make_statistical_outlier_filter() # Set the number of neighboring points to analyze for any given point outlier_filter.set_mean_k(2) # Set threshold scale factor x = .01 # 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 cloud_filtered = outlier_filter.filter() # TODO: Voxel Grid Downsampling # Create a VoxelGrid filter object for our input point cloud vox = cloud_filtered.make_voxel_grid_filter() # Experiment and find the appropriate size! LEAF_SIZE = .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() # filename = 'voxel_downsampled.pcd' # pcl.save(cloud_filtered, filename) # TODO: PassThrough Filter # Create a PassThrough filter object. passthrough = cloud_filtered.make_passthrough_filter() # Assign z-axis and range to the passthrough filter object. filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = .6 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) # Assign x-axis and range cloud_filtered = passthrough.filter() passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'x' passthrough.set_filter_field_name(filter_axis) axis_min = 0.33 axis_max = 0.87 passthrough.set_filter_limits(axis_min, axis_max) # Assign y-axis and range cloud_filtered = passthrough.filter() 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) # Finally use the filter function to obtain the resultant point cloud. cloud_filtered = passthrough.filter() # filename = 'xyz.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 max_distance = .005 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_objects = cloud_filtered.extract(inliers, negative=True) # filename = 'cloud_objects.pcd' # pcl.save(cloud_objects, filename) cloud_table = cloud_filtered.extract(inliers, negative=False) # filename = 'cloud_table.pcd' # pcl.save(cloud_table, filename) # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ(cloud_objects) tree = white_cloud.make_kdtree() ec = white_cloud.make_EuclideanClusterExtraction() # Set tolerances for distance threshold # as well as minimum and maximum cluster size (in points) ec.set_ClusterTolerance(0.01) #0.01 ec.set_MinClusterSize(400) ec.set_MaxClusterSize(7000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] #Excercise 3 denoted by <***> # Classify the clusters! (loop through each detected cluster one at a time) <*** detected_objects_labels = [] detected_objects = [] # *******> for j, indices in enumerate(cluster_indices): # TODO: Exercise 3 Loop Enclosed HERE <****************** # Grab the points for the cluster pcl_cluster = cloud_objects.extract(indices) filename = str(j)+'.pcd' pcl.save(pcl_cluster, filename) # Compute the associated feature vector # Convert cluster to ros ros_cluster = pcl_to_ros(pcl_cluster) # Extract Histogram Features chists = compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # 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[indices[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label,label_pos, j)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) # **************************************************> 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])]) # <**** rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects detected_objects_pub.publish(detected_objects) #****> # Create new cloud containing all clusters, each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # 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) # 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 vox = pcl_data.make_voxel_grid_filter() LEAF_SIZE = 0.01 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) vox_filtered = vox.filter() # TODO: PassThrough Filter passthrough = vox_filtered.make_passthrough_filter() filter_axis = 'z' axis_min = 0.6 axis_max = 1.1 passthrough.set_filter_field_name(filter_axis) passthrough.set_filter_limits(axis_min, axis_max) passthrough_filtered = passthrough.filter() # TODO: RANSAC Plane Segmentation seg = passthrough_filtered.make_segmenter() max_distance = 0.01 seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) seg.set_distance_threshold(max_distance) # TODO: Extract inliers and outliers inliers, coefficients = seg.segment() cloud_table = passthrough_filtered.extract(inliers, negative=False) cloud_objects = passthrough_filtered.extract(inliers, negative=True) # TODO: Euclidean Clustering xyz_cloud = XYZRGB_to_XYZ(cloud_objects) kd_tree = xyz_cloud.make_kdtree() ec = xyz_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.05) ec.set_MinClusterSize(100) # Search the k-d tree for clusters ec.set_SearchMethod(kd_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([ xyz_cloud[indice][0], xyz_cloud[indice][1], xyz_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_cloud_clusters = pcl_to_ros(cluster_cloud) # TODO: Publish ROS messages pcl_table_pub.publish(ros_cloud_table) pcl_objects_pub.publish(ros_cloud_objects) pcl_clusters_pub.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 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(xyz_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): global previous_detected_objects, same_detection_times # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data pcl_data = ros_to_pcl(pcl_msg) # TODO: Voxel Grid Downsampling vox = pcl_data.make_voxel_grid_filter() LEAF_SIZE = 0.004 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # TODO: PassThrough Filter passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.6 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'y' passthrough.set_filter_field_name(filter_axis) axis_min = -0.5 axis_max = 0.5 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() # Extract outliers outlier_filter = cloud_filtered.make_statistical_outlier_filter() outlier_filter.set_mean_k(50) x = 1.0 outlier_filter.set_std_dev_mul_thresh(x) cloud_filtered = outlier_filter.filter() # TODO: 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_objects = cloud_filtered.extract(inliers, negative=True) cloud_table = cloud_filtered.extract(inliers, negative=False) # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ(cloud_objects) tree = white_cloud.make_kdtree() ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.01) ec.set_MinClusterSize(100) ec.set_MaxClusterSize(50000) ec.set_SearchMethod(tree) 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) pcl_cluster_pub.publish(ros_cluster_cloud) # Exercise-3 TODOs: # Classify the clusters! (loop through each detected cluster one at a time) detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster pcl_cluster = cloud_objects.extract(pts_list) ros_cluster = pcl_to_ros(pcl_cluster) # 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 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() if previous_detected_objects == detected_objects_labels: print("same") same_detection_times += 1 else: same_detection_times = 0 if same_detection_times >= 0: print("move") try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass previous_detected_objects = detected_objects_labels
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 # Set the voxel (or leaf) size vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) # Call the filter function to obtain the resultant downsampled point cloud cloud_filtered = vox.filter() # TODO: PassThrough Filter passthrough = cloud_filtered.make_passthrough_filter() # Assign axis and range to the passthrough filter object. filter_axis = 'z' passthrough.set_filter_field_name(filter_axis) axis_min = 0.77 #to get rid of table edges #0.6 axis_max = 1.1 passthrough.set_filter_limits(axis_min, axis_max) # Finally use the filter function to obtain the resultant point cloud. cloud_filtered = passthrough.filter() # 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 extracted_inliers = cloud_filtered.extract(inliers, negative=False) cloud_table = extracted_inliers extracted_outliers = cloud_filtered.extract(inliers, negative=True) cloud_objects = extracted_outliers pcl.save(extracted_inliers, 'table.pcd') pcl.save(extracted_outliers, 'objects.pcd') # 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.04) ec.set_MinClusterSize(50) ec.set_MaxClusterSize(2500) # 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)) print "no colors-->", 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_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)) #labeled_features.append([feature, model_name]) # Format the features and labels for use with scikit learn #feature_list = [] --- feature #label_list = [] --- models # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1, -1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label, label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format( len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects)
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) # TODO: Voxel Grid Downsampling vox = cloud.make_voxel_grid_filter() LEAF_SIZE = 0.03 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() filename = 'voxel_downsampled.pcd' pcl.save(cloud_filtered, filename) # TODO: Statistical Outlier Filtering # Much like the previous filters, we start by creating a filter object: outlier_filter = cloud_filtered.make_statistical_outlier_filter() # Set the number of neighboring points to analyze for any given point outlier_filter.set_mean_k(50) # Set threshold scale factor x = 1.0 # Any point with a mean distance larger than global (mean distance+x*std_dev) will be considered outlier outlier_filter.set_std_dev_mul_thresh(x) # Finally call the filter function for magic cloud_filtered = outlier_filter.filter() # TODO: Voxel Grid Downsampling vox = cloud.make_voxel_grid_filter() LEAF_SIZE = 0.003 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() filename = 'voxel_downsampled.pcd' pcl.save(cloud_filtered, filename) # TODO: PassThrough Filter along z axis 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() filename = 'pass_through_filtered_z.pcd' pcl.save(cloud_filtered, filename) # TODO: PassThrough Filter along y axis passthrough = cloud_filtered.make_passthrough_filter() filter_axis = 'y' passthrough.set_filter_field_name(filter_axis) axis_min = -0.5 axis_max = 0.5 passthrough.set_filter_limits(axis_min, axis_max) cloud_filtered = passthrough.filter() filename = 'pass_through_filtered_y.pcd' pcl.save(cloud_filtered, filename) # TODO: RANSAC Plane Segmentation seg = cloud_filtered.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) max_distance = 0.01 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # TODO: Extract inliers and outliers extracted_inliers = cloud_filtered.extract(inliers, negative=False) filename = 'extracted_inliers.pcd' pcl.save(extracted_inliers, filename) extracted_outliers = cloud_filtered.extract(inliers, negative=True) filename = 'extracted_outliers.pcd' pcl.save(extracted_outliers, filename) # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ( extracted_outliers) # Apply function to convert $ tree = white_cloud.make_kdtree() ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.02) ec.set_MinClusterSize(100) ec.set_MaxClusterSize(50000) 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) filename = 'cluster_cloud.pcd' pcl.save(cluster_cloud, filename) # TODO: Convert PCL data to ROS messages ros_cloud_objects = pcl_to_ros(extracted_outliers) ros_cloud_table = pcl_to_ros(extracted_inliers) ros_cloud_cluster = 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_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 = extracted_outliers.extract(pts_list) # TODO: convert the cluster from pcl to ROS using helper function 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 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)) # Get the pick list labels object_list_param = rospy.get_param('/object_list') pick_list_labels = [] for i in range(len(object_list_param)): pick_list_labels.append(object_list_param[i]['name']) # 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() #compare if the set of detected object label is same as the set of pick list objects if set(detected_objects_labels) == set(pick_list_labels): print("detected object matched listed objects") try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) # TODO: 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 cloud = ros_to_pcl(pcl_msg) # TODO: Statistical Outlier Filtering outlier_filter = cloud.make_statistical_outlier_filter() outlier_filter.set_mean_k(5) x = 1.0 outlier_filter.set_std_dev_mul_thresh(x) cloud_filtered = outlier_filter.filter() # TODO: Voxel Grid Downsampling # Create a VoxelGrid filter object for our input point cloud vox = cloud_filtered.make_voxel_grid_filter() # Choose a voxel (also known as leaf) size # 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() # TODO: PassThrough Filter # Create a PassThrough filter object. 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.6255 axis_max = 2 passthroughz.set_filter_limits(axis_min, axis_max) cloud_filtered = passthroughz.filter() passthroughy = cloud_filtered.make_passthrough_filter() filter_axis = 'y' passthroughy.set_filter_field_name(filter_axis) axis_min = -0.4 axis_max = 0.45 passthroughy.set_filter_limits(axis_min, axis_max) # Finally use the filter function to obtain the resultant point cloud. cloud_filtered = passthroughy.filter() # TODO: RANSAC Plane Segmentation # Create the segmentation object seg = cloud_filtered.make_segmenter() # Set the model you wish to fit seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) # Max distance for a point to be considered fitting the model # Experiment with different values for max_distance # for segmenting the table max_distance = .0000001 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # TODO: Extract inliers and outliers extracted_inliers = cloud_filtered.extract(inliers, negative=False) extracted_outliers = cloud_filtered.extract(inliers, negative=True) # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ(extracted_outliers) tree = white_cloud.make_kdtree() # 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.005) ec.set_MinClusterSize(25) ec.set_MaxClusterSize(6000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately #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(extracted_outliers) ros_cloud_table = pcl_to_ros(extracted_inliers) ros_cluster_cloud = pcl_to_ros(cluster_cloud) # TODO: Publish ROS messages pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cluster_cloud) # Exercise-3 TODOs: detected_objects_labels = [] detected_objects = [] # Classify the clusters! (loop through each detected cluster one at a time) for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster pcl_cluster = extracted_outliers.extract(pts_list) ros_cluster = pcl_to_ros(pcl_cluster) # Compute the associated feature vector # Extract histogram features chists = compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # Make the prediction 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() try: pr2_mover(detected_objects) except rospy.ROSInterruptException: pass
for model_name in models: spawn_model(model_name) for i in range(5): # make five attempts to get a valid a point cloud then give up sample_was_good = False try_count = 0 while not sample_was_good and try_count < 5: sample_cloud = capture_sample() sample_cloud_arr = ros_to_pcl(sample_cloud).to_array() # Check for invalid clouds. if sample_cloud_arr.shape[0] == 0: print('Invalid cloud detected') try_count += 1 else: sample_was_good = True # Extract histogram features chists = compute_color_histograms(sample_cloud, using_hsv=False) normals = get_normals(sample_cloud) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) labeled_features.append([feature, model_name]) delete_model() pickle.dump(labeled_features, open('training_set.sav', 'wb'))
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # Convert ROS msg to PCL data pt_cloud = ros_to_pcl(pcl_msg) # Statistical Outlier Filtering out_filt = pt_cloud.make_statistical_outlier_filter(); out_filt.set_mean_k(20); # Number of neighboring points to analyze out_filt.set_std_dev_mul_thresh(0.5); # Standard deviation threshold cloud_filtered = out_filt.filter(); # Voxel Grid Downsampling LEAF_SIZE = 0.01; # Voxel size [m] vox = cloud_filtered.make_voxel_grid_filter(); vox.set_leaf_size(LEAF_SIZE,LEAF_SIZE,LEAF_SIZE); cloud_downsampled = vox.filter(); # PassThrough Filter # First, do this on the Z to remove the ground Z_MIN = 0.25; # Minimum Z value [m] Z_MAX = 2; # Maximum Z value [m] passthru = cloud_downsampled.make_passthrough_filter(); passthru.set_filter_field_name('z'); passthru.set_filter_limits(Z_MIN,Z_MAX); cloud_passthru = passthru.filter(); # Then, do this on the Y to remove boxes on the side X_MIN = -0.5; # Minimum X value [m] X_MAX = 0.5; # Maximum X value [m] passthru = cloud_passthru.make_passthrough_filter(); passthru.set_filter_field_name('y'); passthru.set_filter_limits(X_MIN,X_MAX); cloud_passthru = passthru.filter(); # RANSAC Plane Segmentation DIST_THRESH = 0.03; # Distance threshold for plane fitting [m] seg = cloud_passthru.make_segmenter(); seg.set_model_type(pcl.SACMODEL_PLANE); seg.set_method_type(pcl.SAC_RANSAC); seg.set_distance_threshold(DIST_THRESH); inliers, coefs = seg.segment(); # Extract inliers and outliers cloud_inliers = cloud_passthru.extract(inliers,negative=False); cloud_outliers = cloud_passthru.extract(inliers,negative=True); # Euclidean Clustering cloud_xyz = XYZRGB_to_XYZ(cloud_outliers); tree = cloud_xyz.make_kdtree(); ec = cloud_xyz.make_EuclideanClusterExtraction(); ec.set_ClusterTolerance(0.05); # Distance tolerance [m] ec.set_MinClusterSize(100); # Minimum cluster size ec.set_MaxClusterSize(10000); # Maximum cluster size ec.set_SearchMethod(tree); cluster_indices = ec.Extract(); # Create Cluster-Mask Point Cloud to visualize each cluster separately cluster_color = get_color_list(len(cluster_indices)); color_cluster_point_list = []; for j, indices in enumerate(cluster_indices): for i, index in enumerate(indices): color_cluster_point_list.append([ cloud_xyz[index][0], cloud_xyz[index][1], cloud_xyz[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 cluster_msg = pcl_to_ros(cluster_cloud); # Publish ROS messages pcl_cluster_pub.publish(cluster_msg); # Exercise-3 TODOs: # 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_outliers.extract(pts_list) ros_cluster = pcl_to_ros(pcl_cluster) # Compute the associated feature vector feat_cloud = pcl_cluster.to_array() 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 = classifier.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(cloud_xyz[pts_list[0]]) label_pos[2] += 0.4 object_markers_pub.publish(make_label(label,label_pos,idx)) # Add the detected object to the list of detected objects. det_obj = DetectedObject() det_obj.label = label det_obj.cloud = ros_cluster detected_objects.append(det_obj) # 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