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	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...

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...

rail_manipulation_msgs::SegmentedObject	findSurface (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::IndicesConstPtr &indices_in, const SegmentationZone &zone, const pcl::IndicesPtr &indices_out) const
	Find and remove a surface from the given point cloud. More...

const SegmentationZone &	getCurrentZone () 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...

void	pointCloudCallback (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &pc)
	Callback for the point cloud topic. 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...

Private Attributes
ros::ServiceServer	clear_srv_

bool	debug_

ros::Publisher	debug_img_pub_

ros::Publisher	debug_pc_pub_

bool	first_pc_in_

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_

pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr	pc_

boost::mutex	pc_mutex_

ros::Subscriber	point_cloud_sub_

ros::NodeHandle	private_node_

ros::ServiceServer	remove_object_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_

tf2_ros::TransformListener	tf2_

tf::TransformListener	tf_

tf2_ros::Buffer	tf_buffer_

bool	use_color_

std::vector< SegmentationZone >	zones_

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 76 of file Segmenter.h.

Constructor & Destructor Documentation

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 24 of file Segmenter.cpp.

Member Function Documentation

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

v	The vector of points to average.

Returns: The average Z value of the provided points.

Definition at line 1024 of file Segmenter.cpp.

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

req	The empty request (unused).
res	The empty response (unused).

Returns: Will always return true.

Definition at line 319 of file Segmenter.cpp.

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

in	The original organized point cloud.
cluster	The indicies of the current cluster in the point cloud.

Returns: The corresponding image for the given cluster.

Definition at line 875 of file Segmenter.cpp.

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

pc	The PCL point cloud to create a marker for.

Returns: The corresponding marker for the given point cloud.

Definition at line 937 of file Segmenter.cpp.

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

in	The point cloud to take points from.
indices_in	The indices to create a new point cloud from.
out	The point cloud to fill.

Definition at line 999 of file Segmenter.cpp.

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

in	The point cloud to search for point clouds from.
indices_in	The indices in the point cloud to consider.
clusters	The indices of each cluster in the point cloud.

Definition at line 822 of file Segmenter.cpp.

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

in	The point cloud to search for point clouds from.
indices_in	The indices in the point cloud to consider.
clusters	The indices of each cluster in the point cloud.

Definition at line 848 of file Segmenter.cpp.

