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
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
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
00136 mMapFrame = mTransformListener.resolve(mMapFrame);
00137 mOdometryFrame = mTransformListener.resolve(mOdometryFrame);
00138 mRobotFrame = mTransformListener.resolve(mRobotFrame);
00139 mLaserFrame = mTransformListener.resolve(mLaserFrame);
00140
00141
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
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
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
00191
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
00201
00202
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
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
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;
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
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
00254 z = obsRange - mapRange;
00255 pz += sZHit * exp(-(z * z) / (2 * sSigmaHit * sSigmaHit));
00256
00257
00258 if(z < 0)
00259 pz += sZShort * sLamdaShort * exp(- sLamdaShort * obsRange);
00260
00261
00262 if(obsRange == data->mRangeMax)
00263 pz += sZMax * 1.0;
00264
00265
00266 if(obsRange < data->mRangeMax)
00267 pz += sZRand * 1.0/data->mRangeMax;
00268
00269
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
00292 for (int j = 0; j < set->sample_count; j++)
00293 {
00294 sample = set->samples + j;
00295 pose = sample->pose;
00296
00297
00298 pose = pf_vector_coord_add(sLaserPose, pose);
00299
00300
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
00312 if(obsRange >= data->mRangeMax)
00313 continue;
00314
00315 double z = 0.0;
00316 double pz = 0.0;
00317
00318
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
00323 int mi = MAP_GXWX(sMap, hit.v[0]);
00324 int mj = MAP_GYWY(sMap, hit.v[1]);
00325
00326
00327
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
00334
00335 pz += sZHit * exp(-(z * z) / zHitDenom);
00336
00337
00338 pz += sZRand * zRandMult;
00339
00340
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
00346
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
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
00371 pf_init_model(mParticleFilter, SelfLocalizer::distributeParticles, sMap);
00372
00373
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
00396 if(sMap == NULL) return;
00397
00398
00399
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
00421 mLastPose = tfPose;
00422 mFirstScanReceived = true;
00423 mProcessedScans = 0;
00424 }
00425
00426
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
00438 LaserData laser(scan);
00439 pf_update_sensor(mParticleFilter, mSensorModelFunction, (void*) &laser);
00440
00441
00442 pf_update_resample(mParticleFilter);
00443
00444
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
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
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
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
00547
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
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 }