diff --git a/docker-entrypoint.sh b/docker-entrypoint.sh
new file mode 100755
index 0000000..6fe6cd2
--- /dev/null
+++ b/docker-entrypoint.sh
@@ -0,0 +1,9 @@
+#!/usr/bin/env bash
+
+set -e
+
+find . \! -user doogie -exec chown doogie '{}' +
+
+source "/opt/ros/$ROS_DISTRO/setup.bash"
+
+exec gosu doogie "$@"
\ No newline at end of file
diff --git a/dockerfile b/dockerfile
new file mode 100644
index 0000000..4d6c082
--- /dev/null
+++ b/dockerfile
@@ -0,0 +1,32 @@
+FROM ros:kinetic-ros-base
+
+ARG DOCKER_USER=doogie
+
+RUN useradd -s /bin/bash ${DOCKER_USER}
+
+# install gosu
+RUN set -eux; \
+    apt-get update; \
+    apt-get install -y gosu; \
+    rm -rf /var/lib/apt/lists/*; \
+    gosu nobody true
+
+# install common dev tools
+RUN export DEBIAN_FRONTEND=noninteractive; \
+    apt-get update && \
+    apt-get install -y --no-install-recommends \
+    bash-completion \
+    pkg-config \
+    git \
+    vim \
+    && rm -rf /var/lib/apt/lists/*
+
+RUN mkdir /home/${DOCKER_USER} && chown ${DOCKER_USER}:${DOCKER_USER} /home/${DOCKER_USER};
+
+WORKDIR /home/${DOCKER_USER}
+
+COPY docker-entrypoint.sh /usr/bin/docker-entrypoint.sh
+
+ENTRYPOINT ["docker-entrypoint.sh"]
+
+CMD ["bash"]
\ No newline at end of file
diff --git a/doogie_gazebo/CMakeLists.txt b/doogie_gazebo/CMakeLists.txt
index 6ec2a4b..051a717 100644
--- a/doogie_gazebo/CMakeLists.txt
+++ b/doogie_gazebo/CMakeLists.txt
@@ -4,10 +4,19 @@ project(doogie_gazebo)
 add_compile_options(-std=c++11)
 
 SET(CATKIN_BUILD_DEPS
-  roscpp
-  sensor_msgs
+  angles
   doogie_msgs
+  doogie_localization
+  gazebo_msgs
+  geometry_msgs
   message_filters
+  nav_msgs
+  pluginlib
+  roscpp
+  sensor_msgs
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
 )
 
 find_package(catkin REQUIRED COMPONENTS ${CATKIN_BUILD_DEPS})
@@ -15,6 +24,7 @@ find_package(Boost REQUIRED COMPONENTS system)
 
 catkin_package(
   INCLUDE_DIRS include
+  LIBRARIES gazebo_ground_truth
   CATKIN_DEPENDS ${CATKIN_BUILD_DEPS}
   DEPENDS Boost
 )
@@ -25,6 +35,13 @@ include_directories(
   ${Boost_INCLUDE_DIRS}
 )
 
+add_library(gazebo_ground_truth
+  src/ground_truth.cpp
+)
+target_link_libraries(gazebo_ground_truth
+  ${catkin_LIBRARIES}
+)
+
 add_executable(ir_sensor_data_acc_node
   src/ir_sensor_data_acc_node.cpp  
   src/ir_sensor_data_acc.cpp
diff --git a/doogie_gazebo/ground_truth_plugin_description.xml b/doogie_gazebo/ground_truth_plugin_description.xml
new file mode 100644
index 0000000..76e8bcd
--- /dev/null
+++ b/doogie_gazebo/ground_truth_plugin_description.xml
@@ -0,0 +1,9 @@
+<library path="lib/libgazebo_ground_truth">
+
+  <class name="doogie_gazebo/GroundTruth" type="doogie_gazebo::GroundTruth" base_class_type="doogie_localization::BaseLocalization">
+    <description>
+      A gazebo ground truth implementation using nav_msgs/Odom msg.
+    </description>
+  </class>
+  
+</library>
\ No newline at end of file
diff --git a/doogie_gazebo/include/doogie_gazebo/ground_truth.hpp b/doogie_gazebo/include/doogie_gazebo/ground_truth.hpp
new file mode 100644
index 0000000..a0f4c86
--- /dev/null
+++ b/doogie_gazebo/include/doogie_gazebo/ground_truth.hpp
@@ -0,0 +1,48 @@
+#ifndef DOOGIE_GAZEBO_GROUND_TRUTH_HPP_
+#define DOOGIE_GAZEBO_GROUND_TRUTH_HPP_
+
+#include <gazebo_msgs/ModelStates.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Odometry.h>
+
+#include <message_filters/subscriber.h>
+#include <tf2_ros/message_filter.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include "doogie_localization/base_localization.hpp"
+
+namespace doogie_gazebo {
+
+class GroundTruth : public doogie_localization::BaseLocalization {
+ public:
+  GroundTruth();
+  explicit GroundTruth(const std::string& odom_frame);
+  virtual ~GroundTruth() = default;
+  void odomCallBack(const nav_msgs::OdometryConstPtr &odometry_msg);
+  double getCurrentXPosition() override;
+  double getCurrentYPosition() override;
+  double getCurrentYawOrientation() override;
+  double getCurrentNormalizedYawOrientation() override;
+
+ private:
+  ros::NodeHandle nh_;
+  ros::NodeHandle pn_{"~"};
+
+  std::string odom_frame_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_{tf_buffer_};
+  tf2_ros::TransformBroadcaster tf_broadcaster_;  
+  
+  message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
+  tf2_ros::MessageFilter<nav_msgs::Odometry> tf2_filter_;
+
+  geometry_msgs::PoseStamped current_pose_;
+  geometry_msgs::Twist current_velocity_;
+};
+
+}  // doogie_gazebo
+
+#endif // DOOGIE_GAZEBO_GROUND_TRUTH_HPP_
\ No newline at end of file
diff --git a/doogie_gazebo/include/doogie_gazebo/ir_sensor_data_acc.hpp b/doogie_gazebo/include/doogie_gazebo/ir_sensor_data_acc.hpp
index ca5bda6..9884011 100644
--- a/doogie_gazebo/include/doogie_gazebo/ir_sensor_data_acc.hpp
+++ b/doogie_gazebo/include/doogie_gazebo/ir_sensor_data_acc.hpp
@@ -11,7 +11,7 @@
 #include <message_filters/sync_policies/approximate_time.h>
 
 #include <sensor_msgs/Range.h>
-#include "doogie_msgs/DoogiePosition.h"
+#include "doogie_msgs/DoogiePose.h"
 
 namespace doogie_gazebo {
 
@@ -65,7 +65,7 @@ class IRSensorDataAcc {
    * 
    * @param msg Not used in this application.
    */
