diff --git a/aeplanner/CMakeLists.txt b/aeplanner/CMakeLists.txt
index 5287adb..7edd90a 100644
--- a/aeplanner/CMakeLists.txt
+++ b/aeplanner/CMakeLists.txt
@@ -8,11 +8,15 @@ find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   visualization_msgs
   message_generation
+  rqt_gui_cpp
+  rqt_gui
   tf
   tf2
   genmsg 
   actionlib_msgs 
   actionlib
+  nodelet
+  std_msgs
 
   kdtree
   pigain
@@ -44,9 +48,8 @@ generate_messages(
 )
 
 catkin_package(
-  INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS} ${OCTOMAP_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
   LIBRARIES aeplanner ${catkin_LIBRARIES} ${OCTOMAP_LIBRARIES}
-  CATKIN_DEPENDS message_runtime roscpp geometry_msgs visualization_msgs tf kdtree
+  CATKIN_DEPENDS message_runtime roscpp geometry_msgs visualization_msgs tf kdtree actionlib_msgs std_msgs
 )
 
 include_directories(
diff --git a/aeplanner/package.xml b/aeplanner/package.xml
index 16e57ee..9ebe181 100644
--- a/aeplanner/package.xml
+++ b/aeplanner/package.xml
@@ -12,6 +12,14 @@
 
   <buildtool_depend>catkin</buildtool_depend>
   
+  <build_depend>genmsg</build_depend>
+  <build_depend>rqt_gui_cpp</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>cmake_modules</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>octomap</build_depend>
+  <build_depend>rqt_gui</build_depend>
+  <build_depend>std_msgs</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>nodelet</build_depend>
   <build_depend>tf</build_depend>
@@ -23,6 +31,8 @@
   <build_depend>pigain</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
   
+  <run_depend>actionlib_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>nodelet</run_depend>
   <run_depend>tf</run_depend>
diff --git a/pigain/package.xml b/pigain/package.xml
index 511cad1..9751b70 100644
--- a/pigain/package.xml
+++ b/pigain/package.xml
@@ -54,6 +54,7 @@
   <build_depend>geometry_msgs</build_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
   <build_depend>message_generation</build_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
diff --git a/rpl_exploration/config/exploration.yaml b/rpl_exploration/config/exploration.yaml
index 2be4bc1..a87c2bd 100644
--- a/rpl_exploration/config/exploration.yaml
+++ b/rpl_exploration/config/exploration.yaml
@@ -22,13 +22,13 @@ rrt/min_nodes: 100
 
 # bounding box: necessary to limit the simulation 
 # scenario (smaller than actual gazebo scenario)
-boundary/min: [-30.0, -30.0, 0.0]
-boundary/max: [ 30.0,  30.0, 2.0]
+boundary/min: [-10.0, -10.0, 0.0]
+boundary/max: [ 10.0,  10.0, 1.0]
 
 visualize_rays: false
 visualize_tree: true
 visualize_exploration_area: true
 
-robot_frame: "fcu"
-world_frame: "map"
+robot_frame: "base_link"
+world_frame: "odom"
 
diff --git a/rpl_exploration/src/fly_to.cpp b/rpl_exploration/src/fly_to.cpp
index ff5e1a1..016ef0f 100644
--- a/rpl_exploration/src/fly_to.cpp
+++ b/rpl_exploration/src/fly_to.cpp
@@ -27,7 +27,7 @@ namespace rpl_exploration {
                                                  << goal->pose.pose.position.y << ", "
                                                  << goal->pose.pose.position.z << ") ");
 
-        ros::Rate r(20);
+        ros::Rate r(2);
         geometry_msgs::Point p = goal->pose.pose.position;
 
         float distance_to_goal = 9001; // Distance is over 9000
@@ -52,8 +52,8 @@ namespace rpl_exploration {
           ROS_INFO_STREAM("Publishing goal to (" << p.x << ", " << p.y << ", " << p.z << ") ");
           pub_.publish(goal->pose);
 
-          listener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(10.0) );
-          listener.lookupTransform("/map", "/base_link", ros::Time(0), transform);
+          listener.waitForTransform("/odom", "/base_link", ros::Time(0), ros::Duration(10.0) );
+          listener.lookupTransform("/odom", "/base_link", ros::Time(0), transform);
 
           geometry_msgs::Point q;
           q.x = (float)transform.getOrigin().x(); 
@@ -81,7 +81,7 @@ namespace rpl_exploration {
           yaw_diff = fabs(atan2(sin(goal_yaw-current_yaw), cos(goal_yaw-current_yaw)));
 
           r.sleep();
-        } while(distance_to_goal > 0.8 or yaw_diff > 0.6*M_PI);
+        } while(ros::ok() && (distance_to_goal > 0.8 or yaw_diff > 0.6*M_PI));
 
 
         as->setSucceeded();
