diff --git a/crosbot_explore/include/crosbot_explore/explorer.hpp b/crosbot_explore/include/crosbot_explore/explorer.hpp
index 4ac498c9204137435bcc3b357975f40cf0209649..1dd0cafdf06d9c64cb91ecd3d0db465036803eb3 100644
--- a/crosbot_explore/include/crosbot_explore/explorer.hpp
+++ b/crosbot_explore/include/crosbot_explore/explorer.hpp
@@ -200,8 +200,9 @@ public:
 	virtual ~Explorer();
 
 	virtual void shutdown();
-	virtual void pause() { paused = true; }
-	virtual void resume() { drive.setDriveTarget(Pose(INFINITY, INFINITY, INFINITY)); paused = false; }
+	virtual void pause() { paused = true; statusChanged("Paused"); }
+	virtual void resume() { drive.setDriveTarget(Pose(INFINITY, INFINITY, INFINITY)); paused = false; statusChanged("Running"); }
+	virtual void statusChanged(const std::string& status) {}
 
 	virtual VoronoiGridPtr getLatestVoronoi()=0;
 protected:
diff --git a/crosbot_explore/src/explorer.cpp b/crosbot_explore/src/explorer.cpp
index 36e72f189bd0ac1eb6e1a557f3975d0ba213cbfc..b579e19266236a0afb0154213111f91d2cf4a425 100644
--- a/crosbot_explore/src/explorer.cpp
+++ b/crosbot_explore/src/explorer.cpp
@@ -332,6 +332,7 @@ Pose Explorer::findWaypointTarget(const VoronoiGrid& voronoi, const Pose& robot)
 				search.atDestination = true;
 				LOG("Explorer: Arrived at destination (%.2lf, %.2lf, %.2lf)\n",
 						destination.x, destination.y, destination.z);
+				statusChanged("Arrived");
 			}
 			search.waypoint = Point(INFINITY, INFINITY, INFINITY);
 			return Pose(INFINITY, INFINITY, INFINITY);
@@ -379,6 +380,7 @@ Pose Explorer::findWaypointTarget(const VoronoiGrid& voronoi, const Pose& robot)
 				search.waypoint.x, search.waypoint.y, search.waypoint.z);
 	}
 	search.pathBlocked = true;
+	statusChanged("Blocked");
 	return Pose(INFINITY, INFINITY, INFINITY);
 }
 
diff --git a/crosbot_explore/src/nodes/astar.cpp b/crosbot_explore/src/nodes/astar.cpp
index 4eb44b85913ce6d1d89331307984f4a157d45752..c4cf41bd29566e51e7b9e4760a5e3f7e2633dcff 100644
--- a/crosbot_explore/src/nodes/astar.cpp
+++ b/crosbot_explore/src/nodes/astar.cpp
@@ -9,6 +9,7 @@
 #include <crosbot_explore/astar.hpp>
 #include <crosbot/utils.hpp>
 #include <crosbot_map/voronoi.hpp>
+#include <crosbot/config.hpp>
 
 #include <tf/transform_listener.h>
 #include <nav_msgs/OccupancyGrid.h>
@@ -75,8 +76,10 @@ public:
 			}
     	}}
 
-    	if (voronoi == NULL)
+    	if (voronoi == NULL) {
+    		ERROR("AstarNode::GetPath(): No voronoi map available.\n");
     		return false;
+    	}
 
     	ImagePtr image;
     	if (imagePub.getNumSubscribers() > 0) {
@@ -94,7 +97,7 @@ public:
 
     	// return the plan
     	res.path.header.stamp = ros::Time::now();
-    	std::string frame_id = latestMap->header.frame_id;
+    	std::string frame_id = voronoi->frame;
     	res.path.header.frame_id = frame_id;
     	res.path.poses.resize(path.size());
     	for (size_t i = 0; i < path.size(); ++i) {
@@ -102,7 +105,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());
     	return true;
     }
 
@@ -110,8 +113,16 @@ public:
 		// read configuration/parameters
 		ros::NodeHandle paramNH("~");	// Because ROS's search order is buggered
 		paramNH.param<std::string>("base_frame", baseFrame, DEFAULT_BASEFRAME);
-
+		ConfigElementPtr cfg = new ROSConfigElement(paramNH);
+		
 		// TODO: read voronoi constraints