-  void UpdateMatrixCallback(const doogie_msgs::DoogiePositionConstPtr& msg);
+  void UpdateMatrixCallback(const doogie_msgs::DoogiePoseConstPtr& msg);
 
   /** Global NodeHandle to publishers and subsribers. */
   ros::NodeHandle nh_;
diff --git a/doogie_gazebo/launch/robot_launch.launch b/doogie_gazebo/launch/robot_launch.launch
index e0cfaa4..ff3faa4 100644
--- a/doogie_gazebo/launch/robot_launch.launch
+++ b/doogie_gazebo/launch/robot_launch.launch
@@ -7,12 +7,12 @@
   <arg name="gui" default="true"/>
   <arg name="debug" default="false"/>
   <arg name="physics" default="ode"/>
-  <arg name="verbose" default="false"/>
+  <arg name="verbose" default="true"/>
   <arg name="world_name" default="worlds/empty.world"/>
 
   <!-- The arguments you can pass spawn node, for example name:=mouse_1 -->
-  <arg name="robot_name" default="doogie"/>
-  <arg name="robot_param" default="$(arg robot_name)/robot_description"/> 
+  <arg name="namespace" default="doogie"/>
+  <arg name="robot_param" default="$(arg namespace)/robot_description"/> 
 
   <arg name="maze_name" default="minus"/> 
 
@@ -43,36 +43,29 @@
     <arg name="robot_param" value="$(arg robot_param)"/>
   </include>
 
-  <group ns="$(arg robot_name)">
-    <!--Note that because of 'group ns' all doogie params will be prefixed by
+  <group ns="$(arg namespace)">
+    <!--NOTE: all names [such as nodes, topics, params...] called inside '<group ns>' will be prefixed by
     /doogie (e.g., /doogie/robot_description) -->
 
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>
+
     <rosparam file="$(find doogie_description)/config/mazes.yaml" command="load"/>
 
     <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" output="screen"
-      args="-urdf -param /$(arg robot_param) -model $(arg robot_name) 
+      args="-urdf -param /$(arg robot_param) -model $(arg namespace)
       -x $(arg x) -y $(arg y) -z $(arg z) 
       -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
 
+    <node pkg="tf2_ros" type="static_transform_publisher" name="world_to_odom" 
+      args="$(arg x) $(arg y) $(arg z) 
+            $(arg Y) $(arg P) $(arg R) 
+            world odom"/>
+    
+    <!-- wheel controller -->
     <include file="$(find doogie_control)/launch/robot_control.launch">
