def __init__(self):
ecto_ros.init(sys.argv,"ecto_pcl_demo")
parser = argparse.ArgumentParser(description='My awesome program thing.')
scheduler_options(parser)
options = parser.parse_args()
plasm = ecto.Plasm()
voxel_grid = VoxelGrid("voxel_grid", leaf_size=0.01)
cropper = Cropper("cropper", x_min=-0.25, x_max=0.25, y_min=-0.25, y_max=0.25, z_min=0.0, z_max=1.0)
ror = RadiusOutlierRemoval("ror", min_neighbors=1, search_radius=1.0)
cloud_sub = ecto_sensor_msgs.Subscriber_PointCloud2("cloud_sub", topic_name='/camera/depth_registered/points')
msg2cloud = ecto_pcl_ros.Message2PointCloud("msg2cloud", format=ecto_pcl.XYZRGB)
cloud2msg = ecto_pcl_ros.PointCloud2Message("cloud2msg")
cloud_pub = ecto_sensor_msgs.Publisher_PointCloud2("cloud_pub",topic_name='/ecto_pcl/cloud_filtered')
plasm.connect(cloud_sub[:] >> msg2cloud[:],
msg2cloud[:] >> voxel_grid[:],
voxel_grid[:] >> ror[:],
ror[:] >> cloud2msg[:],
cloud2msg[:] >> cloud_pub[:])
run_plasm(options, plasm, locals=vars())
def connect_ros_point_cloud(self):
self.depthTo3d = calib.DepthTo3d('Depth ~> 3D')
self.erode = imgproc.Erode('Mask Erosion', kernel=3) # -> 7x7
# this is for SXGA mode scale handling.
self.rescale_depth = RescaledRegisteredDepth('Depth scaling')
self.point_cloud_converter = MatToPointCloudXYZRGB('To Point Cloud')
self.to_ecto_pcl = ecto_pcl.PointCloudT2PointCloud(
'converter', format=ecto_pcl.XYZRGB)
self.cloud2msg = ecto_pcl_ros.PointCloud2Message("cloud2msg")
self.cloud_pub = ecto_sensor_msgs.Publisher_PointCloud2(
"cloud_pub", topic_name='/ecto_pcl/sample_output')
self.graph += [
self.observation_renderer['depth', 'image',
'mask'] >> self.rescale_depth['depth', 'image', 'mask'],
self.observation_renderer['K'] >> self.rescale_depth['K'],
self.rescale_depth['K'] >> self.depthTo3d['K'],
self.rescale_depth['depth'] >> self.depthTo3d['depth'],
self.depthTo3d['points3d'] >> self.point_cloud_converter['points'],
self.observation_renderer['image'] >> self.point_cloud_converter['image'],
self.rescale_depth['mask'] >> self.erode['image'],
self.erode['image'] >> self.point_cloud_converter['mask'],
self.point_cloud_converter['point_cloud'] >> self.to_ecto_pcl['input'],
self.to_ecto_pcl['output'] >> self.cloud2msg['input'],
self.cloud2msg['output'] >> self.cloud_pub['input'],
]
def __init__(self):
ecto_ros.init(sys.argv, "ecto_pcl_demo")
parser = argparse.ArgumentParser(
description='My awesome program thing.')
scheduler_options(parser)
options = parser.parse_args()
plasm = ecto.Plasm()
voxel_grid = VoxelGrid("voxel_grid", leaf_size=0.01)
cropper = Cropper("cropper",
x_min=-0.25,
x_max=0.25,
y_min=-0.25,
y_max=0.25,
z_min=0.0,
z_max=1.0)
convex_hull = ConvexHull("convex_hull")
ror = RadiusOutlierRemoval("ror", min_neighbors=1, search_radius=1.0)
sor = StatisticalOutlierRemoval("sor", mean_k=1)
extract_indices = ExtractIndices("extract_indices", negative=False)
extract_clusters = EuclideanClusterExtraction("extract_clusters",
min_cluster_size=50,
cluster_tolerance=0.005)
extract_largest_cluster = ExtractLargestCluster(
"extract_largest_cluster")
cloud_sub = ecto_sensor_msgs.Subscriber_PointCloud2(
"cloud_sub", topic_name='/camera/depth_registered/points')
msg2cloud = ecto_pcl_ros.Message2PointCloud("msg2cloud",
format=ecto_pcl.XYZRGB)
cloud2msg = ecto_pcl_ros.PointCloud2Message("cloud2msg")
cloud_pub = ecto_sensor_msgs.Publisher_PointCloud2(
"cloud_pub", topic_name='/ecto_pcl/sample_output')
plasm.connect(
cloud_sub[:] >> msg2cloud[:], msg2cloud[:] >> extract_clusters[:],
extract_clusters[:] >> extract_largest_cluster["clusters"],
msg2cloud[:] >> extract_largest_cluster["input"],
extract_largest_cluster[:] >> cloud2msg[:],
cloud2msg[:] >> cloud_pub[:])
# plasm.connect(cloud_sub[:] >> msg2cloud[:],
# msg2cloud[:] >> voxel_grid[:],
# msg2cloud[:] >> extract_indices["input"],
# voxel_grid[:] >> cropper[:],
# cropper[:] >> extract_clusters[:],
# extract_indices[:] >> extract_clusters[:],
# extract_clusters[:] >> extract_largest_cluster["clusters"],
# cropper[:] >> extract_largest_cluster["input"],
# extract_largest_cluster[:] >> cloud2msg[:],
# cloud2msg[:] >> cloud_pub[:])
run_plasm(options, plasm, locals=vars())
def __init__(self):
ecto_ros.init(sys.argv, "ecto_pcl_demo")
parser = argparse.ArgumentParser(description='Ecto Largest Cluster')
scheduler_options(parser)
options = parser.parse_args()
plasm = ecto.Plasm()
voxel_grid = VoxelGrid("voxel_grid", leaf_size=0.015)
cropper = Cropper("cropper",
x_min=-0.25,
x_max=0.25,
y_min=-0.25,
y_max=0.25,
z_min=0.0,
z_max=1.0)
extract_clusters = EuclideanClusterExtraction("extract_clusters",
min_cluster_size=50,
cluster_tolerance=0.02)
extract_largest_cluster = ExtractLargestCluster(
"extract_largest_cluster")
nan_filter = PassThrough("nan_removal")
colorize = ColorizeClusters("colorize", max_clusters=100)
extract_indices = ExtractIndices("extract_indices", negative=False)
cloud_sub = ecto_sensor_msgs.Subscriber_PointCloud2(
"cloud_sub", topic_name='/camera/depth_registered/points')
msg2cloud = ecto_pcl_ros.Message2PointCloud("msg2cloud",
format=ecto_pcl.XYZRGB)
cloud2msg = ecto_pcl_ros.PointCloud2Message("cloud2msg")
cloud_pub = ecto_sensor_msgs.Publisher_PointCloud2(
"cloud_pub", topic_name='/ecto_pcl/cloud_filtered')
output = ecto_pcl.XYZRGB
# plasm.connect(cloud_sub[:] >> msg2cloud[:],
# msg2cloud[:] >> nan_filter[:],
# nan_filter[:] >> voxel_grid[:],
# voxel_grid[:] >> extract_clusters[:],
# extract_clusters[:] >> extract_largest_cluster["clusters"],
# voxel_grid[:] >> extract_largest_cluster["input"],
# extract_largest_cluster[:] >> cloud2msg[:],
# cloud2msg[:] >> cloud_pub[:])
#
# run_plasm(options, plasm, locals=vars())
plasm.connect(
cloud_sub[:] >> msg2cloud[:], msg2cloud[:] >> nan_filter[:],
nan_filter[:] >> voxel_grid[:],
voxel_grid[:] >> extract_clusters[:],
extract_clusters[:] >> extract_largest_cluster["clusters"],
voxel_grid[:] >> extract_largest_cluster["input"],
extract_largest_cluster["output"] >> 'out')
run_plasm(options, plasm, locals=output)
def __init__(self):
ecto_ros.init(sys.argv, "ecto_pcl_demo")
parser = argparse.ArgumentParser(
description='My awesome program thing.')
scheduler_options(parser)
options = parser.parse_args()
plasm = ecto.Plasm()
voxel_grid = VoxelGrid("voxel_grid", leaf_size=0.01)
passthru_z = PassThrough("passthru_z",
filter_field_name="z",
filter_limit_min=0.0,
filter_limit_max=1.0)
passthru_x = PassThrough("passthru_x",
filter_field_name="x",
filter_limit_min=-0.25,
filter_limit_max=0.25)
passthru_y = PassThrough("passthru_y",
filter_field_name="y",
filter_limit_min=-0.25,
filter_limit_max=0.25)
cropper = Cropper("cropper",
x_min=-0.25,
x_max=0.25,
y_min=-0.25,
y_max=0.25,
z_min=0.0,
z_max=1.0)
viewer = CloudViewer("viewer", window_name="Clouds!")
cloud_sub = ecto_sensor_msgs.Subscriber_PointCloud2(
"cloud_sub", topic_name='/camera/depth_registered/points')
msg2cloud = ecto_pcl_ros.Message2PointCloud("msg2cloud",
format=ecto_pcl.XYZRGB)
cloud2msg = ecto_pcl_ros.PointCloud2Message("cloud2msg")
cloud_pub = ecto_sensor_msgs.Publisher_PointCloud2(
"cloud_pub", topic_name='/ecto_pcl/sample_output')
plasm.connect(
cloud_sub[:] >> msg2cloud[:], msg2cloud[:] >> voxel_grid[:],
voxel_grid[:] >> passthru_z[:], passthru_z[:] >> passthru_x[:],
passthru_x[:] >> passthru_y[:], passthru_y[:] >> cloud2msg[:],
cloud2msg[:] >> cloud_pub[:])
# plasm.connect(cloud_sub[:] >> msg2cloud[:],
# msg2cloud[:] >> voxel_grid[:],
# voxel_grid[:] >> cropper[:],
# cropper[:] >> cloud2msg[:],
# cloud2msg[:] >> cloud_pub[:])
run_plasm(options, plasm, locals=vars())
def __init__(self):
ecto_ros.init(sys.argv, "ecto_pcl_demo")
parser = argparse.ArgumentParser(
description='Ecto Euclidean Clustering')
scheduler_options(parser)
options = parser.parse_args()
plasm = ecto.Plasm()
voxel_grid = VoxelGrid("voxel_grid", leaf_size=0.015)
cropper = Cropper("cropper",
x_min=-0.25,
x_max=0.25,
y_min=-0.25,
y_max=0.25,
z_min=0.3,
z_max=1.0)
passthru_z = PassThrough("passthru_z",
filter_field_name="z",
filter_limit_min=0.3,
filter_limit_max=1.0)
passthru_x = PassThrough("passthru_x",
filter_field_name="x",
filter_limit_min=-0.25,
filter_limit_max=0.25)
passthru_y = PassThrough("passthru_y",
filter_field_name="y",
filter_limit_min=-0.25,
filter_limit_max=0.25)
extract_clusters = EuclideanClusterExtraction("extract_clusters",
min_cluster_size=50,
cluster_tolerance=0.02)
nan_filter = PassThrough("nan_removal")
colorize = ColorizeClusters("colorize", max_clusters=100)
cloud_sub = ecto_sensor_msgs.Subscriber_PointCloud2(
"cloud_sub", topic_name='/camera/depth_registered/points')
msg2cloud = ecto_pcl_ros.Message2PointCloud("msg2cloud",
format=ecto_pcl.XYZRGB)
cloud2msg = ecto_pcl_ros.PointCloud2Message("cloud2msg")
cloud_pub = ecto_sensor_msgs.Publisher_PointCloud2(
"cloud_pub", topic_name='/ecto_pcl/cloud_filtered')
plasm.connect(
cloud_sub[:] >> msg2cloud[:], msg2cloud[:] >> nan_filter[:],
nan_filter[:] >> voxel_grid[:], voxel_grid[:] >> cropper[:],
cropper["output"] >> extract_clusters["input"],
extract_clusters[:] >> colorize["clusters"],
cropper[:] >> colorize["input"], colorize[:] >> cloud2msg[:],
cloud2msg[:] >> cloud_pub[:])
run_plasm(options, plasm, locals=vars())
def __init__(self):
ecto_ros.init(sys.argv, "ecto_pcl_demo")
# Create the argument parser
parser = argparse.ArgumentParser(description='Ecto PCL Cropper Demo')
# Use the parser to create scheduler options
scheduler_options(parser)
options = parser.parse_args()
# Create the overall plasm
plasm = ecto.Plasm()
# Create the Cropper cell with the desired dimensions (in meters)
cropper = ecto_pcl.Cropper("cropper",
x_min=-0.25,
x_max=0.25,
y_min=-0.25,
y_max=0.25,
z_min=0.0,
z_max=1.0)
# 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 point cloud ROS message to an Ecto/PCL point cloud object
cloud2msg = ecto_pcl_ros.PointCloud2Message("cloud2msg")
# A publisher cell with the desired topic name
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[:] >> cropper[:],
cropper[:] >> cloud2msg[:], cloud2msg[:] >> cloud_pub[:])
# Run the plasm
run_plasm(options, plasm, locals=vars())
def __init__(self):
ecto_ros.init(sys.argv,"ecto_pcl_demo")
# Create the argument parser
parser = argparse.ArgumentParser(description='Ecto PCL Demo')
# Use the parser to create scheduler options
scheduler_options(parser)
options = parser.parse_args()
# Create the overall plasm
plasm = ecto.Plasm()
# Create three PassThrough cells along the x, y and z dimensions
passthru_z = ecto_pcl.PassThrough("passthru_z", filter_field_name="z", filter_limit_min=0.0, filter_limit_max=1.0)
passthru_x = ecto_pcl.PassThrough("passthru_x", filter_field_name="x", filter_limit_min=-0.25, filter_limit_max=0.25)
passthru_y = ecto_pcl.PassThrough("passthru_y", filter_field_name="y", filter_limit_min=-0.25, filter_limit_max=0.25)
# 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 point cloud ROS message to an Ecto/PCL point cloud object
cloud2msg = ecto_pcl_ros.PointCloud2Message("cloud2msg")
# A publisher cell with the desired topic name
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[:] >> passthru_z[:],
passthru_z[:] >> passthru_x[:],
passthru_x[:] >> passthru_y[:],
passthru_y[:] >> cloud2msg[:],
cloud2msg[:] >> cloud_pub[:])
# Run the plasm
run_plasm(options, plasm, locals=vars())
default='ladder_test_5.pcd',
help='The pcdfile to input')
options = parser.parse_args()
pcdfilename = options.pcdfile
plasm = ecto.Plasm()
reader = cloud_treatment.PCDReaderCell("Reader_ecto", filename=pcdfilename)
cloud_sub = ecto_sensor_msgs.Subscriber_PointCloud2(
"cloud_sub", topic_name='/worldmodel_main/pointcloud_vis')
msg2cloud = ecto_pcl_ros.Message2PointCloud("msg2cloud",
format=ecto_pcl.XYZRGB)
cloud2msg_main = ecto_pcl_ros.PointCloud2Message("cloud2msg_main")
cloud2msg_seg = ecto_pcl_ros.PointCloud2Message("cloud2msg_seg")
cloud_pub_main = ecto_sensor_msgs.Publisher_PointCloud2(
"cloud_pub_main", topic_name='/ecto/main_cloud')
cloud_pub_seg = ecto_sensor_msgs.Publisher_PointCloud2(
"cloud_pub_seg", topic_name='/ecto/seg_cloud')
passthrough3d = cloud_treatment.PassThrough3DCell("passthrough3D",
x_min=-1.0,
x_max=2.0,
y_min=-1.9,
y_max=-0.5,
z_min=-1.0,
z_max=5.0)
stepsegmenter = cloud_treatment.StepSegmentationCell("Step_Seg",
z_step_1=0.0,
z_step_2=0.30,
z_step_3=0.61,
#!/usr/bin/env python
"""
This sample shows how to interact with ROS cloud subscribers and publishers.
"""
import sys, ecto, ecto_pcl, ecto_ros, ecto_pcl_ros, ecto_ros.ecto_sensor_msgs as ecto_sensor_msgs
plasm = ecto.Plasm()
cloud_sub = ecto_sensor_msgs.Subscriber_PointCloud2("cloud_sub", topic_name='/camera/depth_registered/points')
msg2cloud = ecto_pcl_ros.Message2PointCloud("msg2cloud", format=ecto_pcl.XYZRGB)
cloud2msg = ecto_pcl_ros.PointCloud2Message("cloud2msg")
cloud_pub = ecto_sensor_msgs.Publisher_PointCloud2("cloud_pub",topic_name='/ecto_pcl/sample_output')
plasm.connect(cloud_sub['output'] >> msg2cloud[:],
msg2cloud[:] >> cloud2msg[:],
cloud2msg[:] >> cloud_pub[:])
if __name__ == "__main__":
from ecto.opts import doit
ecto_ros.init(sys.argv,"ecto_pcl_rossample")
ecto.view_plasm(plasm)
doit(plasm, description='Read a pcd through ROS.')
def __init__(self):
ecto_ros.init(sys.argv,"ecto_pcl_demo")
# Create the argument parser
parser = argparse.ArgumentParser(description='Ecto Tabletop Segmentation')
# Use the parser to create scheduler options
scheduler_options(parser)
options = parser.parse_args()
# Create the overall plasm
plasm = ecto.Plasm()
cloud_sub = ecto_sensor_msgs.Subscriber_PointCloud2("cloud_sub", topic_name='/camera/depth_registered/points')
msg2cloud = ecto_pcl_ros.Message2PointCloud("msg2cloud", format=ecto_pcl.XYZRGB)
cloud2msg = ecto_pcl_ros.PointCloud2Message("cloud2msg")
cloud_pub = ecto_sensor_msgs.Publisher_PointCloud2("cloud_pub",topic_name='ecto_pcl/cloud_filtered')
voxel_grid = VoxelGrid("voxel_grid", leaf_size=0.01)
graph = [cloud_sub["output"] >> msg2cloud[:],
msg2cloud[:] >> voxel_grid[:]
]
# estimate normals, segment and find convex hull
normals = NormalEstimation("normals", k_search=0, radius_search=0.02)
planar_segmentation = SACSegmentationFromNormals("planar_segmentation",
model_type=SACMODEL_NORMAL_PLANE,
eps_angle=0.09, distance_threshold=0.1)
project_inliers = ProjectInliers("project_inliers", model_type=SACMODEL_NORMAL_PLANE)
nan_filter = PassThrough('nan_removal')
convex_hull = ConvexHull("convex_hull")
graph += [voxel_grid[:] >> normals[:],
voxel_grid[:] >> planar_segmentation["input"],
normals[:] >> planar_segmentation["normals"],
voxel_grid[:] >> project_inliers["input"],
planar_segmentation["model"] >> project_inliers["model"],
project_inliers[:] >> nan_filter[:],
nan_filter[:] >> convex_hull[:]
]
# extract stuff on table from original high-res cloud and show in viewer
extract_stuff = ExtractPolygonalPrismData("extract_stuff", height_min=0.01, height_max=0.2)
extract_indices = ExtractIndices("extract_indices", negative=False)
viewer = CloudViewer("viewer", window_name="Clouds!")
graph += [msg2cloud[:] >> extract_stuff["input"],
convex_hull[:] >> extract_stuff["planar_hull"],
extract_stuff[:] >> extract_indices["indices"],
msg2cloud[:] >> extract_indices["input"],
extract_indices[:] >> cloud2msg[:],
cloud2msg[:] >> cloud_pub[:]
]
plasm.connect(graph)
# Run the plasm
run_plasm(options, plasm, locals=vars())
def configure(self, p, _i, _o):
#self._recognized_object_array = Publisher_RecognizedObjectArray(topic_name=p.recognized_object_array_topic, latched=p.latched)
self._clusters_pointcloud = ecto_sensor_msgs.Publisher_PointCloud2(
topic_name=p.cluster_pointcloud_topic, latched=p.latched)
def __init__(self):
ecto_ros.init(sys.argv,"ecto_tracker")
parser = argparse.ArgumentParser(description='Ecto Tracker.')
scheduler_options(parser)
options = parser.parse_args()
plasm = ecto.Plasm()
voxel_grid = VoxelGrid("voxel_grid", leaf_size=0.01)
passthru_z = PassThrough("passthru_z", filter_field_name="z", filter_limit_min=0.2, filter_limit_max=1.0)
passthru_x = PassThrough("passthru_x", filter_field_name="x", filter_limit_min=-0.5, filter_limit_max=0.5)
passthru_y = PassThrough("passthru_y", filter_field_name="y", filter_limit_min=-0.5, filter_limit_max=0.5)
nan_filter = PassThrough("nan_removal")
cropper = Cropper("cropper", x_min=-0.5, x_max=0.5, y_min=-0.5, y_max=0.5, z_min=0.0, z_max=1.0)
#convex_hull = ConvexHull("convex_hull")
#
extract_stuff = ExtractPolygonalPrismData("extract_stuff", height_min=0.01, height_max=0.2)
extract_indices = ExtractIndices("extract_indices", negative=False)
extract_clusters = EuclideanClusterExtraction("extract_clusters", min_cluster_size=50, cluster_tolerance=0.005)
extract_largest_cluster = ExtractLargestCluster("extract_largest_cluster")
colorize = ColorizeClusters("colorize", max_clusters=3)
merge = MergeClouds("merge")
viewer = CloudViewer("viewer", window_name="Clouds!")
#
cloud_sub = ecto_sensor_msgs.Subscriber_PointCloud2("cloud_sub", topic_name='/camera/depth_registered/points')
msg2cloud = ecto_pcl_ros.Message2PointCloud("msg2cloud", format=ecto_pcl.XYZRGB)
cloud2msg = ecto_pcl_ros.PointCloud2Message("cloud2msg")
cloud_pub = ecto_sensor_msgs.Publisher_PointCloud2("cloud_pub",topic_name='/ecto_pcl/sample_output')
# plasm.connect(cloud_sub["output"] >> msg2cloud[:],
# msg2cloud[:] >> voxel_grid[:],
## voxel_grid[:] >> extract_indices[:],
# voxel_grid[:] >> cloud2msg[:],
# cloud2msg[:] >> cloud_pub[:],)
#
## plasm += [voxel_grid['output'] >> extract_clusters[:],
## extract_clusters[:] >> colorize["clusters"],
## cloud_generator[:] >> merge["input"],
## colorize[:] >> merge["input2"],
## colorize[:] >> viewer[:]
## ]
#
#
plasm.connect(cloud_sub[:] >> msg2cloud[:],
msg2cloud[:] >> voxel_grid[:],
voxel_grid[:] >> cropper[:],
cropper[:] >> extract_clusters[:],
msg2cloud[:] >> extract_indices["input"],
extract_clusters[:] >> colorize["clusters"],
extract_indices[:] >> colorize["input"],
msg2cloud[:] >> merge["input"],
colorize[:] >> merge["input2"],
merge[:] >> cloud2msg[:],
cloud2msg[:] >> cloud_pub[:],)
# plasm.connect(cloud_sub["output"] >> msg2cloud[:],
# msg2cloud[:] >> voxel_grid[:],
# #voxel_grid[:] >> nan_filter[:],
# #nan_filter[:] >> extract_indices["indices"],
# voxel_grid[:] >> extract_indices["indices"],
# msg2cloud[:] >> extract_indices["input"],)
# #extract_indices[:] >> extract_clusters[:],)
## extract_clusters[:] >> colorize["clusters"],
## extract_indices[:] >> colorize["input"],
## msg2cloud[:] >> merge["input"],
## colorize[:] >> merge["input2"],
## colorize[:] >> viewer[:],)
# plasm.connect(cloud_sub["output"] >> msg2cloud[:],
# msg2cloud[:] >> voxel_grid["input"],
# voxel_grid["output"] >> passthru_z["input"],
# passthru_z["output"] >> passthru_x["input"],
# passthru_x["output"] >> passthru_y["input"],
# passthru_y["output"] >> extract_indices["input"],
# passthru_y["output"] >> extract_clusters["input"],
# extract_clusters[:] >> colorize["clusters"],
# extract_indices[:] >> colorize["input"],
# msg2cloud[:] >> merge["input"],
# colorize[:] >> merge["input2"],
# colorize[:] >> viewer[:],)
#
# #passthru_y["output"] >> cloud2msg[:],
# #cloud2msg[:] >> cloud_pub[:],)
run_plasm(options, plasm, locals=vars())
#sched = ecto.Scheduler(plasm)
#sched.execute_async()
time.sleep(10)
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())
