00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <humanoid_localization/HumanoidLocalization.h>
00025 #include <iostream>
00026 #include <pcl/keypoints/uniform_sampling.h>
00027
00028
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
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);
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);
00083 m_privateNh.param("initial_std/y", m_initNoiseStd(1), 0.1);
00084 m_privateNh.param("initial_std/z", m_initNoiseStd(2), 0.02);
00085 m_privateNh.param("initial_std/roll", m_initNoiseStd(3), 0.04);
00086 m_privateNh.param("initial_std/pitch", m_initNoiseStd(4), 0.04);
00087 m_privateNh.param("initial_std_yaw", m_initNoiseStd(5), 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
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
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
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
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
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
00157 reset();
00158
00159
00160 m_globalLocSrv = m_nh.advertiseService("global_localization", &HumanoidLocalization::globalLocalizationCallback, this);
00161
00162
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
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
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){
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");
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
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
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
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
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
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
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){
00355
00356
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
00373 if (!m_constrainMotionZ && !m_constrainMotionRP)
00374 return;
00375
00376
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
00412 m_motionModel->applyOdomTransformTemporal(m_particles, t, m_temporalSamplingRange);
00413
00414
00415 tf::Stamped<tf::Transform> odomPose;
00416 assert(m_motionModel->lookupOdomPose(t, odomPose));
00417 constrainMotion(odomPose);
00418
00419
00420
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
00428 toLogForm();
00429
00430
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
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
00463 m_mapModel->verifyPoses(m_particles);
00464
00465
00466 normalizeWeights();
00467
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()){
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
00494
00495
00496 unsigned step = 1;
00497
00498
00499
00500
00501 unsigned int numBeamsSkipped = 0;
00502
00503
00504 double laserMin = std::max(double(laser->range_min), m_filterMinRange);
00505
00506
00507
00508 ranges.reserve(50);
00509
00510
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
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
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
00580 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00581 pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00582
00583
00584 pcl::SACSegmentation<pcl::PointXYZ> seg;
00585 seg.setOptimizeCoefficients (true);
00586
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
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
00619
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
00637
00638
00639
00640
00641
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
00654 if (!groundPlaneFound){
00655 ROS_WARN("No ground plane found in scan");
00656
00657
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
00669
00670
00671
00672
00673
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
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
00695
00696
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
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
00711 pcl::transformPointCloud(pc, pc, matSensorToBaseFootprint );
00712 filterGroundPlane(pc, ground, nonground, m_groundFilterDistance, m_groundFilterAngle, m_groundFilterPlaneDistance);
00713
00714
00715 pc.clear();
00716 pcl::PointCloud<int> sampledIndices;
00717
00718
00719 int numFloorPoints = 0;
00720 if (ground.size() > 0){
00721
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
00729 int numNonFloorPoints = 0;
00730 if (nonground.size() > 0){
00731
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
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
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
00756
00757 pcl::PointCloud<int> sampledIndices;
00758 voxelGridSampling(pc, sampledIndices, m_sensorSampleDist);
00759 pcl::copyPointCloud(pc, sampledIndices.points, pc);
00760
00761
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
00767
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));
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
00805 if (!m_motionModel->lookupOdomPose(msg->header.stamp, odomPose))
00806 return;
00807
00808
00809 bool sensor_integrated = false;
00810
00811
00812
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;
00818
00819
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
00828
00829 if (!m_paused && (!m_receivedSensorData || isAboveHeadMotionThreshold || isAboveMotionThreshold(odomPose))) {
00830
00831
00832 PointCloud pc_filtered;
00833 std::vector<float> rangesSparse;
00834 prepareGeneralPointCloud(msg, pc_filtered, rangesSparse);
00835
00836 double maxRange = 10.0;
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){
00842
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
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
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
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
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
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
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
01014 Matrix6d initCovL = initCov.llt().matrixL();
01015 tf::Transform transformNoise;
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;
01023
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
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
01051 m_motionModel->reset();
01052
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;
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
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
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
01147 double interval=getCumParticleWeight()/numParticles;
01148
01149
01150 double target=interval*m_rngUniform();
01151
01152
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
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
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
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
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
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);
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
01268
01269
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
01314
01315
01316 meanPose.getOrigin() /= totalWeight;
01317
01318 meanPose.getBasis() = meanPose.getBasis().scaled(tf::Vector3(1.0/m_numParticles, 1.0/m_numParticles, 1.0/m_numParticles));
01319
01320
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
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
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
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