$search

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

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_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
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_numParticles
int m_numSensorBeams
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
UniformGeneratorT m_rngUniform
double m_sensorSampleDist
bool m_syncedTruepose
double m_temporalSamplingRange
tf::TransformBroadcaster m_tfBroadcaster
tf::TransformListener m_tfListener
double m_transformTolerance
bool m_useIMU
 True = use IMU for initialization and observation models, false = use orientation from odometry.
bool m_useRaycasting

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 166 of file HumanoidLocalization.cpp.


Member Function Documentation

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

Definition at line 702 of file HumanoidLocalization.cpp.

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

Definition at line 329 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 529 of file HumanoidLocalization.cpp.

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

Definition at line 510 of file HumanoidLocalization.cpp.

unsigned humanoid_localization::HumanoidLocalization::getBestParticleIdx (  )  const

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

Definition at line 1203 of file HumanoidLocalization.cpp.

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

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

Definition at line 1216 of file HumanoidLocalization.cpp.

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

cumulative weight of all particles (=1 when normalized)

Definition at line 1055 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 793 of file HumanoidLocalization.cpp.

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

Returns the 6D pose of the weighted mean particle.

Definition at line 1220 of file HumanoidLocalization.cpp.

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

Returns the 6D pose of a particle.

Definition at line 1212 of file HumanoidLocalization.cpp.

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

Definition at line 987 of file HumanoidLocalization.cpp.

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

Definition at line 789 of file HumanoidLocalization.cpp.

void humanoid_localization::HumanoidLocalization::initGlobal (  ) 

function call for global initialization (called by globalLocalizationCallback)

Definition at line 1106 of file HumanoidLocalization.cpp.

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

Definition at line 842 of file HumanoidLocalization.cpp.

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

Definition at line 356 of file HumanoidLocalization.cpp.

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

absolute, current odom pose

Definition at line 278 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 366 of file HumanoidLocalization.cpp.

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

Definition at line 1318 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 1248 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 995 of file HumanoidLocalization.cpp.

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

Definition at line 1270 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 1292 of file HumanoidLocalization.cpp.

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

absolute, current odom pose

Definition at line 716 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 637 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 449 of file HumanoidLocalization.cpp.

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

Definition at line 1121 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 1066 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 179 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 1304 of file HumanoidLocalization.cpp.

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

Converts particles into log scale

Definition at line 1261 of file HumanoidLocalization.cpp.


Member Data Documentation

Definition at line 221 of file HumanoidLocalization.h.

Definition at line 220 of file HumanoidLocalization.h.

Definition at line 246 of file HumanoidLocalization.h.

Definition at line 241 of file HumanoidLocalization.h.

Definition at line 211 of file HumanoidLocalization.h.

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

Definition at line 273 of file HumanoidLocalization.h.

Definition at line 272 of file HumanoidLocalization.h.

Definition at line 211 of file HumanoidLocalization.h.

Definition at line 236 of file HumanoidLocalization.h.

Definition at line 237 of file HumanoidLocalization.h.

Definition at line 222 of file HumanoidLocalization.h.

Definition at line 215 of file HumanoidLocalization.h.

absolute, summed pitch angle since last measurement integraton

Definition at line 269 of file HumanoidLocalization.h.

absolute, summed yaw angle since last measurement integraton

Definition at line 267 of file HumanoidLocalization.h.

Definition at line 214 of file HumanoidLocalization.h.

Definition at line 225 of file HumanoidLocalization.h.

Definition at line 249 of file HumanoidLocalization.h.

Definition at line 248 of file HumanoidLocalization.h.

Definition at line 233 of file HumanoidLocalization.h.

Definition at line 232 of file HumanoidLocalization.h.

Definition at line 209 of file HumanoidLocalization.h.

Definition at line 234 of file HumanoidLocalization.h.

Definition at line 208 of file HumanoidLocalization.h.

Definition at line 205 of file HumanoidLocalization.h.

Definition at line 204 of file HumanoidLocalization.h.

Definition at line 244 of file HumanoidLocalization.h.

Definition at line 259 of file HumanoidLocalization.h.

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

Definition at line 264 of file HumanoidLocalization.h.

Definition at line 260 of file HumanoidLocalization.h.

Definition at line 199 of file HumanoidLocalization.h.

Definition at line 231 of file HumanoidLocalization.h.

Definition at line 197 of file HumanoidLocalization.h.

Definition at line 230 of file HumanoidLocalization.h.

Definition at line 211 of file HumanoidLocalization.h.

Definition at line 201 of file HumanoidLocalization.h.

Definition at line 226 of file HumanoidLocalization.h.

Definition at line 227 of file HumanoidLocalization.h.

Definition at line 198 of file HumanoidLocalization.h.

Definition at line 256 of file HumanoidLocalization.h.

Definition at line 255 of file HumanoidLocalization.h.

Definition at line 254 of file HumanoidLocalization.h.

Definition at line 253 of file HumanoidLocalization.h.

Definition at line 219 of file HumanoidLocalization.h.

Definition at line 242 of file HumanoidLocalization.h.

Definition at line 240 of file HumanoidLocalization.h.

Definition at line 250 of file HumanoidLocalization.h.

Definition at line 202 of file HumanoidLocalization.h.

Definition at line 215 of file HumanoidLocalization.h.

Definition at line 207 of file HumanoidLocalization.h.

Definition at line 206 of file HumanoidLocalization.h.

Definition at line 243 of file HumanoidLocalization.h.

Definition at line 211 of file HumanoidLocalization.h.

Definition at line 211 of file HumanoidLocalization.h.

Definition at line 211 of file HumanoidLocalization.h.

Definition at line 211 of file HumanoidLocalization.h.

Definition at line 211 of file HumanoidLocalization.h.

Definition at line 201 of file HumanoidLocalization.h.

Definition at line 247 of file HumanoidLocalization.h.

Definition at line 215 of file HumanoidLocalization.h.

Definition at line 194 of file HumanoidLocalization.h.

Definition at line 195 of file HumanoidLocalization.h.

Definition at line 196 of file HumanoidLocalization.h.

Definition at line 228 of file HumanoidLocalization.h.

Definition at line 251 of file HumanoidLocalization.h.

Definition at line 257 of file HumanoidLocalization.h.

Definition at line 217 of file HumanoidLocalization.h.

Definition at line 216 of file HumanoidLocalization.h.

Definition at line 258 of file HumanoidLocalization.h.

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

Definition at line 271 of file HumanoidLocalization.h.

Definition at line 224 of file HumanoidLocalization.h.


The documentation for this class was generated from the following files:
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


humanoid_localization
Author(s): Armin Hornung, Stefan Osswald, Daniel Maier
autogenerated on Tue Mar 5 11:39:45 2013