#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 |
Definition at line 84 of file HumanoidLocalization.h.
humanoid_localization::HumanoidLocalization::HumanoidLocalization | ( | unsigned | randomSeed | ) |
Definition at line 32 of file HumanoidLocalization.cpp.
Definition at line 193 of file HumanoidLocalization.cpp.
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.
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.
[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 864 of file HumanoidLocalization.cpp.
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.
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
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] |
Definition at line 777 of file HumanoidLocalization.cpp.
std::string humanoid_localization::HumanoidLocalization::m_baseFootprintId [protected] |
Definition at line 235 of file HumanoidLocalization.h.
std::string humanoid_localization::HumanoidLocalization::m_baseFrameId [protected] |
Definition at line 234 of file HumanoidLocalization.h.
Definition at line 259 of file HumanoidLocalization.h.
int humanoid_localization::HumanoidLocalization::m_bestParticleIdx [protected] |
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.
double humanoid_localization::HumanoidLocalization::m_filterMaxRange [protected] |
Definition at line 249 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_filterMinRange [protected] |
Definition at line 250 of file HumanoidLocalization.h.
std::string humanoid_localization::HumanoidLocalization::m_globalFrameId [protected] |
Definition at line 236 of file HumanoidLocalization.h.
Definition at line 227 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_groundFilterAngle [protected] |
Definition at line 280 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_groundFilterDistance [protected] |
Definition at line 279 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_groundFilterPlaneDistance [protected] |
Definition at line 281 of file HumanoidLocalization.h.
Definition at line 278 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_headPitchRotationLastScan [protected] |
absolute, summed pitch angle since last measurement integraton
Definition at line 294 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_headYawRotationLastScan [protected] |
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.
tf::MessageFilter<sensor_msgs::LaserScan>* humanoid_localization::HumanoidLocalization::m_laserFilter [protected] |
Definition at line 217 of file HumanoidLocalization.h.
message_filters::Subscriber<sensor_msgs::LaserScan>* humanoid_localization::HumanoidLocalization::m_laserSub [protected] |
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.
boost::shared_ptr<MapModel> humanoid_localization::HumanoidLocalization::m_mapModel [protected] |
Definition at line 211 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_minParticleWeight [protected] |
Definition at line 244 of file HumanoidLocalization.h.
boost::shared_ptr<MotionModel> humanoid_localization::HumanoidLocalization::m_motionModel [protected] |
Definition at line 209 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_nEffFactor [protected] |
Definition at line 243 of file HumanoidLocalization.h.
Definition at line 223 of file HumanoidLocalization.h.
Definition at line 213 of file HumanoidLocalization.h.
int humanoid_localization::HumanoidLocalization::m_numFloorPoints [protected] |
Definition at line 283 of file HumanoidLocalization.h.
int humanoid_localization::HumanoidLocalization::m_numNonFloorPoints [protected] |
Definition at line 284 of file HumanoidLocalization.h.
int humanoid_localization::HumanoidLocalization::m_numParticles [protected] |
Definition at line 240 of file HumanoidLocalization.h.
boost::shared_ptr<ObservationModel> humanoid_localization::HumanoidLocalization::m_observationModel [protected] |
Definition at line 210 of file HumanoidLocalization.h.
Definition at line 269 of file HumanoidLocalization.h.
Definition at line 268 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_observationThresholdRot [protected] |
Definition at line 267 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_observationThresholdTrans [protected] |
Definition at line 266 of file HumanoidLocalization.h.
std::string humanoid_localization::HumanoidLocalization::m_odomFrameId [protected] |
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.
tf::MessageFilter< PointCloud >* humanoid_localization::HumanoidLocalization::m_pointCloudFilter [protected] |
Definition at line 219 of file HumanoidLocalization.h.
message_filters::Subscriber< PointCloud >* humanoid_localization::HumanoidLocalization::m_pointCloudSub [protected] |
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.
double humanoid_localization::HumanoidLocalization::m_sensorSampleDist [protected] |
Definition at line 241 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_sensorSampleDistGroundFactor [protected] |
Definition at line 282 of file HumanoidLocalization.h.
Definition at line 264 of file HumanoidLocalization.h.
std::string humanoid_localization::HumanoidLocalization::m_targetFrameId [protected] |
Definition at line 233 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_temporalSamplingRange [protected] |
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.
double humanoid_localization::HumanoidLocalization::m_timerPeriod [protected] |
Definition at line 302 of file HumanoidLocalization.h.
double humanoid_localization::HumanoidLocalization::m_transformTolerance [protected] |
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.