rail_manipulation_msgs::SegmentedObject Segmenter::findSurface	(	const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &	in,
		const pcl::IndicesConstPtr &	indices_in,
		const SegmentationZone &	zone,
		const pcl::IndicesPtr &	indices_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

in	The input point cloud.
indices_in	The indices in the point cloud to consider.
z_min	The minimum height of a surface to remove.
z_max	The maximum height of a surface to remove.
indices_out	The set of points that are not part of the surface.

Returns: The average height of the surface that was removed or negative infinity if no valid surface was found.

Definition at line 647 of file Segmenter.cpp.

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 264 of file Segmenter.cpp.

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

in	The point cloud to take points from.
indices_in	The indices in the point cloud to consider.
conditions	The conditions specifying which points to ignore.
indices_out	The set of points that pass the condition test.

Definition at line 1008 of file Segmenter.cpp.

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 250 of file Segmenter.cpp.

void Segmenter::pointCloudCallback ( const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr & pc )

private

Callback for the point cloud topic.

Saves a copy of the latest point cloud internally.

Parameters

pc	The current point cloud message.

Definition at line 255 of file Segmenter.cpp.

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

req	The request with the index to use.
res	The empty response (unused).

Returns: Returns true if a valid index was provided.

Definition at line 291 of file Segmenter.cpp.

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

req	The empty request (unused).
res	The empty response (unused).

Returns: Returns true if the segmentation was successful.

Definition at line 344 of file Segmenter.cpp.

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

objects List for resulting segmented objects.

Definition at line 356 of file Segmenter.cpp.

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

req	The empty request (unused).
res	The resulting segmented object list.

Returns: Returns true if the segmentation was successful.

Definition at line 350 of file Segmenter.cpp.

Member Data Documentation

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

private

Definition at line 338 of file Segmenter.h.

const double rail::segmentation::Segmenter::CLUSTER_TOLERANCE = 0.02

static

The cluster tolerance level.

Definition at line 120 of file Segmenter.h.

bool rail::segmentation::Segmenter::debug_

private

The debug, okay check, first point cloud, and color segmentation flags.

Definition at line 327 of file Segmenter.h.

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

private

Definition at line 340 of file Segmenter.h.

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

private

Definition at line 340 of file Segmenter.h.

const bool Segmenter::DEFAULT_DEBUG = false

static

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

Definition at line 106 of file Segmenter.h.

const int Segmenter::DEFAULT_MAX_CLUSTER_SIZE = 10000

static

The maximum cluster size.

Definition at line 118 of file Segmenter.h.

const int Segmenter::DEFAULT_MIN_CLUSTER_SIZE = 200

static

The minimum cluster size.

Definition at line 116 of file Segmenter.h.

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

static

Leaf size of the voxel grid for downsampling.

Definition at line 126 of file Segmenter.h.

bool rail::segmentation::Segmenter::first_pc_in_

private

Definition at line 327 of file Segmenter.h.

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

static

Size of the marker visualization scale factor.

Definition at line 128 of file Segmenter.h.

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

private

Current marker array.

Definition at line 357 of file Segmenter.h.

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

private

Definition at line 340 of file Segmenter.h.

int rail::segmentation::Segmenter::max_cluster_size_

private

Definition at line 329 of file Segmenter.h.

int rail::segmentation::Segmenter::min_cluster_size_

private

Cluster parameters.

Definition at line 329 of file Segmenter.h.

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

private

Definition at line 331 of file Segmenter.h.

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

private

The global and private ROS node handles.

Definition at line 336 of file Segmenter.h.

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

private

Current object list.

Definition at line 353 of file Segmenter.h.

bool rail::segmentation::Segmenter::okay_

private

Definition at line 327 of file Segmenter.h.

pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr rail::segmentation::Segmenter::pc_

private

Latest point cloud.

Definition at line 351 of file Segmenter.h.

boost::mutex rail::segmentation::Segmenter::pc_mutex_

private

Mutex for locking on the point cloud and current messages.

Definition at line 331 of file Segmenter.h.

ros::Subscriber rail::segmentation::Segmenter::point_cloud_sub_

private

Subscribers used in the node.

Definition at line 342 of file Segmenter.h.

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

static

The color tolerance level, only for RGB segmentation

Definition at line 122 of file Segmenter.h.

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

private

Definition at line 336 of file Segmenter.h.

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 124 of file Segmenter.h.

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

private

Definition at line 338 of file Segmenter.h.

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

static

The distance threshold for the plane segmenter.

Definition at line 110 of file Segmenter.h.

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

static

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

Definition at line 108 of file Segmenter.h.

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

static

The maximum interations for the plane segmenter

Definition at line 112 of file Segmenter.h.

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

private

Definition at line 338 of file Segmenter.h.

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

private

Services advertised by this node

Definition at line 338 of file Segmenter.h.

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

private

Publishers used in the node.

Definition at line 340 of file Segmenter.h.

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

static

The padding for surface removal.

Definition at line 114 of file Segmenter.h.

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

private

Current table object.

Definition at line 355 of file Segmenter.h.

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

private

Current table marker.

Definition at line 359 of file Segmenter.h.

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

private

Definition at line 340 of file Segmenter.h.

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

private

Definition at line 340 of file Segmenter.h.

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

private

The buffered trasnform client.

Definition at line 348 of file Segmenter.h.

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

private

Main transform listener.

Definition at line 344 of file Segmenter.h.

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

private

The transform tree buffer for the tf2 listener.

Definition at line 346 of file Segmenter.h.

bool rail::segmentation::Segmenter::use_color_

private

Definition at line 327 of file Segmenter.h.

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

private

List of segmentation zones.

Definition at line 333 of file Segmenter.h.

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

Public Member Functions

Static Public Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation