Nodelet implementation of jsk_pcl/EnvironmentPlaneModeling. More...

#include <environment_plane_modeling.h>

Public Types
typedef EnvironmentPlaneModelingConfig	Config
typedef message_filters::sync_policies::ExactTime < sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, jsk_recognition_msgs::PolygonArray, jsk_recognition_msgs::ModelCoefficientsArray, jsk_recognition_msgs::ClusterPointIndices >	SyncPolicy
Public Member Functions
	EnvironmentPlaneModeling ()
Protected Member Functions
virtual void	boundingBoxCallback (const jsk_recognition_msgs::BoundingBox::ConstPtr &box_array)
virtual std::vector < GridPlane::Ptr >	buildGridPlanes (pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > convexes, std::set< int > &non_plane_indices)
	make GridPlane from jsk_recognition_utils::ConvexPolygon and PointCloud
virtual std::vector < GridPlane::Ptr >	completeFootprintRegion (const std_msgs::Header &header, std::vector< GridPlane::Ptr > &grid_maps)
virtual GridPlane::Ptr	completeGridMapByBoundingBox (const jsk_recognition_msgs::BoundingBox::ConstPtr &box, const std_msgs::Header &header, GridPlane::Ptr grid_map)
virtual void	configCallback (Config &config, uint32_t level)
	Callback method of dynamic reconfigure.
virtual std::vector < jsk_recognition_utils::ConvexPolygon::Ptr >	convertToConvexPolygons (const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &indices_msg, const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &coefficients_msg)
virtual std::vector < GridPlane::Ptr >	erodeFiltering (std::vector< GridPlane::Ptr > &grid_maps)
virtual void	inputCallback (const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, const sensor_msgs::PointCloud2::ConstPtr &full_cloud_msg, const jsk_recognition_msgs::PolygonArray::ConstPtr &polygon_msg, const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &coefficients_msg, const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &indices_msg)
	main callback function
virtual bool	isValidFrameIds (const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, const sensor_msgs::PointCloud2::ConstPtr &full_cloud_msg, const jsk_recognition_msgs::PolygonArray::ConstPtr &polygon_msg, const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &coefficients_msg, const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &indices_msg)
virtual int	lookupGroundPlaneForFootprint (const std::string &footprint_frame_id, const std_msgs::Header &header, const std::vector< GridPlane::Ptr > &grid_maps)
virtual int	lookupGroundPlaneForFootprint (const Eigen::Affine3f &pose, const std::vector< GridPlane::Ptr > &grid_maps)
virtual std::vector < jsk_recognition_utils::ConvexPolygon::Ptr >	magnifyConvexes (std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > &convexes)
	Magnify jsk_recognition_utils::ConvexPolygons according to maginify_distance_ parameter.
virtual std::vector < GridPlane::Ptr >	morphologicalFiltering (std::vector< GridPlane::Ptr > &raw_grid_maps)
virtual void	moveBaseSimpleGoalCallback (const geometry_msgs::PoseStamped::ConstPtr &msg)
virtual void	onInit ()
virtual void	printInputData (const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, const sensor_msgs::PointCloud2::ConstPtr &full_cloud_msg, const jsk_recognition_msgs::PolygonArray::ConstPtr &polygon_msg, const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &coefficients_msg, const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &indices_msg)
virtual void	publishConvexPolygons (ros::Publisher &pub, const std_msgs::Header &header, std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > &convexes)
	Publish array of jsk_recognition_utils::ConvexPolygon::Ptr by using specified publisher.
virtual void	publishConvexPolygonsBoundaries (ros::Publisher &pub, const std_msgs::Header &header, std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > &convexes)
virtual void	publishGridMaps (ros::Publisher &pub, const std_msgs::Header &header, std::vector< GridPlane::Ptr > &grids)
	Publish array of GridPlane::Ptr by using specified publisher.
virtual void	subscribe ()
	subscription callback function of jsk_topic_tools::DiagnosticNodelet. This method is empty method because EnvironmentPlaneModeling needs to always run
virtual void	unsubscribe ()
	unsubscription callback function of jsk_topic_tools::DiagnosticNodelet. This method is empty method because EnvironmentPlaneModeling needs to always run
Protected Attributes
bool	complete_footprint_region_
double	distance_threshold_
int	erode_filter_size_
std::vector< std::string >	footprint_frames_
double	footprint_plane_angular_threshold_
double	footprint_plane_distance_threshold_
std_msgs::Header	latest_global_header_
std::vector< GridPlane::Ptr >	latest_grid_maps_
jsk_recognition_msgs::BoundingBox::ConstPtr	latest_leg_bounding_box_
double	magnify_distance_
int	morphological_filter_size_
boost::mutex	mutex_
double	normal_threshold_
ros::Publisher	pub_debug_convex_point_cloud_
ros::Publisher	pub_debug_magnified_plane_coords_
ros::Publisher	pub_debug_magnified_polygons_
ros::Publisher	pub_debug_noeroded_grid_map_
ros::Publisher	pub_debug_plane_coords_
ros::Publisher	pub_debug_raw_grid_map_
ros::Publisher	pub_grid_map_
ros::Publisher	pub_non_plane_indices_
ros::Publisher	pub_snapped_move_base_simple_goal_
double	resolution_
boost::shared_ptr < dynamic_reconfigure::Server < Config > >	srv_
message_filters::Subscriber < sensor_msgs::PointCloud2 >	sub_cloud_
message_filters::Subscriber < jsk_recognition_msgs::ModelCoefficientsArray >	sub_coefficients_
message_filters::Subscriber < sensor_msgs::PointCloud2 >	sub_full_cloud_
message_filters::Subscriber < jsk_recognition_msgs::ClusterPointIndices >	sub_indices_
ros::Subscriber	sub_leg_bbox_
ros::Subscriber	sub_move_base_simple_goal_
message_filters::Subscriber < jsk_recognition_msgs::PolygonArray >	sub_polygons_
boost::shared_ptr < message_filters::Synchronizer < SyncPolicy > >	sync_
tf::TransformListener *	tf_listener_

Detailed Description

Nodelet implementation of jsk_pcl/EnvironmentPlaneModeling.

Definition at line 80 of file environment_plane_modeling.h.

Member Typedef Documentation

typedef EnvironmentPlaneModelingConfig jsk_pcl_ros::EnvironmentPlaneModeling::Config

Definition at line 83 of file environment_plane_modeling.h.

typedef message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, jsk_recognition_msgs::PolygonArray, jsk_recognition_msgs::ModelCoefficientsArray, jsk_recognition_msgs::ClusterPointIndices > jsk_pcl_ros::EnvironmentPlaneModeling::SyncPolicy

Definition at line 90 of file environment_plane_modeling.h.

Constructor & Destructor Documentation

jsk_pcl_ros::EnvironmentPlaneModeling::EnvironmentPlaneModeling ( ) [inline]

Definition at line 91 of file environment_plane_modeling.h.

Member Function Documentation

void jsk_pcl_ros::EnvironmentPlaneModeling::boundingBoxCallback ( const jsk_recognition_msgs::BoundingBox::ConstPtr & box_array ) [protected, virtual]

Definition at line 236 of file environment_plane_modeling_nodelet.cpp.

std::vector< GridPlane::Ptr > jsk_pcl_ros::EnvironmentPlaneModeling::buildGridPlanes	(	pcl::PointCloud< pcl::PointNormal >::Ptr &	cloud,
		std::vector< jsk_recognition_utils::ConvexPolygon::Ptr >	convexes,
		std::set< int > &	non_plane_indices
	)		`[protected, virtual]`

make GridPlane from jsk_recognition_utils::ConvexPolygon and PointCloud

Definition at line 516 of file environment_plane_modeling_nodelet.cpp.

std::vector< GridPlane::Ptr > jsk_pcl_ros::EnvironmentPlaneModeling::completeFootprintRegion	(	const std_msgs::Header &	header,
		std::vector< GridPlane::Ptr > &	grid_maps
	)		`[protected, virtual]`

Definition at line 431 of file environment_plane_modeling_nodelet.cpp.

GridPlane::Ptr jsk_pcl_ros::EnvironmentPlaneModeling::completeGridMapByBoundingBox	(	const jsk_recognition_msgs::BoundingBox::ConstPtr &	box,
		const std_msgs::Header &	header,
		GridPlane::Ptr	grid_map
	)		`[protected, virtual]`

Definition at line 402 of file environment_plane_modeling_nodelet.cpp.

void jsk_pcl_ros::EnvironmentPlaneModeling::configCallback	(	Config &	config,
		uint32_t	level
	)		`[protected, virtual]`

Callback method of dynamic reconfigure.

Definition at line 118 of file environment_plane_modeling_nodelet.cpp.

std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > jsk_pcl_ros::EnvironmentPlaneModeling::convertToConvexPolygons	(	const pcl::PointCloud< pcl::PointNormal >::Ptr &	cloud,
		const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &	indices_msg,
		const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &	coefficients_msg
	)		`[protected, virtual]`

Definition at line 569 of file environment_plane_modeling_nodelet.cpp.

std::vector< GridPlane::Ptr > jsk_pcl_ros::EnvironmentPlaneModeling::erodeFiltering ( std::vector< GridPlane::Ptr > & grid_maps ) [protected, virtual]

Definition at line 342 of file environment_plane_modeling_nodelet.cpp.

void jsk_pcl_ros::EnvironmentPlaneModeling::inputCallback	(	const sensor_msgs::PointCloud2::ConstPtr &	cloud_msg,
		const sensor_msgs::PointCloud2::ConstPtr &	full_cloud_msg,
		const jsk_recognition_msgs::PolygonArray::ConstPtr &	polygon_msg,
		const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &	coefficients_msg,
		const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &	indices_msg
	)		`[protected, virtual]`

main callback function

Definition at line 243 of file environment_plane_modeling_nodelet.cpp.

bool jsk_pcl_ros::EnvironmentPlaneModeling::isValidFrameIds	(	const sensor_msgs::PointCloud2::ConstPtr &	cloud_msg,
		const sensor_msgs::PointCloud2::ConstPtr &	full_cloud_msg,
		const jsk_recognition_msgs::PolygonArray::ConstPtr &	polygon_msg,
		const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &	coefficients_msg,
		const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &	indices_msg
	)		`[protected, virtual]`

Definition at line 146 of file environment_plane_modeling_nodelet.cpp.

int jsk_pcl_ros::EnvironmentPlaneModeling::lookupGroundPlaneForFootprint	(	const std::string &	footprint_frame_id,
		const std_msgs::Header &	header,
		const std::vector< GridPlane::Ptr > &	grid_maps
	)		`[protected, virtual]`

Definition at line 386 of file environment_plane_modeling_nodelet.cpp.

int jsk_pcl_ros::EnvironmentPlaneModeling::lookupGroundPlaneForFootprint	(	const Eigen::Affine3f &	pose,
		const std::vector< GridPlane::Ptr > &	grid_maps
	)		`[protected, virtual]`

Definition at line 353 of file environment_plane_modeling_nodelet.cpp.

std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > jsk_pcl_ros::EnvironmentPlaneModeling::magnifyConvexes ( std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > & convexes ) [protected, virtual]

Magnify jsk_recognition_utils::ConvexPolygons according to maginify_distance_ parameter.

Definition at line 553 of file environment_plane_modeling_nodelet.cpp.

std::vector< GridPlane::Ptr > jsk_pcl_ros::EnvironmentPlaneModeling::morphologicalFiltering ( std::vector< GridPlane::Ptr > & raw_grid_maps ) [protected, virtual]

Definition at line 467 of file environment_plane_modeling_nodelet.cpp.

void jsk_pcl_ros::EnvironmentPlaneModeling::moveBaseSimpleGoalCallback ( const geometry_msgs::PoseStamped::ConstPtr & msg ) [protected, virtual]

Definition at line 184 of file environment_plane_modeling_nodelet.cpp.

void jsk_pcl_ros::EnvironmentPlaneModeling::onInit ( void ) [protected, virtual]

Definition at line 54 of file environment_plane_modeling_nodelet.cpp.

void jsk_pcl_ros::EnvironmentPlaneModeling::printInputData	(	const sensor_msgs::PointCloud2::ConstPtr &	cloud_msg,
		const sensor_msgs::PointCloud2::ConstPtr &	full_cloud_msg,
		const jsk_recognition_msgs::PolygonArray::ConstPtr &	polygon_msg,
		const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &	coefficients_msg,
		const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &	indices_msg
	)		`[protected, virtual]`

Definition at line 131 of file environment_plane_modeling_nodelet.cpp.

void jsk_pcl_ros::EnvironmentPlaneModeling::publishConvexPolygons	(	ros::Publisher &	pub,
		const std_msgs::Header &	header,
		std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > &	convexes
	)		`[protected, virtual]`

Publish array of jsk_recognition_utils::ConvexPolygon::Ptr by using specified publisher.

Definition at line 537 of file environment_plane_modeling_nodelet.cpp.

void jsk_pcl_ros::EnvironmentPlaneModeling::publishConvexPolygonsBoundaries	(	ros::Publisher &	pub,
		const std_msgs::Header &	header,
		std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > &	convexes
	)		`[protected, virtual]`

Definition at line 480 of file environment_plane_modeling_nodelet.cpp.

void jsk_pcl_ros::EnvironmentPlaneModeling::publishGridMaps	(	ros::Publisher &	pub,
		const std_msgs::Header &	header,
		std::vector< GridPlane::Ptr > &	grids
	)		`[protected, virtual]`

Publish array of GridPlane::Ptr by using specified publisher.

Definition at line 500 of file environment_plane_modeling_nodelet.cpp.

virtual void jsk_pcl_ros::EnvironmentPlaneModeling::subscribe ( ) [inline, protected, virtual]

subscription callback function of jsk_topic_tools::DiagnosticNodelet. This method is empty method because EnvironmentPlaneModeling needs to always run

Definition at line 100 of file environment_plane_modeling.h.

virtual void jsk_pcl_ros::EnvironmentPlaneModeling::unsubscribe ( ) [inline, protected, virtual]

unsubscription callback function of jsk_topic_tools::DiagnosticNodelet. This method is empty method because EnvironmentPlaneModeling needs to always run

Definition at line 107 of file environment_plane_modeling.h.