From 1dee5c21f8382127be5e10c073d238615d4ac590 Mon Sep 17 00:00:00 2001
From: Timothy Wiley <timothyw@cse.unsw.edu.au>
Date: Thu, 27 Apr 2017 17:20:15 +1000
Subject: [PATCH] crosbot_fastslam: remove mid-function returns from
 moveAndUpdate

---
 crosbot_fastslam/src/fastslam.cpp | 90 ++++++++++++++++---------------
 1 file changed, 47 insertions(+), 43 deletions(-)

diff --git a/crosbot_fastslam/src/fastslam.cpp b/crosbot_fastslam/src/fastslam.cpp
index 786859c..68306fc 100644
--- a/crosbot_fastslam/src/fastslam.cpp
+++ b/crosbot_fastslam/src/fastslam.cpp
@@ -438,6 +438,7 @@ void FastSLAMMap::insertTag(TagPtr tag) {
 void FastSLAMMap::moveAndUpdate(MapCloudPtr cloud) {
 	if (motion == NULL) {
 		// No motion particle means FastSLAM has not yet been started
+	    // Generate the motion particle
 		ParticlePtr newMean = mean, newMotion;
 		if (newMean == NULL) {
 			newMean = new Particle(parameters);
@@ -465,51 +466,54 @@ void FastSLAMMap::moveAndUpdate(MapCloudPtr cloud) {
 
 		resample();
 		mapChanged();
-		return;
-	}
-
-	// Get motion
-	double yaw;
-	double pitch;
-	double roll;
-	motion->trackerPose.getYPR(yaw, pitch, roll);
-
-	Pose motionPose;
-	tf::Transform motionTransform;
-	tf::Transform trackerTransform = cloud->odometry.toTF();
-
-	motionTransform = motion->trackerPose.toTF().inverse() * trackerTransform;
-	motionPose = motionTransform;
-
-	motionPose.getYPR(yaw, pitch, roll);
-	motionSumTheta += fabs(yaw);
-	motionSumXY += sqrt(motionPose.position.x*motionPose.position.x +
-			motionPose.position.y*motionPose.position.y);
-
-	bool resampleNeeded = true;
-	if (motionSumXY < parameters.updateThresholdXY && motionSumTheta < parameters.updateThresholdTheta) {
-		resampleNeeded = false;
-	}
-
-	moveParticles(motionPose, cloud, resampleNeeded);
-
-	if (!resampleNeeded) {
-		motionTracked();
-		return;
-	}
-
-	motionSumXY = motionSumTheta = 0.0;
-
-	// Combine update and resample into one more efficient block
-	if (slamUpdating) {
-	    //ROS_INFO("%s Updating map", LOG_START);
-		resampleAndUpdate(cloud);
 	} else {
-	    //ROS_INFO("%s Resampling only", LOG_START);
-		resample();
+	    // Update the motion particle, then update fastslam map
+
+        // Get motion
+        double yaw;
+        double pitch;
+        double roll;
+        motion->trackerPose.getYPR(yaw, pitch, roll);
+
+        Pose motionPose;
+        tf::Transform motionTransform;
+        tf::Transform trackerTransform = cloud->odometry.toTF();
+
+        motionTransform = motion->trackerPose.toTF().inverse() * trackerTransform;
+        motionPose = motionTransform;
+
+        motionPose.getYPR(yaw, pitch, roll);
+        motionSumTheta += fabs(yaw);
+        motionSumXY += sqrt(motionPose.position.x*motionPose.position.x + motionPose.position.y*motionPose.position.y);
+
+        // Decide is resampling is required
+        bool resampleNeeded = true;
+        if (motionSumXY < parameters.updateThresholdXY && motionSumTheta < parameters.updateThresholdTheta) {
+            resampleNeeded = false;
+        }
+
+        // Update particles with motion
+        moveParticles(motionPose, cloud, resampleNeeded);
+
+        if (!resampleNeeded) {
+            // If resampling not required, then track motion
+            motionTracked();
+        } else {
+            // If resampling, then update particles and map
+            motionSumXY = motionSumTheta = 0.0;
+
+            // Combine update and resample into one more efficient block
+            if (slamUpdating) {
+                //ROS_INFO("%s Updating map", LOG_START);
+                resampleAndUpdate(cloud);
+            } else {
+                //ROS_INFO("%s Resampling only", LOG_START);
+                resample();
+            }
+
+            mapChanged();
+        }
 	}
-
-	mapChanged();
 }
 
 
-- 
GitLab