$search
#include <HumanoidLocalization.h>
Definition at line 84 of file HumanoidLocalization.h.
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.
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.
[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. |
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
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.
std::string humanoid_localization::HumanoidLocalization::m_baseFootprintId [protected] |
Definition at line 221 of file HumanoidLocalization.h.
std::string humanoid_localization::HumanoidLocalization::m_baseFrameId [protected] |
Definition at line 220 of file HumanoidLocalization.h.
bool humanoid_localization::HumanoidLocalization::m_bestParticleAsMean [protected] |
Definition at line 246 of file HumanoidLocalization.h.
int humanoid_localization::HumanoidLocalization::m_bestParticleIdx [protected] |
Definition at line 241 of file HumanoidLocalization.h.
Definition at line 211 of file HumanoidLocalization.h.
bool humanoid_localization::HumanoidLocalization::m_constrainMotionRP [protected] |
< True = do not estimate height, directly use odometry pose
Definition at line 273 of file HumanoidLocalization.h.
bool humanoid_localization::HumanoidLocalization::m_constrainMotionZ [protected] |
Definition at line 272 of file HumanoidLocalization.h.
Definition at line 211 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_filterMaxRange [protected] |
Definition at line 236 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_filterMinRange [protected] |
Definition at line 237 of file HumanoidLocalization.h.
std::string humanoid_localization::HumanoidLocalization::m_globalFrameId [protected] |
Definition at line 222 of file HumanoidLocalization.h.
Definition at line 215 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_headPitchRotationLastScan [protected] |
absolute, summed pitch angle since last measurement integraton
Definition at line 269 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_headYawRotationLastScan [protected] |
absolute, summed yaw angle since last measurement integraton
Definition at line 267 of file HumanoidLocalization.h.
Definition at line 214 of file HumanoidLocalization.h.
bool humanoid_localization::HumanoidLocalization::m_initFromTruepose [protected] |
Definition at line 225 of file HumanoidLocalization.h.
bool humanoid_localization::HumanoidLocalization::m_initGlobal [protected] |
Definition at line 249 of file HumanoidLocalization.h.
bool humanoid_localization::HumanoidLocalization::m_initialized [protected] |
Definition at line 248 of file HumanoidLocalization.h.
Definition at line 233 of file HumanoidLocalization.h.
Definition at line 232 of file HumanoidLocalization.h.
tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>* humanoid_localization::HumanoidLocalization::m_initPoseFilter [protected] |
Definition at line 209 of file HumanoidLocalization.h.
bool humanoid_localization::HumanoidLocalization::m_initPoseRealZRP [protected] |
Definition at line 234 of file HumanoidLocalization.h.
message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* humanoid_localization::HumanoidLocalization::m_initPoseSub [protected] |
Definition at line 208 of file HumanoidLocalization.h.
tf::MessageFilter<sensor_msgs::LaserScan>* humanoid_localization::HumanoidLocalization::m_laserFilter [protected] |
Definition at line 205 of file HumanoidLocalization.h.
message_filters::Subscriber<sensor_msgs::LaserScan>* humanoid_localization::HumanoidLocalization::m_laserSub [protected] |
Definition at line 204 of file HumanoidLocalization.h.
boost::circular_buffer<sensor_msgs::Imu> humanoid_localization::HumanoidLocalization::m_lastIMUMsgBuffer [protected] |
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.
boost::shared_ptr<MapModel> humanoid_localization::HumanoidLocalization::m_mapModel [protected] |
Definition at line 199 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_minParticleWeight [protected] |
Definition at line 231 of file HumanoidLocalization.h.
boost::shared_ptr<MotionModel> humanoid_localization::HumanoidLocalization::m_motionModel [protected] |
Definition at line 197 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_nEffFactor [protected] |
Definition at line 230 of file HumanoidLocalization.h.
Definition at line 211 of file HumanoidLocalization.h.
Definition at line 201 of file HumanoidLocalization.h.
int humanoid_localization::HumanoidLocalization::m_numParticles [protected] |
Definition at line 226 of file HumanoidLocalization.h.
int humanoid_localization::HumanoidLocalization::m_numSensorBeams [protected] |
Definition at line 227 of file HumanoidLocalization.h.
boost::shared_ptr<ObservationModel> humanoid_localization::HumanoidLocalization::m_observationModel [protected] |
Definition at line 198 of file HumanoidLocalization.h.
Definition at line 256 of file HumanoidLocalization.h.
Definition at line 255 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_observationThresholdRot [protected] |
Definition at line 254 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_observationThresholdTrans [protected] |
Definition at line 253 of file HumanoidLocalization.h.
std::string humanoid_localization::HumanoidLocalization::m_odomFrameId [protected] |
Definition at line 219 of file HumanoidLocalization.h.
Definition at line 242 of file HumanoidLocalization.h.
Definition at line 240 of file HumanoidLocalization.h.
bool humanoid_localization::HumanoidLocalization::m_paused [protected] |
Definition at line 250 of file HumanoidLocalization.h.
Definition at line 202 of file HumanoidLocalization.h.
Definition at line 215 of file HumanoidLocalization.h.
tf::MessageFilter< PointCloud >* humanoid_localization::HumanoidLocalization::m_pointCloudFilter [protected] |
Definition at line 207 of file HumanoidLocalization.h.
message_filters::Subscriber< PointCloud >* humanoid_localization::HumanoidLocalization::m_pointCloudSub [protected] |
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.
bool humanoid_localization::HumanoidLocalization::m_receivedSensorData [protected] |
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.
double humanoid_localization::HumanoidLocalization::m_sensorSampleDist [protected] |
Definition at line 228 of file HumanoidLocalization.h.
bool humanoid_localization::HumanoidLocalization::m_syncedTruepose [protected] |
Definition at line 251 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_temporalSamplingRange [protected] |
Definition at line 257 of file HumanoidLocalization.h.
Definition at line 217 of file HumanoidLocalization.h.
Definition at line 216 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_transformTolerance [protected] |
Definition at line 258 of file HumanoidLocalization.h.
bool humanoid_localization::HumanoidLocalization::m_useIMU [protected] |
True = use IMU for initialization and observation models, false = use orientation from odometry.
Definition at line 271 of file HumanoidLocalization.h.
bool humanoid_localization::HumanoidLocalization::m_useRaycasting [protected] |
Definition at line 224 of file HumanoidLocalization.h.