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())
parser.add_argument('-p',
'--pcdfile',
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",
def create_capture_plasm(bag_name, angle_thresh, segmentation_cell, n_desired,
sensor, res=SXGA_RES, fps=FPS_30,
orb_matches=False,
preview=False, use_turn_table=True):
'''
Creates a plasm that will capture openni data into a bag, using a dot pattern to sparsify views.
@param bag_name: A filename for the bag, will write to this file.
@param angle_thresh: The angle threshhold in radians to sparsify the views with.
'''
graph = []
# Create ros node, subscribing to openni topics
argv = sys.argv[:]
ecto_ros.strip_ros_args(argv)
ecto_ros.init(argv, "object_capture_msd_pcl", False)
cloud_sub = ecto_ros.ecto_sensor_msgs.Subscriber_PointCloud2("cloud_sub", topic_name='/camera/depth_registered/points')
# openni&freenect requires '/camera/rgb/image_rect_color', openni2 just need '/camera/rgb/image_raw'
if sensor in ['kinect']:
rgb_image_sub = ecto_ros.ecto_sensor_msgs.Subscriber_Image("rgb_image_sub", topic_name='/camera/rgb/image_color')
elif sensor in ['xtion']:
rgb_image_sub = ecto_ros.ecto_sensor_msgs.Subscriber_Image("rgb_image_sub", topic_name='/camera/rgb/image_raw')
depth_image_sub = ecto_ros.ecto_sensor_msgs.Subscriber_Image("depth_image_sub", topic_name='/camera/depth_registered/image_raw')
rgb_camera_info_sub = ecto_ros.ecto_sensor_msgs.Subscriber_CameraInfo("rgb_camera_info_sub", topic_name='/camera/rgb/camera_info')
depth_camera_info_sub = ecto_ros.ecto_sensor_msgs.Subscriber_CameraInfo("depth_camera_info_sub", topic_name='/camera/depth_registered/camera_info')
# Converte ros topics to cv types
msg2cloud = ecto_pcl_ros.Message2PointCloud("msg2cloud", format=XYZRGB)
image2cv = ecto_ros.Image2Mat()
depth2cv = ecto_ros.Image2Mat()
rgb_info2cv = ecto_ros.CameraInfo2Cv()
depth_info2cv = ecto_ros.CameraInfo2Cv()
graph += [ cloud_sub['output'] >> msg2cloud[:],
rgb_image_sub[:] >> image2cv[:],
depth_image_sub[:] >> depth2cv[:],
rgb_camera_info_sub[:] >> rgb_info2cv[:],
depth_camera_info_sub[:] >> depth_info2cv[:]
]
"""
rgb_display = highgui.imshow(name='rgb')
depth_display = highgui.imshow(name='depth')
graph += [image2cv[:] >> rgb_display[:],
depth2cv[:] >> depth_display[:],
]
# Create pointcloud viewer
viewer = CloudViewer("viewer", window_name="Clouds!")
graph += [ msg2cloud[:] >> viewer[:] ]
"""
# Process pointcloud
# Cut off some parts
cut_x = PassThrough(filter_field_name="x",
filter_limit_min=-0.2,
filter_limit_max=0.2)
cut_y = PassThrough(filter_field_name="y",
filter_limit_min=-0.5,
filter_limit_max=0.5)
cut_z = PassThrough(filter_field_name="z",
filter_limit_min=-0.0,
filter_limit_max=1.0)
graph += [ msg2cloud[:] >> cut_x[:],
cut_x[:] >> cut_y[:],
cut_y[:] >> cut_z[:] ]
# Voxel filter
voxel_grid = VoxelGrid("voxel_grid", leaf_size=0.002)
graph += [ cut_z[:] >> voxel_grid[:] ]
# Find plane and delete it
normals = NormalEstimation("normals", k_search=0, radius_search=0.02)
graph += [ voxel_grid[:] >> normals[:] ]
plane_finder = SACSegmentationFromNormals("planar_segmentation",
model_type=SACMODEL_NORMAL_PLANE,
eps_angle=0.09, distance_threshold=0.1)
plane_deleter = ExtractIndices(negative=True)
graph += [ voxel_grid[:] >> plane_finder['input'],
normals[:] >> plane_finder['normals'] ]
graph += [ voxel_grid[:] >> plane_deleter['input'],
plane_finder['inliers'] >> plane_deleter['indices'] ]
# Create pointcloud viewer
viewer = CloudViewer("viewer", window_name="Clouds!")
graph += [ plane_deleter[:] >> viewer[:] ]
# Compute vfh
vfh_signature = VFHEstimation(radius_search=0.01)
graph += [ plane_deleter[:] >> vfh_signature['input'],
normals[:] >> vfh_signature['normals'] ]
# convert the image to grayscale
rgb2gray = imgproc.cvtColor('rgb -> gray', flag=imgproc.Conversion.RGB2GRAY)
graph += [image2cv[:] >> rgb2gray[:] ]
# get a pose use the dot pattern: there might be a scale ambiguity as this is 3d only
poser = OpposingDotPoseEstimator(rows=5, cols=3,
pattern_type=calib.ASYMMETRIC_CIRCLES_GRID,
square_size=0.04, debug=True)
graph += [ image2cv[:] >> poser['color_image'],
rgb_info2cv['K'] >> poser['K_image'],
rgb2gray[:] >> poser['image'] ] #poser gives us R&T from camera to dot pattern
points3d = calib.DepthTo3d()
graph += [ depth_info2cv['K'] >> points3d['K'],
depth2cv['image'] >> points3d['depth']
]
plane_est = PlaneFinder(min_size=10000)
compute_normals = ComputeNormals()
# Convert K if the resolution is different (the camera should output that)
graph += [ # find the normals
depth_info2cv['K'] >> compute_normals['K'],
points3d['points3d'] >> compute_normals['points3d'],
# find the planes
compute_normals['normals'] >> plane_est['normals'],
depth_info2cv['K'] >> plane_est['K'],
points3d['points3d'] >> plane_est['points3d'] ]
pose_filter = object_recognition_capture.ecto_cells.capture.PlaneFilter();
# make sure the pose is centered at the origin of the plane
graph += [ depth_info2cv['K'] >> pose_filter['K_depth'],
poser['R', 'T'] >> pose_filter['R', 'T'],
plane_est['planes', 'masks'] >> pose_filter['planes', 'masks'] ]
delta_pose = ecto.If('delta R|T', cell=object_recognition_capture.DeltaRT(angle_thresh=angle_thresh,
n_desired=n_desired))
poseMsg = RT2PoseStamped(frame_id='/camera_rgb_optical_frame')
graph += [ pose_filter['R', 'T', 'found'] >> delta_pose['R', 'T', 'found'],
pose_filter['R', 'T'] >> poseMsg['R', 'T'] ]
trainer_ = Trainer()
graph += [ pose_filter['R', 'T'] >> trainer_['R', 'T'],
vfh_signature['output'] >> trainer_['features'],
delta_pose['last'] >> trainer_['last'],
delta_pose['novel'] >> trainer_['novel'],
plane_deleter[:] >> trainer_['input'] ]
novel = delta_pose['novel']
cloud2msg = ecto_pcl_ros.PointCloud2Message("cloud2msg")
graph += [ plane_deleter[:] >> cloud2msg[:] ]
baggers = dict(points=PointCloudBagger(topic_name='/camera/depth_registered/points'),
pose=PoseBagger(topic_name='/camera/pose')
)
bagwriter = ecto.If('Bag Writer if R|T', cell=ecto_ros.BagWriter(baggers=baggers, bag=bag_name))
pcd_writer = ecto.If('msd pcd writer', cell = PCDWriter())
graph += [ cloud2msg[:] >> bagwriter['points'],
poseMsg['pose'] >> bagwriter['pose']
]
graph += [ plane_deleter['output'] >> pcd_writer['input'] ]
delta_pose.inputs.__test__ = True
pcd_writer.inputs.__test__ = True
graph += [novel >> (bagwriter['__test__'])]
graph += [novel >> (pcd_writer['__test__'])]
rt2pose = RT2PoseStamped(frame_id = "camera_rgb_optical_frame")
rt2pose_filtered = RT2PoseStamped(frame_id = "camera_rgb_optical_frame")
posePub = PosePublisher("pose_pub", topic_name='pattern_pose')
posePub_filtered = PosePublisher("pose_pub_filtered", topic_name='pattern_pose_filtered')
graph += [ poser['R', 'T'] >> rt2pose['R', 'T'],
rt2pose['pose'] >> posePub['input'],
pose_filter['R', 'T'] >> rt2pose_filtered['R', 'T'],
rt2pose_filtered['pose'] >> posePub_filtered['input'] ]
plasm = ecto.Plasm()
plasm.connect(graph)
return (plasm, segmentation_cell) # return segmentation for tuning of parameters.
#!/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 __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())