#include <HumanoidLocalization.h>

Public Member Functions
unsigned	getBestParticleIdx () const
	Returns index of particle with highest weight (log or normal scale)
tf::Pose	getBestParticlePose () const
	Returns the 6D pose of the best particle (highest weight)
tf::Pose	getMeanParticlePose () const
	Returns the 6D pose of the weighted mean particle.
tf::Pose	getParticlePose (unsigned particleIdx) const
	Returns the 6D pose of a particle.
bool	globalLocalizationCallback (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
	HumanoidLocalization (unsigned randomSeed)
void	imuCallback (const sensor_msgs::ImuConstPtr &msg)
void	initGlobal ()
	function call for global initialization (called by globalLocalizationCallback)
void	initPoseCallback (const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
virtual void	laserCallback (const sensor_msgs::LaserScanConstPtr &msg)
void	pauseLocalizationCallback (const std_msgs::BoolConstPtr &msg)
bool	pauseLocalizationSrvCallback (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
	pause localization by service call
virtual void	pointCloudCallback (const PointCloud::ConstPtr &msg)
void	resample (unsigned numParticles=0)
bool	resumeLocalizationSrvCallback (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
	unpause localization by service call
virtual	~HumanoidLocalization ()
Static Public Member Functions
static void	filterGroundPlane (const PointCloud &pc, PointCloud &ground, PointCloud &nonground, double groundFilterDistance, double groundFilterAngle, double groundFilterPlaneDistance)
Protected Member Functions
unsigned	computeBeamStep (unsigned numBeams) const
void	constrainMotion (const tf::Pose &odomPose)
int	filterUniform (const PointCloud &cloud_in, PointCloud &cloud_out, int numSamples) const
double	getCumParticleWeight () const
	cumulative weight of all particles (=1 when normalized)
bool	getImuMsg (const ros::Time &stamp, ros::Time &imuStamp, double &angleX, double &angleY) const
void	initZRP (double &z, double &roll, double &pitch)
bool	isAboveMotionThreshold (const tf::Pose &odomTransform)
bool	localizeWithMeasurement (const PointCloud &pc_filtered, const std::vector< float > &ranges, double max_range)
bool	lookupPoseHeight (const ros::Time &t, double &poseHeight) const
double	nEff () const
void	normalizeWeights ()
void	prepareGeneralPointCloud (const PointCloud::ConstPtr &msg, PointCloud &pc, std::vector< float > &ranges) const
void	prepareLaserPointCloud (const sensor_msgs::LaserScanConstPtr &laser, PointCloud &pc, std::vector< float > &ranges) const
void	publishPoseEstimate (const ros::Time &time, bool publish_eval)
void	reset ()
void	timerCallback (const ros::TimerEvent &e)
void	toLogForm ()
void	voxelGridSampling (const PointCloud &pc, pcl::PointCloud< int > &sampledIndices, double searchRadius) const
Protected Attributes
std::string	m_baseFootprintId
std::string	m_baseFrameId
bool	m_bestParticleAsMean
int	m_bestParticleIdx
ros::Publisher	m_bestPosePub
bool	m_constrainMotionRP
	< True = do not estimate height, directly use odometry pose
bool	m_constrainMotionZ
ros::Publisher	m_filteredPointCloudPub
double	m_filterMaxRange
double	m_filterMinRange
std::string	m_globalFrameId
ros::ServiceServer	m_globalLocSrv
double	m_groundFilterAngle
double	m_groundFilterDistance
double	m_groundFilterPlaneDistance
bool	m_groundFilterPointCloud
double	m_headPitchRotationLastScan
	absolute, summed pitch angle since last measurement integraton
double	m_headYawRotationLastScan
	absolute, summed yaw angle since last measurement integraton
ros::Subscriber	m_imuSub
bool	m_initFromTruepose
bool	m_initGlobal
bool	m_initialized
Vector6d	m_initNoiseStd
Vector6d	m_initPose
tf::MessageFilter < geometry_msgs::PoseWithCovarianceStamped > *	m_initPoseFilter
bool	m_initPoseRealZRP
message_filters::Subscriber < geometry_msgs::PoseWithCovarianceStamped > *	m_initPoseSub
tf::MessageFilter < sensor_msgs::LaserScan > *	m_laserFilter
message_filters::Subscriber < sensor_msgs::LaserScan > *	m_laserSub
boost::circular_buffer < sensor_msgs::Imu >	m_lastIMUMsgBuffer
ros::Time	m_lastLaserTime
tf::Pose	m_lastLocalizedPose
	sensor data last integrated at this odom pose, to check if moved enough since then
ros::Time	m_lastPointCloudTime
tf::StampedTransform	m_latest_transform
boost::shared_ptr< MapModel >	m_mapModel
double	m_minParticleWeight
boost::shared_ptr< MotionModel >	m_motionModel
double	m_nEffFactor
ros::Publisher	m_nEffPub
ros::NodeHandle	m_nh
int	m_numFloorPoints
int	m_numNonFloorPoints
int	m_numParticles
boost::shared_ptr < ObservationModel >	m_observationModel
double	m_observationThresholdHeadPitchRot
double	m_observationThresholdHeadYawRot
double	m_observationThresholdRot
double	m_observationThresholdTrans
std::string	m_odomFrameId
tf::Pose	m_odomPose
Particles	m_particles
bool	m_paused
ros::Subscriber	m_pauseIntegrationSub
ros::ServiceServer	m_pauseLocSrv
tf::MessageFilter< PointCloud > *	m_pointCloudFilter
message_filters::Subscriber < PointCloud > *	m_pointCloudSub
geometry_msgs::PoseArray	m_poseArray
ros::Publisher	m_poseArrayPub
ros::Publisher	m_poseEvalPub
ros::Publisher	m_poseOdomPub
ros::Publisher	m_posePub
ros::Publisher	m_poseTruePub
ros::NodeHandle	m_privateNh
bool	m_receivedSensorData
ros::ServiceServer	m_resumeLocSrv
EngineT	m_rngEngine
NormalGeneratorT	m_rngNormal
	standard normal distribution
UniformGeneratorT	m_rngUniform
	uniform distribution [0:1]
double	m_sensorSampleDist
double	m_sensorSampleDistGroundFactor
bool	m_syncedTruepose
std::string	m_targetFrameId
double	m_temporalSamplingRange
tf::TransformBroadcaster	m_tfBroadcaster
tf::TransformListener	m_tfListener
ros::Timer	m_timer
double	m_timerPeriod
double	m_transformTolerance
bool	m_useIMU
	True = use IMU for initialization and observation models, false = use orientation from odometry.
bool	m_useRaycasting
bool	m_useTimer
	< True = do not estimate roll and pitch, directly use odometry pose

Detailed Description

Definition at line 84 of file HumanoidLocalization.h.

Constructor & Destructor Documentation

humanoid_localization::HumanoidLocalization::HumanoidLocalization ( unsigned randomSeed )

Definition at line 32 of file HumanoidLocalization.cpp.

humanoid_localization::HumanoidLocalization::~HumanoidLocalization ( ) [virtual]

Definition at line 193 of file HumanoidLocalization.cpp.

Member Function Documentation

unsigned humanoid_localization::HumanoidLocalization::computeBeamStep ( unsigned numBeams ) const [protected]

void humanoid_localization::HumanoidLocalization::constrainMotion ( const tf::Pose & odomPose ) [protected]

Definition at line 371 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::filterGroundPlane	(	const PointCloud &	pc,
		PointCloud &	ground,
		PointCloud &	nonground,
		double	groundFilterDistance,
		double	groundFilterAngle,
		double	groundFilterPlaneDistance
	)		`[static]`

Definition at line 571 of file HumanoidLocalization.cpp.

int humanoid_localization::HumanoidLocalization::filterUniform	(	const PointCloud &	cloud_in,
		PointCloud &	cloud_out,
		int	numSamples
	)		const `[protected]`

Definition at line 552 of file HumanoidLocalization.cpp.

unsigned humanoid_localization::HumanoidLocalization::getBestParticleIdx ( ) const

Returns index of particle with highest weight (log or normal scale)

Definition at line 1281 of file HumanoidLocalization.cpp.

tf::Pose humanoid_localization::HumanoidLocalization::getBestParticlePose ( ) const

Returns the 6D pose of the best particle (highest weight)

Definition at line 1294 of file HumanoidLocalization.cpp.

double humanoid_localization::HumanoidLocalization::getCumParticleWeight ( ) const [protected]

cumulative weight of all particles (=1 when normalized)

Definition at line 1130 of file HumanoidLocalization.cpp.

bool humanoid_localization::HumanoidLocalization::getImuMsg	(	const ros::Time &	stamp,
		ros::Time &	imuStamp,
		double &	angleX,
		double &	angleY
	)		const `[protected]`

Returns the IMU message with stamp closest to a given stamp.

Parameters:

[in]	stamp	Timestamp to search.
[out]	imuStamp	Stamp of closest IMU message (or interpolation of two IMU messages).
[out]	angleX	Interpolated roll angle.
[out]	angleY	Interpolated pitch angle.

Returns:: Success.

Definition at line 864 of file HumanoidLocalization.cpp.

tf::Pose humanoid_localization::HumanoidLocalization::getMeanParticlePose ( ) const

Returns the 6D pose of the weighted mean particle.

Definition at line 1298 of file HumanoidLocalization.cpp.

tf::Pose humanoid_localization::HumanoidLocalization::getParticlePose ( unsigned particleIdx ) const

Returns the 6D pose of a particle.

Definition at line 1290 of file HumanoidLocalization.cpp.

bool humanoid_localization::HumanoidLocalization::globalLocalizationCallback	(	std_srvs::Empty::Request &	req,
		std_srvs::Empty::Response &	res
	)

Definition at line 1062 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::imuCallback ( const sensor_msgs::ImuConstPtr & msg )

Definition at line 860 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::initGlobal ( )

function call for global initialization (called by globalLocalizationCallback)

Definition at line 1181 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::initPoseCallback ( const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg )

Definition at line 913 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::initZRP	(	double &	z,
		double &	roll,
		double &	pitch
	)		`[protected]`

Initializes z, roll and pitch values either from parameters or fromo real (odom) values

Definition at line 290 of file HumanoidLocalization.cpp.

bool humanoid_localization::HumanoidLocalization::isAboveMotionThreshold ( const tf::Pose & odomTransform ) [protected]

Definition at line 398 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::laserCallback ( const sensor_msgs::LaserScanConstPtr & msg ) [virtual]

absolute, current odom pose

Definition at line 320 of file HumanoidLocalization.cpp.

bool humanoid_localization::HumanoidLocalization::localizeWithMeasurement	(	const PointCloud &	pc_filtered,
		const std::vector< float > &	ranges,
		double	max_range
	)		`[protected]`

Definition at line 408 of file HumanoidLocalization.cpp.

bool humanoid_localization::HumanoidLocalization::lookupPoseHeight	(	const ros::Time &	t,
		double &	poseHeight
	)		const `[protected]`

Definition at line 1396 of file HumanoidLocalization.cpp.

double humanoid_localization::HumanoidLocalization::nEff ( ) const [protected]

nEff - returns the number of effective particles = 1/sum(w_i^2)

Needed for selective resampling (Doucet 98, Arulampalam 01), when nEff < n/2

Definition at line 1326 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::normalizeWeights ( ) [protected]

Normalizes the weights and transforms from log to normal scale m_minWeight gives the lower bound for weight (normal scale). No adjustment will be done for minWeight = 0 (default)

Definition at line 1070 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::pauseLocalizationCallback ( const std_msgs::BoolConstPtr & msg )

Definition at line 1348 of file HumanoidLocalization.cpp.

bool humanoid_localization::HumanoidLocalization::pauseLocalizationSrvCallback	(	std_srvs::Empty::Request &	req,
		std_srvs::Empty::Response &	res
	)

pause localization by service call

Definition at line 1370 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::pointCloudCallback ( const PointCloud::ConstPtr & msg ) [virtual]

absolute, current odom pose

Definition at line 787 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::prepareGeneralPointCloud	(	const PointCloud::ConstPtr &	msg,
		PointCloud &	pc,
		std::vector< float > &	ranges
	)		const `[protected]`

Prepares a PointCloud msg to be integrated into the observations model. Filters near range, floor and subsamples a sparse point cloud (out of m_numSensorBeams points)

Definition at line 679 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::prepareLaserPointCloud	(	const sensor_msgs::LaserScanConstPtr &	laser,
		PointCloud &	pc,
		std::vector< float > &	ranges
	)		const `[protected]`

Prepares a LaserScan msg to be integrated into the observations model. Filters near range measurements out and creates a sparse point cloud (out of m_numSensorBeams points)

Definition at line 491 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::publishPoseEstimate	(	const ros::Time &	time,
		bool	publish_eval
	)		`[protected]`

Definition at line 1199 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::resample ( unsigned numParticles = 0 )

Importance sampling from m_particles according to weights, resets weight to 1/numParticles. Uses low variance sampling

Parameters:

numParticles how many particles to sample, 0 (default): keep size of particle distribution

Definition at line 1141 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::reset ( ) [protected]

General reset of the filter: sets pose around initial pose (or truepose, if requested) and resets the internal state. Calls initPoseCallback().

Definition at line 213 of file HumanoidLocalization.cpp.

bool humanoid_localization::HumanoidLocalization::resumeLocalizationSrvCallback	(	std_srvs::Empty::Request &	req,
		std_srvs::Empty::Response &	res
	)

unpause localization by service call

Definition at line 1382 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::timerCallback ( const ros::TimerEvent & e ) [protected]

Definition at line 206 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::toLogForm ( ) [protected]

Converts particles into log scale

Definition at line 1339 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::voxelGridSampling	(	const PointCloud &	pc,
		pcl::PointCloud< int > &	sampledIndices,
		double	searchRadius
	)		const `[protected]`