The main grasp collector node object. More...

#include <Segmenter.h>

Public Member Functions
bool	okay () const
	A check for a valid Segmenter.
	Segmenter ()
	Create a Segmenter and associated ROS information.
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.
bool	clearCallback (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
	Callback for the clear request.
sensor_msgs::Image	createImage (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::PointIndices &cluster) const
	Create a cropped image of the segmented object.
visualization_msgs::Marker	createMarker (const pcl::PCLPointCloud2::ConstPtr &pc) const
	Create a Marker from the given point cloud.
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.
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.
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.
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.
const SegmentationZone &	getCurrentZone () const
	Determine the current zone based on the latest state of the TF tree.
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.
void	pointCloudCallback (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &pc)
	Callback for the point cloud topic.
bool	removeObjectCallback (rail_segmentation::RemoveObject::Request &req, rail_segmentation::RemoveObject::Response &res)
	Callback for the remove object request.
bool	segmentCallback (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
	Callback for the main segmentation request.
bool	segmentObjects (rail_manipulation_msgs::SegmentedObjectList &objects)
	Callback for the main segmentation request.
bool	segmentObjectsCallback (rail_manipulation_msgs::SegmentObjects::Request &req, rail_manipulation_msgs::SegmentObjects::Response &res)
	Callback for the main segmentation request.
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 1022 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]`