Added new coursework, cleaned up structure

2025-11-09 13:41:13 +00:00 · 2017-11-29 10:11:54 -08:00
parent b300c76103
commit 808a0f1724
345 changed files with 126653 additions and 0 deletions
--- a/Navigation/README.md
+++ b/Navigation/README.md
@@ -0,0 +1,6 @@
+This was my final group project for ROB 456, Intelligent Robotics.
+
+This was a group project with Joel Goodman and Dylan Thrush.
+I also worked with Ben Davidson and Bryan Cmelak.
+
+These are Robot Operating System packages that will need to be put in catkin_ws/src.
--- a/Navigation/ROB456_Final_Project_Presentation.pdf
+++ b/Navigation/ROB456_Final_Project_Presentation.pdf
--- a/Navigation/ROB456_Final_Project_Report.pdf
+++ b/Navigation/ROB456_Final_Project_Report.pdf
--- a/Navigation/nav_bundle/CMakeLists.txt
+++ b/Navigation/nav_bundle/CMakeLists.txt
@@ -0,0 +1,198 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(nav_bundle)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  gmapping
+  move_base
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES nav_bundle
+#  CATKIN_DEPENDS gmapping move_base
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/nav_bundle.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/nav_bundle_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_nav_bundle.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- a/Navigation/nav_bundle/launch/nav_bundle.launch
+++ b/Navigation/nav_bundle/launch/nav_bundle.launch
@@ -0,0 +1,6 @@
+<launch>
+	<master auto="start"/>
+	<param name="/use_sim_time" value="true"/>
+	<include file="$(find rob456_project)/move_base_config/move_base.xml"/>
+	<include file="$(find rob456_project)/move_base_config/slam_gmapping.xml"/>
+</launch>
--- a/Navigation/nav_bundle/package.xml
+++ b/Navigation/nav_bundle/package.xml
@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>nav_bundle</name>
+  <version>0.0.0</version>
+  <description>The nav_bundle package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="caperren@todo.todo">caperren</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/nav_bundle</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>gmapping</build_depend>
+  <build_depend>move_base</build_depend>
+  <build_export_depend>gmapping</build_export_depend>
+  <build_export_depend>move_base</build_export_depend>
+  <exec_depend>gmapping</exec_depend>
+  <exec_depend>move_base</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
--- a/Navigation/rob456_project
+++ b/Navigation/rob456_project
--- a/Navigation/stagebot_2dnav
+++ b/Navigation/stagebot_2dnav
--- a/Avoidance/CMakeLists.txt
+++ b/Avoidance/CMakeLists.txt
@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rob456_hw2)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  stage_ros
+  geometry_msgs
+  roscpp
+  rospy
+)
+
+catkin_package()
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
--- a/Avoidance/HW2.pdf
+++ b/Avoidance/HW2.pdf
--- a/Robotics/Homework
+++ b/Robotics/Homework
--- a/Avoidance/launch/hw2.launch
+++ b/Avoidance/launch/hw2.launch
@@ -0,0 +1,7 @@
+<launch>
+  <node pkg="stage_ros" type="stageros" name="simulator" args="$(find rob456_hw2)/worlds/manyDots.world"/>
+  <node pkg="rob456_hw2" type="hw2.py" name="hw2" output="screen">
+	  <param name="goalX" value="20" />
+    <param name="goalY" value="-20" />
+  </node>
+</launch>
--- a/Avoidance/package.xml
+++ b/Avoidance/package.xml
@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<package>
+  <name>rob456_hw2</name>
+  <version>0.0.0</version>
+  <description>ROB456 HW2</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="rob456@todo.todo">rob456</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/pioneer_delivery</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>stage_ros</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>stage_ros</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
--- a/Avoidance/src/hw2.py
+++ b/Avoidance/src/hw2.py
@@ -0,0 +1,181 @@
+#!/usr/bin/env python
+
+import rospy
+import math
+import tf
+from tf.transformations import euler_from_quaternion
+import message_filters
+from pprint import pprint
+import time
+# The laser scan message
+from sensor_msgs.msg import LaserScan
+
+# The odometry message
+from nav_msgs.msg import Odometry
+
+# the velocity command message
+from geometry_msgs.msg import Twist
+
+# instantiate global variables "globalOdom"
+globalOdom = Odometry()
+
+# global pi - this may come in handy
+pi = math.pi
+
+
+def angle_between_points((x_1, y_1), (x_2, y_2)):
+    return math.atan2(y_2 - y_1, x_2 - x_1)
+
+
+# method to control the robot
+def callback(scan, odom):
+    # the odometry parameter should be global
+    global globalOdom
+    globalOdom = odom
+
+    # make a new twist message
+    command = Twist()
+
+    # Fill in the fields.  Field values are unspecified 
+    # until they are actually assigned. The Twist message 
+    # holds linear and angular velocities.
+    command.linear.x = 0.0
+    command.linear.y = 0.0
+    command.linear.z = 0.0
+    command.angular.x = 0.0
+    command.angular.y = 0.0
+    command.angular.z = 0.0
+
+    # get goal x and y locations from the launch file
+    goalX = rospy.get_param('hw2/goalX', 0.0)
+    goalY = rospy.get_param('hw2/goalY', 0.0)
+    
+    # find current (x,y) position of robot based on odometry
+    currentX = globalOdom.pose.pose.position.x
+    currentY = globalOdom.pose.pose.position.y
+
+    # find current orientation of robot based on odometry (quaternion coordinates)
+    xOr = globalOdom.pose.pose.orientation.x
+    yOr = globalOdom.pose.pose.orientation.y
+    zOr = globalOdom.pose.pose.orientation.z
+    wOr = globalOdom.pose.pose.orientation.w
+
+    # find orientation of robot (Euler coordinates)
+    (roll, pitch, yaw) = euler_from_quaternion([xOr, yOr, zOr, wOr])
+
+    # find currentAngle of robot (equivalent to yaw)
+    # now that you have yaw, the robot's pose is completely defined by (currentX, currentY, currentAngle)
+    currentAngle = yaw
+
+    # find laser scanner properties (min scan angle, max scan angle, scan angle increment)
+    maxAngle = scan.angle_max
+    minAngle = scan.angle_min
+    angleIncrement = scan.angle_increment
+
+    # find current laser angle, max scan length, distance array for all scans, and number of laser scans
+    currentLaserTheta = minAngle
+    maxScanLength = scan.range_max 
+    distanceArray = scan.ranges
+    numScans = len(distanceArray)
+
+    # ######### My Modifications ##########
+    # Get the angle from the robot to the goal position
+    desired_angle = angle_between_points((currentX, currentY), (goalX, goalY))
+
+    # Precision variables
+    angle_offset = 0.05  # Handles how much deviance from our desired angle we can be
+    goal_precision = 0.1  # Sets how close to the goal is determined to be the actual goal
+    precision = 0.02  # Sets how forcefully we try to maintain a path
+
+    min_theta = desired_angle - angle_offset  # Sets the min theta to be within from settings above
+    max_theta = desired_angle + angle_offset  # Same but for max theta
+
+    abs_min_theta = abs(currentAngle - min_theta)  # Get the absolute value of how far off we are, in regards to limits
+    abs_max_theta = abs(currentAngle - max_theta)  # Same as previous
+
+    # If we've reached the goal, within our goal precision, stop the rover from moving
+    if abs(goalY - currentY) < goal_precision and abs(goalX - currentX) < goal_precision:
+        return
+
+    # Otherwise, make the rover move forward always
+    command.linear.x = 0.35
+
+    # Check if our current angle is within our precision for direction
+    # This is what sets the rover heading towards the goal
+    if abs_min_theta > precision and abs_max_theta > precision:
+        # If we're not, turn left or right depending on which limit we're closer to
+        if abs_max_theta > abs_min_theta:
+            command.angular.z = 0.25  # Counter-clockwise
+        else:
+            command.angular.z = -0.25  # Clockwise
+
+    # Sets up obstacle avoidance
+    num_scans = 400  # Set the number of samples around the center-forward of the rover's vision
+    threshold = 1.0  # Set a threshold for how close objects are allowed to be
+    results = ["Good" for _ in range(num_scans)]  # Set up an array for the number of scans, all set to good
+    middle_point = numScans / 2  # Get the middle point for the scan information we have
+
+    # Get our limits for upper and lower scan boundaries based on the scan inputs and num_scans
+    lower_range = middle_point - (num_scans / 2)
+    upper_range = middle_point + (num_scans / 2)
+
+    position = 0  # Variable to keep track of the results array position
+
+    # Go through our scans in the range, and mark any that are out of bounds.
+    for current_scan in range(lower_range, upper_range):
+        if distanceArray[current_scan] < threshold:
+            results[position] = "Bad"
+        position += 1
+
+    # Split the results up into measurements from the left vs right side of the rover, counting how many sensor
+    # readings on each side were too close to an object
+    right_count = results[: (len(results) / 2) - 10].count("Bad")
+    left_count = results[(len(results) / 2) + 10:].count("Bad")
+
+    # Debug statements while testing
+    # print time.time()
+    # print "Left: %s Right: %s" % (left_count, right_count)
+
+    # Threshold for how many positions on one side of the rover need to be too close to an object before avoiding
+    count_thresh = 20
+
+    # How forcefully to turn the rover one direction or another when it is trying to avoid obstacles
+    increment = 0.8
+
+    # If either side of the rover has too many counts for an object being too closer
+    if left_count > count_thresh or right_count > count_thresh:
+        # Turn the rover left or right, depending on which side has more counts than the other
+        if left_count and right_count:
+            if left_count > right_count or right_count == left_count:
+                command.angular.z += -increment
+            else:
+                command.angular.z += increment
+        elif left_count:
+            command.angular.z += -increment
+        elif right_count:
+            command.angular.z += increment
+
+    # Send the commands
+    pub.publish(command)
+
+# main function call
+if __name__ == "__main__":
+    # Initialize the node
+    rospy.init_node('lab2', log_level=rospy.DEBUG)
+
+    # subscribe to laser scan message
+    sub = message_filters.Subscriber('base_scan', LaserScan)
+
+    # subscribe to odometry message    
+    sub2 = message_filters.Subscriber('odom', Odometry)
+
+    # synchronize laser scan and odometry data
+    ts = message_filters.TimeSynchronizer([sub, sub2], 10)
+    ts.registerCallback(callback)
+
+    # publish twist message
+    pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
+
+    # Turn control over to ROS
+    rospy.spin()
+
--- a/Avoidance/worlds/manyDots.pgm
+++ b/Avoidance/worlds/manyDots.pgm
--- a/Avoidance/worlds/manyDots.world
+++ b/Avoidance/worlds/manyDots.world
@@ -0,0 +1,71 @@
+define block model
+(
+  size [0.5 0.5 0.5]
+  gui_nose 0
+)
+
+define topurg ranger
+(
+	sensor( 			
+    range [ 0.0  30.0 ]
+    fov 270.25
+   samples 1081
+  )
+
+  # generic model properties
+  color "black"
+  size [ 0.05 0.05 0.1 ]
+)
+
+define erratic position
+(
+  size [0.35 0.35 0.25]
+  origin [-0.05 0 0 0]
+  gui_nose 0
+  drive "diff"
+  topurg(pose [ 0.050 0.000 0 0.000 ])
+  odom_error [0.00 0.00 0.00 0.00 0.00 0.00]
+)
+
+define floorplan model
+(
+  # sombre, sensible, artistic
+  color "gray30"
+
+  # most maps will need a bounding box
+  boundary 1
+
+  gui_nose 0
+  gui_grid 0
+
+  gui_outline 0
+  gripper_return 0
+  fiducial_return 0
+  laser_return 1
+)
+
+# set the resolution of the underlying raytrace model in meters
+resolution 0.005
+
+interval_sim 100  # simulation timestep in milliseconds
+
+
+window
+( 
+  size [ 745.000 448.000 ] 
+
+  rotate [ 0.000 0.000 ]
+  scale 5
+)
+
+# load an environment bitmap
+floorplan
+( 
+  name "manyDots"
+  bitmap "manyDots.pgm"
+  size [54.0 58.7 0.5]
+  pose [ 0 0.0 0 90.000 ]
+)
+
+# throw in a robot
+erratic( pose [ -10.000 10.000 0 90.000 ] name "era" color "blue" localizaion "gps" localization_origin [0 0 0 0])
--- a/Robotics/Homework
+++ b/Robotics/Homework
--- a/Grids/CMakeLists.txt
+++ b/Grids/CMakeLists.txt
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rob456_hw3)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  gmapping
+  nav_msgs
+  roscpp
+  rospy
+  stage_ros
+)
+
+catkin_package()
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
--- a/Robotics/Homework
+++ b/Robotics/Homework
--- a/Grids/gmapping/CHANGELOG.rst
+++ b/Grids/gmapping/CHANGELOG.rst
@@ -0,0 +1,175 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package gmapping
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.3.9 (2017-10-22)
+------------------
+* remove unused file
+* add missing nodelet dependency to find_package
+* make rostest in CMakeLists optional (`ros/rosdistro#3010 <https://github.com/ros/rosdistro/issues/3010>`_)
+* Add nodelet implementation. (`#41 <https://github.com/ros-perception/slam_gmapping/issues/41>`_)
+  * Add nodelet implementation.
+  Add additional nodelet layer to mirror the node
+  implementation. This allows the Slam GMapping
+  library to be run as a nodelet instead. This
+  would allow you to, for example, run it under
+  the same nodelet manager as the nodelet producing
+  the /scan output for greater efficiency.
+  * Remove superfluous semicolons
+  Removed superfluous semicolons and
+  mildly clarified info stream output.
+* fix comment, change type from double to int (`#40 <https://github.com/ros-perception/slam_gmapping/issues/40>`_)
+  * fix comment, change type from double to int
+  * fix comment, iterations param is not double but int
+* Contributors: David Hodo, Kevin Wells, Lukas Bulwahn, Oscar Lima, Vincent Rabaud
+
+1.3.8 (2015-07-31)
+------------------
+* fix a test that would take too long sometimes
+* better verbosity
+* add a test for upside down lasers
+* add a test for symmetry
+* make sure the laser sent to gmapping is always centered
+* do not display warning message if scan is processed at some point
+* Contributors: Vincent Rabaud
+
+1.3.7 (2015-07-04)
+------------------
+* get replay to behave like live processing
+* Contributors: Vincent Rabaud
+
+1.3.6 (2015-06-26)
+------------------
+* Don't crash on exit from replay.
+* replay: Add "on_done" command line parameter.
+  Example usage:
+  ros run gmapping slam_gmapping_replay --scan_topic=/scan --bag_filename=/tmp/in.bag --on_done "rosrun map_server map_saver -f foo" _particles:=100 _maxUrange:=10
+* replay: Handle order-of-data-in-bag-file issues better at startup.
+  Silently discard scans at startup that arrive prior to the TF frames
+  required by those scans.
+  TF can't interpolate from non-existant data, so discard scans that
+  arrive with timestamps prior to the first TF frame.  We do this
+  expediently by discarding laser scans that throw
+  ExtrapolationException's until/unless we successfully process our first
+  scan.
+  Similarly, ignore LookupException's at startup.
+* Fix indexing error for inverting scan.
+* add test dependencies
+* get replay test to pass
+* rename test
+* fixing include order
+  removing pointer for Gmapping object
+* Minor Fix thanks to vrabaud comments
+* [new feature] replay on bag file
+  The aim is to provide a way to get exactly the same map after running
+  gmapping multiple times on the same rosbag file. It wasn't possible with the
+  tool 'rosbag play', indeed one missing laser scan could provide really
+  different results.
+  Moreover, this modification allow to process rosbag offline at the maximum
+  speed with the guarantee that all lasers scans are processed. It is
+  useful in automatic tests and when finding optimal gmapping parameters with a script.
+  Example usage:
+  rosrun gmapping slam_gmapping_replay --scan_topic=/scan --bag_filename=/tmp/in.bag _particles:=100 _maxUrange:=10
+  more options:
+  rosrun gmapping slam_gmapping_replay --help
+* spliting init and constructor
+  The objective is to allow future handling of replay offline rosbag files.
+  This commit also add a variable for the seed, with the objective is to allow to
+  have repeateable run (for future offline rosbag replay)
+* Added cfloat include
+* Change arbitrary constant to FLT_EPSILON
+* Added check that scan goes from -x to x
+* Contributors: Laurent GEORGE, Patrick Doyle, Qiao Huang, Vincent Rabaud, Windel Bouwman
+
+1.3.5 (2014-08-28)
+------------------
+* Fixed typo in slam_gmapping_pr2.launch
+  Fixed a typo in the launchfile in the parameter "map_update_interval".
+* Contributors: DaMalo
+
+1.3.4 (2014-08-07)
+------------------
+* Reenabled temporal update in slam_gmapping.cpp
+* Contributors: DaMalo
+
+1.3.3 (2014-06-23)
+------------------
+* Adding the ability to set openslam_gmapping miminumScore through the ros parameter minimumScore
+* Contributors: Koen Lekkerkerker, William Woodall
+
+1.3.2 (2014-01-14)
+------------------
+* Contributors: Vincent Rabaud
+
+1.3.1 (2014-01-13)
+------------------
+* Fix usage of scoped locks so that they are not immediately destroyed.
+  fixes `#11 <https://github.com/ros-perception/slam_gmapping/issues/11>`_
+* check for CATKIN_ENABLE_TESTING
+* Contributors: Lukas Bulwahn, Stefan Kohlbrecher, William Woodall
+
+1.3.0 (2013-06-28)
+------------------
+* Renamed to gmapping, adding metapackage for slam_gmapping
+* catkinize slam_gmapping
+* Changed reference frame from base to laser to account for upside down and/or back facing laserscanners.
+  - Added a check if the scanner is facing down
+  - Added a safety check if the scanner is aligned planar
+  - Made laserscan min- and max-angles global as they are needed later for scanners with a negative angle-increment
+  - Replaced the base->laser pose for gmapping with the identity transform and included the base->laser part into the gmap_pose
+  - Removed a transform-lookup from the map->odom transformation process as it is not needed anymore
+  These changes should make gmapping more robust against laserscanners that are mounted upside down, facing backwards or are rotating counter-clockwise.
+  It will also allow gmapping to work with panning laserscanners, since the transform base->laser is no longer considered fixed.
+* Fix poorly formed paths in patches
+  These patches won't apply in Fedora because they contain "..", which is considered "unsafe"
+* Fixed test files to use the new rosbag command layout.
+* Respect tf_prefix when sending maps
+* Fixed tf expiration
+* Added tf_delay param
+* Add gcc 4.7 patch and Precise support by removing wiped during installed
+* Oneiric linker fixes, bump version to 1.2.6
+* Convert to not use bullet datatypes directly
+* Rejiggered linker lines to accommodate Oneiric's stricter linker behavior.
+* Now uses angle_increment provided in laser scan message, instead of computing it myself (not sure why I was doing that, anyway), `#4730 <https://github.com/ros-perception/slam_gmapping/issues/4730>`_
+* Applied patch to avoid assert when laser gives varying number of beams per
+  scan, `#4856 <https://github.com/ros-perception/slam_gmapping/issues/4856>`_.  Added the bag from that ticket as a test case.
+* Applied patch from `#4984 <https://github.com/ros-perception/slam_gmapping/issues/4984>`_ to fix occ grid generation with lasers that provide scans in reverse order
+* Applied patch from `#4583 <https://github.com/ros-perception/slam_gmapping/issues/4583>`_ with misc fixes to our patch against gmapping
+* Excluded test program from all build
+* Applied typo fix from Maurice Fallon
+* Added Ubuntu platform tags to manifest
+* Removed unused inverted_laser parameter
+* Added transform logic necessary to account for non-horizontal lasers. This
+  change is intended to handle upside-down lasers, but should also work for
+  non-planar lasers (as long as the vertical structure of the environment is
+  continuous), `#3052 <https://github.com/ros-perception/slam_gmapping/issues/3052>`_. I tested minimally with a hacked version of Stage, but
+  this functionality still needs to be validated on data from a real robot
+  with an upside-down laser.
+* Reindexed bag used in testing
+* Added publication of entropy
+* add entropy computation method
+* Added occ_thresh parameter
+* Turning time based updates off by default
+* Updating so that gmapping updates on a timer when not moving. Added the temporalUpdate parameter and updated docs.
+* Updated md5sums for new bags
+* Threading publishing of transforms so that they are published regularly regardless of how long map updates take.
+* Updated patch to fix gcc 4.4 warning, and made top-level Makefile call through to Makefile.gmapping on clean
+* Updating to work with the navigation stack. Now publishes header information on map messages.
+* Applied patch to update tf usage, `#3457 <https://github.com/ros-perception/slam_gmapping/issues/3457>`_
+* Remove use of deprecated rosbuild macros
+* Removed unused parameter
+* Fix the position gmapping gives to the map's info.  Was trying to center the map on the origin, when it should just have been using the world positiong of the map's origin (`#3037 <https://github.com/ros-perception/slam_gmapping/issues/3037>`_)
+* Added doc cleared to manifest
+* Switched sleep to WallDuration, to avoid getting stuck after rosplay has run out of time data to publish
+* Converted from tf::MessageNotifier to tf::MessageFilter.
+* Reverted accidental change to CMakeLists.txt
+* Added advertisement and publication of MapMetaData (docs are updated to
+  match).  Added minimal test for the resulting map.  Updated local params to use
+  NodeHandle("~").
+* Added latched topic version of map, API cleared
+* Updated manifest to explain version that we're using
+* Remove ros/node.h inclusion
+* tf publishes on new topic: \tf. See ticket `#2381 <https://github.com/ros-perception/slam_gmapping/issues/2381>`_
+* Merging in changes from reorgnization of laser pipeline.
+* removed redundant code (getOdomPose) that could result in unnecessary warnings
+* Contributors: Ben Struss, Dave Hershberger, Dereck Wonnacott, Mike Ferguson, Scott K Logan, Vincent Rabaud, William Woodall, duhadway-bosch, eitan, gerkey, jfaust, jleibs, kwc, meeussen, vrabaud, wheeler
--- a/Grids/gmapping/CMakeLists.txt
+++ b/Grids/gmapping/CMakeLists.txt
@@ -0,0 +1,90 @@
+cmake_minimum_required(VERSION 2.8)
+project(gmapping)
+
+find_package(catkin REQUIRED nav_msgs nodelet openslam_gmapping roscpp tf rosbag_storage)
+
+find_package(Boost REQUIRED signals)
+
+include_directories(${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
+
+include_directories(src)
+
+catkin_package()
+
+add_executable(slam_gmapping src/slam_gmapping.cpp src/main.cpp)
+target_link_libraries(slam_gmapping ${Boost_LIBRARIES} ${catkin_LIBRARIES})
+if(catkin_EXPORTED_TARGETS)
+  add_dependencies(slam_gmapping ${catkin_EXPORTED_TARGETS})
+endif()
+
+add_library(slam_gmapping_nodelet src/slam_gmapping.cpp src/nodelet.cpp)
+target_link_libraries(slam_gmapping_nodelet ${catkin_LIBRARIES})
+
+add_executable(slam_gmapping_replay src/slam_gmapping.cpp src/replay.cpp)
+target_link_libraries(slam_gmapping_replay ${Boost_LIBRARIES} ${catkin_LIBRARIES})
+if(catkin_EXPORTED_TARGETS)
+add_dependencies(slam_gmapping_replay ${catkin_EXPORTED_TARGETS})
+endif()
+
+install(TARGETS slam_gmapping slam_gmapping_replay
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  if(TARGET tests)
+    add_executable(gmapping-rtest EXCLUDE_FROM_ALL test/rtest.cpp)
+    target_link_libraries(gmapping-rtest ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
+    add_dependencies(tests gmapping-rtest)
+  endif()
+
+  # Need to make the tests more robust; currently the output map can differ
+  # substantially between runs.
+  catkin_download_test_data(
+    ${PROJECT_NAME}_basic_localization_stage_indexed.bag
+    http://download.ros.org/data/gmapping/basic_localization_stage_indexed.bag
+    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+    MD5 322a0014f47bcfbb0ad16a317738b0dc)
+  catkin_download_test_data(
+    ${PROJECT_NAME}_hallway_slow_2011-03-04-21-41-33.bag
+    http://download.ros.org/data/gmapping/hallway_slow_2011-03-04-21-41-33.bag
+    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+    MD5 e772b89713693adc610f4c5b96f5dc03)
+  catkin_download_test_data(
+    ${PROJECT_NAME}_basic_localization_stage_groundtruth.pgm
+    http://download.ros.org/data/gmapping/basic_localization_stage_groundtruth.pgm
+    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+    MD5 abf208f721053915145215b18c98f9b3)
+  catkin_download_test_data(
+    ${PROJECT_NAME}_test_replay_crash.bag
+    https://github.com/ros-perception/slam_gmapping_test_data/raw/master/test_replay_crash.bag
+    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+    MD5 bb0e086207eb4fccf0b13d3406f610a1)
+  catkin_download_test_data(
+    ${PROJECT_NAME}_test_turtlebot.bag
+    https://github.com/ros-perception/slam_gmapping_test_data/raw/master/test_turtlebot.bag
+    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+    MD5 402e1e5f7c00445d2a446e58e3151830)
+  catkin_download_test_data(
+    ${PROJECT_NAME}_test_upside_down.bag
+    https://github.com/ros-perception/slam_gmapping_test_data/raw/master/test_upside_down.bag
+    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
+    MD5 3b026a2144ec14f3fdf218b5c7077d54)
+  set(LOCAL_DEPENDENCIES gmapping-rtest ${PROJECT_NAME}_basic_localization_stage_indexed.bag
+     ${PROJECT_NAME}_test_replay_crash.bag
+     ${PROJECT_NAME}_test_turtlebot.bag
+     ${PROJECT_NAME}_test_upside_down.bag
+     ${PROJECT_NAME}_hallway_slow_2011-03-04-21-41-33.bag
+     ${PROJECT_NAME}_basic_localization_stage_groundtruth.pgm
+     slam_gmapping
+     slam_gmapping_replay
+  )
+  add_rostest(test/basic_localization_stage.launch DEPENDENCIES ${LOCAL_DEPENDENCIES})
+  add_rostest(test/basic_localization_stage_replay.launch DEPENDENCIES ${LOCAL_DEPENDENCIES})
+  add_rostest(test/basic_localization_stage_replay2.launch DEPENDENCIES ${LOCAL_DEPENDENCIES})
+  add_rostest(test/basic_localization_symmetry.launch DEPENDENCIES ${LOCAL_DEPENDENCIES})
+  add_rostest(test/basic_localization_upside_down.launch DEPENDENCIES ${LOCAL_DEPENDENCIES})
+  add_rostest(test/basic_localization_laser_different_beamcount.test DEPENDENCIES ${LOCAL_DEPENDENCIES})
+endif()
--- a/Grids/gmapping/launch/slam_gmapping_pr2.launch
+++ b/Grids/gmapping/launch/slam_gmapping_pr2.launch
@@ -0,0 +1,34 @@
+<launch>
+    <param name="use_sim_time" value="true"/>
+    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
+      <remap from="scan" to="base_scan"/>
+      <param name="map_update_interval" value="5.0"/>
+      <param name="maxUrange" value="16.0"/>
+      <param name="sigma" value="0.05"/>
+      <param name="kernelSize" value="1"/>
+      <param name="lstep" value="0.05"/>
+      <param name="astep" value="0.05"/>
+      <param name="iterations" value="5"/>
+      <param name="lsigma" value="0.075"/>
+      <param name="ogain" value="3.0"/>
+      <param name="lskip" value="0"/>
+      <param name="srr" value="0.1"/>
+      <param name="srt" value="0.2"/>
+      <param name="str" value="0.1"/>
+      <param name="stt" value="0.2"/>
+      <param name="linearUpdate" value="1.0"/>
+      <param name="angularUpdate" value="0.5"/>
+      <param name="temporalUpdate" value="3.0"/>
+      <param name="resampleThreshold" value="0.5"/>
+      <param name="particles" value="30"/>
+      <param name="xmin" value="-50.0"/>
+      <param name="ymin" value="-50.0"/>
+      <param name="xmax" value="50.0"/>
+      <param name="ymax" value="50.0"/>
+      <param name="delta" value="0.05"/>
+      <param name="llsamplerange" value="0.01"/>
+      <param name="llsamplestep" value="0.01"/>
+      <param name="lasamplerange" value="0.005"/>
+      <param name="lasamplestep" value="0.005"/>
+    </node>
+</launch>
--- a/Grids/gmapping/nodelet_plugins.xml
+++ b/Grids/gmapping/nodelet_plugins.xml
@@ -0,0 +1,7 @@
+<library path="lib/libslam_gmapping_nodelet">
+  <class name="SlamGMappingNodelet" type="SlamGMappingNodelet" base_class_type="nodelet::Nodelet">
+  <description>
+  Nodelet ROS wrapper for OpenSlam's Gmapping.
+  </description>
+  </class>
+</library>
--- a/Grids/gmapping/package.xml
+++ b/Grids/gmapping/package.xml
@@ -0,0 +1,33 @@
+<package>
+  <name>gmapping</name>
+  <version>1.3.9</version>
+  <description>This package contains a ROS wrapper for OpenSlam's Gmapping. 
+  The gmapping package provides laser-based SLAM (Simultaneous Localization and Mapping), 
+  as a ROS node called slam_gmapping. Using slam_gmapping, you can create a 2-D occupancy
+  grid map (like a building floorplan) from laser and pose data collected by a mobile robot.
+  </description>
+  <author>Brian Gerkey</author>
+  <maintainer email="vincent.rabaud@gmail.com">Vincent Rabaud</maintainer>
+  <license>CreativeCommons-by-nc-sa-2.0</license>
+
+  <url>http://ros.org/wiki/gmapping</url>
+
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>openslam_gmapping</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rostest</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>nodelet</build_depend>
+
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>openslam_gmapping</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>nodelet</run_depend>
+  
+  <export>
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+  </export>
+</package>
--- a/Grids/gmapping/src/main.cpp
+++ b/Grids/gmapping/src/main.cpp
@@ -0,0 +1,34 @@
+/*
+ * slam_gmapping
+ * Copyright (c) 2008, Willow Garage, Inc.
+ *
+ * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
+ * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
+ * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
+ * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
+ * 
+ * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
+ * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
+ * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
+ * CONDITIONS.
+ *
+ */
+
+/* Author: Brian Gerkey */
+
+#include <ros/ros.h>
+
+#include "slam_gmapping.h"
+
+int
+main(int argc, char** argv)
+{
+  ros::init(argc, argv, "slam_gmapping");
+
+  SlamGMapping gn;
+  gn.startLiveSlam();
+  ros::spin();
+
+  return(0);
+}
+
--- a/Grids/gmapping/src/nodelet.cpp
+++ b/Grids/gmapping/src/nodelet.cpp
@@ -0,0 +1,42 @@
+/*
+ * slam_gmapping
+ * Copyright (c) 2008, Willow Garage, Inc.
+ *
+ * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
+ * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
+ * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
+ * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
+ * 
+ * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
+ * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
+ * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
+ * CONDITIONS.
+ *
+ */
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+
+#include "slam_gmapping.h"
+
+class SlamGMappingNodelet : public nodelet::Nodelet
+{
+  public:
+    SlamGMappingNodelet()  {}
+
+    ~SlamGMappingNodelet() {}
+  
+    virtual void onInit()
+    {
+      NODELET_INFO_STREAM("Initialising Slam GMapping nodelet...");
+      sg_.reset(new SlamGMapping(getNodeHandle(), getPrivateNodeHandle()));
+      NODELET_INFO_STREAM("Starting live SLAM...");
+      sg_->startLiveSlam();
+    }
+
+  private:  
+    boost::shared_ptr<SlamGMapping> sg_;
+};
+
+PLUGINLIB_EXPORT_CLASS(SlamGMappingNodelet, nodelet::Nodelet)
--- a/Grids/gmapping/src/replay.cpp
+++ b/Grids/gmapping/src/replay.cpp
@@ -0,0 +1,88 @@
+/*
+ * Copyright 2015 Aldebaran
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#include "slam_gmapping.h"
+
+#include <iostream>
+#include <string>
+
+#include <boost/program_options.hpp>
+#include <ros/ros.h>
+
+int
+main(int argc, char** argv)
+{
+    /** Define and parse the program options 
+     */ 
+    namespace po = boost::program_options; 
+    po::options_description desc("Options"); 
+    desc.add_options() 
+    ("help", "Print help messages") 
+    ("scan_topic",  po::value<std::string>()->default_value("/scan") ,"topic that contains the laserScan in the rosbag")
+    ("bag_filename", po::value<std::string>()->required(), "ros bag filename") 
+    ("seed", po::value<unsigned long int>()->default_value(0), "seed")
+    ("max_duration_buffer", po::value<unsigned long int>()->default_value(99999), "max tf buffer duration")
+    ("on_done", po::value<std::string>(), "command to execute when done") ;
+    
+    po::variables_map vm; 
+    try 
+    { 
+        po::store(po::parse_command_line(argc, argv, desc),  
+                  vm); // can throw 
+        
+        /** --help option 
+         */ 
+        if ( vm.count("help")  ) 
+        { 
+            std::cout << "Basic Command Line Parameter App" << std::endl 
+            << desc << std::endl; 
+            return 0; 
+        } 
+        
+        po::notify(vm); // throws on error, so do after help in case 
+        // there are any problems 
+    } 
+    catch(po::error& e) 
+    { 
+        std::cerr << "ERROR: " << e.what() << std::endl << std::endl; 
+        std::cerr << desc << std::endl; 
+        return -1; 
+    } 
+    
+    std::string bag_fname = vm["bag_filename"].as<std::string>();
+    std::string scan_topic = vm["scan_topic"].as<std::string>();
+    unsigned long int seed = vm["seed"].as<unsigned long int>();
+    unsigned long int max_duration_buffer = vm["max_duration_buffer"].as<unsigned long int>();
+    
+    ros::init(argc, argv, "slam_gmapping");
+    SlamGMapping gn(seed, max_duration_buffer) ;
+    gn.startReplay(bag_fname, scan_topic);
+    ROS_INFO("replay stopped.");
+
+    if ( vm.count("on_done") )
+    {
+        // Run the "on_done" command and then exit
+        system(vm["on_done"].as<std::string>().c_str());
+    }
+    else
+    {
+        ros::spin(); // wait so user can save the map
+    }
+    return(0);
+    
+    
+}
+
--- a/Grids/gmapping/src/slam_gmapping.cpp
+++ b/Grids/gmapping/src/slam_gmapping.cpp
@@ -0,0 +1,791 @@
+/*
+ * slam_gmapping
+ * Copyright (c) 2008, Willow Garage, Inc.
+ *
+ * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
+ * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
+ * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
+ * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
+ * 
+ * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
+ * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
+ * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
+ * CONDITIONS.
+ *
+ */
+
+/* Author: Brian Gerkey */
+/* Modified by: Charles DuHadway */
+
+
+/**
+
+@mainpage slam_gmapping
+
+@htmlinclude manifest.html
+
+@b slam_gmapping is a wrapper around the GMapping SLAM library. It reads laser
+scans and odometry and computes a map. This map can be
+written to a file using e.g.
+
+  "rosrun map_server map_saver static_map:=dynamic_map"
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- @b "scan"/<a href="../../sensor_msgs/html/classstd__msgs_1_1LaserScan.html">sensor_msgs/LaserScan</a> : data from a laser range scanner 
+- @b "/tf": odometry from the robot
+
+
+Publishes to (name/type):
+- @b "/tf"/tf/tfMessage: position relative to the map
+
+
+@section services
+ - @b "~dynamic_map" : returns the map
+
+
+@section parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+Parameters used by our GMapping wrapper:
+
+- @b "~throttle_scans": @b [int] throw away every nth laser scan
+- @b "~base_frame": @b [string] the tf frame_id to use for the robot base pose
+- @b "~map_frame": @b [string] the tf frame_id where the robot pose on the map is published
+- @b "~odom_frame": @b [string] the tf frame_id from which odometry is read
+- @b "~map_update_interval": @b [double] time in seconds between two recalculations of the map
+
+
+Parameters used by GMapping itself:
+
+Laser Parameters:
+- @b "~/maxRange" @b [double] maximum range of the laser scans. Rays beyond this range get discarded completely. (default: maximum laser range minus 1 cm, as received in the the first LaserScan message)
+- @b "~/maxUrange" @b [double] maximum range of the laser scanner that is used for map building (default: same as maxRange)
+- @b "~/sigma" @b [double] standard deviation for the scan matching process (cell)
+- @b "~/kernelSize" @b [int] search window for the scan matching process
+- @b "~/lstep" @b [double] initial search step for scan matching (linear)
+- @b "~/astep" @b [double] initial search step for scan matching (angular)
+- @b "~/iterations" @b [int] number of refinement steps in the scan matching. The final "precision" for the match is lstep*2^(-iterations) or astep*2^(-iterations), respectively.
+- @b "~/lsigma" @b [double] standard deviation for the scan matching process (single laser beam)
+- @b "~/ogain" @b [double] gain for smoothing the likelihood
+- @b "~/lskip" @b [int] take only every (n+1)th laser ray for computing a match (0 = take all rays)
+- @b "~/minimumScore" @b [double] minimum score for considering the outcome of the scanmatching good. Can avoid 'jumping' pose estimates in large open spaces when using laser scanners with limited range (e.g. 5m). (0 = default. Scores go up to 600+, try 50 for example when experiencing 'jumping' estimate issues)
+
+Motion Model Parameters (all standard deviations of a gaussian noise model)
+- @b "~/srr" @b [double] linear noise component (x and y)
+- @b "~/stt" @b [double] angular noise component (theta)
+- @b "~/srt" @b [double] linear -> angular noise component
+- @b "~/str" @b [double] angular -> linear noise component
+
+Others:
+- @b "~/linearUpdate" @b [double] the robot only processes new measurements if the robot has moved at least this many meters
+- @b "~/angularUpdate" @b [double] the robot only processes new measurements if the robot has turned at least this many rads
+
+- @b "~/resampleThreshold" @b [double] threshold at which the particles get resampled. Higher means more frequent resampling.
+- @b "~/particles" @b [int] (fixed) number of particles. Each particle represents a possible trajectory that the robot has traveled
+
+Likelihood sampling (used in scan matching)
+- @b "~/llsamplerange" @b [double] linear range
+- @b "~/lasamplerange" @b [double] linear step size
+- @b "~/llsamplestep" @b [double] linear range
+- @b "~/lasamplestep" @b [double] angular step size
+
+Initial map dimensions and resolution:
+- @b "~/xmin" @b [double] minimum x position in the map [m]
+- @b "~/ymin" @b [double] minimum y position in the map [m]
+- @b "~/xmax" @b [double] maximum x position in the map [m]
+- @b "~/ymax" @b [double] maximum y position in the map [m]
+- @b "~/delta" @b [double] size of one pixel [m]
+
+*/
+
+
+
+#include "slam_gmapping.h"
+
+#include <iostream>
+
+#include <time.h>
+
+#include "ros/ros.h"
+#include "ros/console.h"
+#include "nav_msgs/MapMetaData.h"
+
+#include "gmapping/sensor/sensor_range/rangesensor.h"
+#include "gmapping/sensor/sensor_odometry/odometrysensor.h"
+
+#include <rosbag/bag.h>
+#include <rosbag/view.h>
+#include <boost/foreach.hpp>
+#define foreach BOOST_FOREACH
+
+// compute linear index for given map coords
+#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
+
+SlamGMapping::SlamGMapping():
+  map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
+  laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
+{
+  seed_ = time(NULL);
+  init();
+}
+
+SlamGMapping::SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh):
+  map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
+  laser_count_(0),node_(nh), private_nh_(pnh), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
+{
+  seed_ = time(NULL);
+  init();
+}
+
+SlamGMapping::SlamGMapping(long unsigned int seed, long unsigned int max_duration_buffer):
+  map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
+  laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL),
+  seed_(seed), tf_(ros::Duration(max_duration_buffer))
+{
+  init();
+}
+
+
+void SlamGMapping::init()
+{
+  // log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME)->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
+
+  // The library is pretty chatty
+  //gsp_ = new GMapping::GridSlamProcessor(std::cerr);
+  gsp_ = new GMapping::GridSlamProcessor();
+  ROS_ASSERT(gsp_);
+
+  tfB_ = new tf::TransformBroadcaster();
+  ROS_ASSERT(tfB_);
+
+  gsp_laser_ = NULL;
+  gsp_odom_ = NULL;
+
+  got_first_scan_ = false;
+  got_map_ = false;
+  
+
+  
+  // Parameters used by our GMapping wrapper
+  if(!private_nh_.getParam("throttle_scans", throttle_scans_))
+    throttle_scans_ = 1;
+  if(!private_nh_.getParam("base_frame", base_frame_))
+    base_frame_ = "base_link";
+  if(!private_nh_.getParam("map_frame", map_frame_))
+    map_frame_ = "map";
+  if(!private_nh_.getParam("odom_frame", odom_frame_))
+    odom_frame_ = "odom";
+
+  private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);
+
+  double tmp;
+  if(!private_nh_.getParam("map_update_interval", tmp))
+    tmp = 5.0;
+  map_update_interval_.fromSec(tmp);
+  
+  // Parameters used by GMapping itself
+  maxUrange_ = 0.0;  maxRange_ = 0.0; // preliminary default, will be set in initMapper()
+  if(!private_nh_.getParam("minimumScore", minimum_score_))
+    minimum_score_ = 0;
+  if(!private_nh_.getParam("sigma", sigma_))
+    sigma_ = 0.05;
+  if(!private_nh_.getParam("kernelSize", kernelSize_))
+    kernelSize_ = 1;
+  if(!private_nh_.getParam("lstep", lstep_))
+    lstep_ = 0.05;
+  if(!private_nh_.getParam("astep", astep_))
+    astep_ = 0.05;
+  if(!private_nh_.getParam("iterations", iterations_))
+    iterations_ = 5;
+  if(!private_nh_.getParam("lsigma", lsigma_))
+    lsigma_ = 0.075;
+  if(!private_nh_.getParam("ogain", ogain_))
+    ogain_ = 3.0;
+  if(!private_nh_.getParam("lskip", lskip_))
+    lskip_ = 0;
+  if(!private_nh_.getParam("srr", srr_))
+    srr_ = 0.1;
+  if(!private_nh_.getParam("srt", srt_))
+    srt_ = 0.2;
+  if(!private_nh_.getParam("str", str_))
+    str_ = 0.1;
+  if(!private_nh_.getParam("stt", stt_))
+    stt_ = 0.2;
+  if(!private_nh_.getParam("linearUpdate", linearUpdate_))
+    linearUpdate_ = 1.0;
+  if(!private_nh_.getParam("angularUpdate", angularUpdate_))
+    angularUpdate_ = 0.5;
+  if(!private_nh_.getParam("temporalUpdate", temporalUpdate_))
+    temporalUpdate_ = -1.0;
+  if(!private_nh_.getParam("resampleThreshold", resampleThreshold_))
+    resampleThreshold_ = 0.5;
+  if(!private_nh_.getParam("particles", particles_))
+    particles_ = 30;
+  if(!private_nh_.getParam("xmin", xmin_))
+    xmin_ = -100.0;
+  if(!private_nh_.getParam("ymin", ymin_))
+    ymin_ = -100.0;
+  if(!private_nh_.getParam("xmax", xmax_))
+    xmax_ = 100.0;
+  if(!private_nh_.getParam("ymax", ymax_))
+    ymax_ = 100.0;
+  if(!private_nh_.getParam("delta", delta_))
+    delta_ = 0.05;
+  if(!private_nh_.getParam("occ_thresh", occ_thresh_))
+    occ_thresh_ = 0.25;
+  if(!private_nh_.getParam("llsamplerange", llsamplerange_))
+    llsamplerange_ = 0.01;
+  if(!private_nh_.getParam("llsamplestep", llsamplestep_))
+    llsamplestep_ = 0.01;
+  if(!private_nh_.getParam("lasamplerange", lasamplerange_))
+    lasamplerange_ = 0.005;
+  if(!private_nh_.getParam("lasamplestep", lasamplestep_))
+    lasamplestep_ = 0.005;
+    
+  if(!private_nh_.getParam("tf_delay", tf_delay_))
+    tf_delay_ = transform_publish_period_;
+
+}
+
+
+void SlamGMapping::startLiveSlam()
+{
+  entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
+  sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
+  sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
+  ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
+  scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
+  scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
+  scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
+
+  transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
+}
+
+void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_topic)
+{
+  double transform_publish_period;
+  ros::NodeHandle private_nh_("~");
+  entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
+  sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
+  sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
+  ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
+  
+  rosbag::Bag bag;
+  bag.open(bag_fname, rosbag::bagmode::Read);
+  
+  std::vector<std::string> topics;
+  topics.push_back(std::string("/tf"));
+  topics.push_back(scan_topic);
+  rosbag::View viewall(bag, rosbag::TopicQuery(topics));
+
+  // Store up to 5 messages and there error message (if they cannot be processed right away)
+  std::queue<std::pair<sensor_msgs::LaserScan::ConstPtr, std::string> > s_queue;
+  foreach(rosbag::MessageInstance const m, viewall)
+  {
+    tf::tfMessage::ConstPtr cur_tf = m.instantiate<tf::tfMessage>();
+    if (cur_tf != NULL) {
+      for (size_t i = 0; i < cur_tf->transforms.size(); ++i)
+      {
+        geometry_msgs::TransformStamped transformStamped;
+        tf::StampedTransform stampedTf;
+        transformStamped = cur_tf->transforms[i];
+        tf::transformStampedMsgToTF(transformStamped, stampedTf);
+        tf_.setTransform(stampedTf);
+      }
+    }
+
+    sensor_msgs::LaserScan::ConstPtr s = m.instantiate<sensor_msgs::LaserScan>();
+    if (s != NULL) {
+      if (!(ros::Time(s->header.stamp)).is_zero())
+      {
+        s_queue.push(std::make_pair(s, ""));
+      }
+      // Just like in live processing, only process the latest 5 scans
+      if (s_queue.size() > 5) {
+        ROS_WARN_STREAM("Dropping old scan: " << s_queue.front().second);
+        s_queue.pop();
+      }
+      // ignoring un-timestamped tf data 
+    }
+
+    // Only process a scan if it has tf data
+    while (!s_queue.empty())
+    {
+      try
+      {
+        tf::StampedTransform t;
+        tf_.lookupTransform(s_queue.front().first->header.frame_id, odom_frame_, s_queue.front().first->header.stamp, t);
+        this->laserCallback(s_queue.front().first);
+        s_queue.pop();
+      }
+      // If tf does not have the data yet
+      catch(tf2::TransformException& e)
+      {
+        // Store the error to display it if we cannot process the data after some time
+        s_queue.front().second = std::string(e.what());
+        break;
+      }
+    }
+  }
+
+  bag.close();
+}
+
+void SlamGMapping::publishLoop(double transform_publish_period){
+  if(transform_publish_period == 0)
+    return;
+
+  ros::Rate r(1.0 / transform_publish_period);
+  while(ros::ok()){
+    publishTransform();
+    r.sleep();
+  }
+}
+
+SlamGMapping::~SlamGMapping()
+{
+  if(transform_thread_){
+    transform_thread_->join();
+    delete transform_thread_;
+  }
+
+  delete gsp_;
+  if(gsp_laser_)
+    delete gsp_laser_;
+  if(gsp_odom_)
+    delete gsp_odom_;
+  if (scan_filter_)
+    delete scan_filter_;
+  if (scan_filter_sub_)
+    delete scan_filter_sub_;
+}
+
+bool
+SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
+{
+  // Get the pose of the centered laser at the right time
+  centered_laser_pose_.stamp_ = t;
+  // Get the laser's pose that is centered
+  tf::Stamped<tf::Transform> odom_pose;
+  try
+  {
+    tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
+  }
+  catch(tf::TransformException e)
+  {
+    ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
+    return false;
+  }
+  double yaw = tf::getYaw(odom_pose.getRotation());
+
+  gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
+                                      odom_pose.getOrigin().y(),
+                                      yaw);
+  return true;
+}
+
+bool
+SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
+{
+  laser_frame_ = scan.header.frame_id;
+  // Get the laser's pose, relative to base.
+  tf::Stamped<tf::Pose> ident;
+  tf::Stamped<tf::Transform> laser_pose;
+  ident.setIdentity();
+  ident.frame_id_ = laser_frame_;
+  ident.stamp_ = scan.header.stamp;
+  try
+  {
+    tf_.transformPose(base_frame_, ident, laser_pose);
+  }
+  catch(tf::TransformException e)
+  {
+    ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
+             e.what());
+    return false;
+  }
+
+  // create a point 1m above the laser position and transform it into the laser-frame
+  tf::Vector3 v;
+  v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
+  tf::Stamped<tf::Vector3> up(v, scan.header.stamp,
+                                      base_frame_);
+  try
+  {
+    tf_.transformPoint(laser_frame_, up, up);
+    ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
+  }
+  catch(tf::TransformException& e)
+  {
+    ROS_WARN("Unable to determine orientation of laser: %s",
+             e.what());
+    return false;
+  }
+  
+  // gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
+  if (fabs(fabs(up.z()) - 1) > 0.001)
+  {
+    ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
+                 up.z());
+    return false;
+  }
+
+  gsp_laser_beam_count_ = scan.ranges.size();
+
+  double angle_center = (scan.angle_min + scan.angle_max)/2;
+
+  if (up.z() > 0)
+  {
+    do_reverse_range_ = scan.angle_min > scan.angle_max;
+    centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center),
+                                                               tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
+    ROS_INFO("Laser is mounted upwards.");
+  }
+  else
+  {
+    do_reverse_range_ = scan.angle_min < scan.angle_max;
+    centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center),
+                                                               tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
+    ROS_INFO("Laser is mounted upside down.");
+  }
+
+  // Compute the angles of the laser from -x to x, basically symmetric and in increasing order
+  laser_angles_.resize(scan.ranges.size());
+  // Make sure angles are started so that they are centered
+  double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;
+  for(unsigned int i=0; i<scan.ranges.size(); ++i)
+  {
+    laser_angles_[i]=theta;
+    theta += std::fabs(scan.angle_increment);
+  }
+
+  ROS_DEBUG("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max,
+            scan.angle_increment);
+  ROS_DEBUG("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(),
+            laser_angles_.back(), std::fabs(scan.angle_increment));
+
+  GMapping::OrientedPoint gmap_pose(0, 0, 0);
+
+  // setting maxRange and maxUrange here so we can set a reasonable default
+  ros::NodeHandle private_nh_("~");
+  if(!private_nh_.getParam("maxRange", maxRange_))
+    maxRange_ = scan.range_max - 0.01;
+  if(!private_nh_.getParam("maxUrange", maxUrange_))
+    maxUrange_ = maxRange_;
+
+  // The laser must be called "FLASER".
+  // We pass in the absolute value of the computed angle increment, on the
+  // assumption that GMapping requires a positive angle increment.  If the
+  // actual increment is negative, we'll swap the order of ranges before
+  // feeding each scan to GMapping.
+  gsp_laser_ = new GMapping::RangeSensor("FLASER",
+                                         gsp_laser_beam_count_,
+                                         fabs(scan.angle_increment),
+                                         gmap_pose,
+                                         0.0,
+                                         maxRange_);
+  ROS_ASSERT(gsp_laser_);
+
+  GMapping::SensorMap smap;
+  smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
+  gsp_->setSensorMap(smap);
+
+  gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
+  ROS_ASSERT(gsp_odom_);
+
+
+  /// @todo Expose setting an initial pose
+  GMapping::OrientedPoint initialPose;
+  if(!getOdomPose(initialPose, scan.header.stamp))
+  {
+    ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
+    initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
+  }
+
+  gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
+                              kernelSize_, lstep_, astep_, iterations_,
+                              lsigma_, ogain_, lskip_);
+
+  gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
+  gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
+  gsp_->setUpdatePeriod(temporalUpdate_);
+  gsp_->setgenerateMap(false);
+  gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
+                                delta_, initialPose);
+  gsp_->setllsamplerange(llsamplerange_);
+  gsp_->setllsamplestep(llsamplestep_);
+  /// @todo Check these calls; in the gmapping gui, they use
+  /// llsamplestep and llsamplerange intead of lasamplestep and
+  /// lasamplerange.  It was probably a typo, but who knows.
+  gsp_->setlasamplerange(lasamplerange_);
+  gsp_->setlasamplestep(lasamplestep_);
+  gsp_->setminimumScore(minimum_score_);
+
+  // Call the sampling function once to set the seed.
+  GMapping::sampleGaussian(1,seed_);
+
+  ROS_INFO("Initialization complete");
+
+  return true;
+}
+
+bool
+SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
+{
+  if(!getOdomPose(gmap_pose, scan.header.stamp))
+     return false;
+  
+  if(scan.ranges.size() != gsp_laser_beam_count_)
+    return false;
+
+  // GMapping wants an array of doubles...
+  double* ranges_double = new double[scan.ranges.size()];
+  // If the angle increment is negative, we have to invert the order of the readings.
+  if (do_reverse_range_)
+  {
+    ROS_DEBUG("Inverting scan");
+    int num_ranges = scan.ranges.size();
+    for(int i=0; i < num_ranges; i++)
+    {
+      // Must filter out short readings, because the mapper won't
+      if(scan.ranges[num_ranges - i - 1] < scan.range_min)
+        ranges_double[i] = (double)scan.range_max;
+      else
+        ranges_double[i] = (double)scan.ranges[num_ranges - i - 1];
+    }
+  } else 
+  {
+    for(unsigned int i=0; i < scan.ranges.size(); i++)
+    {
+      // Must filter out short readings, because the mapper won't
+      if(scan.ranges[i] < scan.range_min)
+        ranges_double[i] = (double)scan.range_max;
+      else
+        ranges_double[i] = (double)scan.ranges[i];
+    }
+  }
+
+  GMapping::RangeReading reading(scan.ranges.size(),
+                                 ranges_double,
+                                 gsp_laser_,
+                                 scan.header.stamp.toSec());
+
+  // ...but it deep copies them in RangeReading constructor, so we don't
+  // need to keep our array around.
+  delete[] ranges_double;
+
+  reading.setPose(gmap_pose);
+
+  /*
+  ROS_DEBUG("scanpose (%.3f): %.3f %.3f %.3f\n",
+            scan.header.stamp.toSec(),
+            gmap_pose.x,
+            gmap_pose.y,
+            gmap_pose.theta);
+            */
+  ROS_DEBUG("processing scan");
+
+  return gsp_->processScan(reading);
+}
+
+void
+SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
+{
+  laser_count_++;
+  if ((laser_count_ % throttle_scans_) != 0)
+    return;
+
+  static ros::Time last_map_update(0,0);
+
+  // We can't initialize the mapper until we've got the first scan
+  if(!got_first_scan_)
+  {
+    if(!initMapper(*scan))
+      return;
+    got_first_scan_ = true;
+  }
+
+  GMapping::OrientedPoint odom_pose;
+
+  if(addScan(*scan, odom_pose))
+  {
+    ROS_DEBUG("scan processed");
+
+    GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
+    ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
+    ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
+    ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
+
+    tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
+    tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
+
+    map_to_odom_mutex_.lock();
+    map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
+    map_to_odom_mutex_.unlock();
+
+    if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
+    {
+      updateMap(*scan);
+      last_map_update = scan->header.stamp;
+      ROS_DEBUG("Updated the map");
+    }
+  } else
+    ROS_DEBUG("cannot process scan");
+}
+
+double
+SlamGMapping::computePoseEntropy()
+{
+  double weight_total=0.0;
+  for(std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
+      it != gsp_->getParticles().end();
+      ++it)
+  {
+    weight_total += it->weight;
+  }
+  double entropy = 0.0;
+  for(std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
+      it != gsp_->getParticles().end();
+      ++it)
+  {
+    if(it->weight/weight_total > 0.0)
+      entropy += it->weight/weight_total * log(it->weight/weight_total);
+  }
+  return -entropy;
+}
+
+void
+SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
+{
+  ROS_DEBUG("Update map");
+  boost::mutex::scoped_lock map_lock (map_mutex_);
+  GMapping::ScanMatcher matcher;
+
+  matcher.setLaserParameters(scan.ranges.size(), &(laser_angles_[0]),
+                             gsp_laser_->getPose());
+
+  matcher.setlaserMaxRange(maxRange_);
+  matcher.setusableRange(maxUrange_);
+  matcher.setgenerateMap(true);
+
+  GMapping::GridSlamProcessor::Particle best =
+          gsp_->getParticles()[gsp_->getBestParticleIndex()];
+  std_msgs::Float64 entropy;
+  entropy.data = computePoseEntropy();
+  if(entropy.data > 0.0)
+    entropy_publisher_.publish(entropy);
+
+  if(!got_map_) {
+    map_.map.info.resolution = delta_;
+    map_.map.info.origin.position.x = 0.0;
+    map_.map.info.origin.position.y = 0.0;
+    map_.map.info.origin.position.z = 0.0;
+    map_.map.info.origin.orientation.x = 0.0;
+    map_.map.info.origin.orientation.y = 0.0;
+    map_.map.info.origin.orientation.z = 0.0;
+    map_.map.info.origin.orientation.w = 1.0;
+  } 
+
+  GMapping::Point center;
+  center.x=(xmin_ + xmax_) / 2.0;
+  center.y=(ymin_ + ymax_) / 2.0;
+
+  GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_, 
+                                delta_);
+
+  ROS_DEBUG("Trajectory tree:");
+  for(GMapping::GridSlamProcessor::TNode* n = best.node;
+      n;
+      n = n->parent)
+  {
+    ROS_DEBUG("  %.3f %.3f %.3f",
+              n->pose.x,
+              n->pose.y,
+              n->pose.theta);
+    if(!n->reading)
+    {
+      ROS_DEBUG("Reading is NULL");
+      continue;
+    }
+    matcher.invalidateActiveArea();
+    matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
+    matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
+  }
+
+  // the map may have expanded, so resize ros message as well
+  if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY()) {
+
+    // NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor
+    //       so we must obtain the bounding box in a different way
+    GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0));
+    GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY()));
+    xmin_ = wmin.x; ymin_ = wmin.y;
+    xmax_ = wmax.x; ymax_ = wmax.y;
+    
+    ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(),
+              xmin_, ymin_, xmax_, ymax_);
+
+    map_.map.info.width = smap.getMapSizeX();
+    map_.map.info.height = smap.getMapSizeY();
+    map_.map.info.origin.position.x = xmin_;
+    map_.map.info.origin.position.y = ymin_;
+    map_.map.data.resize(map_.map.info.width * map_.map.info.height);
+
+    ROS_DEBUG("map origin: (%f, %f)", map_.map.info.origin.position.x, map_.map.info.origin.position.y);
+  }
+
+  for(int x=0; x < smap.getMapSizeX(); x++)
+  {
+    for(int y=0; y < smap.getMapSizeY(); y++)
+    {
+      /// @todo Sort out the unknown vs. free vs. obstacle thresholding
+      GMapping::IntPoint p(x, y);
+      double occ=smap.cell(p);
+      assert(occ <= 1.0);
+      if(occ < 0)
+        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
+      else if(occ > occ_thresh_)
+      {
+        //map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
+        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
+      }
+      else
+        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
+    }
+  }
+  got_map_ = true;
+
+  //make sure to set the header information on the map
+  map_.map.header.stamp = ros::Time::now();
+  map_.map.header.frame_id = tf_.resolve( map_frame_ );
+
+  sst_.publish(map_.map);
+  sstm_.publish(map_.map.info);
+}
+
+bool 
+SlamGMapping::mapCallback(nav_msgs::GetMap::Request  &req,
+                          nav_msgs::GetMap::Response &res)
+{
+  boost::mutex::scoped_lock map_lock (map_mutex_);
+  if(got_map_ && map_.map.info.width && map_.map.info.height)
+  {
+    res = map_;
+    return true;
+  }
+  else
+    return false;
+}
+
+void SlamGMapping::publishTransform()
+{
+  map_to_odom_mutex_.lock();
+  ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_);
+  tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
+  map_to_odom_mutex_.unlock();
+}
--- a/Grids/gmapping/src/slam_gmapping.h
+++ b/Grids/gmapping/src/slam_gmapping.h
@@ -0,0 +1,140 @@
+/*
+ * slam_gmapping
+ * Copyright (c) 2008, Willow Garage, Inc.
+ *
+ * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
+ * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
+ * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
+ * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
+ * 
+ * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
+ * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
+ * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
+ * CONDITIONS.
+ *
+ */
+
+/* Author: Brian Gerkey */
+
+#include "ros/ros.h"
+#include "sensor_msgs/LaserScan.h"
+#include "std_msgs/Float64.h"
+#include "nav_msgs/GetMap.h"
+#include "tf/transform_listener.h"
+#include "tf/transform_broadcaster.h"
+#include "message_filters/subscriber.h"
+#include "tf/message_filter.h"
+
+#include "gmapping/gridfastslam/gridslamprocessor.h"
+#include "gmapping/sensor/sensor_base/sensor.h"
+
+#include <boost/thread.hpp>
+
+class SlamGMapping
+{
+  public:
+    SlamGMapping();
+    SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh);
+    SlamGMapping(unsigned long int seed, unsigned long int max_duration_buffer);
+    ~SlamGMapping();
+
+    void init();
+    void startLiveSlam();
+    void startReplay(const std::string & bag_fname, std::string scan_topic);
+    void publishTransform();
+  
+    void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
+    bool mapCallback(nav_msgs::GetMap::Request  &req,
+                     nav_msgs::GetMap::Response &res);
+    void publishLoop(double transform_publish_period);
+
+  private:
+    ros::NodeHandle node_;
+    ros::Publisher entropy_publisher_;
+    ros::Publisher sst_;
+    ros::Publisher sstm_;
+    ros::ServiceServer ss_;
+    tf::TransformListener tf_;
+    message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
+    tf::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
+    tf::TransformBroadcaster* tfB_;
+
+    GMapping::GridSlamProcessor* gsp_;
+    GMapping::RangeSensor* gsp_laser_;
+    // The angles in the laser, going from -x to x (adjustment is made to get the laser between
+    // symmetrical bounds as that's what gmapping expects)
+    std::vector<double> laser_angles_;
+    // The pose, in the original laser frame, of the corresponding centered laser with z facing up
+    tf::Stamped<tf::Pose> centered_laser_pose_;
+    // Depending on the order of the elements in the scan and the orientation of the scan frame,
+    // We might need to change the order of the scan
+    bool do_reverse_range_;
+    unsigned int gsp_laser_beam_count_;
+    GMapping::OdometrySensor* gsp_odom_;
+
+    bool got_first_scan_;
+
+    bool got_map_;
+    nav_msgs::GetMap::Response map_;
+
+    ros::Duration map_update_interval_;
+    tf::Transform map_to_odom_;
+    boost::mutex map_to_odom_mutex_;
+    boost::mutex map_mutex_;
+
+    int laser_count_;
+    int throttle_scans_;
+
+    boost::thread* transform_thread_;
+
+    std::string base_frame_;
+    std::string laser_frame_;
+    std::string map_frame_;
+    std::string odom_frame_;
+
+    void updateMap(const sensor_msgs::LaserScan& scan);
+    bool getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t);
+    bool initMapper(const sensor_msgs::LaserScan& scan);
+    bool addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose);
+    double computePoseEntropy();
+    
+    // Parameters used by GMapping
+    double maxRange_;
+    double maxUrange_;
+    double maxrange_;
+    double minimum_score_;
+    double sigma_;
+    int kernelSize_;
+    double lstep_;
+    double astep_;
+    int iterations_;
+    double lsigma_;
+    double ogain_;
+    int lskip_;
+    double srr_;
+    double srt_;
+    double str_;
+    double stt_;
+    double linearUpdate_;
+    double angularUpdate_;
+    double temporalUpdate_;
+    double resampleThreshold_;
+    int particles_;
+    double xmin_;
+    double ymin_;
+    double xmax_;
+    double ymax_;
+    double delta_;
+    double occ_thresh_;
+    double llsamplerange_;
+    double llsamplestep_;
+    double lasamplerange_;
+    double lasamplestep_;
+    
+    ros::NodeHandle private_nh_;
+    
+    unsigned long int seed_;
+    
+    double transform_publish_period_;
+    double tf_delay_;
+};
--- a/Grids/gmapping/test/basic_localization_laser_different_beamcount.test
+++ b/Grids/gmapping/test/basic_localization_laser_different_beamcount.test
@@ -0,0 +1,11 @@
+<launch>
+  <param name="/use_sim_time" value="true"/>
+  <node pkg="rosbag" type="play" name="rosplay"
+        args="-d 5 -r 1 --clock --hz 10 $(find gmapping)/test/hallway_slow_2011-03-04-21-41-33.bag"/>
+  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
+    <remap from="scan" to="base_laser1_scan"/>
+  </node>
+  <!--test time-limit="180" test-name="basic_localization_stage" pkg="gmapping" 
+        type="test_map.py" args="90.0"/-->
+  <test time-limit="180" test-name="map_data_different_beamcount_test" pkg="gmapping" type="gmapping-rtest" args="90.0 0.05 4000 4000 0.002 0.005"/>
+</launch>
--- a/Grids/gmapping/test/basic_localization_stage.launch
+++ b/Grids/gmapping/test/basic_localization_stage.launch
@@ -0,0 +1,11 @@
+<launch>
+  <param name="/use_sim_time" value="true"/>
+  <node pkg="rosbag" type="play" name="rosplay"
+        args="-d 5 -r 1 --clock --hz 10 $(find gmapping)/test/basic_localization_stage_indexed.bag"/>
+  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
+    <remap from="scan" to="base_scan"/>
+  </node>
+  <!--test time-limit="180" test-name="basic_localization_stage" pkg="gmapping" 
+        type="test_map.py" args="90.0"/-->
+  <test time-limit="180" test-name="map_data_test" pkg="gmapping" type="gmapping-rtest" args="90.0 0.05 4000 4000 0.005 0.010"/>
+</launch>
--- a/Grids/gmapping/test/basic_localization_stage_replay.launch
+++ b/Grids/gmapping/test/basic_localization_stage_replay.launch
@@ -0,0 +1,8 @@
+<launch>
+  <node pkg="gmapping" type="slam_gmapping_replay" name="slam_gmapping_replay" output="screen"
+        args="--bag_filename $(find gmapping)/test/basic_localization_stage_indexed.bag --scan_topic /base_scan"/>
+  <!--test time-limit="30" test-name="basic_localization_stage" pkg="gmapping" 
+        type="test_map.py" args="90.0"/-->
+  <!-- TODO: Fix the rtest to work with replay, for now it's not working -->
+  <test time-limit="250" test-name="map_data_test_replay" pkg="gmapping" type="gmapping-rtest" args="200.0 0.05 4000 4000 0.005 0.010"/>
+</launch>
--- a/Grids/gmapping/test/basic_localization_stage_replay2.launch
+++ b/Grids/gmapping/test/basic_localization_stage_replay2.launch
@@ -0,0 +1,5 @@
+<launch>
+  <node pkg="gmapping" type="slam_gmapping_replay" name="slam_gmapping_replay" output="screen"
+        args="--bag_filename $(find gmapping)/test/test_replay_crash.bag --scan_topic /scan"/>
+  <test time-limit="20" test-name="map_data_test_replay" pkg="gmapping" type="gmapping-rtest" args="10.0 0.05 4000 4000 0.0001 0.010"/>
+</launch>
--- a/Grids/gmapping/test/basic_localization_symmetry.launch
+++ b/Grids/gmapping/test/basic_localization_symmetry.launch
@@ -0,0 +1,5 @@
+<launch>
+  <node pkg="gmapping" type="slam_gmapping_replay" name="slam_gmapping_replay" output="screen"
+        args="--bag_filename $(find gmapping)/test/test_turtlebot.bag --scan_topic /scan"/>
+  <test time-limit="120" test-name="test_symmetry" pkg="gmapping" type="gmapping-rtest" args="90.0 0.05 4000 4000 0.0005 0.010"/>
+</launch>
--- a/Grids/gmapping/test/basic_localization_upside_down.launch
+++ b/Grids/gmapping/test/basic_localization_upside_down.launch
@@ -0,0 +1,5 @@
+<launch>
+  <node pkg="gmapping" type="slam_gmapping_replay" name="slam_gmapping_replay" output="screen"
+        args="--bag_filename $(find gmapping)/test/test_upside_down.bag --scan_topic /laserscan/fix/raw _base_frame:=base_floor"/>
+  <test time-limit="190" test-name="test_symmetry" pkg="gmapping" type="gmapping-rtest" args="180.0 0.05 4000 4000 0.002 0.010"/>
+</launch>
--- a/Grids/gmapping/test/rtest.cpp
+++ b/Grids/gmapping/test/rtest.cpp
@@ -0,0 +1,180 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* Author: Brian Gerkey */
+
+#include <gtest/gtest.h>
+#include <ros/service.h>
+#include <ros/ros.h>
+#include <nav_msgs/GetMap.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/MapMetaData.h>
+
+
+ros::NodeHandle* g_n=NULL;
+double g_res, g_width, g_height, g_min_free_ratio, g_max_free_ratio;
+
+class MapClientTest : public testing::Test
+{
+  public:
+    MapClientTest()
+    {
+      got_map_ = false;
+      got_map_metadata_ = false;
+    }
+
+    ~MapClientTest()
+    {
+    }
+
+    bool got_map_;
+    boost::shared_ptr<nav_msgs::OccupancyGrid const> map_;
+    void mapCallback(const boost::shared_ptr<nav_msgs::OccupancyGrid const>& map)
+    {
+      map_ = map;
+      got_map_ = true;
+    }
+
+    bool got_map_metadata_;
+    boost::shared_ptr<nav_msgs::MapMetaData const> map_metadata_;
+    void mapMetaDataCallback(const boost::shared_ptr<nav_msgs::MapMetaData const>& map_metadata)
+    {
+      map_metadata_ = map_metadata;
+      got_map_metadata_ = true;
+    }
+
+    void checkMapMetaData(const nav_msgs::MapMetaData& map_metadata)
+    {
+      EXPECT_FLOAT_EQ(map_metadata.resolution, g_res);
+      EXPECT_FLOAT_EQ(map_metadata.width, g_width);
+      EXPECT_FLOAT_EQ(map_metadata.height, g_height);
+    }
+
+    void checkMapData(const nav_msgs::OccupancyGrid& map)
+    {
+      unsigned int i;
+      unsigned int free_cnt = 0;
+      for(i=0; i < map.info.width * map.info.height; i++)
+      {
+        if(map.data[i] == 0)
+          free_cnt++;
+      }
+      double free_ratio = free_cnt / (double)(i);
+      ROS_INFO("Min / ratio / Max free ratio: %f / %f / %f", g_min_free_ratio, free_ratio, g_max_free_ratio);
+      EXPECT_GE(free_ratio, g_min_free_ratio);
+      EXPECT_LE(free_ratio, g_max_free_ratio);
+    }
+};
+
+/* Try to retrieve the map via service, and compare to ground truth */
+TEST_F(MapClientTest, call_service)
+{
+  nav_msgs::GetMap::Request  req;
+  nav_msgs::GetMap::Response resp;
+  ASSERT_TRUE(ros::service::waitForService("dynamic_map", 5000));
+  ASSERT_TRUE(ros::service::call("dynamic_map", req, resp));
+  checkMapMetaData(resp.map.info);
+  checkMapData(resp.map);
+}
+
+/* Try to retrieve the map via topic, and compare to ground truth */
+TEST_F(MapClientTest, subscribe_topic)
+{
+  ros::Subscriber sub = g_n->subscribe<nav_msgs::OccupancyGrid>("map", 1, boost::bind(&MapClientTest::mapCallback, this, _1));
+
+  // Try a few times, because the server may not be up yet.
+  int i=20;
+  while(!got_map_ && i > 0)
+  {
+    ros::spinOnce();
+    ros::WallDuration d(0.25);
+    d.sleep();
+    i--;
+  }
+  ASSERT_TRUE(got_map_);
+  checkMapMetaData(map_->info);
+  checkMapData(*(map_.get()));
+}
+
+/* Try to retrieve the metadata via topic, and compare to ground truth */
+TEST_F(MapClientTest, subscribe_topic_metadata)
+{
+  ros::Subscriber sub = g_n->subscribe<nav_msgs::MapMetaData>("map_metadata", 1, boost::bind(&MapClientTest::mapMetaDataCallback, this, _1));
+
+  // Try a few times, because the server may not be up yet.
+  int i=20;
+  while(!got_map_metadata_ && i > 0)
+  {
+    ros::spinOnce();
+    ros::WallDuration d(0.25);
+    d.sleep();
+    i--;
+  }
+  ASSERT_TRUE(got_map_metadata_);
+  checkMapMetaData(*(map_metadata_.get()));
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+
+  ros::init(argc, argv, "map_client_test");
+  g_n = new ros::NodeHandle();
+
+  // Required args are, in order:
+  //   <delay> <resolution> <width> <height> <min_free_ratio> <max_free_ratio>
+  ROS_ASSERT(argc == 7);
+  ros::Duration delay = ros::Duration(atof(argv[1]));
+  g_res = atof(argv[2]);
+  g_width = atof(argv[3]);
+  g_height = atof(argv[4]);
+  g_min_free_ratio = atof(argv[5]);
+  g_max_free_ratio = atof(argv[6]);
+
+  while(ros::Time::now().toSec() == 0.0)
+  {
+    ROS_INFO("Waiting for initial time publication");
+    ros::Duration(0.25).sleep();
+    ros::spinOnce();
+  }
+  ros::Time start_time = ros::Time::now();
+  while((ros::Time::now() - start_time) < delay)
+  {
+    ROS_INFO("Waiting for delay expiration (%.3f - %.3f) < %.3f",
+             ros::Time::now().toSec(),
+             start_time.toSec(),
+             delay.toSec());
+    ros::Duration(0.25).sleep();
+    ros::spinOnce();
+  }
+
+  int result = RUN_ALL_TESTS();
+  delete g_n;
+  return result;
+}
--- a/Grids/gmapping/test/test_map.py
+++ b/Grids/gmapping/test/test_map.py
@@ -0,0 +1,104 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the Willow Garage nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+
+import PIL.Image
+import unittest
+import subprocess
+import sys
+
+import roslib
+import os
+roslib.load_manifest('gmapping')
+import rostest
+
+class TestGmapping(unittest.TestCase):
+
+  # Test that 2 map files are approximately the same
+  def cmp_maps(self, f0, f1):
+    im0 = PIL.Image.open(f0+'.pgm')
+    im1 = PIL.Image.open(f1+'.pgm')
+  
+    size = 100,100
+    im0.thumbnail(size,PIL.Image.ANTIALIAS)
+    im1.thumbnail(size,PIL.Image.ANTIALIAS)
+  
+    # get raw data for comparison
+    im0d = im0.getdata()
+    im1d = im1.getdata()
+  
+    # assert len(i0)==len(i1)
+    self.assertTrue(len(im0d) == len(im1d))
+  
+    #compare pixel by pixel for thumbnails
+    error_count = 0
+    error_total = 0
+    pixel_tol = 0
+    total_error_tol = 0.1
+    for i in range(len(im0d)):
+      (p0) = im0d[i]
+      (p1) = im1d[i]
+      if abs(p0-p1) > pixel_tol:
+        error_count = error_count + 1
+        error_total = error_total + abs(p0-p1)
+    error_avg = float(error_total)/float(len(im0d))
+    print '%d / %d = %.6f (%.6f)'%(error_total,len(im0d),error_avg,total_error_tol)
+    self.assertTrue(error_avg <= total_error_tol)
+
+  def test_basic_localization_stage(self):
+    if sys.argv > 1:
+      target_time = float(sys.argv[1])
+
+      import time
+      import rospy
+      rospy.init_node('test', anonymous=True)
+      while(rospy.rostime.get_time() == 0.0):
+        print 'Waiting for initial time publication'
+        time.sleep(0.1)
+      start_time = rospy.rostime.get_time()
+
+      while (rospy.rostime.get_time() - start_time) < target_time:
+        print 'Waiting for end time %.6f (current: %.6f)'%(target_time,(rospy.rostime.get_time() - start_time))
+        time.sleep(0.1)
+
+    f0 = os.path.join(roslib.packages.get_pkg_dir('gmapping'),'test','basic_localization_stage_groundtruth')
+    f1 = os.path.join(roslib.packages.get_pkg_dir('gmapping'),'test','basic_localization_stage_generated')
+
+    cmd = ['rosrun', 'map_server', 'map_saver', 'map:=dynamic_map', '-f', f1]
+    self.assertTrue(subprocess.call(cmd) == 0)
+
+    self.cmp_maps(f0,f1)
+
+if __name__ == '__main__':
+  rostest.run('gmapping', 'gmapping_slam', TestGmapping, sys.argv)
--- a/Grids/launch/.DS_Store
+++ b/Grids/launch/.DS_Store
--- a/Grids/launch/hw3.launch
+++ b/Grids/launch/hw3.launch
@@ -0,0 +1,13 @@
+<launch>
+  <node pkg="stage_ros" type="stageros" name="simulator" args="$(find rob456_hw3)/worlds/manyDotsNoisyOdom.world"/>
+  <node pkg="rob456_hw3" type="hw3.py" name="lab3" output="screen" />
+
+  <!-- Insert a call to the slam_gmapping node -->
+  <node pkg="gmapping" type="slam_gmapping" name="slam">
+    <remap from="scan" to="base_scan"/>
+  </node>
+
+  <!-- Insert a call to rviz (optional) -->
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rob456_hw3)/rvizsetup.rviz"/>
+  <!-- args="-d $(find rob456_hw3)/rvizsetup.rviz" -->
+</launch>
--- a/Grids/logging_outputs/odomlogfile.txt
+++ b/Grids/logging_outputs/odomlogfile.txt
--- a/Grids/logging_outputs/slamlogfile.txt
+++ b/Grids/logging_outputs/slamlogfile.txt
--- a/Grids/package.xml
+++ b/Grids/package.xml
@@ -0,0 +1,60 @@
+<?xml version="1.0"?>
+<package>
+  <name>rob456_hw3</name>
+  <version>0.0.0</version>
+  <description>ROB456 HW3</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="rob456@todo.todo">rob456</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/rob456_hw3</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>gmapping</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>stage_ros</build_depend>
+  <run_depend>gmapping</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>stage_ros</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
--- a/Grids/rvizsetup.rviz
+++ b/Grids/rvizsetup.rviz
@@ -0,0 +1,186 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 775
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 1
+      Min Color: 0; 0; 0
+      Min Intensity: 1
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.00999999978
+      Style: Flat Squares
+      Topic: /base_scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Angle Tolerance: 0.100000001
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.300000012
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 100
+      Name: Odometry
+      Position Tolerance: 0.100000001
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.100000001
+        Color: 255; 25; 0
+        Head Length: 0.300000012
+        Head Radius: 0.100000001
+        Shaft Length: 1
+        Shaft Radius: 0.0500000007
+        Value: Arrow
+      Topic: /base_pose_ground_truth
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 113.321236
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -1.69929993
+        Y: -21.3828716
+        Z: -3.49176645
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 1.24039745
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.295395941
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000031900000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007490000003efc0100000002fb0000000800540069006d00650100000000000007490000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003150000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1865
+  X: 55
+  Y: 24
--- a/Grids/src/.DS_Store
+++ b/Grids/src/.DS_Store
--- a/Robotics/Homework
+++ b/Robotics/Homework
@@ -0,0 +1,118 @@
+#!/usr/bin/env python
+
+import rospy
+import math
+import tf
+from tf.transformations import euler_from_quaternion
+import message_filters
+
+# The laser scan message
+from sensor_msgs.msg import LaserScan
+
+# The odometry message
+from nav_msgs.msg import Odometry
+
+# the velocity command message
+from geometry_msgs.msg import Twist
+
+# instantiate global variables "globalOdom"
+globalOdom = Odometry()
+
+# global pi
+pi = math.pi
+
+# method to control the robot
+def callback(scan,odom):
+    # the odometry parameter should be global
+    global globalOdom
+    globalOdom = odom
+
+    # make a new twist message
+    command = Twist()
+
+    # Fill in the fields.  Field values are unspecified 
+    # until they are actually assigned. The Twist message 
+    # holds linear and angular velocities.
+    command.linear.x = 0.0
+    command.linear.y = 0.0
+    command.linear.z = 0.0
+    command.angular.x = 0.0
+    command.angular.y = 0.0
+    command.angular.z = 0.0
+    
+    # find current (x,y) position of robot based on odometry
+    currentX = globalOdom.pose.pose.position.x
+    currentY = globalOdom.pose.pose.position.y
+
+    # find current orientation of robot based on odometry (quaternion coordinates)
+    xOr = globalOdom.pose.pose.orientation.x
+    yOr = globalOdom.pose.pose.orientation.y
+    zOr = globalOdom.pose.pose.orientation.z
+    wOr = globalOdom.pose.pose.orientation.w
+
+    # find orientation of robot (Euler coordinates)
+    (roll, pitch, yaw) = euler_from_quaternion([xOr, yOr, zOr, wOr])
+
+    # find currentAngle of robot (equivalent to yaw)
+    # now that you have yaw, the robot's pose is completely defined by (currentX, currentY, currentAngle)
+    currentAngle = yaw
+
+    # find laser scanner properties (min scan angle, max scan angle, scan angle increment)
+    maxAngle = scan.angle_max
+    minAngle = scan.angle_min
+    angleIncrement = scan.angle_increment
+
+    # find current laser angle, max scan length, distance array for all scans, and number of laser scans
+    currentLaserTheta = minAngle
+    maxScanLength = scan.range_max 
+    distanceArray = scan.ranges
+    numScans = len(distanceArray)
+   
+    # the code below (currently commented) shows how 
+    # you can print variables to the terminal (may 
+    # be useful for debugging)
+    #print 'x: {0}'.format(currentX)
+    #print 'y: {0}'.format(currentY)
+    #print 'theta: {0}'.format(currentAngle)
+
+    # for each laser scan
+    distThreshold = 2.0   # obstacle avoidance threshold
+    minScan = distanceArray[0]
+    velocity = 5.0
+    bearing = 0.0
+    for curScan in range(0, numScans):
+      if currentLaserTheta > -pi/2 and currentLaserTheta < 0 and distanceArray[curScan] < distThreshold:
+        bearing = 1.0 # turn left
+      elif currentLaserTheta >= 0 and currentLaserTheta < pi/2 and distanceArray[curScan] < distThreshold:
+        bearing = -1.0 # turn right
+      if distanceArray[curScan] < minScan:
+        minScan = distanceArray[curScan]
+      currentLaserTheta = currentLaserTheta + angleIncrement
+    
+    # set the robot motion
+    # commanded velocities
+    command.linear.x = velocity*min(1.0,minScan/distThreshold) # slow down if within threshold distance
+    command.angular.z = bearing
+    pub.publish(command)
+
+# main function call
+if __name__ == "__main__":
+    # Initialize the node
+    rospy.init_node('lab3', log_level=rospy.DEBUG)
+
+    # subscribe to laser scan message
+    sub = message_filters.Subscriber('base_scan', LaserScan)
+
+    # subscribe to odometry message    
+    sub2 = message_filters.Subscriber('odom', Odometry)
+
+    # synchronize laser scan and odometry data
+    ts = message_filters.TimeSynchronizer([sub, sub2], 10)
+    ts.registerCallback(callback)
+
+    # publish twist message
+    pub = rospy.Publisher('cmd_vel',Twist)
+
+    # Turn control over to ROS
+    rospy.spin()
+
--- a/Grids/src/odom_error_printer.py
+++ b/Grids/src/odom_error_printer.py
@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+
+import rospy
+import math
+import tf
+from tf.transformations import euler_from_quaternion
+import message_filters
+
+# The odometry message
+from nav_msgs.msg import Odometry
+
+def callback(truth,odom):
+    # Calculate error between truth and odometry estimate
+    x_ODOM_error = truth.pose.pose.position.x - odom.pose.pose.position.x
+    y_ODOM_error = truth.pose.pose.position.y - odom.pose.pose.position.y
+    
+    xo = odom.pose.pose.orientation.x
+    yo = odom.pose.pose.orientation.y
+    zo = odom.pose.pose.orientation.z
+    wo = odom.pose.pose.orientation.w
+    
+    xt = truth.pose.pose.orientation.x
+    yt = truth.pose.pose.orientation.y
+    zt = truth.pose.pose.orientation.z
+    wt = truth.pose.pose.orientation.w
+
+    # find orientation of robot (Euler coordinates)
+    (ro, po, yo) = euler_from_quaternion([xo, yo, zo, wo])    
+    (rt, pt, yt) = euler_from_quaternion([xt, yt, zt, wt])  
+    t_ODOM_error = yt-yo  
+    
+    print "{0},{1},{2},{3}".format(rospy.get_time(),x_ODOM_error, y_ODOM_error, t_ODOM_error)
+
+
+# main function call
+if __name__ == "__main__":
+    # Initialize the node
+    rospy.init_node('odom_error_printer')
+
+    # subscribe to truth pose message
+    sub_truth = message_filters.Subscriber('base_pose_ground_truth', Odometry)
+
+    # subscribe to odometry message    
+    sub_odom = message_filters.Subscriber('odom', Odometry)
+
+    # synchronize truth pose and odometry data
+    ts = message_filters.TimeSynchronizer([sub_truth, sub_odom], 10)
+    ts.registerCallback(callback)
+
+    # Turn control over to ROS
+    rospy.spin()
--- a/Grids/src/slam_error_printer.py
+++ b/Grids/src/slam_error_printer.py
@@ -0,0 +1,64 @@
+#!/usr/bin/env python
+
+import rospy
+import math
+import tf
+from tf.transformations import euler_from_quaternion
+import message_filters
+
+# The odometry message
+from nav_msgs.msg import Odometry
+
+def callback(truth,odom):
+    # Calculate error between truth and SLAM estimate
+    x_ODOM_error = truth.pose.pose.position.x - odom.pose.pose.position.x
+    y_ODOM_error = truth.pose.pose.position.y - odom.pose.pose.position.y
+    
+    xo = odom.pose.pose.orientation.x
+    yo = odom.pose.pose.orientation.y
+    zo = odom.pose.pose.orientation.z
+    wo = odom.pose.pose.orientation.w
+    
+    xt = truth.pose.pose.orientation.x
+    yt = truth.pose.pose.orientation.y
+    zt = truth.pose.pose.orientation.z
+    wt = truth.pose.pose.orientation.w
+
+    # find orientation of robot (Euler coordinates)
+    (ro, po, yo) = euler_from_quaternion([xo, yo, zo, wo])    
+    (rt, pt, yt) = euler_from_quaternion([xt, yt, zt, wt])
+    t_ODOM_error = yt-yo
+    
+    # query SLAM solution
+    map_listener = tf.TransformListener()
+    try:
+      map_listener.waitForTransform("/map", "/odom", rospy.Time(), rospy.Duration(4.0))
+      (trans,rot) = map_listener.lookupTransform('/map', '/odom', rospy.Time(0))
+      x_SLAM_error = trans[0]-x_ODOM_error
+      y_SLAM_error = trans[1]-y_ODOM_error
+      t_SLAM_error = rot[2]-t_ODOM_error
+  
+      print "{0},{1},{2},{3}".format(rospy.get_time(), x_SLAM_error, y_SLAM_error, t_SLAM_error)
+
+    except(tf.LookupException, tf.ConnectivityException):
+      pass
+          	
+    
+
+# main function call
+if __name__ == "__main__":
+    # Initialize the node
+    rospy.init_node('slam_error_printer')
+
+    # subscribe to truth pose message
+    sub_truth = message_filters.Subscriber('base_pose_ground_truth', Odometry)
+
+    # subscribe to odometry message    
+    sub_odom = message_filters.Subscriber('odom', Odometry)
+
+    # synchronize truth pose and odometry data
+    ts = message_filters.TimeSynchronizer([sub_truth, sub_odom], 10)
+    ts.registerCallback(callback)
+
+    # Turn control over to ROS
+    rospy.spin()
--- a/Grids/worlds/manyDots.pgm
+++ b/Grids/worlds/manyDots.pgm
--- a/Grids/worlds/manyDotsNoisyOdom.world
+++ b/Grids/worlds/manyDotsNoisyOdom.world
@@ -0,0 +1,73 @@
+define block model
+(
+  size [0.5 0.5 0.5]
+  gui_nose 0
+)
+
+define topurg ranger
+(
+	sensor( 			
+    range [ 0.0  20.0 ]
+    fov 270.25
+   samples 1081
+  )
+
+  # generic model properties
+  color "black"
+  size [ 0.05 0.05 0.1 ]
+)
+
+define erratic position
+(
+  #size [0.415 0.392 0.25]
+  size [0.35 0.35 0.25]
+  origin [-0.05 0 0 0]
+  gui_nose 0
+  drive "diff"
+  localization "odom"
+  topurg(pose [ 0.050 0.000 0 0.000 ])
+  odom_error [0.05 0.05 0.01 0.02 0.01 0.02]
+)
+
+define floorplan model
+(
+  # sombre, sensible, artistic
+  color "gray30"
+
+  # most maps will need a bounding box
+  boundary 1
+
+  gui_nose 0
+  gui_grid 0
+
+  gui_outline 0
+  gripper_return 0
+  fiducial_return 0
+  laser_return 1
+)
+
+# set the resolution of the underlying raytrace model in meters
+resolution 0.005
+
+interval_sim 100  # simulation timestep in milliseconds
+
+
+window
+( 
+  size [ 745.000 448.000 ] 
+
+  rotate [ 0.000 0.000 ]
+  scale 5
+)
+
+# load an environment bitmap
+floorplan
+( 
+  name "manyDots"
+  bitmap "manyDots.pgm"
+  size [54.0 58.7 0.5]
+  pose [ 0 0.0 0 90.000 ]
+)
+
+# throw in a robot
+erratic( pose [ -10.000 10.000 0 90.000 ] name "era" color "blue" localizaion "gps" localization_origin [0 0 0 0])
--- a/Plots/odom_error_graphs.pdf
+++ b/Plots/odom_error_graphs.pdf
--- a/Robotics/Homework
+++ b/Robotics/Homework
@@ -0,0 +1,72 @@
+import csv
+import matplotlib.pyplot as plotter
+
+base_path = "/home/caperren/catkin_ws/src/rob456_hw3/logging_outputs"
+odom_path = base_path + "/" + "odomlogfile.txt"
+slam_path = base_path + "/" + "slamlogfile.txt"
+
+if __name__ == '__main__':
+    odom_file = open(odom_path, "r")
+    slam_path = open(slam_path, "r")
+
+    odom_csv = csv.reader(odom_file, delimiter=',')
+    slam_csv = csv.reader(slam_path, delimiter=',')
+
+    odom_time = []
+    odom_x_error = []
+    odom_y_error = []
+    odom_theta_error = []
+
+    for line in odom_csv:
+        odom_time.append(line[0])
+        odom_x_error.append(line[1])
+        odom_y_error.append(line[2])
+        odom_theta_error.append(line[3])
+
+    slam_time = []
+    slam_x_error = []
+    slam_y_error = []
+    slam_theta_error = []
+
+    for line in slam_csv:
+        slam_time.append(line[0])
+        slam_x_error.append(line[1])
+        slam_y_error.append(line[2])
+        slam_theta_error.append(line[3])
+
+    # Plot for odometry
+    figure_object_odom, axes_object_odom = plotter.subplots()
+    axes_object_odom.plot(odom_time, odom_x_error, label="X Error")
+    axes_object_odom.plot(odom_time, odom_y_error, label="Y Error")
+
+    axes_theta = axes_object_odom.twinx()
+    axes_theta.plot(odom_time, odom_theta_error, label="Theta Error", color="r")
+    axes_theta.legend()
+
+    axes_object_odom.set_title("Odometery Error Graphs")
+    axes_object_odom.set_xlabel("Time")
+    axes_object_odom.set_ylabel("Error")
+    axes_object_odom.legend(loc=4)
+    axes_object_odom.autoscale(enable=True, axis='x', tight=True)
+
+    figure_object_odom.savefig("odom_error_graphs.pdf", bbox_inches="tight")
+
+    # Plot for slam
+    figure_object_slam, axes_object_slam = plotter.subplots()
+    axes_object_slam.plot(slam_time, slam_x_error, label="X Error")
+    axes_object_slam.plot(slam_time, slam_y_error, label="Y Error")
+
+    axes_theta = axes_object_slam.twinx()
+    axes_theta.plot(slam_time, slam_theta_error, label="Theta Error", color="r")
+    axes_theta.legend()
+    # axes_theta.plot(slam_time, slam_theta_error, label="Theta Error")
+
+    axes_object_slam.set_title("Slam Error Graphs")
+    axes_object_slam.set_xlabel("Time")
+    axes_object_slam.set_ylabel("Error")
+    axes_object_slam.legend(loc=4)
+    axes_object_slam.autoscale(enable=True, axis='x', tight=True)
+
+    figure_object_slam.savefig("slam_error_graphs.pdf", bbox_inches="tight")
+
+    plotter.show()
--- a/Plots/slam_error_graphs.pdf
+++ b/Plots/slam_error_graphs.pdf
--- a/Pathfinding/HW4.pdf
+++ b/Pathfinding/HW4.pdf
--- a/Pathfinding/final_path.pdf
+++ b/Pathfinding/final_path.pdf
--- a/Pathfinding/final_path.png
+++ b/Pathfinding/final_path.png
--- a/Pathfinding/hw4.py
+++ b/Pathfinding/hw4.py
@@ -0,0 +1,189 @@
+import csv
+from matplotlib import pyplot, patches
+from math import sqrt
+
+from heapq import *
+
+CSV_PATH = "world.csv"
+
+VAL_TO_COLOR = {
+    0: "green",
+    1: "red",
+    -1: "blue"
+}
+
+EDGE_COST = 1
+
+START_POSITION = (0, 0)
+END_POSITION = (19, 19)
+
+
+def import_csv_as_array(csv_path):
+    csv_file = open(csv_path, "rU")  # Open the file
+
+    csv_reader = csv.reader(csv_file)  # Put it through the csv reader
+
+    # Loop through the csv lines and append them to an array
+    output_array = []
+    for line in csv_reader:
+        output_array.append([int(col_val) for col_val in line])
+
+    # Delete the csv reader and close the file
+    del csv_reader
+    csv_file.close()
+
+    # Return our world map array
+    return output_array
+
+
+def plot_grid_map(grid_map, fig_save_path=None):
+    # Make the plot
+    figure_object, axes_object = pyplot.subplots()
+
+    # Plot appropriately colored rectangles for each point on the map
+    for y, row in enumerate(grid_map):
+        for x, col in enumerate(row):
+            axes_object.add_patch(patches.Rectangle((x, y), 1, 1, fill=True, color=VAL_TO_COLOR[col]))
+
+    # Plot some x and y dotted lines to make it nicer to view the underlying grid
+    for y in range(len(grid_map)):
+        axes_object.plot([0, len(grid_map[0])], [y, y], color="black", alpha=0.75, linestyle=":")
+
+    for x in range(len(grid_map[0])):
+        axes_object.plot([x, x], [0, len(grid_map)], color="black", alpha=0.75, linestyle=":")
+
+    # Set the y limit from len(grid_map) to 0 so it matches how the file looks in terms of the map
+    axes_object.set_ylim([len(grid_map), 0])
+    axes_object.autoscale(enable=True, tight=True)
+
+    # If the optional argument to save to a file is added, output that file
+    if fig_save_path:
+        figure_object.savefig(fig_save_path, bbox_inches="tight")
+
+    # Show the plot
+    pyplot.show()
+
+
+class AStarSolver(object):
+    # Directions to be used for children
+    VALID_DIRECTIONS = \
+        [
+            [1, 0],   # E
+            [0, 1],   # N
+            [-1, 0],  # W
+            [0, -1],  # S
+        ]
+
+    def __init__(self, world, start_position, end_position):
+        # Initialize all the class variables
+        self.world_map = world
+        self.world_limit_x = len(self.world_map[0])
+        self.world_limit_y = len(self.world_map)
+
+        self.start_position = start_position
+        self.end_position = end_position
+
+        self.open_set = []
+        self.closed_set = []
+
+        self.g_scores = {}
+        self.f_scores = {}
+
+        self.travel_path = {}
+        self.final_path = []
+        self.solution_map = list(self.world_map)
+
+    @staticmethod
+    def heuristic(start_point, end_point):
+        # Calculate the heuristic from point a to point b using the pythagorean theorem
+        delta_x = abs(end_point[0] - start_point[0])
+        delta_y = abs(end_point[1] - start_point[1])
+
+        return sqrt(pow(delta_x, 2) + pow(delta_y, 2))
+
+    def solve_path(self):
+        # Add the starting node, plus it's initial f_cost
+        self.g_scores[self.start_position] = 0
+        self.f_scores[self.start_position] = self.heuristic(self.start_position, self.end_position)
+
+        # Put the starting node into the open set as (f_score, position)
+        # It needs to be in this form for heap sorting by f_score
+        heappush(self.open_set, (self.f_scores[self.start_position], self.start_position))
+
+        while self.open_set:
+            # Pop off the most recent node in open set with the lowest f_score
+            current_node = heappop(self.open_set)
+
+            # Extract the current position from the node
+            current_position = current_node[1]
+
+            # If we've reached the end, break so we can compute the final path
+            if current_position == self.end_position:
+                break
+
+            # Now that we've reached this node, add it to the closed set
+            self.closed_set.append(current_position)
+
+            # Loop through the cardinal directions we can move to
+            for delta_x, delta_y in self.VALID_DIRECTIONS:
+                # Computer the child position based on the cardinal direction and teh current position
+                child_position = (current_position[0] + delta_x, current_position[1] + delta_y)
+
+                # Compute the child's g_score with an edge cost of 1
+                child_g_score = self.g_scores[current_position] + EDGE_COST
+
+                # Check if location is in the world
+                valid_x_limit = 0 <= child_position[0] < self.world_limit_x
+                valid_y_limit = 0 <= child_position[1] < self.world_limit_y
+
+                # If it's in the world, make sure the child location is not an obstacle
+                valid_not_obstacle = None
+                if valid_x_limit and valid_y_limit:
+                    valid_not_obstacle = self.world_map[child_position[1]][child_position[0]] != 1
+
+                # If the child is in a valid location and not an obstacle:
+                if valid_x_limit and valid_y_limit and valid_not_obstacle:
+
+                    # Skip to the next child if we've already seen this node and the current path is more costly than
+                    # what we've seen previously
+                    if child_position in self.closed_set and child_g_score >= self.g_scores.get(child_position, 0):
+                        continue
+
+                    # Get a list of all positions in our open set
+                    open_set_positions = [x[1] for x in self.open_set]
+
+                    # If the score is better than what we've seen, or if we've never seen this node before, add the node
+                    # to our open set and add this as a potential path
+                    if child_g_score < self.g_scores.get(child_position, 0) or child_position not in open_set_positions:
+                        self.travel_path[child_position] = current_position  # Add this jump to the travel path
+                        self.g_scores[child_position] = child_g_score  # Sets the new g_score
+                        self.f_scores[child_position] = \
+                            child_g_score + self.heuristic(child_position, self.end_position)  # Sets the new f_score
+                        heappush(self.open_set, (self.f_scores[child_position], child_position))  # Add to open set
+
+        # Work our way backwards from the end to find the proper path
+        final_path = [self.end_position]  # Add our last hop manually so the loop below can include our start position
+        current_position = self.end_position  # Set the current position to the end
+        while current_position != self.start_position:  # Keep looping until we've reached the beginning of the path
+            current_position = self.travel_path[current_position]  # Update the current to the last path location
+            final_path.append(current_position)  # Append this location to our final array
+
+        self.final_path = final_path[::-1]  # Now that we've found the path, reverse it so it's in order
+
+        # This applies modifications to the world map with the solution so you can see the path when plotting
+        for x, y in self.final_path:
+            self.solution_map[y][x] = -1
+
+    def get_solution_map(self):
+        # Gives us the solution map once we've found a solution
+        return self.solution_map
+
+
+if __name__ == '__main__':
+    world_map = import_csv_as_array(CSV_PATH)  # Import the map
+
+    solver = AStarSolver(world_map, START_POSITION, END_POSITION)  # Initialize the solver
+    solver.solve_path()  # Solve the path
+    solution_map = solver.get_solution_map()  # Retrieve the solution map
+
+    plot_grid_map(solution_map, "final_path.pdf")  # Plot and save the solution
--- a/Pathfinding/output.pdf
+++ b/Pathfinding/output.pdf
--- a/Pathfinding/world.csv
+++ b/Pathfinding/world.csv
@@ -0,0 +1 @@
+0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,0