SelfLocalizer.cpp
Go to the documentation of this file.
00001 #include "nav2d_localizer/SelfLocalizer.h"
00002 
00003 #include <tf/transform_listener.h>
00004 #include <math.h>
00005 
00006 bool isNaN(double a)
00007 {
00008         //return (a != a);
00009         return boost::math::isnan(a);
00010 }
00011 
00012 double angle_diff(double a, double b)
00013 {
00014         a = atan2(sin(a),cos(a));
00015         b = atan2(sin(b),cos(b));
00016         double d1 = a - b;
00017         double d2 = 2 * M_PI - fabs(d1);
00018         if(d1 > 0)
00019         d2 *= -1.0;
00020         if(fabs(d1) < fabs(d2))
00021                 return(d1);
00022         else
00023                 return(d2);
00024 }
00025 
00026 OdometryData::OdometryData(const tf::StampedTransform& pNewPose, const tf::StampedTransform& pLastPose)
00027 {
00028         mDeltaX = pNewPose.getOrigin().x() - pLastPose.getOrigin().x();
00029         mDeltaY = pNewPose.getOrigin().y() - pLastPose.getOrigin().y();
00030         mDeltaTheta = tf::getYaw(pNewPose.getRotation()) -  tf::getYaw(pLastPose.getRotation());
00031 }
00032 
00033 LaserData::LaserData(const sensor_msgs::LaserScan::ConstPtr& scan)
00034 {
00035         mRangeCount = scan->ranges.size();
00036         mRanges = new double[mRangeCount][2];
00037         mRangeMax = scan->range_max;
00038         
00039         double angle_min = scan->angle_min;
00040         double angle_inc = scan->angle_increment; 
00041         
00042         angle_inc = fmod(angle_inc + 5*M_PI, 2*M_PI) - M_PI;
00043         
00044         for(int i = 0; i < mRangeCount; i++)
00045         {
00046                 if(scan->ranges[i] <= scan->range_min)
00047                         mRanges[i][0] = scan->range_max;
00048                 else
00049                         mRanges[i][0] = scan->ranges[i];
00050 
00051                 // Compute bearing
00052                 mRanges[i][1] = angle_min + (i * angle_inc);
00053         }
00054 }
00055 
00056 LaserData::~LaserData()
00057 {
00058         delete[] mRanges;
00059 }
00060 
00061 map_t* SelfLocalizer::sMap = NULL;
00062 double SelfLocalizer::sSigmaHit;
00063 double SelfLocalizer::sLamdaShort;
00064 double SelfLocalizer::sZHit;
00065 double SelfLocalizer::sZMax;
00066 double SelfLocalizer::sZRand;
00067 double SelfLocalizer::sZShort;
00068 int SelfLocalizer::sMaxBeams;
00069 double SelfLocalizer::sLikelihoodMaxDist;
00070 
00071 double SelfLocalizer::sAlpha1;
00072 double SelfLocalizer::sAlpha2;
00073 double SelfLocalizer::sAlpha3;
00074 double SelfLocalizer::sAlpha4;
00075 
00076 pf_vector_t SelfLocalizer::sLaserPose;
00077 
00078 tf::StampedTransform SelfLocalizer::mLastPose;
00079 
00080 SelfLocalizer::SelfLocalizer(bool publish)
00081  : mPublishParticles(publish)
00082 {
00083         mActionModelFunction = (pf_action_model_fn_t)SelfLocalizer::calculateMoveModel;
00084         
00085         ros::NodeHandle globalNode;
00086         globalNode.param("laser_frame", mLaserFrame, std::string("laser"));
00087         globalNode.param("robot_frame", mRobotFrame, std::string("robot"));
00088         globalNode.param("odometry_frame", mOdometryFrame, std::string("odometry_base"));
00089         globalNode.param("map_frame", mMapFrame, std::string("map"));
00090         
00091         ros::NodeHandle localNode("~/");
00092         localNode.param("min_particles", mMinParticles, 500);
00093         localNode.param("max_particles", mMaxParticles, 2500);
00094         localNode.param("alpha_slow", mAlphaSlow, 0.001);
00095         localNode.param("alpha_fast", mAlphaFast, 0.1);
00096         localNode.param("min_translation", mMinTranslation, 0.2);
00097         localNode.param("min_rotation", mMinRotation, 0.5);
00098         localNode.param("pop_err", mPopulationErr, 0.01);
00099         localNode.param("pop_z", mPopulationZ, 0.99);
00100         
00101         localNode.param("laser_sigma_hit", sSigmaHit, 0.2);
00102         localNode.param("laser_lambda_short", sLamdaShort, 0.1);
00103         localNode.param("laser_z_hit", sZHit, 0.95);
00104         localNode.param("laser_z_max", sZMax, 0.05);
00105         localNode.param("laser_z_rand", sZRand, 0.05);
00106         localNode.param("laser_z_short", sZShort, 0.1);
00107         localNode.param("laser_max_beams", sMaxBeams, 30);
00108         localNode.param("laser_likelihood_max_dist", sLikelihoodMaxDist, 2.0);
00109         
00110         localNode.param("odom_alpha_1", sAlpha1, 0.25);
00111         localNode.param("odom_alpha_2", sAlpha2, 0.25);
00112         localNode.param("odom_alpha_3", sAlpha3, 0.25);
00113         localNode.param("odom_alpha_4", sAlpha4, 0.25);
00114         
00115         localNode.param("laser_model_type", mLaserModelType, 2);
00116         switch(mLaserModelType)
00117         {
00118         case 2:
00119                 mSensorModelFunction = (pf_sensor_model_fn_t)SelfLocalizer::calculateLikelihoodFieldModel;
00120                 break;
00121         case 1:
00122         default:
00123                 mLaserModelType = 1;
00124                 mSensorModelFunction = (pf_sensor_model_fn_t)SelfLocalizer::calculateBeamModel;
00125         }
00126         
00127         if(mPublishParticles)
00128         {
00129                 mParticlePublisher = localNode.advertise<geometry_msgs::PoseArray>("particles", 1, true);
00130         }
00131 
00132         mFirstScanReceived = false;
00133         mParticleFilter = NULL;
00134         
00135         // Apply tf_prefix to all used frame-id's
00136         mMapFrame = mTransformListener.resolve(mMapFrame);
00137         mOdometryFrame = mTransformListener.resolve(mOdometryFrame);
00138         mRobotFrame = mTransformListener.resolve(mRobotFrame);
00139         mLaserFrame = mTransformListener.resolve(mLaserFrame);
00140         
00141         // Use squared distance so we don't need sqrt() later
00142         mMinTranslation *= mMinTranslation;
00143         mMapToOdometry.setIdentity();
00144 }
00145 
00146 SelfLocalizer::~SelfLocalizer()
00147 {
00148         if(mParticleFilter)
00149                 pf_free(mParticleFilter);
00150         if(sMap)        
00151                 map_free(sMap);
00152 }
00153 
00154 pf_vector_t SelfLocalizer::distributeParticles(void* data)
00155 {
00156         map_t* map = (map_t*)data;
00157 
00158         double min_x, max_x, min_y, max_y;
00159         
00160         min_x = map->origin_x - (map->size_x * map->scale / 2.0);
00161         max_x = map->origin_x + (map->size_x * map->scale / 2.0);
00162         min_y = map->origin_y - (map->size_y * map->scale / 2.0);
00163         max_y = map->origin_y + (map->size_y * map->scale / 2.0);
00164 
00165         pf_vector_t p;
00166 
00167         while(true)
00168         {
00169                 p.v[0] = min_x + drand48() * (max_x - min_x);
00170                 p.v[1] = min_y + drand48() * (max_y - min_y);
00171                 p.v[2] = drand48() * 2 * M_PI - M_PI;
00172 
00173                 // Check that it's a free cell
00174                 int i,j;
00175                 i = MAP_GXWX(map, p.v[0]);
00176                 j = MAP_GYWY(map, p.v[1]);
00177                 if(MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1))
00178                         break;
00179         }
00180         return p;
00181 }
00182 
00183 double SelfLocalizer::calculateMoveModel(OdometryData* data, pf_sample_set_t* set)
00184 {
00185         // Implement sample_motion_odometry (Prob Rob p 136)
00186         double delta_rot1, delta_trans, delta_rot2;
00187         double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
00188         double delta_rot1_noise, delta_rot2_noise;
00189 
00190         // Avoid computing a bearing from two poses that are extremely near each
00191         // other (happens on in-place rotation).
00192         delta_trans = sqrt(data->mDeltaX * data->mDeltaX + data->mDeltaY * data->mDeltaY);
00193         if(delta_trans < 0.01)
00194                 delta_rot1 = 0.0;
00195         else
00196                 delta_rot1 = angle_diff(atan2(data->mDeltaY, data->mDeltaX), tf::getYaw(mLastPose.getRotation()));
00197 
00198         delta_rot2 = angle_diff(data->mDeltaTheta, delta_rot1);
00199 
00200         // We want to treat backward and forward motion symmetrically for the
00201         // noise model to be applied below.  The standard model seems to assume
00202         // forward motion.
00203         delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), fabs(angle_diff(delta_rot1,M_PI)));
00204         delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), fabs(angle_diff(delta_rot2,M_PI)));
00205 
00206         for (int i = 0; i < set->sample_count; i++)
00207         {
00208                 pf_sample_t* sample = set->samples + i;
00209 
00210                 // Sample pose differences
00211                 delta_rot1_hat = angle_diff(delta_rot1, pf_ran_gaussian(sAlpha1*delta_rot1_noise*delta_rot1_noise + sAlpha2*delta_trans*delta_trans));
00212                 delta_trans_hat = delta_trans - pf_ran_gaussian(sAlpha3*delta_trans*delta_trans + sAlpha4*delta_rot1_noise*delta_rot1_noise + sAlpha4*delta_rot2_noise*delta_rot2_noise);
00213                 delta_rot2_hat = angle_diff(delta_rot2, pf_ran_gaussian(sAlpha1*delta_rot2_noise*delta_rot2_noise + sAlpha2*delta_trans*delta_trans));
00214 
00215                 // Apply sampled update to particle pose
00216                 sample->pose.v[0] += delta_trans_hat * cos(sample->pose.v[2] + delta_rot1_hat);
00217                 sample->pose.v[1] += delta_trans_hat * sin(sample->pose.v[2] + delta_rot1_hat);
00218                 sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
00219                 sample->weight = 1.0 / set->sample_count;
00220         }
00221         
00222         return 0.0;
00223 }
00224 
00225 double SelfLocalizer::calculateBeamModel(LaserData *data, pf_sample_set_t* set)
00226 {
00227         pf_sample_t *sample;
00228         pf_vector_t pose;
00229         
00230         double mapRange;
00231         double obsRange, obsBearing;
00232         double z, pz; // What is z/pz ?
00233         
00234         double totalWeight = 0.0;
00235         
00236         for (int j = 0; j < set->sample_count; j++)
00237         {
00238                 sample = set->samples + j;
00239                 pose = sample->pose;
00240 
00241                 pose = pf_vector_coord_add(pose, sLaserPose);
00242 //              pose = pf_vector_coord_sub(pose, sLaserPose);
00243 
00244                 double p = 1.0;
00245                 int step = (data->mRangeCount - 1) / (sMaxBeams - 1);
00246                 for (int i = 0; i < data->mRangeCount; i+=step)
00247                 {
00248                         obsRange = data->mRanges[i][0];
00249                         obsBearing = data->mRanges[i][1];
00250                         mapRange = map_calc_range(sMap, pose.v[0], pose.v[1], pose.v[2]+obsBearing, data->mRangeMax);
00251                         pz = 0.0;
00252 
00253                         // Part 1: good, but noisy, hit
00254                         z = obsRange - mapRange;
00255                         pz += sZHit * exp(-(z * z) / (2 * sSigmaHit * sSigmaHit));
00256 
00257                         // Part 2: short reading from unexpected obstacle (e.g., a person)
00258                         if(z < 0)
00259                         pz += sZShort * sLamdaShort * exp(- sLamdaShort * obsRange);
00260 
00261                         // Part 3: Failure to detect obstacle, reported as max-range
00262                         if(obsRange == data->mRangeMax)
00263                         pz += sZMax * 1.0;
00264 
00265                         // Part 4: Random measurements
00266                         if(obsRange < data->mRangeMax)
00267                         pz += sZRand * 1.0/data->mRangeMax;
00268 
00269                         // TODO: outlier rejection for short readings
00270 
00271                         assert(pz <= 1.0);
00272                         assert(pz >= 0.0);
00273 
00274                         p += pz*pz*pz;
00275                 }
00276                 sample->weight *= p;
00277                 totalWeight += sample->weight;
00278         }
00279         return totalWeight;
00280 }
00281 
00282 double SelfLocalizer::calculateLikelihoodFieldModel(LaserData *data, pf_sample_set_t* set)
00283 {
00284         int i, step;
00285         double obsRange, obsBearing;
00286         double totalWeight = 0.0;
00287         pf_sample_t* sample;
00288         pf_vector_t pose;
00289         pf_vector_t hit;
00290 
00291         // Compute the sample weights
00292         for (int j = 0; j < set->sample_count; j++)
00293         {
00294                 sample = set->samples + j;
00295                 pose = sample->pose;
00296 
00297                 // Take into account the laser pose relative to the robot
00298                 pose = pf_vector_coord_add(sLaserPose, pose);
00299 
00300                 // Pre-compute a couple of things
00301                 double zHitDenom = 2.0 * sSigmaHit * sSigmaHit;
00302                 double zRandMult = 1.0 / data->mRangeMax;
00303                 double p = 1.0;
00304 
00305                 step = (data->mRangeCount - 1) / (sMaxBeams - 1);
00306                 for (i = 0; i < data->mRangeCount; i += step)
00307                 {
00308                         obsRange = data->mRanges[i][0];
00309                         obsBearing = data->mRanges[i][1];
00310 
00311                         // This model ignores max range readings
00312                         if(obsRange >= data->mRangeMax)
00313                                 continue;
00314 
00315                         double z = 0.0;
00316                         double pz = 0.0;
00317 
00318                         // Compute the endpoint of the beam
00319                         hit.v[0] = pose.v[0] + obsRange * cos(pose.v[2] + obsBearing);
00320                         hit.v[1] = pose.v[1] + obsRange * sin(pose.v[2] + obsBearing);
00321 
00322                         // Convert to map grid coords.
00323                         int mi = MAP_GXWX(sMap, hit.v[0]);
00324                         int mj = MAP_GYWY(sMap, hit.v[1]);
00325 
00326                         // Part 1: Get distance from the hit to closest obstacle.
00327                         // Off-map penalized as max distance
00328                         if(!MAP_VALID(sMap, mi, mj))
00329                                 z = sMap->max_occ_dist;
00330                         else
00331                                 z = sMap->cells[MAP_INDEX(sMap,mi,mj)].occ_dist;
00332 
00333                         // Gaussian model
00334                         // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
00335                         pz += sZHit * exp(-(z * z) / zHitDenom);
00336 
00337                         // Part 2: random measurements
00338                         pz += sZRand * zRandMult;
00339 
00340                         // TODO: outlier rejection for short readings
00341 
00342                         if(pz < 0.0 || pz > 1.0)
00343                                 ROS_WARN("Value pz = %.2f, but it should be in range 0..1", pz);
00344 
00345                         // here we have an ad-hoc weighting scheme for combining beam probs
00346                         // works well, though...
00347                         p += (pz*pz*pz);
00348                 }
00349 
00350                 sample->weight *= p;
00351                 totalWeight += sample->weight;
00352         }
00353 
00354         return(totalWeight);
00355 }
00356 
00357 bool SelfLocalizer::initialize()
00358 {
00359         // Create the particle filter
00360         mParticleFilter = pf_alloc(     mMinParticles, mMaxParticles, 
00361                                                                 mAlphaSlow, mAlphaFast,
00362                                                                 SelfLocalizer::distributeParticles,
00363                                                                 (void*) sMap);
00364                 
00365         pf_sample_set_t* set = mParticleFilter->sets + mParticleFilter->current_set;
00366         ROS_INFO("Initialized PF with %d samples.", set->sample_count); 
00367         mParticleFilter->pop_err = mPopulationErr;
00368         mParticleFilter->pop_z = mPopulationZ;
00369         
00370         // Distribute particles uniformly in the free space of the map
00371         pf_init_model(mParticleFilter, SelfLocalizer::distributeParticles, sMap);
00372         
00373         // Get the laser pose on the robot
00374         tf::StampedTransform laserPose;
00375         mTransformListener.waitForTransform(mRobotFrame, mLaserFrame, ros::Time(0), ros::Duration(5.0));
00376         
00377         try
00378         {
00379                 mTransformListener.lookupTransform(mRobotFrame, mLaserFrame, ros::Time(0), laserPose);
00380         }
00381         catch(tf::TransformException e)
00382         {
00383                 return false;
00384         }
00385         
00386         sLaserPose.v[0] = laserPose.getOrigin().getX();
00387         sLaserPose.v[1] = laserPose.getOrigin().getY();
00388         sLaserPose.v[2] = tf::getYaw(laserPose.getRotation());
00389         
00390         return true;
00391 }
00392 
00393 void SelfLocalizer::process(const sensor_msgs::LaserScan::ConstPtr& scan)
00394 {
00395         // Ignore all scans unless we got a map
00396         if(sMap == NULL) return;
00397         
00398         // Get the odometry pose from TF. (We use RobotFrame here instead of LaserFrame, 
00399         // so the motion model of the particle filter can work correctly.)
00400         tf::StampedTransform tfPose;
00401         try
00402         {
00403                 mTransformListener.lookupTransform(mOdometryFrame, mRobotFrame, scan->header.stamp, tfPose);
00404         }
00405         catch(tf::TransformException e)
00406         {
00407                 try
00408                 {
00409                         mTransformListener.lookupTransform(mOdometryFrame, mRobotFrame, ros::Time(0), tfPose);
00410                 }
00411                 catch(tf::TransformException e)
00412                 {
00413                         ROS_WARN("Failed to compute odometry pose, skipping scan (%s)", e.what());
00414                         return;
00415                 }
00416         }
00417         
00418         if(!mFirstScanReceived)
00419         {
00420                 // Process the first laser scan after initialization
00421                 mLastPose = tfPose;
00422                 mFirstScanReceived = true;
00423                 mProcessedScans = 0;
00424         }
00425         
00426         // Update motion model with odometry data
00427         OdometryData odom(tfPose, mLastPose);
00428 
00429         double dist = odom.mDeltaX * odom.mDeltaX + odom.mDeltaY * odom.mDeltaY;
00430         if(dist < mMinTranslation && fabs(odom.mDeltaTheta) < mMinRotation)
00431                 return;
00432 
00433         mProcessedScans++;
00434         pf_update_action(mParticleFilter, mActionModelFunction, (void*) &odom);
00435         mLastPose = tfPose;
00436 
00437         // Update sensor model with laser data
00438         LaserData laser(scan);
00439         pf_update_sensor(mParticleFilter, mSensorModelFunction, (void*) &laser);
00440 
00441         // Do the resampling step
00442         pf_update_resample(mParticleFilter);
00443         
00444         // Create the map-to-odometry transform
00445         tf::Transform map2robot = getBestPose();
00446         tf::Stamped<tf::Pose> odom2map;
00447         try
00448         {
00449                 tf::Stamped<tf::Pose> robot2map;
00450                 robot2map.setData(map2robot.inverse());
00451                 robot2map.stamp_ = scan->header.stamp;
00452                 robot2map.frame_id_ = mRobotFrame;
00453 
00454                 mTransformListener.transformPose(mOdometryFrame, robot2map, odom2map);
00455                 mMapToOdometry = odom2map.inverse();
00456         }
00457         catch(tf::TransformException)
00458         {
00459                 ROS_WARN("Failed to subtract base to odom transform");
00460         }
00461 
00462         // Publish the particles for visualization
00463         publishParticleCloud();
00464 }
00465 
00466 void SelfLocalizer::convertMap( const nav_msgs::OccupancyGrid& map_msg )
00467 {
00468         map_t* map = map_alloc();
00469         ROS_ASSERT(map);
00470 
00471         map->size_x = map_msg.info.width;
00472         map->size_y = map_msg.info.height;
00473         map->scale = map_msg.info.resolution;
00474         map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
00475         map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;
00476 
00477         // Convert to pf-internal format
00478         map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
00479         ROS_ASSERT(map->cells);
00480         for(int i = 0; i < map->size_x * map->size_y; i++)
00481         {
00482                 if(map_msg.data[i] == 0)
00483                         map->cells[i].occ_state = -1;
00484                 else if(map_msg.data[i] == 100)
00485                         map->cells[i].occ_state = +1;
00486                 else
00487                         map->cells[i].occ_state = 0;
00488         }
00489 
00490         if(sMap)        
00491                 map_free(sMap);
00492                 
00493         sMap = map;
00494         if(mLaserModelType == 2)
00495         {
00496                 ROS_INFO("Initializing likelihood field model. This can take some time on large maps...");
00497                 map_update_cspace(sMap, sLikelihoodMaxDist);
00498         }
00499 }
00500 
00501 double SelfLocalizer::getCovariance()
00502 {
00503         pf_sample_set_t* set = mParticleFilter->sets + mParticleFilter->current_set;
00504         double max = set->cov.m[0][0];
00505         if(set->cov.m[1][1] > max) max = set->cov.m[1][1];
00506         if(set->cov.m[2][2] > max) max = set->cov.m[2][2];
00507         return max;
00508 }
00509 
00510 tf::Transform SelfLocalizer::getBestPose()
00511 {
00512         pf_vector_t pose;
00513         pose.v[0] = 0;
00514         pose.v[1] = 0;
00515         pose.v[2] = 0;
00516         pf_sample_set_t* set = mParticleFilter->sets + mParticleFilter->current_set;
00517         
00518         // Wow, is this really the only way to do this !?!
00519         double max_weight = 0.0;
00520         
00521         for(int i = 0; i < set->cluster_count; i++)
00522         {
00523                 double weight;
00524                 pf_vector_t pose_mean;
00525                 pf_matrix_t pose_cov;
00526                 if (!pf_get_cluster_stats(mParticleFilter, i, &weight, &pose_mean, &pose_cov))
00527                 {
00528                         ROS_ERROR("Couldn't get stats on cluster %d", i);
00529                         break;
00530                 }
00531 
00532                 if(weight > max_weight)
00533                 {
00534                         max_weight = weight;
00535                         pose = pose_mean;
00536                 }
00537         }
00538 
00539         if(max_weight > 0.0)
00540         {
00541                 ROS_DEBUG("Determined pose at: %.3f %.3f %.3f", pose.v[0], pose.v[1], pose.v[2]);
00542         }else
00543         {
00544                 ROS_ERROR("Could not get pose from particle filter!");
00545         }
00546 //      pose = pf_vector_coord_sub(pose, sLaserPose);
00547 //      return pose;
00548         
00549         tf::Transform map2robot;
00550         map2robot.setOrigin(tf::Vector3(pose.v[0], pose.v[1], 0));
00551         map2robot.setRotation(tf::createQuaternionFromYaw(pose.v[2]));
00552 
00553         return map2robot;
00554 }
00555 
00556 void SelfLocalizer::publishParticleCloud()
00557 {
00558         if(!mPublishParticles) return;
00559         
00560         pf_sample_set_t* set = mParticleFilter->sets + mParticleFilter->current_set;
00561         
00562         geometry_msgs::PoseArray cloud_msg;
00563         cloud_msg.header.stamp = ros::Time::now();
00564         cloud_msg.header.frame_id = mMapFrame.c_str();
00565         cloud_msg.poses.resize(set->sample_count);
00566         for(int i = 0; i < set->sample_count; i++)
00567         {
00568                 double x = set->samples[i].pose.v[0];
00569                 double y = set->samples[i].pose.v[1];
00570                 double yaw = set->samples[i].pose.v[2];
00571                 tf::Pose pose(tf::createQuaternionFromYaw(yaw), tf::Vector3(x, y, 0));
00572                 tf::poseTFToMsg(pose, cloud_msg.poses[i]);
00573 
00574                 // Check content of the message because sometimes NaN values occur and therefore visualization in rviz breaks
00575                 geometry_msgs::Pose pose_check = cloud_msg.poses.at(i);
00576                 geometry_msgs::Point pt = pose_check.position;
00577                 if(isNaN(pt.x))
00578                         ROS_WARN("NaN occured at pt.x before publishing particle cloud...");
00579                 if(isNaN(pt.y))
00580                         ROS_WARN("NaN occured at pt.y before publishing particle cloud...");
00581                 if(isNaN(pt.z))
00582                         ROS_WARN("NaN occured at pt.z before publishing particle cloud...");
00583                 geometry_msgs::Quaternion ori = pose_check.orientation;
00584                 if(isNaN(ori.x)){
00585                         ROS_WARN("NaN occured at ori.x before publishing particle cloud, setting it to zero (original x:%f y:%f yaw:%f) ...", set->samples[i].pose.v[0], set->samples[i].pose.v[1], set->samples[i].pose.v[2]);
00586                         cloud_msg.poses.at(i).orientation.x = 0;
00587                 }
00588                 if(isNaN(ori.y)){
00589                         ROS_WARN("NaN occured at ori.y before publishing particle cloud, setting it to zero (original x:%f y:%f yaw:%f) ...", set->samples[i].pose.v[0], set->samples[i].pose.v[1], set->samples[i].pose.v[2]);
00590                         cloud_msg.poses.at(i).orientation.y = 0;
00591                 }
00592                 if(isNaN(ori.z))
00593                         ROS_WARN("NaN occured at ori.z before publishing particle cloud ...");
00594                 if(isNaN(ori.w))
00595                         ROS_WARN("NaN occured at ori.w before publishing particle cloud ...");
00596 
00597         }
00598         mParticlePublisher.publish(cloud_msg);
00599 }


nav2d_localizer
Author(s): Sebastian Kasperski
autogenerated on Thu Aug 27 2015 14:07:21