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())