+		ConfigElementPtr voronoiCfg = cfg->getChild("voronoi");
+                if (voronoiCfg != NULL) {
+                        voronoiConstraints.restricted = voronoiCfg->getParamAsDouble("restrict", voronoiConstraints.restricted);
+                        voronoiConstraints.partial = voronoiCfg->getParamAsDouble("partial", voronoiConstraints.partial);
+                        voronoiConstraints.expand = voronoiCfg->getParamAsDouble("expand", voronoiConstraints.expand);
+                        voronoiConstraints.orphanThreshold = voronoiCfg->getParamAsInt("orphan", voronoiConstraints.orphanThreshold);
+                }
 
 		gridSub = nh.subscribe("map", 1, &AstarNode::callbackOccGrid, this);
 		historySub = nh.subscribe("history", 1, &AstarNode::callbackHistory, this);
diff --git a/crosbot_explore/src/nodes/explorer.cpp b/crosbot_explore/src/nodes/explorer.cpp
index b1ff7e69436a622987a3d70d528a9248fd4bdcda..2a6e72a5f10aaf6e3e16ddf49e14672f3559a2c7 100644
--- a/crosbot_explore/src/nodes/explorer.cpp
+++ b/crosbot_explore/src/nodes/explorer.cpp
@@ -15,6 +15,7 @@
 #include <crosbot_explore/FollowPath.h>
 #include <crosbot_explore/SetMode.h>
 #include <geometry_msgs/Twist.h>
+#include <std_msgs/String.h>
 
 #include <crosbot/config.hpp>
 
@@ -29,11 +30,10 @@ protected:
 	std::string baseFrame;
 
 	ros::Subscriber gridSub, historySub;
-	ros::Publisher imagePub, velPub;
+	ros::Publisher imagePub, velPub, statusPub;
 	tf::TransformListener tfListener;
 	ros::ServiceServer waypointSrv, setModeSrv;
 
-
 	ReadWriteLock rosLock;
 	nav_msgs::OccupancyGridConstPtr latestMap;
 	nav_msgs::PathConstPtr latestHistory;
@@ -65,10 +65,11 @@ public:
 
     	tf::StampedTransform transform;
     	try {
-    		tfListener.waitForTransform(voronoi->frame, req.path.header.frame_id, ros::Time::now(), ros::Duration(DEFAULT_MAXWAIT4TRANSFORM));
-			tfListener.lookupTransform(voronoi->frame, req.path.header.frame_id, ros::Time::now(), transform);
+		ros::Time now = ros::Time::now();
+    		tfListener.waitForTransform(voronoi->frame, req.path.header.frame_id, now, ros::Duration(DEFAULT_MAXWAIT4TRANSFORM));
+			tfListener.lookupTransform(voronoi->frame, req.path.header.frame_id, now, transform);
 		} catch (tf::TransformException& ex) {
-			ERROR("localmap: Error getting transform. (%s)\n", ex.what());
+			ERROR("explorer: Error getting transform. (%s)\n", ex.what());
 			return false;
 		}
 
@@ -130,6 +131,7 @@ public:
 		historySub = nh.subscribe("history", 1, &ExplorerNode::callbackHistory, this);
 		imagePub = nh.advertise<sensor_msgs::Image>("image", 1);
 		velPub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+		statusPub = nh.advertise<std_msgs::String>("status", 1);
 		waypointSrv = nh.advertiseService("/explore/follow_path", &ExplorerNode::callbackFollowPath, this);
 		setModeSrv = nh.advertiseService("/explore/set_mode", &ExplorerNode::callbackSetMode, this);
 	}
@@ -225,6 +227,12 @@ public:
 		}
 		velPub.publish(twist);
 	}
+
+	virtual void statusChanged(const std::string& status) {
+		std_msgs::String msg;
+		msg.data = status;
+		statusPub.publish(msg);
+	}
 };
 
 int main(int argc, char** argv) {
diff --git a/crosbot_fastslam/include/crosbot_fastslam/module.hpp b/crosbot_fastslam/include/crosbot_fastslam/module.hpp
index c08acf97c510a3d2fa1774123ff32a49ede30728..b120d3ac1c7fa748d719e2fc37abd2f09f92a1c2 100644
--- a/crosbot_fastslam/include/crosbot_fastslam/module.hpp
+++ b/crosbot_fastslam/include/crosbot_fastslam/module.hpp
@@ -113,8 +113,11 @@ public:
 
 		if (gridPub.getNumSubscribers() > 0) {
 			nav_msgs::OccupancyGridPtr grid = asOccupancyGrid(newMean);
-			if (grid != NULL)
+			if (grid != NULL) {
+				grid->header.frame_id = mapFrame;
+				grid->header.stamp = ros::Time::now();
 				gridPub.publish(grid);
+			}
 		}
 
 		// TODO: publish history