HumanoidLocalization.cpp
Go to the documentation of this file.
00001 // SVN $HeadURL$
00002 // SVN $Id$
00003 
00004 /*
00005  * 6D localization for humanoid robots
00006  *
00007  * Copyright 2009-2012 Armin Hornung, University of Freiburg
00008  * http://www.ros.org/wiki/humanoid_localization
00009  *
00010  *
00011  * This program is free software: you can redistribute it and/or modify
00012  * it under the terms of the GNU General Public License as published by
00013  * the Free Software Foundation, version 3.
00014  *
00015  * This program is distributed in the hope that it will be useful,
00016  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  * GNU General Public License for more details.
00019  *
00020  * You should have received a copy of the GNU General Public License
00021  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00022  */
00023 
00024 #include <humanoid_localization/HumanoidLocalization.h>
00025 #include <iostream>
00026 #include <pcl/keypoints/uniform_sampling.h>
00027 
00028 // simple timing benchmark output
00029 #define _BENCH_TIME 0
00030 
00031 namespace humanoid_localization{
00032 HumanoidLocalization::HumanoidLocalization(unsigned randomSeed)
00033 :
00034 m_rngEngine(randomSeed),
00035 m_rngNormal(m_rngEngine, NormalDistributionT(0.0, 1.0)),
00036 m_rngUniform(m_rngEngine, UniformDistributionT(0.0, 1.0)),
00037 m_nh(),m_privateNh("~"),
00038 m_odomFrameId("odom"), m_targetFrameId("odom"), m_baseFrameId("torso"), m_baseFootprintId("base_footprint"), m_globalFrameId("/map"),
00039 m_useRaycasting(true), m_initFromTruepose(false), m_numParticles(500),
00040 m_sensorSampleDist(0.2),
00041 m_nEffFactor(1.0), m_minParticleWeight(0.0),
00042 m_bestParticleIdx(-1), m_lastIMUMsgBuffer(5),
00043 m_bestParticleAsMean(true),
00044 m_receivedSensorData(false), m_initialized(false), m_initGlobal(false), m_paused(false),
00045 m_syncedTruepose(false),
00046 m_observationThresholdTrans(0.1), m_observationThresholdRot(M_PI/6),
00047 m_observationThresholdHeadYawRot(0.1), m_observationThresholdHeadPitchRot(0.1),
00048 m_temporalSamplingRange(0.1), m_transformTolerance(0.1),
00049 m_groundFilterPointCloud(true), m_groundFilterDistance(0.04),
00050 m_groundFilterAngle(0.15), m_groundFilterPlaneDistance(0.07),
00051 m_sensorSampleDistGroundFactor(3),
00052 m_numFloorPoints(20), m_numNonFloorPoints(80),
00053 m_headYawRotationLastScan(0.0), m_headPitchRotationLastScan(0.0),
00054 m_useIMU(false),
00055 m_constrainMotionZ (false), m_constrainMotionRP(false), m_useTimer(false), m_timerPeriod(0.1)
00056 {
00057 
00058    m_latest_transform.setData (tf::Transform(tf::createIdentityQuaternion()) );
00059   // raycasting or endpoint model?
00060   m_privateNh.param("use_raycasting", m_useRaycasting, m_useRaycasting);
00061 
00062   m_privateNh.param("odom_frame_id", m_odomFrameId, m_odomFrameId);
00063   m_privateNh.param("target_frame_id", m_targetFrameId, m_targetFrameId);
00064   m_privateNh.param("base_frame_id", m_baseFrameId, m_baseFrameId);
00065   m_privateNh.param("base_footprint_id", m_baseFootprintId, m_baseFootprintId);
00066   m_privateNh.param("global_frame_id", m_globalFrameId, m_globalFrameId);
00067   m_privateNh.param("init_from_truepose", m_initFromTruepose, m_initFromTruepose);
00068   m_privateNh.param("init_global", m_initGlobal, m_initGlobal);
00069   m_privateNh.param("best_particle_as_mean", m_bestParticleAsMean, m_bestParticleAsMean);
00070   m_privateNh.param("num_particles", m_numParticles, m_numParticles);
00071   m_privateNh.param("neff_factor", m_nEffFactor, m_nEffFactor);
00072   m_privateNh.param("min_particle_weight", m_minParticleWeight, m_minParticleWeight);
00073 
00074   m_privateNh.param("initial_pose/x", m_initPose(0), 0.0);
00075   m_privateNh.param("initial_pose/y", m_initPose(1), 0.0);
00076   m_privateNh.param("initial_pose/z", m_initPose(2), 0.32); // hip height when standing
00077   m_privateNh.param("initial_pose/roll", m_initPose(3), 0.0);
00078   m_privateNh.param("initial_pose/pitch", m_initPose(4), 0.0);
00079   m_privateNh.param("initial_pose/yaw", m_initPose(5), 0.0);
00080   m_privateNh.param("initial_pose_real_zrp", m_initPoseRealZRP, false);
00081 
00082   m_privateNh.param("initial_std/x", m_initNoiseStd(0), 0.1); // 0.1
00083   m_privateNh.param("initial_std/y", m_initNoiseStd(1), 0.1); // 0.1
00084   m_privateNh.param("initial_std/z", m_initNoiseStd(2), 0.02); // 0.02
00085   m_privateNh.param("initial_std/roll", m_initNoiseStd(3), 0.04); // 0.04
00086   m_privateNh.param("initial_std/pitch", m_initNoiseStd(4), 0.04); // 0.04
00087   m_privateNh.param("initial_std_yaw", m_initNoiseStd(5), M_PI/12); // M_PI/12
00088 
00089   if (m_privateNh.hasParam("num_sensor_beams"))
00090     ROS_WARN("Parameter \"num_sensor_beams\" is no longer used, use \"sensor_sampling_dist\" instead");
00091 
00092   // laser observation model parameters:
00093   m_privateNh.param("sensor_sampling_dist", m_sensorSampleDist, m_sensorSampleDist);
00094   m_privateNh.param("max_range", m_filterMaxRange, 30.0);
00095   m_privateNh.param("min_range", m_filterMinRange, 0.05);
00096   ROS_DEBUG("Using a range filter of %f to %f", m_filterMinRange, m_filterMaxRange);
00097 
00098   m_privateNh.param("update_min_trans", m_observationThresholdTrans, m_observationThresholdTrans);
00099   m_privateNh.param("update_min_rot", m_observationThresholdRot, m_observationThresholdRot);
00100   m_privateNh.param("update_min_head_yaw", m_observationThresholdHeadYawRot, m_observationThresholdHeadYawRot);
00101   m_privateNh.param("update_min_head_pitch", m_observationThresholdHeadPitchRot, m_observationThresholdHeadPitchRot);
00102   m_privateNh.param("temporal_sampling_range", m_temporalSamplingRange, m_temporalSamplingRange);
00103   m_privateNh.param("transform_tolerance", m_transformTolerance, m_transformTolerance);
00104 
00105   m_privateNh.param("use_imu", m_useIMU, m_useIMU);
00106   m_privateNh.param("constrain_motion_z", m_constrainMotionZ, m_constrainMotionZ);
00107   m_privateNh.param("constrain_motion_rp", m_constrainMotionRP, m_constrainMotionRP);
00108 
00109   // point cloud observation model parameters
00110   m_privateNh.param("ground_filter_point_cloud", m_groundFilterPointCloud, m_groundFilterPointCloud);
00111   m_privateNh.param("ground_filter_distance", m_groundFilterDistance, m_groundFilterDistance);
00112   m_privateNh.param("ground_filter_angle", m_groundFilterAngle, m_groundFilterAngle); 
00113   m_privateNh.param("ground_filter_plane_distance", m_groundFilterPlaneDistance, m_groundFilterPlaneDistance);
00114   m_privateNh.param("sensor_sampling_dist_ground_factor", m_sensorSampleDistGroundFactor, m_sensorSampleDistGroundFactor);
00115   m_privateNh.param("num_floor_points", m_numFloorPoints, m_numFloorPoints);
00116   m_privateNh.param("num_non_floor_points", m_numNonFloorPoints, m_numNonFloorPoints);
00117   
00118   m_privateNh.param("use_timer", m_useTimer, m_useTimer);
00119   m_privateNh.param("timer_period", m_timerPeriod, m_timerPeriod);
00120 
00121   // motion model parameters
00122 
00123   m_motionModel = boost::shared_ptr<MotionModel>(new MotionModel(&m_privateNh, &m_rngEngine, &m_tfListener, m_odomFrameId, m_baseFrameId));
00124 
00125   if (m_useRaycasting){
00126     m_mapModel = boost::shared_ptr<MapModel>(new OccupancyMap(&m_privateNh));
00127     m_observationModel = boost::shared_ptr<ObservationModel>(new RaycastingModel(&m_privateNh, m_mapModel, &m_rngEngine));
00128   } else{
00129 #ifndef SKIP_ENDPOINT_MODEL
00130     //m_mapModel = boost::shared_ptr<MapModel>(new DistanceMap(&m_privateNh));
00131     m_mapModel = boost::shared_ptr<MapModel>(new OccupancyMap(&m_privateNh));
00132     m_observationModel = boost::shared_ptr<ObservationModel>(new EndpointModel(&m_privateNh, m_mapModel, &m_rngEngine));
00133 #else
00134     ROS_FATAL("EndpointModel not compiled due to missing dynamicEDT3D");
00135     exit(-1);
00136 #endif
00137   }
00138 
00139 
00140   m_particles.resize(m_numParticles);
00141   m_poseArray.poses.resize(m_numParticles);
00142   m_poseArray.header.frame_id = m_globalFrameId;
00143   m_tfListener.clear();
00144 
00145 
00146   // publishers can be advertised first, before needed:
00147   m_posePub = m_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 10);
00148   m_poseEvalPub = m_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose_eval", 10);
00149   m_poseOdomPub = m_privateNh.advertise<geometry_msgs::PoseStamped>("pose_odom_sync", 10);
00150   m_poseArrayPub = m_privateNh.advertise<geometry_msgs::PoseArray>("particlecloud", 10);
00151   m_bestPosePub = m_privateNh.advertise<geometry_msgs::PoseArray>("best_particle", 10);
00152   m_nEffPub = m_privateNh.advertise<std_msgs::Float32>("n_eff", 10);
00153   m_filteredPointCloudPub = m_privateNh.advertise<sensor_msgs::PointCloud2>("filtered_cloud", 1);
00154 
00155 
00156   //TODO Propagate particles independent of sensor callback
00157   reset();
00158 
00159   // ROS subscriptions last:
00160   m_globalLocSrv = m_nh.advertiseService("global_localization", &HumanoidLocalization::globalLocalizationCallback, this);
00161 
00162   // subscription on laser, tf message filter
00163   m_laserSub = new message_filters::Subscriber<sensor_msgs::LaserScan>(m_nh, "scan", 100);
00164   m_laserFilter = new tf::MessageFilter<sensor_msgs::LaserScan>(*m_laserSub, m_tfListener, m_odomFrameId, 100);
00165   m_laserFilter->registerCallback(boost::bind(&HumanoidLocalization::laserCallback, this, _1));
00166 
00167   // subscription on point cloud, tf message filter
00168   m_pointCloudSub = new message_filters::Subscriber<PointCloud>(m_nh, "point_cloud", 100);
00169   m_pointCloudFilter = new tf::MessageFilter< PointCloud >(*m_pointCloudSub, m_tfListener, m_odomFrameId, 100);
00170   m_pointCloudFilter->registerCallback(boost::bind(&HumanoidLocalization::pointCloudCallback, this, _1));
00171 
00172   // subscription on init pose, tf message filter
00173   m_initPoseSub = new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>(m_nh, "initialpose", 2);
00174   m_initPoseFilter = new tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>(*m_initPoseSub, m_tfListener, m_globalFrameId, 2);
00175   m_initPoseFilter->registerCallback(boost::bind(&HumanoidLocalization::initPoseCallback, this, _1));
00176 
00177 
00178   m_pauseIntegrationSub = m_privateNh.subscribe("pause_localization", 1, &HumanoidLocalization::pauseLocalizationCallback, this);
00179   m_pauseLocSrv = m_privateNh.advertiseService("pause_localization_srv", &HumanoidLocalization::pauseLocalizationSrvCallback, this);
00180   m_resumeLocSrv = m_privateNh.advertiseService("resume_localization_srv", &HumanoidLocalization::resumeLocalizationSrvCallback, this);
00181 
00182   if (m_useIMU)
00183     m_imuSub = m_nh.subscribe("imu", 5, &HumanoidLocalization::imuCallback, this);
00184   if (m_useTimer)
00185   {
00186      m_timer = m_nh.createTimer(ros::Duration(m_timerPeriod), &HumanoidLocalization::timerCallback, this);
00187      ROS_INFO("Using timer with a period of %4f s", m_timerPeriod);
00188   }
00189 
00190   ROS_INFO("NaoLocalization initialized with %d particles.", m_numParticles);
00191 }
00192 
00193 HumanoidLocalization::~HumanoidLocalization() {
00194 
00195   delete m_laserFilter;
00196   delete m_laserSub;
00197 
00198   delete m_pointCloudFilter;
00199   delete m_pointCloudSub;
00200 
00201   delete m_initPoseFilter;
00202   delete m_initPoseSub;
00203 
00204 }
00205   
00206 void HumanoidLocalization::timerCallback(const ros::TimerEvent & e){
00207    ros::Time transformExpiration = e.current_real + ros::Duration(m_transformTolerance);
00208    tf::StampedTransform tmp_tf_stamped(m_latest_transform, transformExpiration, m_globalFrameId, m_targetFrameId);
00209    m_tfBroadcaster.sendTransform(tmp_tf_stamped);
00210 }
00211 
00212 
00213 void HumanoidLocalization::reset(){
00214 
00215 #if defined(_BENCH_TIME)
00216   ros::WallTime startTime = ros::WallTime::now();
00217 #endif
00218 
00219   if (m_initGlobal){
00220     this->initGlobal();
00221   } else {
00222     geometry_msgs::PoseWithCovarianceStampedPtr posePtr(new geometry_msgs::PoseWithCovarianceStamped());
00223 
00224     if (m_initFromTruepose){ // useful for evaluation, when ground truth available:
00225       geometry_msgs::PoseStamped truePose;
00226       tf::Stamped<tf::Pose> truePoseTF;
00227       tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,0)), ros::Time::now(), "/torso_real"); // TODO: param
00228 
00229       ros::Time lookupTime = ros::Time::now();
00230       while(m_nh.ok() && !m_tfListener.waitForTransform(m_globalFrameId, ident.frame_id_, lookupTime, ros::Duration(1.0))){
00231         ROS_WARN("Waiting for transform %s --> %s for ground truth initialization failed, trying again...", m_globalFrameId.c_str(), ident.frame_id_.c_str());
00232         lookupTime = ros::Time::now();
00233       }
00234       ident.stamp_ = lookupTime;
00235 
00236       m_tfListener.transformPose(m_globalFrameId, ident, truePoseTF);
00237       tf::poseStampedTFToMsg(truePoseTF, truePose);
00238       tf::poseTFToMsg(truePoseTF, posePtr->pose.pose);
00239       posePtr->header = truePose.header;
00240 
00241 
00242       // initial covariance acc. to params
00243       for(int j=0; j < 6; ++j){
00244         for (int i = 0; i < 6; ++i){
00245           if (i == j)
00246             posePtr->pose.covariance.at(i*6 +j) = m_initNoiseStd(i) * m_initNoiseStd(i);
00247           else
00248             posePtr->pose.covariance.at(i*6 +j) = 0.0;
00249         }
00250       }
00251 
00252     } else{
00253       posePtr.reset(new geometry_msgs::PoseWithCovarianceStamped());
00254       for (int i = 0; i < 6; ++i){
00255         posePtr->pose.covariance.at(i*6 +i) = m_initNoiseStd(i) * m_initNoiseStd(i);
00256       }
00257 
00258       double roll, pitch, z;
00259       initZRP(z, roll, pitch);
00260 
00261 
00262 
00263       posePtr->pose.pose.position.x = m_initPose(0);
00264       posePtr->pose.pose.position.y = m_initPose(1);
00265       posePtr->pose.pose.position.z = z;
00266       tf::Quaternion quat;
00267       quat.setRPY(roll, pitch, m_initPose(5));
00268       tf::quaternionTFToMsg(quat, posePtr->pose.pose.orientation);
00269 
00270     }
00271 
00272     this->initPoseCallback(posePtr);
00273 
00274   }
00275 
00276 
00277 
00278 
00279 
00280 #if defined(_BENCH_TIME)
00281   double dt = (ros::WallTime::now() - startTime).toSec();
00282   ROS_INFO_STREAM("Initialization of "<< m_numParticles << " particles took "
00283                   << dt << "s (="<<dt/m_numParticles<<"s/particle)");
00284 #endif
00285 
00286 
00287 }
00288 
00289 
00290 void HumanoidLocalization::initZRP(double& z, double& roll, double& pitch){
00291   if(m_initPoseRealZRP) {
00292     // Get latest pose height
00293     tf::Stamped<tf::Pose> lastOdomPose;
00294     double poseHeight;
00295     if(m_motionModel->getLastOdomPose(lastOdomPose) &&
00296         lookupPoseHeight(lastOdomPose.stamp_, poseHeight)) {
00297       z = poseHeight;
00298     } else {
00299       ROS_WARN("Could not determine current pose height, falling back to init_pose_z");
00300       z = m_initPose(2);
00301     }
00302 
00303     // Get latest roll and pitch
00304     if(!m_lastIMUMsgBuffer.empty()) {
00305       getRP(m_lastIMUMsgBuffer.back().orientation, roll, pitch);
00306     } else {
00307       ROS_WARN("Could not determine current roll and pitch, falling back to init_pose_{roll,pitch}");
00308       roll = m_initPose(3);
00309       pitch = m_initPose(4);
00310     }
00311   } else {
00312     // Use pose height, roll and pitch from init_pose_{z,roll,pitch} parameters
00313     z = m_initPose(2);
00314     roll = m_initPose(3);
00315     pitch = m_initPose(4);
00316   }
00317 
00318 
00319 }
00320 void HumanoidLocalization::laserCallback(const sensor_msgs::LaserScanConstPtr& msg){
00321   ROS_DEBUG("Laser received (time: %f)", msg->header.stamp.toSec());
00322   
00323   if (!m_initialized){
00324     ROS_WARN("Localization not initialized yet, skipping laser callback.");
00325     return;
00326   }
00327 
00328   double timediff = (msg->header.stamp - m_lastLaserTime).toSec();
00329   if (m_receivedSensorData && timediff < 0){
00330     ROS_WARN("Ignoring received laser data that is %f s older than previous data!", timediff);
00331     return;
00332   }
00333   
00334 
00336   tf::Stamped<tf::Pose> odomPose;
00337   // check if odometry available, skip scan if not.
00338   if (!m_motionModel->lookupOdomPose(msg->header.stamp, odomPose))
00339      return;
00340 
00341 
00342   bool sensor_integrated = false;
00343   if (!m_paused && (!m_receivedSensorData || isAboveMotionThreshold(odomPose))) {
00344 
00345      // convert laser to point cloud first:
00346      PointCloud pc_filtered;
00347      std::vector<float> laserRangesSparse;
00348      prepareLaserPointCloud(msg, pc_filtered, laserRangesSparse);
00349 
00350      sensor_integrated = localizeWithMeasurement(pc_filtered, laserRangesSparse, msg->range_max);
00351 
00352   } 
00353 
00354   if(!sensor_integrated){ // no laser integration: propagate particles forward by full interval
00355 
00356      // relative odom transform to last odomPose
00357      tf::Transform odomTransform = m_motionModel->computeOdomTransform(odomPose);
00358      m_motionModel->applyOdomTransform(m_particles, odomTransform);
00359      constrainMotion(odomPose);
00360   }
00361   else
00362   {
00363      m_lastLocalizedPose = odomPose;
00364   }
00365 
00366   m_motionModel->storeOdomPose(odomPose);
00367   publishPoseEstimate(msg->header.stamp, sensor_integrated);
00368   m_lastLaserTime = msg->header.stamp; 
00369 }
00370 
00371 void HumanoidLocalization::constrainMotion(const tf::Pose& odomPose){
00372   // skip if nothing to do:
00373   if (!m_constrainMotionZ && !m_constrainMotionRP)
00374     return;
00375 
00376   // reset z according to current odomPose:
00377   double z = odomPose.getOrigin().getZ();
00378   double odomRoll, odomPitch, uselessYaw;
00379   odomPose.getBasis().getRPY(odomRoll, odomPitch, uselessYaw);
00380 
00381 #pragma omp parallel for
00382   for (unsigned i=0; i < m_particles.size(); ++i){
00383     if (m_constrainMotionZ){
00384       tf::Vector3 pos = m_particles[i].pose.getOrigin();
00385       double floor_z = m_mapModel->getFloorHeight(m_particles[i].pose);
00386       pos.setZ(z+floor_z);
00387       m_particles[i].pose.setOrigin(pos);
00388     }
00389 
00390     if (m_constrainMotionRP){
00391       double yaw =  tf::getYaw(m_particles[i].pose.getRotation());
00392       m_particles[i].pose.setRotation(tf::createQuaternionFromRPY(odomRoll, odomPitch, yaw));
00393 
00394     }
00395   }
00396 }
00397 
00398 bool HumanoidLocalization::isAboveMotionThreshold(const tf::Pose& odomPose){
00399   tf::Transform odomTransform = m_lastLocalizedPose.inverse() * odomPose;
00400 
00401   double yaw, pitch, roll;
00402   odomTransform.getBasis().getRPY(roll, pitch, yaw);
00403 
00404   return (odomTransform.getOrigin().length() >= m_observationThresholdTrans
00405       || std::abs(yaw) >= m_observationThresholdRot);
00406 }
00407 
00408 bool HumanoidLocalization::localizeWithMeasurement(const PointCloud& pc_filtered, const std::vector<float>& ranges, double max_range){
00409   ros::WallTime startTime = ros::WallTime::now();
00410   ros::Time t = pc_filtered.header.stamp;
00411   // apply motion model with temporal sampling:
00412   m_motionModel->applyOdomTransformTemporal(m_particles, t, m_temporalSamplingRange);
00413   
00414   // constrain to ground plane, if desired:
00415   tf::Stamped<tf::Transform> odomPose;
00416   assert(m_motionModel->lookupOdomPose(t, odomPose));
00417   constrainMotion(odomPose);
00418 
00419   // transformation from torso frame to sensor
00420   // this takes the latest tf, assumes that torso to sensor did not change over temp. sampling!
00421   tf::StampedTransform localSensorFrame;
00422   if (!m_motionModel->lookupLocalTransform(pc_filtered.header.frame_id, t, localSensorFrame))
00423     return false;
00424 
00425   tf::Transform torsoToSensor(localSensorFrame.inverse());
00426   
00427 //### Particles in log-form from here...
00428   toLogForm();
00429 
00430   // skip pose integration if z, roll and pitch constrained to floor by odometry
00431   if (!(m_constrainMotionRP && m_constrainMotionZ)){
00432     bool imuMsgOk = false;
00433     double angleX, angleY;
00434     if(m_useIMU) {
00435       ros::Time imuStamp;
00436       imuMsgOk = getImuMsg(t, imuStamp, angleX, angleY);
00437     } else {
00438       tf::Stamped<tf::Pose> lastOdomPose;
00439       if(m_motionModel->lookupOdomPose(t, lastOdomPose)) {
00440         double dropyaw;
00441         lastOdomPose.getBasis().getRPY(angleX, angleY, dropyaw);
00442         imuMsgOk = true;
00443       }
00444     }
00445 
00446     tf::StampedTransform footprintToTorso;
00447     // integrated pose (z, roll, pitch) meas. only if data OK:
00448     if(imuMsgOk) {
00449       if (!m_motionModel->lookupLocalTransform(m_baseFootprintId, t, footprintToTorso)) {
00450         ROS_WARN("Could not obtain pose height in localization, skipping Pose integration");
00451       } else {
00452         m_observationModel->integratePoseMeasurement(m_particles, angleX, angleY, footprintToTorso);
00453       }
00454     } else {
00455       ROS_WARN("Could not obtain roll and pitch measurement, skipping Pose integration");
00456     }
00457   }
00458 
00459   m_filteredPointCloudPub.publish(pc_filtered);
00460   m_observationModel->integrateMeasurement(m_particles, pc_filtered, ranges, max_range, torsoToSensor);
00461 
00462   // TODO: verify poses before measurements, ignore particles then
00463   m_mapModel->verifyPoses(m_particles);
00464 
00465   // normalize weights and transform back from log:
00466   normalizeWeights();
00467   //### Particles back in regular form now
00468 
00469   double nEffParticles = nEff();
00470 
00471   std_msgs::Float32 nEffMsg;
00472   nEffMsg.data = nEffParticles;
00473   m_nEffPub.publish(nEffMsg);
00474 
00475   if (nEffParticles <= m_nEffFactor*m_particles.size()){ // selective resampling
00476     ROS_INFO("Resampling, nEff=%f, numParticles=%zd", nEffParticles, m_particles.size());
00477     resample();
00478   } else {
00479     ROS_INFO("Skipped resampling, nEff=%f, numParticles=%zd", nEffParticles, m_particles.size());
00480   }
00481 
00482   m_receivedSensorData = true;
00483 
00484   double dt = (ros::WallTime::now() - startTime).toSec();
00485   ROS_INFO_STREAM("Observations for "<< m_numParticles << " particles took "
00486                   << dt << "s (="<<dt/m_numParticles<<"s/particle)");
00487 
00488   return true;
00489 }
00490 
00491 void HumanoidLocalization::prepareLaserPointCloud(const sensor_msgs::LaserScanConstPtr& laser, PointCloud& pc, std::vector<float>& ranges) const{
00492   unsigned numBeams = laser->ranges.size();
00493   // skip every n-th scan:
00494   //unsigned step = computeBeamStep(numBeams);
00495   // build complete pointcloud:
00496   unsigned step = 1;
00497 
00498 
00499 
00500   // prepare laser message:
00501   unsigned int numBeamsSkipped = 0;
00502 
00503   // range_min of laser is also used to filter out wrong messages:
00504   double laserMin = std::max(double(laser->range_min), m_filterMinRange);
00505 
00506   // (range_max readings stay, will be used in the sensor model)
00507 
00508   ranges.reserve(50);
00509 
00510   // build a point cloud
00511   pc.header = laser->header;
00512   pc.points.reserve(50);
00513   for (unsigned beam_idx = 0; beam_idx < numBeams; beam_idx+= step){
00514     float range = laser->ranges[beam_idx];
00515     if (range >= laserMin && range <= m_filterMaxRange){
00516       double laserAngle = laser->angle_min + beam_idx * laser->angle_increment;
00517       tf::Transform laserAngleRotation(tf::Quaternion(tf::Vector3(0.0, 0.0, 1.0), laserAngle));
00518       tf::Vector3 laserEndpointTrans(range, 0.0, 0.0);
00519       tf::Vector3 pt(laserAngleRotation * laserEndpointTrans);
00520 
00521       pc.points.push_back(pcl::PointXYZ(pt.x(), pt.y(), pt.z()));
00522       ranges.push_back(range);
00523 
00524     } else{
00525       numBeamsSkipped++;
00526     }
00527 
00528   }
00529   pc.height = 1;
00530   pc.width = pc.points.size();
00531   pc.is_dense = true;
00532 
00533   // uniform sampling:
00534   pcl::UniformSampling<pcl::PointXYZ> uniformSampling;
00535   pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPtr;
00536   cloudPtr.reset(new pcl::PointCloud<pcl::PointXYZ> (pc));
00537   uniformSampling.setInputCloud(cloudPtr);
00538   uniformSampling.setRadiusSearch(m_sensorSampleDist);
00539   pcl::PointCloud<int> sampledIndices;
00540   uniformSampling.compute(sampledIndices);
00541   pcl::copyPointCloud(*cloudPtr, sampledIndices.points, pc);
00542   // adjust "ranges" array to contain the same points:
00543   std::vector<float> rangesSparse;
00544   rangesSparse.resize(sampledIndices.size());
00545   for (size_t i = 0; i < rangesSparse.size(); ++i){
00546     rangesSparse[i] = ranges[sampledIndices.points[i]];
00547   }
00548   ranges = rangesSparse;
00549   ROS_INFO("Laser PointCloud subsampled: %zu from %zu (%u out of valid range)", pc.size(), cloudPtr->size(), numBeamsSkipped);
00550 }
00551 
00552 int HumanoidLocalization::filterUniform(const PointCloud & cloud_in, PointCloud & cloud_out, int numSamples) const{
00553   int numPoints = static_cast<int>(cloud_in.size() );
00554   numSamples = std::min( numSamples, numPoints);
00555   std::vector<unsigned int> indices;
00556   indices.reserve( numPoints );
00557   for (int i=0; i<numPoints; ++i)
00558     indices.push_back(i);
00559   random_shuffle ( indices.begin(), indices.end());
00560 
00561   cloud_out.reserve( cloud_out.size() + numSamples );
00562   for ( int i = 0; i < numSamples; ++i)
00563   {
00564     cloud_out.push_back( cloud_in.at(indices[i]));
00565   }
00566   return numSamples;
00567 }
00568 
00569 
00570 
00571 void HumanoidLocalization::filterGroundPlane(const PointCloud& pc, PointCloud& ground, PointCloud& nonground, double groundFilterDistance, double groundFilterAngle, double groundFilterPlaneDistance){
00572    ground.header = pc.header;
00573    nonground.header = pc.header;
00574 
00575    if (pc.size() < 50){
00576       ROS_WARN("Pointcloud in HumanoidLocalization::filterGroundPlane too small, skipping ground plane extraction");
00577       nonground = pc;
00578    } else {
00579       // plane detection for ground plane removal:
00580       pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00581       pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00582 
00583       // Create the segmentation object and set up:
00584       pcl::SACSegmentation<pcl::PointXYZ> seg;
00585       seg.setOptimizeCoefficients (true);
00586       // TODO: maybe a filtering based on the surface normals might be more robust / accurate?
00587       seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
00588       seg.setMethodType(pcl::SAC_RANSAC);
00589       seg.setMaxIterations(200);
00590       seg.setDistanceThreshold (groundFilterDistance);
00591       seg.setAxis(Eigen::Vector3f(0,0,1));
00592       seg.setEpsAngle(groundFilterAngle);
00593 
00594 
00595       PointCloud cloud_filtered(pc);
00596       // Create the filtering object
00597       pcl::ExtractIndices<pcl::PointXYZ> extract;
00598       bool groundPlaneFound = false;
00599 
00600       while(cloud_filtered.size() > 10 && !groundPlaneFound){
00601          seg.setInputCloud(cloud_filtered.makeShared());
00602          seg.segment (*inliers, *coefficients);
00603          if (inliers->indices.size () == 0){
00604             ROS_INFO("PCL segmentation did not find any plane.");
00605 
00606             break;
00607          }
00608 
00609          extract.setInputCloud(cloud_filtered.makeShared());
00610          extract.setIndices(inliers);
00611 
00612          if (std::abs(coefficients->values.at(3)) < groundFilterPlaneDistance){
00613             ROS_DEBUG("Ground plane found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(),
00614                   coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3));
00615             extract.setNegative (false);
00616             extract.filter (ground);
00617 
00618             // remove ground points from full pointcloud:
00619             // workaround for PCL bug:
00620             if(inliers->indices.size() != cloud_filtered.size()){
00621                extract.setNegative(true);
00622                PointCloud cloud_out;
00623                extract.filter(cloud_out);
00624                nonground += cloud_out;
00625                cloud_filtered = cloud_out;
00626             }
00627 
00628             groundPlaneFound = true;
00629          } else{
00630             ROS_DEBUG("Horizontal plane (not ground) found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(),
00631                   coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3));
00632             pcl::PointCloud<pcl::PointXYZ> cloud_out;
00633             extract.setNegative (false);
00634             extract.filter(cloud_out);
00635             nonground +=cloud_out;
00636             // debug
00637             //            pcl::PCDWriter writer;
00638             //            writer.write<pcl::PointXYZ>("nonground_plane.pcd",cloud_out, false);
00639 
00640             // remove current plane from scan for next iteration:
00641             // workaround for PCL bug:
00642             if(inliers->indices.size() != cloud_filtered.size()){
00643                extract.setNegative(true);
00644                cloud_out.points.clear();
00645                extract.filter(cloud_out);
00646                cloud_filtered = cloud_out;
00647             } else{
00648                cloud_filtered.points.clear();
00649             }
00650          }
00651 
00652       }
00653       // TODO: also do this if overall starting pointcloud too small?
00654       if (!groundPlaneFound){ // no plane found or remaining points too small
00655          ROS_WARN("No ground plane found in scan");
00656 
00657          // do a rough fitlering on height to prevent spurious obstacles
00658          pcl::PassThrough<pcl::PointXYZ> second_pass;
00659          second_pass.setFilterFieldName("z");
00660          second_pass.setFilterLimits(-groundFilterPlaneDistance, groundFilterPlaneDistance);
00661          second_pass.setInputCloud(pc.makeShared());
00662          second_pass.filter(ground);
00663 
00664          second_pass.setFilterLimitsNegative (true);
00665          second_pass.filter(nonground);
00666       }
00667 
00668       // debug:
00669       //        pcl::PCDWriter writer;
00670       //        if (pc_ground.size() > 0)
00671       //          writer.write<pcl::PointXYZ>("ground.pcd",pc_ground, false);
00672       //        if (pc_nonground.size() > 0)
00673       //          writer.write<pcl::PointXYZ>("nonground.pcd",pc_nonground, false);
00674    }
00675 }
00676 
00677 
00678 
00679 void HumanoidLocalization::prepareGeneralPointCloud(const PointCloud::ConstPtr & msg, PointCloud& pc, std::vector<float>& ranges) const{
00680 
00681     pc.clear();
00682     // lookup Transfrom Sensor to BaseFootprint
00683     tf::StampedTransform sensorToBaseFootprint;
00684     try{
00685       m_tfListener.waitForTransform(m_baseFootprintId, msg->header.frame_id, msg->header.stamp, ros::Duration(0.2));
00686       m_tfListener.lookupTransform(m_baseFootprintId, msg->header.frame_id, msg->header.stamp, sensorToBaseFootprint);
00687 
00688 
00689     }catch(tf::TransformException& ex){
00690       ROS_ERROR_STREAM( "Transform error for pointCloudCallback: " << ex.what() << ", quitting callback.\n");
00691       return;
00692     }
00693 
00694     /*** filter PointCloud and fill pc and ranges ***/
00695 
00696     // pass-through filter to get rid of near and far ranges
00697     pcl::PassThrough<pcl::PointXYZ> pass;
00698     pass.setInputCloud (msg);
00699     pass.setFilterFieldName ("z");
00700     pass.setFilterLimits (m_filterMinRange, m_filterMaxRange);
00701     pass.filter (pc);
00702 
00703     // identify ground plane
00704     PointCloud ground, nonground;
00705     if (m_groundFilterPointCloud)
00706     {
00707         Eigen::Matrix4f matSensorToBaseFootprint, matBaseFootprintToSensor;
00708         pcl_ros::transformAsMatrix(sensorToBaseFootprint, matSensorToBaseFootprint);
00709         pcl_ros::transformAsMatrix(sensorToBaseFootprint.inverse(), matBaseFootprintToSensor);
00710         // TODO:Why transform the point cloud and not just the normal vector?
00711         pcl::transformPointCloud(pc, pc, matSensorToBaseFootprint );
00712         filterGroundPlane(pc, ground, nonground, m_groundFilterDistance, m_groundFilterAngle, m_groundFilterPlaneDistance);
00713 
00714         // clear pc again and refill it based on classification
00715         pc.clear();
00716         pcl::PointCloud<int> sampledIndices;
00717 
00718         //int numFloorPoints = filterUniform( ground, pc, m_numFloorPoints );
00719         int numFloorPoints = 0;
00720         if (ground.size() > 0){ // check for 0 size, otherwise PCL crashes
00721           // transform clouds back to sensor for integration
00722           pcl::transformPointCloud(ground, ground, matBaseFootprintToSensor);
00723           voxelGridSampling(ground, sampledIndices, m_sensorSampleDist*m_sensorSampleDistGroundFactor);
00724           pcl::copyPointCloud(ground, sampledIndices.points, pc);
00725           numFloorPoints = sampledIndices.size();
00726         }
00727 
00728         //int numNonFloorPoints = filterUniform( nonground, pc, m_numNonFloorPoints );
00729         int numNonFloorPoints = 0;
00730         if (nonground.size() > 0){ // check for 0 size, otherwise PCL crashes
00731           // transform clouds back to sensor for integration
00732           pcl::transformPointCloud(nonground, nonground, matBaseFootprintToSensor);
00733           voxelGridSampling( nonground, sampledIndices, m_sensorSampleDist);
00734           pcl::copyPointCloud( nonground, sampledIndices.points, nonground);
00735           numNonFloorPoints = sampledIndices.size();
00736           pc += nonground;
00737         }
00738 
00739         //TODO improve sampling?
00740 
00741 
00742         ROS_INFO("PointCloudGroundFiltering done. Added %d non-ground points and %d ground points (from %zu). Cloud size is %zu", numNonFloorPoints, numFloorPoints, ground.size(), pc.size());
00743         // create sparse ranges..
00744         ranges.resize(pc.size());
00745         for (unsigned int i=0; i<pc.size(); ++i)
00746         {
00747            pcl::PointXYZ p = pc.at(i);
00748            ranges[i] = sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
00749         }
00750 
00751     }
00752     else
00753     {
00754        ROS_INFO("Starting uniform sampling");
00755        //ROS_ERROR("No ground filtering is not implemented yet!");
00756        // uniform sampling:
00757        pcl::PointCloud<int> sampledIndices;
00758        voxelGridSampling(pc, sampledIndices,  m_sensorSampleDist);
00759        pcl::copyPointCloud(pc, sampledIndices.points, pc);
00760 
00761        // adjust "ranges" array to contain the same points:
00762        ranges.resize(sampledIndices.size());
00763        for (size_t i = 0; i < ranges.size(); ++i){
00764           pcl::PointXYZ p = pc[i]; 
00765           ranges[i] = sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
00766           //rangesSparse[i] = ranges[sampledIndices.points[i]];
00767           //ranges[i] = sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
00768        }
00769        ROS_INFO("Done.");
00770 
00771 
00772     }
00773     return;
00774 
00775 }
00776 
00777 void HumanoidLocalization::voxelGridSampling(const PointCloud & pc, pcl::PointCloud<int> & sampledIndices, double search_radius) const
00778 {
00779    pcl::UniformSampling<pcl::PointXYZ> uniformSampling;
00780    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPtr;
00781    cloudPtr.reset(new pcl::PointCloud<pcl::PointXYZ> (pc)); // TODO: Check if this is a shallow copy..
00782    uniformSampling.setInputCloud(cloudPtr);
00783    uniformSampling.setRadiusSearch(search_radius);
00784    uniformSampling.compute(sampledIndices);
00785 }
00786 
00787 void HumanoidLocalization::pointCloudCallback(const PointCloud::ConstPtr & msg) {
00788   ROS_DEBUG("PointCloud received (time: %f)", msg->header.stamp.toSec());
00789 
00790   if (!m_initialized){
00791     ROS_WARN("Loclization not initialized yet, skipping PointCloud callback.");
00792     return;
00793   }
00794 
00795   double timediff = (msg->header.stamp - m_lastPointCloudTime).toSec();
00796   if (m_receivedSensorData && timediff < 0){
00797     ROS_WARN("Ignoring received PointCloud data that is %f s older than previous data!", timediff);
00798     return;
00799   }
00800 
00801 
00803   tf::Stamped<tf::Pose> odomPose;
00804   // check if odometry available, skip scan if not.
00805   if (!m_motionModel->lookupOdomPose(msg->header.stamp, odomPose))
00806     return;
00807 
00808 
00809   bool sensor_integrated = false;
00810 
00811   // TODO #1: Make this nicer: head rotations for integration check
00812   // TODO #2: Initialization of m_headYawRotationLastScan, etc needs to be set correctly
00813   bool isAboveHeadMotionThreshold = false;
00814   double headYaw, headPitch, headRoll;
00815   tf::StampedTransform torsoToSensor;
00816   if (!m_motionModel->lookupLocalTransform(msg->header.frame_id, msg->header.stamp, torsoToSensor))
00817       return; //TODO: should we apply applyOdomTransformTemporal, before returning
00818 
00819   // TODO #3: Invert transform?: tf::Transform torsoToSensor(localSensorFrame.inverse());
00820 
00821   torsoToSensor.getBasis().getRPY(headRoll, headPitch, headYaw);
00822   double headYawRotationSinceScan = std::abs(headYaw - m_headYawRotationLastScan);
00823   double headPitchRotationSinceScan = std::abs(headPitch - m_headPitchRotationLastScan);
00824 
00825   if (headYawRotationSinceScan>= m_observationThresholdHeadYawRot || headPitchRotationSinceScan >= m_observationThresholdHeadPitchRot)
00826       isAboveHeadMotionThreshold = true;
00827   // end #1
00828 
00829   if (!m_paused && (!m_receivedSensorData || isAboveHeadMotionThreshold || isAboveMotionThreshold(odomPose))) {
00830 
00831     // convert laser to point cloud first:
00832     PointCloud pc_filtered;
00833     std::vector<float> rangesSparse;
00834     prepareGeneralPointCloud(msg, pc_filtered, rangesSparse);
00835 
00836     double maxRange = 10.0; // TODO #4: What is a maxRange for pointClouds? NaN? maxRange is expected to be a double and integrateMeasurement checks rangesSparse[i] > maxRange
00837     ROS_DEBUG("Updating Pose Estimate from a PointCloud with %zu points and %zu ranges", pc_filtered.size(), rangesSparse.size());
00838     sensor_integrated = localizeWithMeasurement(pc_filtered, rangesSparse, maxRange);
00839    
00840   } 
00841   if(!sensor_integrated){ // no observation necessary: propagate particles forward by full interval
00842      // relative odom transform to last odomPose
00843      tf::Transform odomTransform = m_motionModel->computeOdomTransform(odomPose);
00844      m_motionModel->applyOdomTransform(m_particles, odomTransform);
00845      constrainMotion(odomPose);
00846   }
00847   else{
00848      m_lastLocalizedPose = odomPose;
00849      // TODO #1
00850      m_headYawRotationLastScan = headYaw;
00851      m_headPitchRotationLastScan = headPitch;
00852   }
00853 
00854   m_motionModel->storeOdomPose(odomPose);
00855   publishPoseEstimate(msg->header.stamp, sensor_integrated);
00856   m_lastPointCloudTime = msg->header.stamp;
00857   ROS_DEBUG("PointCloud callback complete.");
00858 }
00859 
00860 void HumanoidLocalization::imuCallback(const sensor_msgs::ImuConstPtr& msg){
00861   m_lastIMUMsgBuffer.push_back(*msg);
00862 }
00863 
00864 bool HumanoidLocalization::getImuMsg(const ros::Time& stamp, ros::Time& imuStamp, double& angleX, double& angleY) const {
00865   if(m_lastIMUMsgBuffer.empty())
00866     return false;
00867 
00868   typedef boost::circular_buffer<sensor_msgs::Imu>::const_iterator ItT;
00869   const double maxAge = 0.2;
00870   double closestOlderStamp = std::numeric_limits<double>::max();
00871   double closestNewerStamp = std::numeric_limits<double>::max();
00872   ItT closestOlder = m_lastIMUMsgBuffer.end(), closestNewer = m_lastIMUMsgBuffer.end();
00873   for(ItT it = m_lastIMUMsgBuffer.begin(); it != m_lastIMUMsgBuffer.end(); it++) {
00874     const double age = (stamp - it->header.stamp).toSec();
00875     if(age >= 0.0 && age < closestOlderStamp) {
00876       closestOlderStamp = age;
00877       closestOlder = it;
00878     } else if(age < 0.0 && -age < closestNewerStamp) {
00879       closestNewerStamp = -age;
00880       closestNewer = it;
00881     }
00882   }
00883 
00884   if(closestOlderStamp < maxAge && closestNewerStamp < maxAge && closestOlderStamp + closestNewerStamp > 0.0) {
00885     // Linear interpolation
00886     const double weightOlder = closestNewerStamp / (closestNewerStamp + closestOlderStamp);
00887     const double weightNewer = 1.0 - weightOlder;
00888     imuStamp = ros::Time(weightOlder * closestOlder->header.stamp.toSec()
00889                           + weightNewer * closestNewer->header.stamp.toSec());
00890     double olderX, olderY, newerX, newerY;
00891     getRP(closestOlder->orientation, olderX, olderY);
00892     getRP(closestNewer->orientation, newerX, newerY);
00893     angleX   = weightOlder * olderX  + weightNewer * newerX;
00894     angleY   = weightOlder * olderY + weightNewer * newerY;
00895     ROS_DEBUG("Msg: %.3f, Interpolate [%.3f .. %.3f .. %.3f]\n", stamp.toSec(), closestOlder->header.stamp.toSec(),
00896               imuStamp.toSec(), closestNewer->header.stamp.toSec());
00897     return true;
00898   } else if(closestOlderStamp < maxAge || closestNewerStamp < maxAge) {
00899     // Return closer one
00900     ItT it = (closestOlderStamp < closestNewerStamp) ? closestOlder : closestNewer;
00901     imuStamp = it->header.stamp;
00902     getRP(it->orientation, angleX, angleY);
00903     return true;
00904   } else {
00905     if(closestOlderStamp < closestNewerStamp)
00906       ROS_WARN("Closest IMU message is %.2f seconds too old, skipping pose integration", closestOlderStamp);
00907     else
00908       ROS_WARN("Closest IMU message is %.2f seconds too new, skipping pose integration", closestNewerStamp);
00909     return false;
00910   }
00911 }
00912 
00913 void HumanoidLocalization::initPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg){
00914   tf::Pose pose;
00915   tf::poseMsgToTF(msg->pose.pose, pose);
00916 
00917   if (msg->header.frame_id != m_globalFrameId){
00918     ROS_WARN("Frame ID of \"initialpose\" (%s) is different from the global frame %s", msg->header.frame_id.c_str(), m_globalFrameId.c_str());
00919   }
00920 
00921   std::vector<double> heights;
00922   double poseHeight = 0.0;
00923   if (std::abs(pose.getOrigin().getZ()) < 0.01){
00924     m_mapModel->getHeightlist(pose.getOrigin().getX(), pose.getOrigin().getY(), 0.6, heights);
00925     if (heights.size() == 0){
00926       ROS_WARN("No ground level to stand on found at map position, assuming 0");
00927       heights.push_back(0.0);
00928     }
00929 
00930     bool poseHeightOk = false;
00931     if(m_initPoseRealZRP) {
00932       ros::Time stamp(msg->header.stamp);
00933       if(stamp.isZero()) {
00934         // Header stamp is not set (e.g. RViz), use stamp from latest pose message instead
00935         tf::Stamped<tf::Pose> lastOdomPose;
00936         m_motionModel->getLastOdomPose(lastOdomPose);
00937         stamp = lastOdomPose.stamp_;
00938       }
00939       poseHeightOk = lookupPoseHeight(stamp, poseHeight);
00940       if(!poseHeightOk) {
00941         ROS_WARN("Could not determine current pose height, falling back to init_pose_z");
00942       }
00943     }
00944     if(!poseHeightOk) {
00945       ROS_INFO("Use pose height from init_pose_z");
00946       poseHeight = m_initPose(2);
00947     }
00948   }
00949 
00950 
00951   Matrix6d initCov;
00952   if ((std::abs(msg->pose.covariance.at(6*0+0) - 0.25) < 0.1) && (std::abs(msg->pose.covariance.at(6*1+1) -0.25) < 0.1)
00953       && (std::abs(msg->pose.covariance.at(6*3+3) - M_PI/12.0 * M_PI/12.0)<0.1)){
00954     ROS_INFO("Covariance originates from RViz, using default parameters instead");
00955     initCov = Matrix6d::Zero();
00956     initCov.diagonal() = m_initNoiseStd.cwiseProduct(m_initNoiseStd);
00957 
00958     // manually set r&p, rviz values are 0
00959     bool ok = false;
00960     const double yaw = tf::getYaw(pose.getRotation());
00961     if(m_initPoseRealZRP) {
00962       bool useOdometry = true;
00963       if(m_useIMU) {
00964         if(m_lastIMUMsgBuffer.empty()) {
00965           ROS_WARN("Could not determine current roll and pitch because IMU message buffer is empty.");
00966         } else {
00967           double roll, pitch;
00968           if(msg->header.stamp.isZero()) {
00969             // Header stamp is not set (e.g. RViz), use stamp from latest IMU message instead
00970             getRP(m_lastIMUMsgBuffer.back().orientation, roll, pitch);
00971             ok = true;
00972           } else {
00973             ros::Time imuStamp;
00974             ok = getImuMsg(msg->header.stamp, imuStamp, roll, pitch);
00975           }
00976           if(ok) {
00977             ROS_INFO("roll and pitch not set in initPoseCallback, use IMU values (roll = %f, pitch = %f) instead", roll, pitch);
00978             pose.setRotation(tf::createQuaternionFromRPY(roll, pitch, yaw));
00979             useOdometry = false;
00980           } else {
00981             ROS_WARN("Could not determine current roll and pitch from IMU, falling back to odometry roll and pitch");
00982             useOdometry = true;
00983           }
00984         }
00985       }
00986 
00987       if(useOdometry) {
00988         double roll, pitch, dropyaw;
00989         tf::Stamped<tf::Pose> lastOdomPose;
00990         ok = m_motionModel->getLastOdomPose(lastOdomPose);
00991         if(ok) {
00992           lastOdomPose.getBasis().getRPY(roll, pitch, dropyaw);
00993           pose.setRotation(tf::createQuaternionFromRPY(roll, pitch, yaw));
00994           ROS_INFO("roll and pitch not set in initPoseCallback, use odometry values (roll = %f, pitch = %f) instead", roll, pitch);
00995         } else {
00996           ROS_WARN("Could not determine current roll and pitch from odometry, falling back to init_pose_{roll,pitch} parameters");
00997         }
00998       }
00999     }
01000 
01001     if(!ok) {
01002       ROS_INFO("roll and pitch not set in initPoseCallback, use init_pose_{roll,pitch} parameters instead");
01003       pose.setRotation(tf::createQuaternionFromRPY(m_initPose(3), m_initPose(4), yaw));
01004     }
01005   } else{
01006     for(int j=0; j < initCov.cols(); ++j){
01007       for (int i = 0; i < initCov.rows(); ++i){
01008         initCov(i,j) = msg->pose.covariance.at(i*initCov.cols() +j);
01009       }
01010     }
01011   }
01012 
01013   // sample from initial pose covariance:
01014   Matrix6d initCovL = initCov.llt().matrixL();
01015   tf::Transform transformNoise; // transformation on original pose from noise
01016   unsigned idx = 0;
01017   for(Particles::iterator it = m_particles.begin(); it != m_particles.end(); ++it){
01018     Vector6d poseNoise;
01019     for (unsigned i = 0; i < 6; ++i){
01020       poseNoise(i) = m_rngNormal();
01021     }
01022     Vector6d poseCovNoise = initCovL * poseNoise; // is now drawn according to covariance noise
01023     // if a variance is set to 0 => no noise!
01024     for (unsigned i = 0; i < 6; ++i){
01025       if (std::abs(initCov(i,i)) < 0.00001)
01026         poseCovNoise(i) = 0.0;
01027     }
01028 
01029 
01030     transformNoise.setOrigin(tf::Vector3(poseCovNoise(0), poseCovNoise(1), poseCovNoise(2)));
01031     tf::Quaternion q;
01032     q.setRPY(poseCovNoise(3), poseCovNoise(4),poseCovNoise(5));
01033 
01034     transformNoise.setRotation(q);
01035     it->pose = pose;
01036 
01037     if (heights.size() > 0){
01038       // distribute particles evenly between levels:
01039       it->pose.getOrigin().setZ(heights.at(int(double(idx)/m_particles.size() * heights.size())) + poseHeight);
01040     }
01041 
01042     it->pose *= transformNoise;
01043 
01044     it->weight = 1.0/m_particles.size();
01045     idx++;
01046   }
01047 
01048   ROS_INFO("Pose reset around mean (%f %f %f)", pose.getOrigin().getX(), pose.getOrigin().getY(), pose.getOrigin().getZ());
01049 
01050   // reset internal state:
01051   m_motionModel->reset();
01052   // force integration of next laser data:
01053   m_receivedSensorData = false;
01054   m_initialized = true;
01055 
01056   publishPoseEstimate(msg->header.stamp, false);
01057 }
01058 
01059 
01060 
01061 
01062 bool HumanoidLocalization::globalLocalizationCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
01063 {
01064 
01065   initGlobal();
01066 
01067   return true;
01068 }
01069 
01070 void HumanoidLocalization::normalizeWeights() {
01071 
01072   double wmin = std::numeric_limits<double>::max();
01073   double wmax = -std::numeric_limits<double>::max();
01074 
01075   for (unsigned i=0; i < m_particles.size(); ++i){
01076     double weight = m_particles[i].weight;
01077     assert (!isnan(weight));
01078     if (weight < wmin)
01079       wmin = weight;
01080     if (weight > wmax){
01081       wmax = weight;
01082       m_bestParticleIdx = i;
01083     }
01084   }
01085   if (wmin > wmax){
01086     ROS_ERROR_STREAM("Error in weights: min=" << wmin <<", max="<<wmax<<", 1st particle weight="<< m_particles[1].weight<< std::endl);
01087 
01088   }
01089 
01090   double min_normalized_value;
01091   if (m_minParticleWeight > 0.0)
01092     min_normalized_value = std::max(log(m_minParticleWeight), wmin - wmax);
01093   else
01094     min_normalized_value = wmin - wmax;
01095 
01096   double max_normalized_value = 0.0; // = log(1.0);
01097   double dn = max_normalized_value-min_normalized_value;
01098   double dw = wmax-wmin;
01099   if (dw == 0.0) dw = 1;
01100   double scale = dn/dw;
01101   if (scale < 0.0){
01102     ROS_WARN("normalizeWeights: scale is %f < 0, dw=%f, dn=%f", scale, dw, dn );
01103   }
01104   double offset = -wmax*scale;
01105   double weights_sum = 0.0;
01106 
01107 #pragma omp parallel
01108   {
01109 
01110 #pragma omp for
01111     for (unsigned i = 0; i < m_particles.size(); ++i){
01112       double w = m_particles[i].weight;
01113       w = exp(scale*w+offset);
01114       assert(!isnan(w));
01115       m_particles[i].weight = w;
01116 #pragma omp atomic
01117       weights_sum += w;
01118     }
01119 
01120     assert(weights_sum > 0.0);
01121     // normalize sum to 1:
01122 #pragma omp for
01123     for (unsigned i = 0; i < m_particles.size(); ++i){
01124       m_particles[i].weight /= weights_sum;
01125     }
01126 
01127   }
01128 }
01129 
01130 double HumanoidLocalization::getCumParticleWeight() const{
01131   double cumWeight=0.0;
01132 
01133   //compute the cumulative weights
01134   for (Particles::const_iterator it = m_particles.begin(); it != m_particles.end(); ++it){
01135     cumWeight += it->weight;
01136   }
01137 
01138   return cumWeight;
01139 }
01140 
01141 void HumanoidLocalization::resample(unsigned numParticles){
01142 
01143   if (numParticles <= 0)
01144     numParticles = m_numParticles;
01145 
01146   //compute the interval
01147   double interval=getCumParticleWeight()/numParticles;
01148 
01149   //compute the initial target weight
01150   double target=interval*m_rngUniform();
01151 
01152   //compute the resampled indexes
01153   double cumWeight=0;
01154   std::vector<unsigned> indices(numParticles);
01155 
01156   unsigned n=0;
01157   for (unsigned i = 0; i < m_particles.size(); ++i){
01158     cumWeight += m_particles[i].weight;
01159     while(cumWeight > target && n < numParticles){
01160       if (m_bestParticleIdx >= 0 && i == unsigned(m_bestParticleIdx)){
01161         m_bestParticleIdx = n;
01162       }
01163 
01164       indices[n++]=i;
01165       target+=interval;
01166     }
01167   }
01168   // indices now contains the indices to draw from the particles distribution
01169 
01170   Particles oldParticles = m_particles;
01171   m_particles.resize(numParticles);
01172   m_poseArray.poses.resize(numParticles);
01173   double newWeight = 1.0/numParticles;
01174 #pragma omp parallel for
01175   for (unsigned i = 0; i < numParticles; ++i){
01176     m_particles[i].pose = oldParticles[indices[i]].pose;
01177     m_particles[i].weight = newWeight;
01178   }
01179 }
01180 
01181 void HumanoidLocalization::initGlobal(){
01182   ROS_INFO("Initializing with uniform distribution");
01183 
01184   double roll, pitch, z;
01185   initZRP(z, roll, pitch);
01186 
01187   m_mapModel->initGlobal(m_particles, z, roll, pitch, m_initNoiseStd, m_rngUniform, m_rngNormal);
01188 
01189 
01190   ROS_INFO("Global localization done");
01191   m_motionModel->reset();
01192   m_receivedSensorData = false;
01193   m_initialized = true;
01194 
01195   publishPoseEstimate(ros::Time::now(), false);
01196 
01197 }
01198 
01199 void HumanoidLocalization::publishPoseEstimate(const ros::Time& time, bool publish_eval){
01200 
01202   // send all hypotheses as arrows:
01204 
01205   m_poseArray.header.stamp = time;
01206 
01207   if (m_poseArray.poses.size() != m_particles.size())
01208     m_poseArray.poses.resize(m_particles.size());
01209 
01210 #pragma omp parallel for
01211   for (unsigned i = 0; i < m_particles.size(); ++i){
01212     tf::poseTFToMsg(m_particles[i].pose, m_poseArray.poses[i]);
01213   }
01214 
01215   m_poseArrayPub.publish(m_poseArray);
01216 
01218   // send best particle as pose and one array:
01220   geometry_msgs::PoseWithCovarianceStamped p;
01221   p.header.stamp = time;
01222   p.header.frame_id = m_globalFrameId;
01223 
01224   tf::Pose bestParticlePose;
01225   if (m_bestParticleAsMean)
01226     bestParticlePose = getMeanParticlePose();
01227   else
01228     bestParticlePose = getBestParticlePose();
01229 
01230   tf::poseTFToMsg(bestParticlePose,p.pose.pose);
01231   m_posePub.publish(p);
01232 
01233   if (publish_eval){
01234     m_poseEvalPub.publish(p);
01235   }
01236 
01237   geometry_msgs::PoseArray bestPose;
01238   bestPose.header = p.header;
01239   bestPose.poses.resize(1);
01240   tf::poseTFToMsg(bestParticlePose, bestPose.poses[0]);
01241   m_bestPosePub.publish(bestPose);
01242 
01244   // send incremental odom pose (synced to localization)
01246   tf::Stamped<tf::Pose> lastOdomPose;
01247   if (m_motionModel->getLastOdomPose(lastOdomPose)){
01248     geometry_msgs::PoseStamped odomPoseMsg;
01249     tf::poseStampedTFToMsg(lastOdomPose, odomPoseMsg);
01250     m_poseOdomPub.publish(odomPoseMsg);
01251   }
01252 
01253 
01255   // Send tf target->map (where target is typically odom)
01256   tf::Stamped<tf::Pose> targetToMapTF;
01257   try{
01258     tf::Stamped<tf::Pose> baseToMapTF(bestParticlePose.inverse(),time, m_baseFrameId);
01259     m_tfListener.transformPose(m_targetFrameId, baseToMapTF, targetToMapTF); // typically target == odom
01260   } catch (const tf::TransformException& e){
01261     ROS_WARN("Failed to subtract base to %s transform, will not publish pose estimate: %s", m_targetFrameId.c_str(), e.what());
01262     return;
01263   }
01264 
01265   tf::Transform latestTF(tf::Quaternion(targetToMapTF.getRotation()), tf::Point(targetToMapTF.getOrigin()));
01266 
01267   // We want to send a transform that is good up until a
01268   // tolerance time so that odom can be used
01269   // see ROS amcl_node
01270 
01271   ros::Duration transformTolerance(m_transformTolerance);
01272   ros::Time transformExpiration = (time + transformTolerance);
01273 
01274   tf::StampedTransform tmp_tf_stamped(latestTF.inverse(), transformExpiration, m_globalFrameId, m_targetFrameId);
01275   m_latest_transform = tmp_tf_stamped;
01276 
01277   m_tfBroadcaster.sendTransform(tmp_tf_stamped);
01278 
01279 }
01280 
01281 unsigned HumanoidLocalization::getBestParticleIdx() const{
01282   if (m_bestParticleIdx < 0 || m_bestParticleIdx >= m_numParticles){
01283     ROS_WARN("Index (%d) of best particle not valid, using 0 instead", m_bestParticleIdx);
01284     return 0;
01285   }
01286 
01287   return m_bestParticleIdx;
01288 }
01289 
01290 tf::Pose HumanoidLocalization::getParticlePose(unsigned particleIdx) const{
01291   return m_particles.at(particleIdx).pose;
01292 }
01293 
01294 tf::Pose HumanoidLocalization::getBestParticlePose() const{
01295   return getParticlePose(getBestParticleIdx());
01296 }
01297 
01298 tf::Pose HumanoidLocalization::getMeanParticlePose() const{
01299   tf::Pose meanPose = tf::Pose::getIdentity();
01300 
01301   double totalWeight = 0.0;
01302 
01303   meanPose.setBasis(tf::Matrix3x3(0,0,0,0,0,0,0,0,0));
01304   for (Particles::const_iterator it = m_particles.begin(); it != m_particles.end(); ++it){
01305     meanPose.getOrigin() += it->pose.getOrigin() * it->weight;
01306     meanPose.getBasis()[0] += it->pose.getBasis()[0];
01307     meanPose.getBasis()[1] += it->pose.getBasis()[1];
01308     meanPose.getBasis()[2] += it->pose.getBasis()[2];
01309     totalWeight += it->weight;
01310   }
01311   assert(!isnan(totalWeight));
01312 
01313   //assert(totalWeight == 1.0);
01314 
01315   // just in case weights are not normalized:
01316   meanPose.getOrigin() /= totalWeight;
01317   // TODO: only rough estimate of mean rotation, asserts normalized weights!
01318   meanPose.getBasis() = meanPose.getBasis().scaled(tf::Vector3(1.0/m_numParticles, 1.0/m_numParticles, 1.0/m_numParticles));
01319 
01320   // Apparently we need to normalize again
01321   meanPose.setRotation(meanPose.getRotation().normalized());
01322 
01323   return meanPose;
01324 }
01325 
01326 double HumanoidLocalization::nEff() const{
01327 
01328   double sqrWeights=0.0;
01329   for (Particles::const_iterator it=m_particles.begin(); it!=m_particles.end(); ++it){
01330     sqrWeights+=(it->weight * it->weight);
01331   }
01332 
01333   if (sqrWeights > 0.0)
01334     return 1./sqrWeights;
01335   else
01336     return 0.0;
01337 }
01338 
01339 void HumanoidLocalization::toLogForm(){
01340   // TODO: linear offset needed?
01341 #pragma omp parallel for
01342   for (unsigned i = 0; i < m_particles.size(); ++i){
01343     assert(m_particles[i].weight > 0.0);
01344     m_particles[i].weight = log(m_particles[i].weight);
01345   }
01346 }
01347 
01348 void HumanoidLocalization::pauseLocalizationCallback(const std_msgs::BoolConstPtr& msg){
01349   if (msg->data){
01350     if (!m_paused){
01351       m_paused = true;
01352       ROS_INFO("Localization paused");
01353     } else{
01354       ROS_WARN("Received a msg to pause localizatzion, but is already paused.");
01355     }
01356   } else{
01357     if (m_paused){
01358       m_paused = false;
01359       ROS_INFO("Localization resumed");
01360       // force laser integration:
01361       m_receivedSensorData = false;
01362     } else {
01363       ROS_WARN("Received a msg to resume localization, is not paused.");
01364     }
01365 
01366   }
01367 
01368 }
01369 
01370 bool HumanoidLocalization::pauseLocalizationSrvCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
01371 {
01372   if (!m_paused){
01373     m_paused = true;
01374     ROS_INFO("Localization paused");
01375   } else{
01376     ROS_WARN("Received a request to pause localizatzion, but is already paused.");
01377   }
01378 
01379   return true;
01380 }
01381 
01382 bool HumanoidLocalization::resumeLocalizationSrvCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
01383 {
01384   if (m_paused){
01385     m_paused = false;
01386     ROS_INFO("Localization resumed");
01387     // force next laser integration:
01388     m_receivedSensorData = false;
01389   } else {
01390     ROS_WARN("Received a request to resume localization, but is not paused.");
01391   }
01392 
01393   return true;
01394 }
01395 
01396 bool HumanoidLocalization::lookupPoseHeight(const ros::Time& t, double& poseHeight) const{
01397   tf::StampedTransform tf;
01398   if (m_motionModel->lookupLocalTransform(m_baseFootprintId, t, tf)){
01399     poseHeight = tf.getOrigin().getZ();
01400     return true;
01401   } else
01402     return false;
01403 }
01404 
01405 }
01406 


humanoid_localization
Author(s): Armin Hornung, Stefan Osswald, Daniel Maier
autogenerated on Mon Oct 6 2014 00:41:01