Public Member Functions | Static Public Member Functions | Protected Member Functions | Protected Attributes
humanoid_localization::HumanoidLocalization Class Reference

#include <HumanoidLocalization.h>

List of all members.

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< MapModelm_mapModel
double m_minParticleWeight
boost::shared_ptr< MotionModelm_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

Definition at line 32 of file HumanoidLocalization.cpp.

Definition at line 193 of file HumanoidLocalization.cpp.


Member Function Documentation

unsigned humanoid_localization::HumanoidLocalization::computeBeamStep ( unsigned  numBeams) const [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.

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

Definition at line 1281 of file HumanoidLocalization.cpp.

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

Definition at line 1294 of file HumanoidLocalization.cpp.

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]stampTimestamp to search.
[out]imuStampStamp of closest IMU message (or interpolation of two IMU messages).
[out]angleXInterpolated roll angle.
[out]angleYInterpolated pitch angle.
Returns:
Success.

Definition at line 864 of file HumanoidLocalization.cpp.

Returns the 6D pose of the weighted mean particle.

Definition at line 1298 of file HumanoidLocalization.cpp.

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.

Definition at line 860 of file HumanoidLocalization.cpp.

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.

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.

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.

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:
numParticleshow many particles to sample, 0 (default): keep size of particle distribution

Definition at line 1141 of file HumanoidLocalization.cpp.

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.

Definition at line 206 of file HumanoidLocalization.cpp.

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]

Definition at line 777 of file HumanoidLocalization.cpp.


Member Data Documentation

Definition at line 235 of file HumanoidLocalization.h.

Definition at line 234 of file HumanoidLocalization.h.

Definition at line 259 of file HumanoidLocalization.h.

Definition at line 254 of file HumanoidLocalization.h.

Definition at line 223 of file HumanoidLocalization.h.

< True = do not estimate height, directly use odometry pose

Definition at line 298 of file HumanoidLocalization.h.

Definition at line 297 of file HumanoidLocalization.h.

Definition at line 223 of file HumanoidLocalization.h.

Definition at line 249 of file HumanoidLocalization.h.

Definition at line 250 of file HumanoidLocalization.h.

Definition at line 236 of file HumanoidLocalization.h.

Definition at line 227 of file HumanoidLocalization.h.

Definition at line 280 of file HumanoidLocalization.h.

Definition at line 279 of file HumanoidLocalization.h.

Definition at line 281 of file HumanoidLocalization.h.

Definition at line 278 of file HumanoidLocalization.h.

absolute, summed pitch angle since last measurement integraton

Definition at line 294 of file HumanoidLocalization.h.

absolute, summed yaw angle since last measurement integraton

Definition at line 292 of file HumanoidLocalization.h.

Definition at line 226 of file HumanoidLocalization.h.

Definition at line 239 of file HumanoidLocalization.h.

Definition at line 262 of file HumanoidLocalization.h.

Definition at line 261 of file HumanoidLocalization.h.

Definition at line 246 of file HumanoidLocalization.h.

Definition at line 245 of file HumanoidLocalization.h.

tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>* humanoid_localization::HumanoidLocalization::m_initPoseFilter [protected]

Definition at line 221 of file HumanoidLocalization.h.

Definition at line 247 of file HumanoidLocalization.h.

message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* humanoid_localization::HumanoidLocalization::m_initPoseSub [protected]

Definition at line 220 of file HumanoidLocalization.h.

Definition at line 217 of file HumanoidLocalization.h.

Definition at line 216 of file HumanoidLocalization.h.

boost::circular_buffer<sensor_msgs::Imu> humanoid_localization::HumanoidLocalization::m_lastIMUMsgBuffer [protected]

Definition at line 257 of file HumanoidLocalization.h.

Definition at line 272 of file HumanoidLocalization.h.

sensor data last integrated at this odom pose, to check if moved enough since then

Definition at line 288 of file HumanoidLocalization.h.

Definition at line 273 of file HumanoidLocalization.h.

Definition at line 289 of file HumanoidLocalization.h.

Definition at line 211 of file HumanoidLocalization.h.

Definition at line 244 of file HumanoidLocalization.h.

Definition at line 209 of file HumanoidLocalization.h.

Definition at line 243 of file HumanoidLocalization.h.

Definition at line 223 of file HumanoidLocalization.h.

Definition at line 213 of file HumanoidLocalization.h.

Definition at line 283 of file HumanoidLocalization.h.

Definition at line 284 of file HumanoidLocalization.h.

Definition at line 240 of file HumanoidLocalization.h.

Definition at line 210 of file HumanoidLocalization.h.

Definition at line 269 of file HumanoidLocalization.h.

Definition at line 268 of file HumanoidLocalization.h.

Definition at line 267 of file HumanoidLocalization.h.

Definition at line 266 of file HumanoidLocalization.h.

Definition at line 232 of file HumanoidLocalization.h.

Definition at line 255 of file HumanoidLocalization.h.

Definition at line 253 of file HumanoidLocalization.h.

Definition at line 263 of file HumanoidLocalization.h.

Definition at line 214 of file HumanoidLocalization.h.

Definition at line 227 of file HumanoidLocalization.h.

Definition at line 219 of file HumanoidLocalization.h.

Definition at line 218 of file HumanoidLocalization.h.

geometry_msgs::PoseArray humanoid_localization::HumanoidLocalization::m_poseArray [protected]

Definition at line 256 of file HumanoidLocalization.h.

Definition at line 223 of file HumanoidLocalization.h.

Definition at line 223 of file HumanoidLocalization.h.

Definition at line 223 of file HumanoidLocalization.h.

Definition at line 223 of file HumanoidLocalization.h.

Definition at line 223 of file HumanoidLocalization.h.

Definition at line 213 of file HumanoidLocalization.h.

Definition at line 260 of file HumanoidLocalization.h.

Definition at line 227 of file HumanoidLocalization.h.

Definition at line 204 of file HumanoidLocalization.h.

standard normal distribution

Definition at line 206 of file HumanoidLocalization.h.

uniform distribution [0:1]

Definition at line 208 of file HumanoidLocalization.h.

Definition at line 241 of file HumanoidLocalization.h.

Definition at line 282 of file HumanoidLocalization.h.

Definition at line 264 of file HumanoidLocalization.h.

Definition at line 233 of file HumanoidLocalization.h.

Definition at line 270 of file HumanoidLocalization.h.

Definition at line 229 of file HumanoidLocalization.h.

Definition at line 228 of file HumanoidLocalization.h.

Definition at line 230 of file HumanoidLocalization.h.

Definition at line 302 of file HumanoidLocalization.h.

Definition at line 271 of file HumanoidLocalization.h.

True = use IMU for initialization and observation models, false = use orientation from odometry.

Definition at line 296 of file HumanoidLocalization.h.

Definition at line 238 of file HumanoidLocalization.h.

< True = do not estimate roll and pitch, directly use odometry pose

Definition at line 301 of file HumanoidLocalization.h.


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


humanoid_localization
Author(s): Armin Hornung, Stefan Osswald, Daniel Maier
autogenerated on Wed Aug 26 2015 11:54:40