diff --git a/crosbot_explore/CMakeLists.txt b/crosbot_explore/CMakeLists.txt
index ff5929d3b2efc67c2e6074eab6ee4167e317756f..a9fa427660ec1d83fa8ff802a6611145b2a52ebe 100644
--- a/crosbot_explore/CMakeLists.txt
+++ b/crosbot_explore/CMakeLists.txt
@@ -75,6 +75,7 @@ add_executable(astar src/nodes/astar.cpp)
 add_executable(explorer src/nodes/explorer.cpp)
 
 ## Add dependencies to the executable
+add_dependencies(crosbot_explore ${PROJECT_NAME}_gencpp)
 add_dependencies(astar ${PROJECT_NAME} crosbot_explore)
 add_dependencies(explorer ${PROJECT_NAME} crosbot_explore)
 
diff --git a/crosbot_explore/src/explorer.cpp b/crosbot_explore/src/explorer.cpp
index 2d58e5da8dad930f69cfb9013a2af92745ac46ab..78833c4538cb900b0472a776299e18eb0cdecbde 100644
--- a/crosbot_explore/src/explorer.cpp
+++ b/crosbot_explore/src/explorer.cpp
@@ -8,6 +8,8 @@
 #include <crosbot_explore/explorer.hpp>
 #include <crosbot/utils.hpp>
 
