stepsegmenter = cloud_treatment.StepSegmentationCell("Step_Seg", z_step_1=0.0, z_step_2=0.30, z_step_3=0.61, z_step_4=0.91, z_step_5=1.21, z_step_6=1.52, z_step_7=1.83, positive_threshold=0.01, negative_threshold=0.0) principalcomponent = cloud_treatment.PrincipalComponentExtractionCell( "Principal_component", length_rectangles=0.8128, width_rectangles=0.127) colorize = ecto_pcl.ColorizeClusters("colorize") viewer = cloud_treatment.CloudViewerCell("Viewer_ecto", window_name="PCD Viewer") graph = [ cloud_sub["output"] >> msg2cloud[:], msg2cloud[:] >> passthrough3d["input"], passthrough3d["output"] >> stepsegmenter["input"], #principalcomponent["centroids"] >> viewer["VIPoints"], #principalcomponent["rectangles"] >> viewer["rectangles"], stepsegmenter["clusters"] >> colorize["clusters"], passthrough3d["output"] >> colorize["input"], stepsegmenter["clusters"] >> principalcomponent["clusters"], passthrough3d["output"] >> principalcomponent["input"], #colorize[:] >> viewer["input"]
def __init__(self): ecto_ros.init(sys.argv, "ecto_pcl_demo") # Create the argument parser parser = argparse.ArgumentParser( description='Ecto Euclidean Clustering') # Use the parser to create scheduler options scheduler_options(parser) options = parser.parse_args() # Create the overall plasm plasm = ecto.Plasm() # A VoxelGrid cell voxel_grid = ecto_pcl.VoxelGrid("voxel_grid", leaf_size=0.01) # A Cropper cell cropper = ecto_pcl.Cropper("cropper", x_min=-0.35, x_max=0.35, y_min=-0.35, y_max=0.35, z_min=0.3, z_max=1.5) # A EuclideanClusterExtraction cell extract_clusters = ecto_pcl.EuclideanClusterExtraction( "extract_clusters", min_cluster_size=50, cluster_tolerance=0.02) # A cell to filter NaN values from the point cloud nan_filter = ecto_pcl.PassThrough("nan_removal") # A cell to colorize the clusters colorize = ecto_pcl.ColorizeClusters("colorize", max_clusters=100) # Create the subscriber cell with the appropriate point cloud topic cloud_sub = ecto_sensor_msgs.Subscriber_PointCloud2( "cloud_sub", topic_name='camera/depth_registered/points') # A cell to convert a point cloud ROS message to an Ecto/PCL point cloud object msg2cloud = ecto_pcl_ros.Message2PointCloud("msg2cloud", format=ecto_pcl.XYZRGB) # A cell to convert a Ecto/PCL point cloud back to a point cloud message cloud2msg = ecto_pcl_ros.PointCloud2Message("cloud2msg") # A cell to convert a point cloud ROS message to an Ecto/PCL point cloud object cloud_pub = ecto_sensor_msgs.Publisher_PointCloud2( "cloud_pub", topic_name='ecto_pcl/cloud_filtered') # Create the plasm by connecting the cells plasm.connect( cloud_sub[:] >> msg2cloud[:], msg2cloud[:] >> nan_filter[:], nan_filter[:] >> voxel_grid[:], voxel_grid[:] >> cropper[:], cropper[:] >> extract_clusters[:], extract_clusters[:] >> colorize["clusters"], cropper[:] >> colorize["input"], colorize[:] >> cloud2msg[:], cloud2msg[:] >> cloud_pub[:]) # Run the plasm run_plasm(options, plasm, locals=vars())