diff --git a/crosbot_fastslam/include/crosbot_fastslam/module.hpp b/crosbot_fastslam/include/crosbot_fastslam/module.hpp index 2cf37aa7e9c17fd860363a7dfe47daa0cd157d6e..5512eb0d4ab0e9fe5568d7bfd907029b880d4cfd 100644 --- a/crosbot_fastslam/include/crosbot_fastslam/module.hpp +++ b/crosbot_fastslam/include/crosbot_fastslam/module.hpp @@ -30,13 +30,19 @@ typedef Handle FastSLAMModulePtr; class FastSLAMModule : public FastSLAMMap, virtual public MapListener { protected: - std::string mapFrame, odomFrame, baseFrame; + // ROS Configuration + std::string mapFrame; + std::string odomFrame; + std::string baseFrame; - ros::Subscriber scanSub, snapSub; - tf::TransformListener tfListener; - tf::TransformBroadcaster tfBroadcast; + ros::Subscriber scanSub; + ros::Subscriber snapSub; + + ros::Publisher gridPub; + ros::Publisher historyPub; - ros::Publisher gridPub, historyPub; + tf::TransformListener tfListener; + tf::TransformBroadcaster tfBroadcast; public: FastSLAMModule(std::string name = ""); diff --git a/crosbot_fastslam/src/fastslam.cpp b/crosbot_fastslam/src/fastslam.cpp index 18706b6eef3e034e650734c755bec4ddcd687e17..622e6999ba3ec61fad35a5998c5f67666a1d597b 100644 --- a/crosbot_fastslam/src/fastslam.cpp +++ b/crosbot_fastslam/src/fastslam.cpp @@ -152,8 +152,10 @@ bool FastSLAMMap::load(std::string filename) throw (IOException) { serialization::FileInputStream fis(filename); serializer.read(*(newMean), fis); - { Lock lock(particlesLock, true); - mean = newMean; + { + Lock lock(particlesLock, true); + + mean = newMean; meanPose = mean->pose; motion = new Particle(newMean); @@ -169,8 +171,9 @@ bool FastSLAMMap::load(std::string filename) throw (IOException) { } bool FastSLAMMap::save(std::string filename) throw (IOException) { - if (mean == NULL) + if (mean == NULL) { return false; + } ParticlePtr particle = new Particle(mean); @@ -184,28 +187,35 @@ bool FastSLAMMap::save(std::string filename) throw (IOException) { Pose FastSLAMMap::getCurrentPose(Pose* trackerPose) { ParticlePtr particle = mean; if (particle == NULL) { - if (trackerPose != NULL) + if (trackerPose != NULL) { *trackerPose = Pose(NAN, NAN, NAN); + } return Pose(); } - { Lock lock(particle->rwLock); - if (trackerPose != NULL) + + { + Lock lock(particle->rwLock); + + if (trackerPose != NULL) { *trackerPose = particle->trackerPose; + } return particle->pose; } } Map::TagListPtr FastSLAMMap::getTags() { ParticlePtr particle = mean; - if (particle == NULL) + if (particle == NULL) { return NULL; + } return particle->tags; } PathPtr FastSLAMMap::getPath() { ParticlePtr particle = mean; - if (particle == NULL) + if (particle == NULL) { return NULL; + } return particle->getPath(); } @@ -251,8 +261,9 @@ nav_msgs::OccupancyGridPtr FastSLAMMap::asOccupancyGrid(ParticlePtr particle) { Lock lock(particle->rwLock); HeightMultiMapPtr hmm; - if (particle != NULL) + if (particle != NULL) { hmm = particle->map; + } if (particle == NULL || hmm == NULL) { rval->info.origin = Pose().toROS(); @@ -272,14 +283,18 @@ nav_msgs::OccupancyGridPtr FastSLAMMap::asOccupancyGrid(ParticlePtr particle) { for (unsigned int j = 0; j < hmm->columns; j++) { HeightMapPtr hm = hmm->patches[i][j]; if (hm != NULL) { - if (i < minHMY) + if (i < minHMY) { minHMY = i; - if (i > maxHMY) + } + if (i > maxHMY) { maxHMY = i; - if (j < minHMX) + } + if (j < minHMX) { minHMX = j; - if (j > maxHMX) + } + if (j > maxHMX) { maxHMX = j; + } if (hm->xOrig < rval->info.origin.position.x) { rval->info.origin.position.x = hm->xOrig; @@ -292,14 +307,14 @@ nav_msgs::OccupancyGridPtr FastSLAMMap::asOccupancyGrid(ParticlePtr particle) { } if (minHMX > maxHMX || minHMY > maxHMY) { - ERROR("FastSLAMRender: No data in map to export to GeoTIFF.\n"); + ROS_WARN("FastSLAMRender: No data in map to export to GeoTIFF.\n"); rval->info.origin = Pose().toROS(); return rval; } // create image - unsigned int imageWidth = (maxHMX + 1 - minHMX) * hmm->parameters.patchColumns, - imageHeight = (maxHMY + 1 - minHMY) * hmm->parameters.patchRows; + unsigned int imageWidth = (maxHMX + 1 - minHMX) * hmm->parameters.patchColumns; + unsigned int imageHeight = (maxHMY + 1 - minHMY) * hmm->parameters.patchRows; rval->info.height = imageHeight; rval->info.width = imageWidth; rval->info.resolution = CELL_WIDTH; @@ -308,8 +323,8 @@ nav_msgs::OccupancyGridPtr FastSLAMMap::asOccupancyGrid(ParticlePtr particle) { for (unsigned int i = minHMY; i <= maxHMY; i++) { for (unsigned int j = minHMX; j <= maxHMX; j++) { HeightMapPtr hm = hmm->patches[i][j]; - int offX = (j - minHMX) * hmm->parameters.patchColumns, - offY = (i - minHMY) * hmm->parameters.patchRows; + int offX = (j - minHMX) * hmm->parameters.patchColumns; + int offY = (i - minHMY) * hmm->parameters.patchRows; if (hm == NULL) { for (unsigned int il = 0; il < hmm->parameters.patchRows; il++) { for (unsigned int jl = 0; jl < hmm->parameters.patchColumns; jl++) { @@ -340,12 +355,14 @@ nav_msgs::OccupancyGridPtr FastSLAMMap::asOccupancyGrid(ParticlePtr particle) { } void FastSLAMMap::slam() { + // TODO: Get rid of "continues" from the loop - I missed them the first time! while (slamOperating) { slamSemaphore.wait(); MapCloudPtr update; - {{ + { Lock lock(incomingMutex); + if (!slamPaused && receivedTags.size() > 0) { TagPtr tag = receivedTags.front(); receivedTags.pop(); @@ -363,15 +380,16 @@ void FastSLAMMap::slam() { slamPaused = true; } else if (action.type == QueuedAction::Resume) { slamPaused = false; - } else if (action.type == QueuedAction::ResetPose) {{ + } else if (action.type == QueuedAction::ResetPose) { // Reset pose in FastSLAM map. Lock lock2(particlesLock, true); motion->setPose(action.pose); - for (uint32_t i = 0; i < particles.size(); i++) + for (uint32_t i = 0; i < particles.size(); i++) { particles[i]->setPose(action.pose); + } mapChanged(); - }} + } slamSemaphore.notify(); continue; @@ -379,7 +397,7 @@ void FastSLAMMap::slam() { update = latestPointCloud; latestPointCloud = NULL; - }} + } if (slamPaused || update == NULL) { continue; @@ -387,7 +405,8 @@ void FastSLAMMap::slam() { moveAndUpdate(update); - // TODO: use ros::Rate to spin a short time rather than just always process at max frame-rate + // TODO: use ros::Rate to spin (separate thread?) a short time rather than just always process at max frame-rate + // Not the heavy use of "continues" slamSemaphore.notify(); } @@ -397,7 +416,6 @@ void FastSLAMMap::slam() { void FastSLAMMap::insertTag(TagPtr tag) { if (mean == NULL) { // No mean particle so create one - mean = new Particle(parameters); mean->addTag(tag, true); return; @@ -407,8 +425,9 @@ void FastSLAMMap::insertTag(TagPtr tag) { motion->addTag(tag); Lock lock(particlesLock, true); for (size_t i = 0; i < particles.size(); i++) { - if (particles[i] != mean) + if (particles[i] != mean) { particles[i]->addTag(tag); + } } } @@ -416,24 +435,23 @@ void FastSLAMMap::moveAndUpdate(MapCloudPtr cloud) { if (motion == NULL) { // No motion particle means FastSLAM has not yet been started ParticlePtr newMean = mean, newMotion; - if (newMean == NULL) + if (newMean == NULL) { newMean = new Particle(parameters); + } newMean->trackerPose = cloud->odometry; newMean->history.push_back(Particle::History(newMean->pose, cloud, true)); newMean->poseTransform = newMean->pose.toTF(); if (slamUpdating) { -// double Y,P,R; -// cloud->odometry.orientation.getYPR(Y,P,R); newMean->motionCloud = cloud; newMean->update(newMean->motionCloud); } newMotion = new Particle(newMean); + { + Lock lock(particlesLock, true); - - { Lock lock(particlesLock, true); - mean = newMean; + mean = newMean; motion = newMotion; particles.clear(); particles.push_back(mean); @@ -446,14 +464,15 @@ void FastSLAMMap::moveAndUpdate(MapCloudPtr cloud) { return; } -// LOG("FastSLAM - Moving\n"); // Get motion - double yaw, pitch, roll; + double yaw; + double pitch; + double roll; motion->trackerPose.getYPR(yaw, pitch, roll); Pose motionPose; - tf::Transform motionTransform, - trackerTransform = cloud->odometry.toTF(); + tf::Transform motionTransform; + tf::Transform trackerTransform = cloud->odometry.toTF(); motionTransform = motion->trackerPose.toTF().inverse() * trackerTransform; motionPose = motionTransform; @@ -464,8 +483,9 @@ void FastSLAMMap::moveAndUpdate(MapCloudPtr cloud) { motionPose.position.y*motionPose.position.y); bool resampleNeeded = true; - if (motionSumXY < parameters.updateThresholdXY && motionSumTheta < parameters.updateThresholdTheta) + if (motionSumXY < parameters.updateThresholdXY && motionSumTheta < parameters.updateThresholdTheta) { resampleNeeded = false; + } moveParticles(motionPose, cloud, resampleNeeded); @@ -478,12 +498,10 @@ void FastSLAMMap::moveAndUpdate(MapCloudPtr cloud) { // Combine update and resample into one more efficient block if (slamUpdating) { -// LOG("FastSLAM - Updating\n"); + //ROS_INFO("%s Updating map", LOG_START); resampleAndUpdate(cloud); -// updateParticles(cloud); -// resample(); } else { -// LOG("FastSLAM - Resampling\n"); + //ROS_INFO("%s Resampling only", LOG_START); resample(); } @@ -508,8 +526,10 @@ void FastSLAMMap::moveParticles(Pose relativeMotion, MapCloudPtr cloud, bool cal usleep(5000); } - { Lock lock(particlesLock); - motion->applyMotion(motionTransform, cloud, calculateWeight); + { + Lock lock(particlesLock); + + motion->applyMotion(motionTransform, cloud, calculateWeight); particlePoses = newPoses; meanPose = mean->pose; } @@ -536,7 +556,8 @@ void FastSLAMMap::updateParticles(MapCloudPtr cloud) { } for (size_t i = 0; i < jobs.size(); i++) { delete jobs[i]; - } jobs.clear(); + } + jobs.clear(); } void FastSLAMMap::resample() { @@ -589,8 +610,10 @@ void FastSLAMMap::resample() { } } - { Lock lock(particlesLock, true); - mean = newMean; + { + Lock lock(particlesLock, true); + + mean = newMean; particles.clear(); particles = newParticles; newParticles.clear(); @@ -600,8 +623,9 @@ void FastSLAMMap::resample() { } void FastSLAMMap::resampleAndUpdate(MapCloudPtr cloud) { - if (particles.size() == 0) + if (particles.size() == 0) { return; + } double weightMax, weightSum = 0; ParticlePtr newMean = particles[0]; @@ -663,8 +687,7 @@ void FastSLAMMap::resampleAndUpdate(MapCloudPtr cloud) { } } -// LOG("FastSLAM %Zu particle updates, %Zu particle copies.\n", -// updates.size(), copies.size()); + //ROS_INFO("FastSLAM %Zu particle updates, %Zu particle copies.\n", updates.size(), copies.size()); // Wait for updates to finish while (workers.jobCount() > 0) { @@ -677,7 +700,8 @@ void FastSLAMMap::resampleAndUpdate(MapCloudPtr cloud) { for (size_t u = 0; u < updates.size(); u++) { delete updates[u]; - } updates.clear(); + } + updates.clear(); // Wait for copies to finish while (workers.jobCount() > 0) { @@ -686,11 +710,14 @@ void FastSLAMMap::resampleAndUpdate(MapCloudPtr cloud) { for (size_t c = 0; c < copies.size(); c++) { delete copies[c]; - } copies.clear(); + } + copies.clear(); // Swap in the new mean and new particles - { Lock lock(particlesLock, true); - mean = newMean; + { + Lock lock(particlesLock, true); + + mean = newMean; particles = newParticles; particlePoses = newPoses; meanPose = mean->pose; diff --git a/crosbot_fastslam/src/module.cpp b/crosbot_fastslam/src/module.cpp index 1fd844170ddcf9354e9c930b74d9694863a960e1..aca7a0a1fb9137b3e1b5fba03d17ab09651e4054 100644 --- a/crosbot_fastslam/src/module.cpp +++ b/crosbot_fastslam/src/module.cpp @@ -98,17 +98,22 @@ void FastSLAMModule::callbackScan(const sensor_msgs::LaserScanConstPtr& latestSc tfListener.waitForTransform(odomFrame, latestScan->header.frame_id, latestScan->header.stamp, ros::Duration(DEFAULT_MAXWAIT4TRANSFORM)); + + // Load transform laser to base tfListener.lookupTransform(baseFrame, latestScan->header.frame_id, latestScan->header.stamp, transform); sensorPose = transform; + + // Load transform base to odom estimate tfListener.lookupTransform(odomFrame, baseFrame, latestScan->header.stamp, transform); robotPose = transform; } catch (tf::TransformException& ex) { - ERROR("%s Error getting transform. (%s)\n", LOG_START, ex.what()); + ROS_WARN("%s Problem obtaining transform (%s)\n", LOG_START, ex.what()); return; } + // Load new laser scan point cloud PointCloud pc(latestScan); newPointCloud(new PointCloud(baseFrame, pc, sensorPose.toTF()), robotPose, sensorPose); } @@ -145,13 +150,14 @@ void FastSLAMModule::motionTracked(MapPtr map) { void FastSLAMModule::mapUpdated(MapPtr map) { ParticlePtr newMean = new Particle(mean); - // publish transform + // Publish transform MapCloudPtr update = newMean->getLatestUpdate(); if (update != NULL) { Pose pose = newMean->getPose(); publishTransform(newMean->getPose(), update->odometry, update->timestamp); } + // Publish Map Occupancy Grid if (gridPub.getNumSubscribers() > 0) { nav_msgs::OccupancyGridPtr grid = asOccupancyGrid(newMean); if (grid != NULL) { @@ -161,7 +167,7 @@ void FastSLAMModule::mapUpdated(MapPtr map) { } } - // publish history + // Publish history of robot path if (historyPub.getNumSubscribers() > 0) { nav_msgs::PathPtr rosPath(new nav_msgs::Path()); rosPath->header.frame_id = mapFrame;