+#include <unistd.h>
+
 namespace crosbot {
 
 #define DEFAULT_SLEEP_TIME		50000
diff --git a/crosbot_explore/src/nodes/astar.cpp b/crosbot_explore/src/nodes/astar.cpp
index c4cf41bd29566e51e7b9e4760a5e3f7e2633dcff..c2aa670b6be8c2c436ad944af1908644a637fe3c 100644
--- a/crosbot_explore/src/nodes/astar.cpp
+++ b/crosbot_explore/src/nodes/astar.cpp
@@ -65,7 +65,24 @@ public:
 			if (latestMap != NULL) {
 				voronoi = latestVoronoi;
 				if (voronoi == NULL || voronoi->timestamp < Time(latestMap->header.stamp)) {
-					voronoi = new VoronoiGrid(*latestMap, voronoiConstraints);  // TODO: add robot pose
+					Pose robot(INFINITY,INFINITY,INFINITY);
+					std::string mapFrame = latestMap->header.frame_id;
+					ros::Time timestamp = latestMap->header.stamp;
+
+					if (baseFrame != "") {
+						try {
+							tf::StampedTransform transform;
+							tfListener.waitForTransform(mapFrame, baseFrame,
+									timestamp, ros::Duration(DEFAULT_MAXWAIT4TRANSFORM));
+							tfListener.lookupTransform(mapFrame,
+									baseFrame, timestamp, transform);
+							robot = transform;
+						} catch (tf::TransformException& ex) {
+							ERROR("astar: Error getting current robot transform. (%s)\n", ex.what());
+							robot = Pose(INFINITY,INFINITY,INFINITY);
+						}
+					}
+					voronoi = new VoronoiGrid(*latestMap, voronoiConstraints, robot);
 					lock.unlock();
 
 					Lock lock2(voronoiLock, true);
@@ -105,7 +122,7 @@ public:
     		pose.header.frame_id = frame_id;
     		pose.pose = path[i].toROS();
     	}
-    	LOG("AstarNode::GetPath(): Returning a path with %d poses.\n", path.size());
+    	LOG("AstarNode::GetPath(): Returning a path with %Zd poses.\n", path.size());
     	return true;
     }
 
diff --git a/crosbot_fastslam/include/crosbot_fastslam/serialization.hpp b/crosbot_fastslam/include/crosbot_fastslam/serialization.hpp
index 32e15435d1f2f63533b9cff83d3b5eafed424969..a4b2b3f6f3ed591b780d196b9c9abe49288e6886 100644
--- a/crosbot_fastslam/include/crosbot_fastslam/serialization.hpp
+++ b/crosbot_fastslam/include/crosbot_fastslam/serialization.hpp
@@ -167,7 +167,7 @@ public:
 		uint32_t nonNulls = 0;
 		rval += serUInt32.read(nonNulls, stream);
 
-		for (uint32_t i = 0; i < nonNulls; i++) {
+		for (uint32_t n = 0; n < nonNulls; n++) {
 			uint32_t i, j;
 			rval += serUInt32.read(i, stream);
 			rval += serUInt32.read(j, stream);
diff --git a/crosbot_fastslam/src/fastslam.cpp b/crosbot_fastslam/src/fastslam.cpp
index 840fd63f2604e3e37f424c404810b4e183761d57..0874e55da6236c4b6d65eafc0fa40558720bee5d 100644
--- a/crosbot_fastslam/src/fastslam.cpp
+++ b/crosbot_fastslam/src/fastslam.cpp
@@ -78,7 +78,7 @@ void FastSLAMMap::configure(ConfigElementPtr config) {
 		parameters.searchPose = searchConfig->getParamAsPose("pose", parameters.searchPose);
 	}
 
-	parameters.addMotionCopy = searchConfig->getParamAsBool("add_motion_copy", parameters.addMotionCopy);
+	parameters.addMotionCopy = config->getParamAsBool("add_motion_copy", parameters.addMotionCopy);
 }
 
 void FastSLAMMap::start() {
diff --git a/crosbot_map/CMakeLists.txt b/crosbot_map/CMakeLists.txt
index 84072875e746ea6270c0fd7ff845df6a07d13155..93af01609eabea908a450791ae8fdae56595c57d 100644
--- a/crosbot_map/CMakeLists.txt
+++ b/crosbot_map/CMakeLists.txt
@@ -79,7 +79,7 @@ add_executable(localmap src/nodes/localmap.cpp)
 ## Add dependencies to the executable
 # add_dependencies(crosbot_localmap_node ${PROJECT_NAME})
 add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp roscpp)
-add_dependencies(localmap ${PROJECT_NAME} roscpp crosbot_map_gencpp)
+add_dependencies(localmap ${PROJECT_NAME})
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(localmap
diff --git a/crosbot_mbicp/src/nodes/mbicp.cpp b/crosbot_mbicp/src/nodes/mbicp.cpp
index be92edb0c9bb2c3992fae1e8a4c8d20199b6f18b..86b59ac32d985ed7c64d4c4da9e89239f44254e0 100644
--- a/crosbot_mbicp/src/nodes/mbicp.cpp
+++ b/crosbot_mbicp/src/nodes/mbicp.cpp
@@ -21,6 +21,8 @@ using namespace crosbot;
 #define DEFAULT_ODOMFRAME			""
 #define DEFAULT_MAXWAIT4TRANSFORM	2.0			// [s]
 
+#define MAX_CONSECUTIVE_FAILURES	5
+
 class MBICPNode {
 	std::string icpFrame, baseFrame, odomFrame;
 
@@ -33,7 +35,7 @@ class MBICPNode {
 	bool useOdometry;
 
 	mbicp::MBICPStd mbicpStd;
-
+	unsigned int consecutiveFails;
 
 	// MBICP Parameters
 
@@ -52,7 +54,7 @@ public:
 	MBICPNode() :
 		icpFrame(DEFAULT_ICPFRAME), baseFrame(DEFAULT_BASEFRAME),
 		odomFrame(DEFAULT_ODOMFRAME), tfListener(ros::Duration(100000000,0)), useOdometry(false),
-		mbicpInitialized(false)
+		mbicpInitialized(false), consecutiveFails(0)
 	{
 
         maxAlignTheta = 0.4;        // MB: can't align cells further than this apart
@@ -232,9 +234,15 @@ public:
             } else {
                 ERROR("MBICP: MBICP failed(%d) ignoring current scan.\n", err);
             }
-            memcpy(&mbicpStd.ptosRef, &mbicpStd.ptosNew, sizeof(mbicp::Tscan));
+
+            if (consecutiveFails >= MAX_CONSECUTIVE_FAILURES) {
+                memcpy(&mbicpStd.ptosRef, &mbicpStd.ptosNew, sizeof(mbicp::Tscan));
+            }
+            consecutiveFails++;
+            return;
         }
 
+        consecutiveFails = 0;
         previousMove.position.x = x; previousMove.position.y = y; previousMove.position.z = 0;
         previousMove.orientation.setYPR(theta, 0, 0);
 
diff --git a/crosbot_ui/src/joystick.cpp b/crosbot_ui/src/joystick.cpp
index 581caa99930439b0893fadf3f5f94f467b745bec..f90e41ffbfe688c71d2f605886e09ae0553cc3fe 100644
--- a/crosbot_ui/src/joystick.cpp
+++ b/crosbot_ui/src/joystick.cpp
@@ -11,6 +11,8 @@
 #include <linux/joystick.h>
 #include <fcntl.h>
 #include <sys/ioctl.h>
+#include <unistd.h>
+#include <stdio.h>
 
 namespace crosbot {