-      <arg name="ns" value="$(arg robot_name)"/>
-    </include>
-
-    <include file="$(find doogie_navigation)/launch/move_base.launch">
-      <arg name="ns" value="$(arg robot_name)"/>
+      <arg name="ns" value="$(arg namespace)"/>
     </include>
 
-    <include file="$(find doogie_perception)/launch/doogie_perception.launch">
-      <arg name="ns" value="$(arg robot_name)"/>
-    </include>
-  
-    <node name="ir_sensor_data_acc_node" pkg="doogie_gazebo" type="ir_sensor_data_acc_node" output="screen">
-      <rosparam file="$(find doogie_gazebo)/config/sensor_commons.yaml" command="load"/>
-    </node>
-    
-    <include file="$(find doogie_rviz)/launch/maze_map.launch">
-      <arg name="ns" value="$(arg robot_name)"/>
-    </include>
   </group>
 
 </launch>
diff --git a/doogie_gazebo/launch/sim_move_base_interface.launch b/doogie_gazebo/launch/sim_move_base_interface.launch
new file mode 100644
index 0000000..7bd140a
--- /dev/null
+++ b/doogie_gazebo/launch/sim_move_base_interface.launch
@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<launch>
+
+  <arg name="namespace" default="doogie"/>
+
+  <include file="$(find doogie_gazebo)/launch/robot_launch.launch">
+    <arg name="namespace" value="$(arg namespace)"/>
+  </include>
+
+  <group ns="$(arg namespace)">
+    <!--Note: all names [ such as nodes, topics, params...] called inside '<group ns>' will be prefixed by
+    /doogie (e.g., /doogie/robot_description) -->
+
+    <!-- doogie_move_in_maze controller -->
+    <include file="$(find doogie_navigation)/launch/move_in_maze.launch"/>
+
+    <!-- doogie_perception -->
+    <include file="$(find doogie_perception)/launch/doogie_perception.launch">
+      <arg name="ns" value="$(arg namespace)"/>
+    </include>
+
+    <!-- wall_distances publisher -->
+    <node name="ir_sensor_data_acc_node" pkg="doogie_gazebo" type="ir_sensor_data_acc_node" output="screen">
+      <rosparam file="$(find doogie_gazebo)/config/sensor_commons.yaml" command="load"/>
+    </node>
+    
+    <!-- rviz occupancy grid -->
+    <include file="$(find doogie_rviz)/launch/maze_map.launch">
+      <arg name="ns" value="$(arg namespace)"/>
+    </include>
+
+  </group>
+
+</launch>
\ No newline at end of file
diff --git a/doogie_gazebo/launch/sim_solver_application.launch b/doogie_gazebo/launch/sim_solver_application.launch
new file mode 100644
index 0000000..8f32b85
--- /dev/null
+++ b/doogie_gazebo/launch/sim_solver_application.launch
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<launch>
+
+  <arg name="namespace" default="doogie"/>
+
+  <include file="$(find doogie_gazebo)/launch/sim_move_base_interface.launch">
+    <arg name="namespace" value="$(arg namespace)"/>
+  </include>
+  
+  <group ns="$(arg namespace)">
+    <!--Note: all names [ such as nodes, topics, params...] called inside '<group ns>' will be prefixed by
+    /doogie (e.g., /doogie/robot_description) -->
+
+    <include file="$(find doogie_navigation)/launch/maze_solver.launch"/>
+
+  </group>
+
+</launch>
\ No newline at end of file
diff --git a/doogie_gazebo/package.xml b/doogie_gazebo/package.xml
index e593f59..05545cd 100644
--- a/doogie_gazebo/package.xml
+++ b/doogie_gazebo/package.xml
@@ -14,15 +14,29 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <depend>roscpp</depend>
-  <depend>sensor_msgs</depend>
+  <depend>angles</depend>
+  <depend>doogie_localization</depend>
   <depend>doogie_msgs</depend>
+  <depend>gazebo_msgs</depend>
+  <depend>geometry_msgs</depend>
   <depend>message_filters</depend>
-  
+  <depend>nav_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+
+  <exec_depend>doogie_control</exec_depend>
+  <exec_depend>doogie_description</exec_depend>
+  <exec_depend>doogie_navigation</exec_depend>
+  <exec_depend>gazebo_plugins</exec_depend>
   <exec_depend>gazebo_ros</exec_depend>
   <exec_depend>gazebo_ros_control</exec_depend>
