From 61c5a7797fd871dc3df43051aa5e9b6038ad0323 Mon Sep 17 00:00:00 2001 From: Timothy Wiley Date: Thu, 27 Apr 2017 16:40:28 +1000 Subject: [PATCH] crosbot_fastslam: Remove job dispatcher from fastslam --- .../include/crosbot_fastslam/fastslam.hpp | 45 ---- crosbot_fastslam/src/fastslam.cpp | 199 +++++------------- crosbot_fastslam/src/module.cpp | 1 + 3 files changed, 51 insertions(+), 194 deletions(-) diff --git a/crosbot_fastslam/include/crosbot_fastslam/fastslam.hpp b/crosbot_fastslam/include/crosbot_fastslam/fastslam.hpp index 7654f5f..a41f5dd 100644 --- a/crosbot_fastslam/include/crosbot_fastslam/fastslam.hpp +++ b/crosbot_fastslam/include/crosbot_fastslam/fastslam.hpp @@ -11,7 +11,6 @@ #include #include #include -#include namespace crosbot { @@ -182,50 +181,6 @@ protected: void resample(); void resampleAndUpdate(MapCloudPtr cloud); - class ParticleMotionJob : public Job { - public: - ParticlePtr particle; - Pose motion; - Pose& pose; - const MotionModel& model; - MapCloudPtr cloud; - bool calculateWeight; - double gain; - - ParticleMotionJob(ParticlePtr particle, const Pose& motion, Pose& pose, - const MotionModel& model, MapCloudPtr cloud, bool calculateWeight, - double gain); - - void run(); - }; - - class ParticleUpdateJob : public Job { - public: - ParticlePtr particle; - MapCloudPtr cloud; - - ParticleUpdateJob(ParticlePtr particle, MapCloudPtr cloud); - - void run(); - }; - - class ParticleCopyJob : public Job { - public: - ParticlePtr particle; - std::vector& particleList; - std::vector& poseList; - Mutex& listMutex; - - ParticleCopyJob(ParticlePtr particle, std::vector& particleList, std::vector& poseList, Mutex& listMutex); - - void run(); - }; - - /** - * A set of threads for multi-threading map updates - */ - JobDispatcher workers; - /** * Friend classes */ diff --git a/crosbot_fastslam/src/fastslam.cpp b/crosbot_fastslam/src/fastslam.cpp index 622e699..7679614 100644 --- a/crosbot_fastslam/src/fastslam.cpp +++ b/crosbot_fastslam/src/fastslam.cpp @@ -20,7 +20,7 @@ namespace fastslam { FastSLAMMap::FastSLAMMap() : motionSumXY(0), motionSumTheta(0), slamOperating(true), slamPaused(false), slamUpdating(true), - slamThread(this), workers("FastSLAMMap::WorkerThread") + slamThread(this) { } @@ -33,7 +33,6 @@ void FastSLAMMap::configure(ConfigElementPtr config) { parameters.gain = config->getParamAsDouble("gain", parameters.gain); parameters.numberOfParticles = config->getParamAsInt("particles", parameters.numberOfParticles); - workers.setThreads(config->getParamAsInt("threads", 1)); if (config->hasParam("initialOdds")) { //LOG("BEFORE: %lf\n", parameters.initialOccupiedOdds); parameters.initialOccupiedOdds = @@ -130,7 +129,6 @@ MapPtr FastSLAMMap::clone() { FastSLAMMapPtr rval = new FastSLAMMap(); rval->parameters = parameters; rval->motionModel = motionModel; - rval->workers.setThreads(workers.threads()); rval->start(); return rval; @@ -511,19 +509,42 @@ void FastSLAMMap::moveAndUpdate(MapCloudPtr cloud) { void FastSLAMMap::moveParticles(Pose relativeMotion, MapCloudPtr cloud, bool calculateWeight) { tf::Transform motionTransform = relativeMotion.toTF(); - std::vector jobs; std::vector newPoses(particles.size()); for (size_t i = 0; i < particles.size(); i++) { - ParticleMotionJob* job = new ParticleMotionJob(particles[i], + /*ParticleMotionJob* job = new ParticleMotionJob(particles[i], relativeMotion, newPoses[i], motionModel, cloud, calculateWeight, parameters.gain); workers.addJob(job); - jobs.push_back(job); - } + jobs.push_back(job);*/ + + Pose randomMotion; + double yaw, pitch, roll; + relativeMotion.getYPR(yaw, pitch, roll); + + double& dx = relativeMotion.position.x; + double& dy = relativeMotion.position.y; + double dxy = sqrt(dx*dx + dy*dy); + + randomMotion.position.x = ProbabilityTable.NormRand48(dx, + motionModel.range * fabs(dx) + motionModel.rangeXY * fabs(dxy) + + motionModel.slip * fabs(yaw)); + randomMotion.position.y = ProbabilityTable.NormRand48(dy, + motionModel.shift * fabs(dy) + motionModel.shiftXY * fabs(dxy) + + motionModel.slip * fabs(yaw)); + + yaw = ProbabilityTable.NormRand48(yaw, + motionModel.theta * fabs(yaw) + motionModel.drift * fabs(dxy)); - // Wait for all jobs to finish. - while (workers.jobCount() > 0) { - usleep(5000); + // Apply smearing. + if (calculateWeight) { + randomMotion.position.x += ProbabilityTable.NormRand48(0, motionModel.smearX); + randomMotion.position.y += ProbabilityTable.NormRand48(0, motionModel.smearY); + yaw += ProbabilityTable.NormRand48(0, motionModel.smearTheta); + } + + randomMotion.setYPR(yaw, pitch, roll); + particles[i]->applyMotion(randomMotion.toTF(), cloud, calculateWeight, parameters.gain); + newPoses[i] = particles[i]->pose; } { @@ -533,36 +554,22 @@ void FastSLAMMap::moveParticles(Pose relativeMotion, MapCloudPtr cloud, bool cal particlePoses = newPoses; meanPose = mean->pose; } - - for (size_t i = 0; i < jobs.size(); i++) { - delete jobs[i]; - } - jobs.clear(); } void FastSLAMMap::updateParticles(MapCloudPtr cloud) { - std::vector jobs; - Job* job = new ParticleUpdateJob(motion, cloud); - jobs.push_back(job); - workers.addJob(job); + // Update motion particle + motion->update(cloud); + for (size_t i = 0; i < particles.size(); i++) { - job = new ParticleUpdateJob(particles[i], cloud); - workers.addJob(job); - jobs.push_back(job); - } - // Wait for all jobs to finish. - while (workers.jobCount() > 0) { - usleep(5000); - } - for (size_t i = 0; i < jobs.size(); i++) { - delete jobs[i]; + particles[i]->update(cloud); } - jobs.clear(); } void FastSLAMMap::resample() { - if (particles.size() == 0) + if (particles.size() == 0) { return; + } + double weightMax, weightSum = 0; ParticlePtr newMean = particles[0]; weightMax = newMean->weight; @@ -645,23 +652,17 @@ void FastSLAMMap::resampleAndUpdate(MapCloudPtr cloud) { std::vector newParticles; std::vector newPoses; - Mutex newParticleMutex; - - std::vector updates; - std::vector copies; - - // Update motion particle - ParticleUpdateJob *motionJob = new ParticleUpdateJob(motion, cloud); - updates.push_back(motionJob); - workers.addJob(motionJob); - + // Update and add motion particle + motion->update(cloud); size_t i = 0; if (parameters.addMotionCopy) { - ParticleCopyJob *copy = new ParticleCopyJob(motion, newParticles, newPoses, newParticleMutex); - copies.push_back(copy); + ParticlePtr newParticle = new Particle(motion); + newParticles.push_back(newParticle); + newPoses.push_back(newParticle->pose); ++i; } + // Update or add new particles for (; i < parameters.numberOfParticles; ++i) { double rand = ProbabilityTable.randxy(0, weightSum); ParticlePtr p = particles[0]; @@ -672,47 +673,21 @@ void FastSLAMMap::resampleAndUpdate(MapCloudPtr cloud) { wCurrent += p->weight; } + ParticlePtr updatedParticle; j--; if (!marks[j]) { - ParticlePtr p = particles[j]; - ParticleUpdateJob *update = new ParticleUpdateJob(p, cloud); - updates.push_back(update); - workers.addJob(update); - newParticles.push_back(p); - newPoses.push_back(p->pose); + updatedParticle = particles[j]; + updatedParticle->update(cloud); marks[j] = true; } else { - ParticleCopyJob *copy = new ParticleCopyJob(particles[j], newParticles, newPoses, newParticleMutex); - copies.push_back(copy); + updatedParticle = new Particle(particles[j]); } + newParticles.push_back(updatedParticle); + newPoses.push_back(updatedParticle->pose); } //ROS_INFO("FastSLAM %Zu particle updates, %Zu particle copies.\n", updates.size(), copies.size()); - // Wait for updates to finish - while (workers.jobCount() > 0) { - usleep(5000); - } - - for (size_t c = 0; c < copies.size(); c++) { - workers.addJob(copies[c]); - } - - for (size_t u = 0; u < updates.size(); u++) { - delete updates[u]; - } - updates.clear(); - - // Wait for copies to finish - while (workers.jobCount() > 0) { - usleep(5000); - } - - for (size_t c = 0; c < copies.size(); c++) { - delete copies[c]; - } - copies.clear(); - // Swap in the new mean and new particles { Lock lock(particlesLock, true); @@ -724,80 +699,6 @@ void FastSLAMMap::resampleAndUpdate(MapCloudPtr cloud) { } } - - -FastSLAMMap::ParticleMotionJob::ParticleMotionJob( - ParticlePtr particle, const Pose& motion, Pose& pose, - const MotionModel& model, MapCloudPtr cloud, bool calculateWeight, - double gain) : - particle(particle), - motion(motion), - pose(pose), - model(model), - cloud(cloud), - calculateWeight(calculateWeight), - gain(gain) -{} - -void FastSLAMMap::ParticleMotionJob::run() { - Pose randomMotion; - double yaw, pitch, roll; - motion.getYPR(yaw, pitch, roll); - - double& dx = motion.position.x; - double& dy = motion.position.y; - double dxy = sqrt(dx*dx + dy*dy); - - randomMotion.position.x = ProbabilityTable.NormRand48(dx, - model.range * fabs(dx) + model.rangeXY * fabs(dxy) + - model.slip * fabs(yaw)); - randomMotion.position.y = ProbabilityTable.NormRand48(dy, - model.shift * fabs(dy) + model.shiftXY * fabs(dxy) + - model.slip * fabs(yaw)); - - yaw = ProbabilityTable.NormRand48(yaw, - model.theta * fabs(yaw) + model.drift * fabs(dxy)); - - // Apply smearing. - if (calculateWeight) { - randomMotion.position.x += ProbabilityTable.NormRand48(0, model.smearX); - randomMotion.position.y += ProbabilityTable.NormRand48(0, model.smearY); - yaw += ProbabilityTable.NormRand48(0, model.smearTheta); - } - - randomMotion.setYPR(yaw, pitch, roll); - particle->applyMotion(randomMotion.toTF(), cloud, calculateWeight, gain); - pose = particle->pose; -} - -FastSLAMMap::ParticleUpdateJob::ParticleUpdateJob(ParticlePtr particle, MapCloudPtr cloud) : - particle(particle), - cloud(cloud) -{} - -void FastSLAMMap::ParticleUpdateJob::run() { - particle->update(cloud); -} - -FastSLAMMap::ParticleCopyJob::ParticleCopyJob( - ParticlePtr particle, std::vector& particleList, - std::vector& poseList, Mutex& listMutex) : - particle(particle), - particleList(particleList), - poseList(poseList), - listMutex(listMutex) -{} - -void FastSLAMMap::ParticleCopyJob::run() { - ParticlePtr newParticle = new Particle(particle); - { - Lock lock(listMutex); - - particleList.push_back(newParticle); - poseList.push_back(newParticle->pose); - } -} - } // namespace fastslam } // namespace crosbot diff --git a/crosbot_fastslam/src/module.cpp b/crosbot_fastslam/src/module.cpp index b7872cf..ad53b90 100644 --- a/crosbot_fastslam/src/module.cpp +++ b/crosbot_fastslam/src/module.cpp @@ -164,6 +164,7 @@ void FastSLAMModule::mapUpdated(MapPtr map) { // Publish transform MapCloudPtr update = newMean->getLatestUpdate(); if (update != NULL) { + ROS_INFO("%s map updating", LOG_START); Pose pose = newMean->getPose(); publishTransform(newMean->getPose(), update->odometry, update->timestamp); } -- GitLab