Nodelet implementation of jsk_pcl/EnvironmentPlaneModeling. More...

#include <environment_plane_modeling.h>

Inheritance diagram for jsk_pcl_ros::EnvironmentPlaneModeling:

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 ()

virtual	~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 More...

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

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

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 Eigen::Affine3f &pose, const std::vector< GridPlane::Ptr > &grid_maps)

virtual int	lookupGroundPlaneForFootprint (const std::string &footprint_frame_id, const std_msgs::Header &header, 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. More...

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

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

virtual void	subscribe ()
	subscription callback function of jsk_topic_tools::DiagnosticNodelet. This method is empty method because EnvironmentPlaneModeling needs to always run More...

virtual void	unsubscribe ()
	unsubscription callback function of jsk_topic_tools::DiagnosticNodelet. This method is empty method because EnvironmentPlaneModeling needs to always run More...

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 112 of file environment_plane_modeling.h.

Member Typedef Documentation

◆ Config

typedef EnvironmentPlaneModelingConfig jsk_pcl_ros::EnvironmentPlaneModeling::Config

Definition at line 147 of file environment_plane_modeling.h.

◆ SyncPolicy

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 154 of file environment_plane_modeling.h.

Constructor & Destructor Documentation

◆ EnvironmentPlaneModeling()

jsk_pcl_ros::EnvironmentPlaneModeling::EnvironmentPlaneModeling ( )

inline

Definition at line 155 of file environment_plane_modeling.h.

◆ ~EnvironmentPlaneModeling()

jsk_pcl_ros::EnvironmentPlaneModeling::~EnvironmentPlaneModeling ( )

virtual

Definition at line 118 of file environment_plane_modeling_nodelet.cpp.

Member Function Documentation

◆ boundingBoxCallback()

void jsk_pcl_ros::EnvironmentPlaneModeling::boundingBoxCallback ( const jsk_recognition_msgs::BoundingBox::ConstPtr & box_array )

protectedvirtual

Definition at line 247 of file environment_plane_modeling_nodelet.cpp.

◆ buildGridPlanes()

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
	)

protectedvirtual

make GridPlane from jsk_recognition_utils::ConvexPolygon and PointCloud

Definition at line 527 of file environment_plane_modeling_nodelet.cpp.

◆ completeFootprintRegion()

std::vector< GridPlane::Ptr > jsk_pcl_ros::EnvironmentPlaneModeling::completeFootprintRegion	(	const std_msgs::Header &	header,
		std::vector< GridPlane::Ptr > &	grid_maps
	)

protectedvirtual

Definition at line 442 of file environment_plane_modeling_nodelet.cpp.

◆ completeGridMapByBoundingBox()

GridPlane::Ptr jsk_pcl_ros::EnvironmentPlaneModeling::completeGridMapByBoundingBox	(	const jsk_recognition_msgs::BoundingBox::ConstPtr &	box,
		const std_msgs::Header &	header,
		GridPlane::Ptr	grid_map
	)

protectedvirtual

Definition at line 413 of file environment_plane_modeling_nodelet.cpp.

◆ configCallback()

void jsk_pcl_ros::EnvironmentPlaneModeling::configCallback	(	Config &	config,
		uint32_t	level
	)

protectedvirtual

Callback method of dynamic reconfigure.

Definition at line 129 of file environment_plane_modeling_nodelet.cpp.

◆ convertToConvexPolygons()

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
	)

protectedvirtual

Definition at line 580 of file environment_plane_modeling_nodelet.cpp.

◆ erodeFiltering()

std::vector< GridPlane::Ptr > jsk_pcl_ros::EnvironmentPlaneModeling::erodeFiltering ( std::vector< GridPlane::Ptr > & grid_maps )

protectedvirtual

Definition at line 353 of file environment_plane_modeling_nodelet.cpp.

◆ inputCallback()

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
	)

protectedvirtual

main callback function

Definition at line 254 of file environment_plane_modeling_nodelet.cpp.

◆ isValidFrameIds()

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
	)

protectedvirtual

Definition at line 157 of file environment_plane_modeling_nodelet.cpp.

◆ lookupGroundPlaneForFootprint() [1/2]

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

protectedvirtual

Definition at line 364 of file environment_plane_modeling_nodelet.cpp.

◆ lookupGroundPlaneForFootprint() [2/2]

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

protectedvirtual

Definition at line 397 of file environment_plane_modeling_nodelet.cpp.

◆ magnifyConvexes()

std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > jsk_pcl_ros::EnvironmentPlaneModeling::magnifyConvexes ( std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > & convexes )

protectedvirtual

Magnify jsk_recognition_utils::ConvexPolygons according to maginify_distance_ parameter.

Definition at line 564 of file environment_plane_modeling_nodelet.cpp.

◆ morphologicalFiltering()

std::vector< GridPlane::Ptr > jsk_pcl_ros::EnvironmentPlaneModeling::morphologicalFiltering ( std::vector< GridPlane::Ptr > & raw_grid_maps )

protectedvirtual

Definition at line 478 of file environment_plane_modeling_nodelet.cpp.

◆ moveBaseSimpleGoalCallback()

void jsk_pcl_ros::EnvironmentPlaneModeling::moveBaseSimpleGoalCallback ( const geometry_msgs::PoseStamped::ConstPtr & msg )

protectedvirtual

Definition at line 195 of file environment_plane_modeling_nodelet.cpp.

◆ onInit()

void jsk_pcl_ros::EnvironmentPlaneModeling::onInit ( )

protectedvirtual

Definition at line 54 of file environment_plane_modeling_nodelet.cpp.

◆ printInputData()

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
	)

protectedvirtual

Definition at line 142 of file environment_plane_modeling_nodelet.cpp.

◆ publishConvexPolygons()

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

protectedvirtual

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

Definition at line 548 of file environment_plane_modeling_nodelet.cpp.

◆ publishConvexPolygonsBoundaries()

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

protectedvirtual

Definition at line 491 of file environment_plane_modeling_nodelet.cpp.

◆ publishGridMaps()

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

protectedvirtual

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

Definition at line 511 of file environment_plane_modeling_nodelet.cpp.

◆ subscribe()

virtual void jsk_pcl_ros::EnvironmentPlaneModeling::subscribe ( )

inlineprotectedvirtual

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

Definition at line 165 of file environment_plane_modeling.h.

◆ unsubscribe()

virtual void jsk_pcl_ros::EnvironmentPlaneModeling::unsubscribe ( )

inlineprotectedvirtual

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

Definition at line 172 of file environment_plane_modeling.h.