diff --git a/rpl_exploration/src/rpl_exploration.cpp b/rpl_exploration/src/rpl_exploration.cpp
index 8aaacec..b052e59 100644
--- a/rpl_exploration/src/rpl_exploration.cpp
+++ b/rpl_exploration/src/rpl_exploration.cpp
@@ -63,13 +63,20 @@ int main(int argc, char** argv)
                                                              "local_position/pose");
   double init_yaw = tf2::getYaw(init_pose->pose.orientation);
   // Up 2 meters and then forward one meter
-  double initial_positions[8][4] = {
-    { init_pose->pose.position.x, init_pose->pose.position.y,
-      init_pose->pose.position.z + 2.0, init_yaw },
-    { init_pose->pose.position.x + 1.0 * std::cos(init_yaw),
-      init_pose->pose.position.y + 1.0 * std::sin(init_yaw),
-      init_pose->pose.position.z + 2.0, init_yaw },
-  };
+//  double initial_positions[8][4] = {
+//    { init_pose->pose.position.x, init_pose->pose.position.y,
+//      init_pose->pose.position.z + 2.0, init_yaw },
+//    { init_pose->pose.position.x + 1.0 * std::cos(init_yaw),
+//      init_pose->pose.position.y + 1.0 * std::sin(init_yaw),
+//      init_pose->pose.position.z + 2.0, init_yaw },
+//  };
+
+  // Modification for UGV to reduce z to zero
+    double initial_positions[8][4] = {
+      { init_pose->pose.position.x + 5, init_pose->pose.position.y, 0.0, init_yaw },
+      { init_pose->pose.position.x + 5 * std::cos(init_yaw),
+        init_pose->pose.position.y + 1.0 * std::sin(init_yaw), 0.0, init_yaw },
+    };
 
   // This is the initialization motion, necessary that the known free space
   // allows the planning of initial paths.
@@ -79,17 +86,24 @@ int main(int argc, char** argv)
   for (int i = 0; i < 2; ++i)
   {
     rpl_exploration::FlyToGoal goal;
+    // Added header to support move base
+    goal.pose.header.stamp = ros::Time::now();
+    goal.pose.header.frame_id  = "odom";
     goal.pose.pose.position.x = initial_positions[i][0];
     goal.pose.pose.position.y = initial_positions[i][1];
     goal.pose.pose.position.z = initial_positions[i][2];
     goal.pose.pose.orientation =
         tf::createQuaternionMsgFromYaw(initial_positions[i][3]);
     last_pose.pose = goal.pose.pose;
-
-    ROS_INFO_STREAM("Sending initial goal...");
+    last_pose.header.stamp = ros::Time::now();
+    last_pose.header.frame_id  = "odom";
+    ROS_INFO_STREAM("Current position: (" << init_pose->pose.position.x << ", " << init_pose->pose.position.y << ", " << init_pose->pose.position.z << ") ");
+    ROS_INFO_STREAM("Sending initial goal..." << goal);
     ac.sendGoal(goal);
 
     ac.waitForResult(ros::Duration(0));
+
+	printf("Current State: %s\n", ac.getState().toString().c_str());
   }
 
   // Start planning: The planner is called and the computed path sent to the
@@ -104,14 +118,17 @@ int main(int argc, char** argv)
     aeplanner::aeplannerGoal aep_goal;
     aep_goal.header.stamp = ros::Time::now();
     aep_goal.header.seq = iteration;
-    aep_goal.header.frame_id = "map";
+    aep_goal.header.frame_id = "odom";
     aep_goal.actions_taken = actions_taken;
+    ROS_INFO_STREAM("Sending goal..." << aep_goal);
     aep_ac.sendGoal(aep_goal);
 
     while (!aep_ac.waitForResult(ros::Duration(0.05)))
     {
       pub.publish(last_pose);
+      ROS_INFO_STREAM("Not waiting for the result, sending last pose." << last_pose);
     }
+    ROS_INFO_STREAM("Current result" << aep_ac.getResult());
 
     ros::Duration fly_time;
     if (aep_ac.getResult()->is_clear)
@@ -126,6 +143,9 @@ int main(int argc, char** argv)
 
       last_pose.pose = goal_pose.pose;
       rpl_exploration::FlyToGoal goal;
+      // Added header to support move base
+      goal.pose.header.stamp = ros::Time::now();
+      goal.pose.header.frame_id  = "odom";
       goal.pose = goal_pose;
       ac.sendGoal(goal);
 
@@ -137,7 +157,7 @@ int main(int argc, char** argv)
     {
       rrtplanner::rrtGoal rrt_goal;
       rrt_goal.start.header.stamp = ros::Time::now();
-      rrt_goal.start.header.frame_id = "map";
+      rrt_goal.start.header.frame_id = "odom";
       rrt_goal.start.pose = last_pose.pose;
       if (!aep_ac.getResult()->frontiers.poses.size())
       {
@@ -167,6 +187,9 @@ int main(int argc, char** argv)
 
         last_pose.pose = goal_pose;
         rpl_exploration::FlyToGoal goal;
+        // Added header to support move base
+	goal.pose.header.stamp = ros::Time::now();
+      	goal.pose.header.frame_id  = "odom";
         goal.pose.pose = goal_pose;
         ac.sendGoal(goal);