-  <exec_depend>gazebo_plugins</exec_depend>
-  <exec_depend>doogie_description</exec_depend>
-  <exec_depend>doogie_control</exec_depend>
+
+  <export>
+    <doogie_localization plugin="${prefix}/ground_truth_plugin_description.xml"/>
+  </export>
 
 </package>
\ No newline at end of file
diff --git a/doogie_gazebo/src/ground_truth.cpp b/doogie_gazebo/src/ground_truth.cpp
new file mode 100644
index 0000000..7ad5e53
--- /dev/null
+++ b/doogie_gazebo/src/ground_truth.cpp
@@ -0,0 +1,58 @@
+#include "doogie_gazebo/ground_truth.hpp"
+
+#include <angles/angles.h>
+#include <pluginlib/class_list_macros.h>
+#include <tf2/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+namespace doogie_gazebo {
+
+GroundTruth::GroundTruth() : GroundTruth("odom") {}
+
+GroundTruth::GroundTruth(const std::string& odom_frame)
+: odom_frame_(odom_frame)
+, tf2_filter_(odom_sub_, tf_buffer_, odom_frame, 1, nullptr) {
+  std::string topic_name = "/gazebo/ground_truth";
+  pn_.getParam("input_topic", topic_name);
+  odom_sub_.subscribe(nh_, topic_name, 1);
+  tf2_filter_.registerCallback(boost::bind(&GroundTruth::odomCallBack, this, _1));
+}
+
+void GroundTruth::odomCallBack(const nav_msgs::OdometryConstPtr &odometry_msg) {
+  ROS_DEBUG_THROTTLE(0.5, "odom callback");
+  current_pose_.header = odometry_msg->header;
+  current_pose_.pose = odometry_msg->pose.pose;
+  current_velocity_ = odometry_msg->twist.twist;
+
+  tf_buffer_.transform(current_pose_, current_pose_, odom_frame_);
+  ROS_DEBUG_THROTTLE(1.0, "Doogie Position(x:%f y:%f z:%f)\n", 
+            current_pose_.pose.position.x,
+            current_pose_.pose.position.y,
+            current_pose_.pose.position.z);
+  
+  tf2::Stamped<tf2::Transform> temp;
+  tf2::fromMsg(current_pose_, temp);
+  geometry_msgs::TransformStamped gt_tf = tf2::toMsg(temp);
+  gt_tf.child_frame_id = "ground_truth";
+  tf_broadcaster_.sendTransform(gt_tf);
+}
+
+double GroundTruth::getCurrentXPosition() {
+  return current_pose_.pose.position.x;
+}
+
+double GroundTruth::getCurrentYPosition() {
+  return current_pose_.pose.position.y;
+}
+
+double GroundTruth::getCurrentYawOrientation() {
+  return tf2::getYaw(current_pose_.pose.orientation);
+}
+
+double GroundTruth::getCurrentNormalizedYawOrientation() {
+  return angles::normalize_angle(getCurrentYawOrientation());
+}
+  
+}  // doogie_gazebo
+
+PLUGINLIB_EXPORT_CLASS(doogie_gazebo::GroundTruth, doogie_localization::BaseLocalization)
\ No newline at end of file
diff --git a/doogie_gazebo/src/ir_sensor_data_acc.cpp b/doogie_gazebo/src/ir_sensor_data_acc.cpp
index f21f87f..b791610 100644
--- a/doogie_gazebo/src/ir_sensor_data_acc.cpp
+++ b/doogie_gazebo/src/ir_sensor_data_acc.cpp
@@ -37,7 +37,7 @@ void IRSensorDataAcc::init() {
 
   ir_sensors_sync_.registerCallback(boost::bind(&IRSensorDataAcc::accCallback, this, _1, _2, _3, _4));
   ir_sensors_pub_ = nh_.advertise<doogie_msgs::WallDistances>("wall_distances", 1);
-  update_matrix_sub_ = nh_.subscribe("doogie_position", 1, &IRSensorDataAcc::UpdateMatrixCallback, this);
+  update_matrix_sub_ = nh_.subscribe("doogie_pose", 1, &IRSensorDataAcc::UpdateMatrixCallback, this);
 }
 
 void IRSensorDataAcc::run() {
@@ -63,7 +63,7 @@ void IRSensorDataAcc::accCallback(const sensor_msgs::RangeConstPtr& left_ir,
   is_to_publish_wall_distances_ = false;
 }
 
-void IRSensorDataAcc::UpdateMatrixCallback(const doogie_msgs::DoogiePositionConstPtr& msg) {
+void IRSensorDataAcc::UpdateMatrixCallback(const doogie_msgs::DoogiePoseConstPtr& msg) {
   is_to_publish_wall_distances_ = true;
 }
 
diff --git a/doogie_rviz/include/doogie_rviz/maze_map.hpp b/doogie_rviz/include/doogie_rviz/maze_map.hpp
index 85d52df..bb39af2 100644
--- a/doogie_rviz/include/doogie_rviz/maze_map.hpp
+++ b/doogie_rviz/include/doogie_rviz/maze_map.hpp
@@ -9,7 +9,7 @@
 
 #include "doogie_msgs/MazeCellMultiArray.h"
 #include "doogie_msgs/MazeCell.h"
-#include "doogie_msgs/DoogiePosition.h"
+#include "doogie_msgs/DoogiePose.h"
 
 namespace doogie_rviz {
 
@@ -29,16 +29,16 @@ class MazeMap {
   void loadParameters();
   void drawWall(grid_map::Position start, grid_map::Position end);
   grid_map::Position computeStartPoint(uint8_t row, uint8_t column);
-  void doogiePositionCallback(const doogie_msgs::DoogiePositionConstPtr& doogie_position);
+  void doogiePoseCallback(const doogie_msgs::DoogiePoseConstPtr& doogie_pose);
   void mazeObstacleMatrixCallback(const doogie_msgs::MazeCellMultiArrayConstPtr& maze_obstacle_matrix);
 
   grid_map::GridMap maze_map_;
   ros::NodeHandle nh_;
   ros::Publisher maze_map_pub_;
-  ros::Subscriber doogie_position_sub_;
+  ros::Subscriber doogie_pose_sub_;
   ros::Subscriber maze_obstacle_matrix_sub_;
 
-  doogie_msgs::DoogiePosition doogie_position_;
+  doogie_msgs::DoogiePose doogie_pose_;
   doogie_msgs::MazeCell cell_walls_;
   nav_msgs::OccupancyGrid maze_map_grid_;
 
diff --git a/doogie_rviz/package.xml b/doogie_rviz/package.xml
index 0d579d3..94a0a7f 100644
--- a/doogie_rviz/package.xml
+++ b/doogie_rviz/package.xml
@@ -17,6 +17,8 @@
   <depend>nav_msgs</depend>
   <depend>doogie_msgs</depend>
   <depend>doogie_core</depend>
+  
+  <exec_depend>rviz</exec_depend>
 
   <export>
   </export>
diff --git a/doogie_rviz/src/maze_map.cpp b/doogie_rviz/src/maze_map.cpp
index 77209ef..07af473 100644
--- a/doogie_rviz/src/maze_map.cpp
+++ b/doogie_rviz/src/maze_map.cpp
@@ -14,7 +14,7 @@ MazeMap::MazeMap()
   , is_to_update_map_(false) {
   maze_map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("maze_map", 1);
   maze_obstacle_matrix_sub_ = nh_.subscribe("maze_matrix", 1, &MazeMap::mazeObstacleMatrixCallback, this);
-  doogie_position_sub_ = nh_.subscribe("doogie_position", 1, &MazeMap::doogiePositionCallback, this);
+  doogie_pose_sub_ = nh_.subscribe("doogie_pose", 1, &MazeMap::doogiePoseCallback, this);
 }
 
 void MazeMap::loadParameters() {
@@ -107,7 +107,7 @@ void MazeMap::drawWalls(uint8_t row, uint8_t column, doogie_msgs::MazeCell cell_
 
 void MazeMap::publishMap() {
   if (is_to_update_map_) {
-    this->drawWalls(doogie_position_.row, doogie_position_.column, cell_walls_);
+    this->drawWalls(doogie_pose_.position.row, doogie_pose_.position.column, cell_walls_);
     is_to_update_map_ = false;
   }
 
@@ -125,13 +125,13 @@ void MazeMap::run() {
   }
 }
 
-void MazeMap::doogiePositionCallback(const doogie_msgs::DoogiePositionConstPtr& doogie_position) {
-  this->doogie_position_ = *doogie_position;
+void MazeMap::doogiePoseCallback(const doogie_msgs::DoogiePoseConstPtr& doogie_pose) {
+  this->doogie_pose_ = *doogie_pose;
 }
 
 void MazeMap::mazeObstacleMatrixCallback(const doogie_msgs::MazeCellMultiArrayConstPtr& maze_obstacle_matrix) {
   cell_walls_ = MazeMatrixHandle::getMazeMatrixCell(*maze_obstacle_matrix,
-                                                    doogie_position_.row, doogie_position_.column);
+                                                    doogie_pose_.position.row, doogie_pose_.position.column);
   is_to_update_map_ = true;
 }