Example #1
0
    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'],
        ]
Example #3
0
    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())
Example #4
0
    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)
Example #5
0
    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())
Example #6
0
    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())
Example #7
0
    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())
Example #8
0
    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",
Example #10
0
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.
Example #11
0
#!/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.')
Example #12
0
    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())
Example #13
0
    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)
Example #14
0
    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())