Public Member Functions | Static Public Attributes | Private Member Functions | Private Attributes | List of all members
rail::segmentation::Segmenter Class Reference

The main grasp collector node object. More...

#include <Segmenter.h>

Public Member Functions

bool okay () const
 A check for a valid Segmenter. More...
 
 Segmenter ()
 Create a Segmenter and associated ROS information. More...
 

Static Public Attributes

static const double CLUSTER_TOLERANCE = 0.02
 
static const bool DEFAULT_DEBUG = false
 
static const int DEFAULT_MAX_CLUSTER_SIZE = 10000
 
static const int DEFAULT_MIN_CLUSTER_SIZE = 200
 
static const float DOWNSAMPLE_LEAF_SIZE = 0.01
 
static const double MARKER_SCALE = 0.01
 
static const double POINT_COLOR_THRESHOLD = 10
 
static const double REGION_COLOR_THRESHOLD = 10
 
static const double SAC_DISTANCE_THRESHOLD = 0.01
 
static const double SAC_EPS_ANGLE = 0.15
 
static const int SAC_MAX_ITERATIONS = 100
 
static const double SURFACE_REMOVAL_PADDING = 0.005
 

Private Member Functions

double averageZ (const std::vector< pcl::PointXYZRGB, Eigen::aligned_allocator< pcl::PointXYZRGB > > &v) const
 Find the average Z value of the point vector. More...
 
bool calculateFeaturesCallback (rail_manipulation_msgs::ProcessSegmentedObjects::Request &req, rail_manipulation_msgs::ProcessSegmentedObjects::Response &res)
 
bool clearCallback (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
 Callback for the clear request. More...
 
sensor_msgs::Image createImage (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::PointIndices &cluster) const
 Create a cropped image of the segmented object. More...
 
visualization_msgs::Marker createMarker (const pcl::PCLPointCloud2::ConstPtr &pc) const
 Create a Marker from the given point cloud. More...
 
bool executeSegmentation (pcl::PointCloud< pcl::PointXYZRGB >::Ptr pc, rail_manipulation_msgs::SegmentedObjectList &objects)
 Main segmentation routine. More...
 
void extract (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::IndicesConstPtr &indices_in, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &out) const
 Extract a new point cloud based on the given indices. More...
 
void extractClustersEuclidean (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::IndicesConstPtr &indices_in, std::vector< pcl::PointIndices > &clusters) const
 Find clusters in a point cloud. More...
 
void extractClustersRGB (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::IndicesConstPtr &indices_in, std::vector< pcl::PointIndices > &clusters) const
 Find clusters in a point cloud. More...
 
bool findSurface (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::IndicesConstPtr &indices_in, const SegmentationZone &zone, const pcl::IndicesPtr &indices_out, rail_manipulation_msgs::SegmentedObject &table_out) const
 Find and remove a surface from the given point cloud. More...
 
const SegmentationZonegetCurrentZone () const
 Determine the current zone based on the latest state of the TF tree. More...
 
void inverseBound (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::IndicesConstPtr &indices_in, const pcl::ConditionBase< pcl::PointXYZRGB >::Ptr &conditions, const pcl::IndicesPtr &indices_out) const
 Bound a point cloud based on the inverse of a set of conditions. More...
 
bool removeObjectCallback (rail_segmentation::RemoveObject::Request &req, rail_segmentation::RemoveObject::Response &res)
 Callback for the remove object request. More...
 
bool segmentCallback (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
 Callback for the main segmentation request. More...
 
bool segmentObjects (rail_manipulation_msgs::SegmentedObjectList &objects)
 Callback for the main segmentation request. More...
 
bool segmentObjectsCallback (rail_manipulation_msgs::SegmentObjects::Request &req, rail_manipulation_msgs::SegmentObjects::Response &res)
 Callback for the main segmentation request. More...
 
bool segmentObjectsFromPointCloudCallback (rail_manipulation_msgs::SegmentObjectsFromPointCloud::Request &req, rail_manipulation_msgs::SegmentObjectsFromPointCloud::Response &res)
 Callback for the main segmentation request. More...
 

Private Attributes

ros::ServiceServer calculate_features_srv_
 
ros::ServiceServer clear_srv_
 
double cluster_tolerance_
 
bool crop_first_
 
bool debug_
 
ros::Publisher debug_img_pub_
 
ros::Publisher debug_pc_pub_
 
bool label_markers_
 
visualization_msgs::MarkerArray markers_
 
ros::Publisher markers_pub_
 
int max_cluster_size_
 
int min_cluster_size_
 
boost::mutex msg_mutex_
 
ros::NodeHandle node_
 
rail_manipulation_msgs::SegmentedObjectList object_list_
 
bool okay_
 
std::string point_cloud_topic_
 
ros::NodeHandle private_node_
 
ros::ServiceServer remove_object_srv_
 
ros::ServiceServer segment_objects_from_point_cloud_srv_
 
ros::ServiceServer segment_objects_srv_
 
ros::ServiceServer segment_srv_
 
ros::Publisher segmented_objects_pub_
 
rail_manipulation_msgs::SegmentedObject table_
 
visualization_msgs::Marker table_marker_
 
ros::Publisher table_marker_pub_
 
ros::Publisher table_pub_
 
visualization_msgs::MarkerArray text_markers_
 
tf2_ros::TransformListener tf2_
 
tf::TransformListener tf_
 
tf2_ros::Buffer tf_buffer_
 
bool use_color_
 
std::vector< SegmentationZonezones_
 

Detailed Description

The main grasp collector node object.

The grasp collector is responsible for capturing and storing grasps. An action server is started is the main entry point to grasp collecting.

Definition at line 78 of file Segmenter.h.

Constructor & Destructor Documentation

◆ Segmenter()

Segmenter::Segmenter ( )

Create a Segmenter and associated ROS information.

Creates a ROS node handle, subscribes to the relevant topics and servers, and creates services for requesting segmenations.

Definition at line 32 of file Segmenter.cpp.

Member Function Documentation

◆ averageZ()

double Segmenter::averageZ ( const std::vector< pcl::PointXYZRGB, Eigen::aligned_allocator< pcl::PointXYZRGB > > &  v) const
private

Find the average Z value of the point vector.

Finds the average Z value of the point vector. An empty vector will return an average of 0.

Parameters
vThe vector of points to average.
Returns
The average Z value of the provided points.

Definition at line 1334 of file Segmenter.cpp.

◆ calculateFeaturesCallback()

bool Segmenter::calculateFeaturesCallback ( rail_manipulation_msgs::ProcessSegmentedObjects::Request &  req,
rail_manipulation_msgs::ProcessSegmentedObjects::Response &  res 
)
private

Definition at line 843 of file Segmenter.cpp.

◆ clearCallback()

bool Segmenter::clearCallback ( std_srvs::Empty::Request &  req,
std_srvs::Empty::Response &  res 
)
private

Callback for the clear request.

Clears the current segmented object list. This will publish both an empty segmented object list and a marker array with delete actions from the last segmentation request.

Parameters
reqThe empty request (unused).
resThe empty response (unused).
Returns
Will always return true.

Definition at line 355 of file Segmenter.cpp.

◆ createImage()

sensor_msgs::Image Segmenter::createImage ( const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &  in,
const pcl::PointIndices &  cluster 
) const
private

Create a cropped image of the segmented object.

Creates a new ROS image based on the cropped segmented object.

Parameters
inThe original organized point cloud.
clusterThe indicies of the current cluster in the point cloud.
Returns
The corresponding image for the given cluster.

Definition at line 1185 of file Segmenter.cpp.

◆ createMarker()

visualization_msgs::Marker Segmenter::createMarker ( const pcl::PCLPointCloud2::ConstPtr &  pc) const
private

Create a Marker from the given point cloud.

Creates a new Marker message based on the PCL point cloud. The point cloud will first be downsampled.

Parameters
pcThe PCL point cloud to create a marker for.
Returns
The corresponding marker for the given point cloud.

Definition at line 1247 of file Segmenter.cpp.

◆ executeSegmentation()

bool Segmenter::executeSegmentation ( pcl::PointCloud< pcl::PointXYZRGB >::Ptr  pc,
rail_manipulation_msgs::SegmentedObjectList &  objects 
)
private

Main segmentation routine.

Parameters
pcinput point cloud to be segmented
Returns
true on success, to be passed to service return

Definition at line 453 of file Segmenter.cpp.

◆ extract()

void Segmenter::extract ( const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &  in,
const pcl::IndicesConstPtr &  indices_in,
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &  out 
) const
private

Extract a new point cloud based on the given indices.

Extract a new point cloud from the given indices. The resulting point cloud will be unorganized.

Parameters
inThe point cloud to take points from.
indices_inThe indices to create a new point cloud from.
outThe point cloud to fill.

Definition at line 1309 of file Segmenter.cpp.

◆ extractClustersEuclidean()

void Segmenter::extractClustersEuclidean ( const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &  in,
const pcl::IndicesConstPtr &  indices_in,
std::vector< pcl::PointIndices > &  clusters 
) const
private

Find clusters in a point cloud.

Find the clusters in the given point cloud using euclidean cluster extraction and a KD search tree.

Parameters
inThe point cloud to search for point clouds from.
indices_inThe indices in the point cloud to consider.
clustersThe indices of each cluster in the point cloud.

Definition at line 1132 of file Segmenter.cpp.

◆ extractClustersRGB()

void Segmenter::extractClustersRGB ( const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &  in,
const pcl::IndicesConstPtr &  indices_in,
std::vector< pcl::PointIndices > &  clusters 
) const
private

Find clusters in a point cloud.

Find the clusters in the given point cloud using RGB region growing and a KD search tree.

Parameters
inThe point cloud to search for point clouds from.
indices_inThe indices in the point cloud to consider.
clustersThe indices of each cluster in the point cloud.

Definition at line 1158 of file Segmenter.cpp.

◆ findSurface()

bool Segmenter::findSurface ( const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &  in,
const pcl::IndicesConstPtr &  indices_in,
const SegmentationZone zone,
const pcl::IndicesPtr &  indices_out,
rail_manipulation_msgs::SegmentedObject &  table_out 
) const
private

Find and remove a surface from the given point cloud.

Find a surface in the input point cloud and attempt to remove it. The surface must be within the bounds provided in order to be removed. The resulting point cloud is placed in the output cloud. If no surface is found, no effect is made to the output cloud and negative infinity is returned.

Parameters
inThe input point cloud.
indices_inThe indices in the point cloud to consider.
z_minThe minimum height of a surface to remove.
z_maxThe maximum height of a surface to remove.
indices_outThe set of points that are not part of the surface.
table_outthe table as a segmented object, to be published on a separate topic
Returns
true if a surface was found, false otherwise

Definition at line 959 of file Segmenter.cpp.

◆ getCurrentZone()

const SegmentationZone & Segmenter::getCurrentZone ( ) const
private

Determine the current zone based on the latest state of the TF tree.

Checks each segmenation zone criteria based on teh latest state of the TF tree and returns a reference to that zone. If multiple zones are met, the first is returned. If no valid zone is found, the first zone is returned and a warning is sent to ROS_WARN.

Returns
The zone that matches the current state of the TF tree.

Definition at line 279 of file Segmenter.cpp.

◆ inverseBound()

void Segmenter::inverseBound ( const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &  in,
const pcl::IndicesConstPtr &  indices_in,
const pcl::ConditionBase< pcl::PointXYZRGB >::Ptr &  conditions,
const pcl::IndicesPtr &  indices_out 
) const
private

Bound a point cloud based on the inverse of a set of conditions.

Extract a new point cloud based on the inverse of a set of conditions.

Parameters
inThe point cloud to take points from.
indices_inThe indices in the point cloud to consider.
conditionsThe conditions specifying which points to ignore.
indices_outThe set of points that pass the condition test.

Definition at line 1318 of file Segmenter.cpp.

◆ okay()

bool Segmenter::okay ( ) const

A check for a valid Segmenter.

This function will return true if valid segmenation zones were parsed from a YAML config file.

Returns
True if valid segmenation zones were parsed.

Definition at line 274 of file Segmenter.cpp.

◆ removeObjectCallback()

bool Segmenter::removeObjectCallback ( rail_segmentation::RemoveObject::Request &  req,
rail_segmentation::RemoveObject::Response &  res 
)
private

Callback for the remove object request.

Remote the object from the segmented object list with a given ID. This will publish both an updated segmented object list and a marker array with a delete action for the given marker index.

Parameters
reqThe request with the index to use.
resThe empty response (unused).
Returns
Returns true if a valid index was provided.

Definition at line 306 of file Segmenter.cpp.

◆ segmentCallback()

bool Segmenter::segmentCallback ( std_srvs::Empty::Request &  req,
std_srvs::Empty::Response &  res 
)
private

Callback for the main segmentation request.

Performs a segmenation with the latest point cloud. This will publish both a segmented object list and a marker array of the resulting segmentation.

Parameters
reqThe empty request (unused).
resThe empty response (unused).
Returns
Returns true if the segmentation was successful.

Definition at line 404 of file Segmenter.cpp.

◆ segmentObjects()

bool Segmenter::segmentObjects ( rail_manipulation_msgs::SegmentedObjectList &  objects)
private

Callback for the main segmentation request.

Performs a segmenation with the latest point cloud. This will publish both a segmented object list and a marker array of the resulting segmentation.

Parameters
objectsList for resulting segmented objects.

Definition at line 427 of file Segmenter.cpp.

◆ segmentObjectsCallback()

bool Segmenter::segmentObjectsCallback ( rail_manipulation_msgs::SegmentObjects::Request &  req,
rail_manipulation_msgs::SegmentObjects::Response &  res 
)
private

Callback for the main segmentation request.

Performs a segmenation with the latest point cloud. This will publish both a segmented object list and a marker array of the resulting segmentation.

Parameters
reqThe empty request (unused).
resThe resulting segmented object list.
Returns
Returns true if the segmentation was successful.

Definition at line 410 of file Segmenter.cpp.

◆ segmentObjectsFromPointCloudCallback()

bool Segmenter::segmentObjectsFromPointCloudCallback ( rail_manipulation_msgs::SegmentObjectsFromPointCloud::Request &  req,
rail_manipulation_msgs::SegmentObjectsFromPointCloud::Response &  res 
)
private

Callback for the main segmentation request.

Performs a segmenation with the provided point cloud. This will publish both a segmented object list and a marker array of the resulting segmentation.

Parameters
reqThe empty request (unused).
resThe resulting segmented object list.
Returns
Returns true if the segmentation was successful.

Definition at line 416 of file Segmenter.cpp.

Member Data Documentation

◆ calculate_features_srv_

ros::ServiceServer rail::segmentation::Segmenter::calculate_features_srv_
private

Definition at line 362 of file Segmenter.h.

◆ clear_srv_

ros::ServiceServer rail::segmentation::Segmenter::clear_srv_
private

Definition at line 362 of file Segmenter.h.

◆ CLUSTER_TOLERANCE

const double Segmenter::CLUSTER_TOLERANCE = 0.02
static

The cluster tolerance level.

Definition at line 122 of file Segmenter.h.

◆ cluster_tolerance_

double rail::segmentation::Segmenter::cluster_tolerance_
private

Settable euclidean distance tolerance for including a point in a cluster

Definition at line 357 of file Segmenter.h.

◆ crop_first_

bool rail::segmentation::Segmenter::crop_first_
private

Flag for cropping the point cloud before table detection or after

Definition at line 353 of file Segmenter.h.

◆ debug_

bool rail::segmentation::Segmenter::debug_
private

The debug, okay check, and color segmentation flags.

Definition at line 345 of file Segmenter.h.

◆ debug_img_pub_

ros::Publisher rail::segmentation::Segmenter::debug_img_pub_
private

Definition at line 364 of file Segmenter.h.

◆ debug_pc_pub_

ros::Publisher rail::segmentation::Segmenter::debug_pc_pub_
private

Definition at line 364 of file Segmenter.h.

◆ DEFAULT_DEBUG

const bool Segmenter::DEFAULT_DEBUG = false
static

If a topic should be created to display debug information such as point clouds.

Definition at line 108 of file Segmenter.h.

◆ DEFAULT_MAX_CLUSTER_SIZE

const int Segmenter::DEFAULT_MAX_CLUSTER_SIZE = 10000
static

The maximum cluster size.

Definition at line 120 of file Segmenter.h.

◆ DEFAULT_MIN_CLUSTER_SIZE

const int Segmenter::DEFAULT_MIN_CLUSTER_SIZE = 200
static

The minimum cluster size.

Definition at line 118 of file Segmenter.h.

◆ DOWNSAMPLE_LEAF_SIZE

const float rail::segmentation::Segmenter::DOWNSAMPLE_LEAF_SIZE = 0.01
static

Leaf size of the voxel grid for downsampling.

Definition at line 128 of file Segmenter.h.

◆ label_markers_

bool rail::segmentation::Segmenter::label_markers_
private

Flag for labeling cluster markers with their index

Definition at line 355 of file Segmenter.h.

◆ MARKER_SCALE

const double rail::segmentation::Segmenter::MARKER_SCALE = 0.01
static

Size of the marker visualization scale factor.

Definition at line 130 of file Segmenter.h.

◆ markers_

visualization_msgs::MarkerArray rail::segmentation::Segmenter::markers_
private

Current marker array.

Definition at line 381 of file Segmenter.h.

◆ markers_pub_

ros::Publisher rail::segmentation::Segmenter::markers_pub_
private

Definition at line 364 of file Segmenter.h.

◆ max_cluster_size_

int rail::segmentation::Segmenter::max_cluster_size_
private

Definition at line 347 of file Segmenter.h.

◆ min_cluster_size_

int rail::segmentation::Segmenter::min_cluster_size_
private

Cluster parameters.

Definition at line 347 of file Segmenter.h.

◆ msg_mutex_

boost::mutex rail::segmentation::Segmenter::msg_mutex_
private

Mutex for locking on the point cloud and current messages.

Definition at line 349 of file Segmenter.h.

◆ node_

ros::NodeHandle rail::segmentation::Segmenter::node_
private

The global and private ROS node handles.

Definition at line 360 of file Segmenter.h.

◆ object_list_

rail_manipulation_msgs::SegmentedObjectList rail::segmentation::Segmenter::object_list_
private

Current object list.

Definition at line 377 of file Segmenter.h.

◆ okay_

bool rail::segmentation::Segmenter::okay_
private

Definition at line 345 of file Segmenter.h.

◆ point_cloud_topic_

std::string rail::segmentation::Segmenter::point_cloud_topic_
private

Definition at line 374 of file Segmenter.h.

◆ POINT_COLOR_THRESHOLD

const double rail::segmentation::Segmenter::POINT_COLOR_THRESHOLD = 10
static

The color tolerance level, only for RGB segmentation

Definition at line 124 of file Segmenter.h.

◆ private_node_

ros::NodeHandle rail::segmentation::Segmenter::private_node_
private

Definition at line 360 of file Segmenter.h.

◆ REGION_COLOR_THRESHOLD

const double rail::segmentation::Segmenter::REGION_COLOR_THRESHOLD = 10
static

The region color tolerance, only for small region merging in RGB segmentation

Definition at line 126 of file Segmenter.h.

◆ remove_object_srv_

ros::ServiceServer rail::segmentation::Segmenter::remove_object_srv_
private

Definition at line 362 of file Segmenter.h.

◆ SAC_DISTANCE_THRESHOLD

const double rail::segmentation::Segmenter::SAC_DISTANCE_THRESHOLD = 0.01
static

The distance threshold for the plane segmenter.

Definition at line 112 of file Segmenter.h.

◆ SAC_EPS_ANGLE

const double rail::segmentation::Segmenter::SAC_EPS_ANGLE = 0.15
static

The angle epsilon (delta) threshold for the plane segmenter.

Definition at line 110 of file Segmenter.h.

◆ SAC_MAX_ITERATIONS

const int rail::segmentation::Segmenter::SAC_MAX_ITERATIONS = 100
static

The maximum interations for the plane segmenter

Definition at line 114 of file Segmenter.h.

◆ segment_objects_from_point_cloud_srv_

ros::ServiceServer rail::segmentation::Segmenter::segment_objects_from_point_cloud_srv_
private

Definition at line 362 of file Segmenter.h.

◆ segment_objects_srv_

ros::ServiceServer rail::segmentation::Segmenter::segment_objects_srv_
private

Definition at line 362 of file Segmenter.h.

◆ segment_srv_

ros::ServiceServer rail::segmentation::Segmenter::segment_srv_
private

Services advertised by this node

Definition at line 362 of file Segmenter.h.

◆ segmented_objects_pub_

ros::Publisher rail::segmentation::Segmenter::segmented_objects_pub_
private

Publishers used in the node.

Definition at line 364 of file Segmenter.h.

◆ SURFACE_REMOVAL_PADDING

const double rail::segmentation::Segmenter::SURFACE_REMOVAL_PADDING = 0.005
static

The padding for surface removal.

Definition at line 116 of file Segmenter.h.

◆ table_

rail_manipulation_msgs::SegmentedObject rail::segmentation::Segmenter::table_
private

Current table object.

Definition at line 379 of file Segmenter.h.

◆ table_marker_

visualization_msgs::Marker rail::segmentation::Segmenter::table_marker_
private

Current table marker.

Definition at line 385 of file Segmenter.h.

◆ table_marker_pub_

ros::Publisher rail::segmentation::Segmenter::table_marker_pub_
private

Definition at line 364 of file Segmenter.h.

◆ table_pub_

ros::Publisher rail::segmentation::Segmenter::table_pub_
private

Definition at line 364 of file Segmenter.h.

◆ text_markers_

visualization_msgs::MarkerArray rail::segmentation::Segmenter::text_markers_
private

Current marker label array (only used if label_markers_ is true).

Definition at line 383 of file Segmenter.h.

◆ tf2_

tf2_ros::TransformListener rail::segmentation::Segmenter::tf2_
private

The buffered trasnform client.

Definition at line 372 of file Segmenter.h.

◆ tf_

tf::TransformListener rail::segmentation::Segmenter::tf_
private

Subscribers used in the node.

Main transform listener.

Definition at line 368 of file Segmenter.h.

◆ tf_buffer_

tf2_ros::Buffer rail::segmentation::Segmenter::tf_buffer_
private

The transform tree buffer for the tf2 listener.

Definition at line 370 of file Segmenter.h.

◆ use_color_

bool rail::segmentation::Segmenter::use_color_
private

Definition at line 345 of file Segmenter.h.

◆ zones_

std::vector<SegmentationZone> rail::segmentation::Segmenter::zones_
private

List of segmentation zones.

Definition at line 351 of file Segmenter.h.


The documentation for this class was generated from the following files:


rail_segmentation
Author(s): Russell Toris , David Kent
autogenerated on Mon Feb 28 2022 23:23:52