diff --git a/src/lib/handlers/ROS/.gitignore b/src/lib/handlers/ROS/.gitignore
new file mode 100644
index 00000000..9c88f4d3
--- /dev/null
+++ b/src/lib/handlers/ROS/.gitignore
@@ -0,0 +1,2 @@
+ltlmop_map.world
+*.png
diff --git a/src/lib/handlers/ROS/RosActuatorHandler.py b/src/lib/handlers/ROS/RosActuatorHandler.py
deleted file mode 100755
index e0e599f6..00000000
--- a/src/lib/handlers/ROS/RosActuatorHandler.py
+++ /dev/null
@@ -1,307 +0,0 @@
-#!/usr/bin/env python
-"""
-===================================================
-rosActuator.py - Actuation Halder for ROS intefarce
-===================================================
-
-Control functions using ROS
-"""
-
-import time
-import threading, thread, subprocess
-import roslib
-roslib.load_manifest('rospy')
-roslib.load_manifest('actionlib')
-roslib.load_manifest('pr2_controllers_msgs')
-roslib.load_manifest('pr2_tuckarm')
-
-import rospy
-import actionlib
-from actionlib_msgs.msg import *
-from pr2_controllers_msgs.msg import *
-from trajectory_msgs.msg import *
-from pr2_tuck_arms_action import tuck_arms_main
-
-roslib.load_manifest('pr2_pick_and_place_demos')
-from pr2_pick_and_place_demos.pick_and_place_demo import *
-from pr2_controllers_msgs.msg import SingleJointPositionAction, SingleJointPositionGoal
-
-#import signal
-#roslib.load_manifest('pr2_tuck_arms_action')
-
-#import os
-#import sys
-#import math
-
-#from pr2_common_action_msgs.msg import *
-#import getopt
-
-
-
-import lib.handlers.handlerTemplates as handlerTemplates
-
-class RosActuatorHandler(handlerTemplates.ActuatorHandler):
-    def __init__(self, executor, shared_data):
-        """
-        Actuator Handler for ROS type applications
-        """
-        self.rosInitHandler = shared_data['ROS_INIT_HANDLER']
-        self.loco = executor.hsub.getHandlerInstanceByType(handlerTemplates.LocomotionCommandHandler)
-
-    #####################################
-    ### Available actuator functions: ###
-    #####################################
-
-
-    def _actionTemplate(self, actuatorVal, initial=False, position=.2, max_effort=100.0):
-        """
-        This is a template for future creation of actions
-
-        position (float): The position the gripper should go to (default=.2)
-        max_effort (float): The force the gripper should apply during this process (default=100.0)
-        """
-        if initial:
-
-            # this is the topic that the command should be published on
-            # to perform the action
-            # $(rostopic list) issued during a running scenario will
-            # list all of the topics that can be published to
-            topic='r_gripper_controller/command'
-
-            # this is the command type that is published on the topic
-            # $(rostopic info $topic) when used with a topic of your
-            # choice will list the message type to use
-            messageType='Pr2GripperCommand'
-
-            # this creates the publisher on the given topic with the
-            # given message type
-            self.templateAction=rospy.Publisher(topic, eval(messageType))
-
-            # this creates a new message with the given type
-            # general form:
-            # self.templateMessage=eval(messageType+'()')
-            # nongeneral form:
-            self.templateMessage=Pr2GripperCommand()
-
-            # here you can set the attributes of the command to send
-            # we grap the attributes from the arguments
-            # nongeneral form (there is no general form):
-            self.templateMessage.position=position
-            self.templateMessage.max_effort=max_effort
-        else:
-            if int(actuatorVal)==1:
-                # publish the message on the publisher created earlier
-                # a note: the message will only work if it has time to
-                # to complete the task before locomotion commands begin
-                # to send again.  Try the goal oriented action function
-                # if you want better completions
-                self.templateAction.publish(self.templateMessage)
-
-
-    def pickUpObject(self, actuatorVal, initial=False):
-        """
-        Pick Up Object with the Gripper and sensor on pr2
-        """
-        if initial:
-            #rospy.init_node('pick_and_place_demo', anonymous=False)
-            use_slip_controller = rospy.get_param('/reactive_grasp_node_right/use_slip_controller', 0)
-            use_slip_detection = rospy.get_param('/reactive_grasp_node_right/use_slip_detection', 0)
-            self.pick_and_place_demo = PickAndPlaceDemo(use_slip_controller = use_slip_controller, \
-                                                       use_slip_detection = use_slip_detection)
-        else:
-            if int(actuatorVal)==1:
-                #subprocess.Popen(['rosrun', 'pr2_pick_and_place_demos', 'IROS_2013_pick.py'],stdout=subprocess.PIPE)
-                torso_action_client = actionlib.SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction);
-                rospy.loginfo("run_pick_and_place_demo: waiting for torso action server")
-                torso_action_client.wait_for_server()
-                rospy.loginfo("run_pick_and_place_demo: torso action server found")
-                goal = SingleJointPositionGoal()
-                goal.position = 0.295
-                rospy.loginfo("sending command to lift torso")
-                torso_action_client.send_goal(goal)
-                torso_action_client.wait_for_result(rospy.Duration(30))
-
-                #start the pick and place demo
-                rospy.loginfo("starting pick and place demo")
-                self.pick_and_place_demo.run_pick_up()
-                print "PICK UP COMPLETED"
-                #self.pick_and_place_demo.start_autonomous_thread(True)
-                #pose = self.pose_handler.getPose()
-                #self.drive_handler.setVelocity(self.Velocity[0,0], self.Velocity[1,0],pose[2])
-                self.loco.sendCommand([-1,0,0])
-                time.sleep(3)
-                self.loco.sendCommand([0,0,0])
-
-    def putDownObject(self, actuatorVal, initial=False,position=.6, max_effort=50.0):
-        """
-        Put down Object in the Gripper
-        position (float): The position the gripper should go to (default=.6)
-        max_effort (float): The force the gripper should apply during this process (default=50.0)
-        """
-        if initial:
-
-            topicLeft='l_gripper_controller/command'
-            topicRight='r_gripper_controller/command'
-            messageType='Pr2GripperCommand'
-            self.gripActionLeft=rospy.Publisher(topicLeft, eval(messageType))
-            self.gripActionRight=rospy.Publisher(topicRight, eval(messageType))
-            self.gripMessage=Pr2GripperCommand()
-            self.gripMessage.position=position
-            self.gripMessage.max_effort=max_effort
-
-
-
-        else:
-            if int(actuatorVal)==1:
-                #action_name = 'tuck_arms'
-                #tuck_arms_action_server = tuck_arms_main.TuckArmsActionServer(action_name)
-                #goal = TuckArmsGoal()
-                #goal.tuck_left = False
-                #goal.tuck_right = False
-                #tuck_arm_client = actionlib.SimpleActionClient(action_name, TuckArmsAction)
-                #rospy.logdebug('Waiting for action server to start')
-                #tuck_arm_client.wait_for_server(rospy.Duration(10.0))
-                #rospy.logdebug('Sending goal to action server')
-                #tuck_arm_client.send_goal_and_wait(goal, rospy.Duration(30.0), rospy.Duration(5.0))
-                #self.loco.sendCommand([0,0,0])
-                #time.sleep(15)
-                #subprocess.Popen(['rosrun', 'pr2_tuckarm', 'tuck_arms.py','-r','u','-l','u'],stdout=subprocess.PIPE)
-                #time.sleep(10)
-                self.gripActionLeft.publish(self.gripMessage)
-                self.gripActionRight.publish(self.gripMessage)
-                time.sleep(20)
-
-
-    def movePr2Gripper(self, actuatorVal, initial=False, whichHand='right', position=.2, max_effort=100.0):
-        """
-        This is a sample function for actuation of the Pr2's gripper
-
-        whichHand (str): Specification of either the right or left gripper (default="right")
-        position (float): The position the gripper should go to (default=.2)
-        max_effort (float): The force the gripper should apply during this process (default=100.0)
-        """
-        if initial:
-
-            if whichHand=='left':
-                topic='l_gripper_controller/command'
-            else:
-                topic='r_gripper_controller/command'
-            messageType='Pr2GripperCommand'
-            self.gripAction=rospy.Publisher(topic, eval(messageType))
-            self.gripMessage=Pr2GripperCommand()
-            self.gripMessage.position=position
-            self.gripMessage.max_effort=max_effort
-        else:
-            if int(actuatorVal)==1:
-                self.gripAction.publish(self.gripMessage)
-
-
-    def _templateGoalAction(self, actuatorVal, position=0.2, initial=False):
-        """
-        This is a template for a goal oriented action function
-
-        position (float): The goal position of the torso (default=0.2)
-        """
-        if initial:
-            # this is the topic you want to publish the message on
-            # $(rostopic list) during a running session will list
-            # the available topics
-            topic='torso_controller/position_joint_action'
-
-            # for goals, the messageType has two parts, the Action
-            # and the Goal, in most cases it will be
-            # messageType+'Action' for the action part and
-            # messageType+'Goal' for the goal part
-            messageType='SingleJointPosition'
-
-            # here we create the action/goal client with the Action
-            # messageType
-            self.templateAction = actionlib.SimpleActionClient(topic, eval(messageType+'Action'))
-
-            # this is the creation of the message, notice the message
-            # has a Goal format
-            self.templateMessage=eval(messageType+'Goal()')
-
-            # set the goal attributes of the message, here we just
-            # have position (torso)
-            self.templateMessage.position=position
-        else:
-            if int(actuatorVal)==1:
-                # bring up the client to allow for message sending
-                self.templateAction.wait_for_server()
-
-                # send the goal message
-                self.templateAction.send_goal(self.templateMessage)
-
-                # wait for a result
-                self.templateAction.wait_for_result()
-
-                # Don't burden the user with success messages
-                # however, if the action fails, the user will want
-                # to know
-                if not templateAction.get_state() == GoalStatus.SUCCEEDED:
-                    print "Action failed"
-
-    def movePr2Torso(self, actuatorVal, position=0.2, initial=False):
-        """
-        This is a sample goal oriented actuation for moving the Pr2's torso
-
-        position (float): The goal position of the torso (default=0.2)
-        """
-        if initial:
-            topic='torso_controller/position_joint_action'
-            messageType='SingleJointPosition'
-            self.torsoAction = actionlib.SimpleActionClient(topic, eval(messageType+'Action'))
-            self.torsoMessage=eval(messageType+'Goal()')
-            self.torsoMessage.position=position
-        else:
-            if int(actuatorVal)==1:
-                self.torsoAction.wait_for_server()
-                self.torsoAction.send_goal(self.torsoMessage)
-                self.torsoAction.wait_for_result()
-                if not self.torsoAction.get_state() == GoalStatus.SUCCEEDED:
-                    print "Action failed"
-
-    def movePr2Head(self, actuatorVal, target_frame_id='r_gripper_tool_frame', target_x=0.0, target_y=0.0, target_z=0.0, min_duration=1.0, initial=False):
-        """
-        This is a sample goal oriented actuation for moving the Pr2's head
-
-        target_frame_id (str): The frame of refernce you want to look at (default='r_gripper_tool_frame')
-        target_x (float): The x position in the frame of reference (default=0.0)
-        target_y (float): The y position in the frame of reference (default=0.0)
-        target_z (float): The z position in the frame of reference (default=0.0)
-        min_duration (float): The minimum movement time (default=1.0)
-        """
-        if initial:
-            topic='head_traj_controller/point_head_action'
-            messageType='PointHead'
-            self.headAction = actionlib.SimpleActionClient(topic, eval(messageType+'Action'))
-            self.headMessage=eval(messageType+'Goal()')
-            self.headMessage.target.header.frame_id=target_frame_id
-            # for a list of frame_ids: http://www.ros.org/wiki/pr2_description
-            self.headMessage.target.point.x=target_x
-            self.headMessage.target.point.y=target_y
-            self.headMessage.target.point.z=target_z
-            self.headMessage.min_duration=rospy.Duration(min_duration)
-        else:
-            if int(actuatorVal)==1:
-                self.headAction.wait_for_server()
-                self.headAction.send_goal(self.headMessage)
-                self.headAction.wait_for_result()
-                if not self.headAction.get_state() == GoalStatus.SUCCEEDED:
-                    print "Action failed"
-
-    def pr2TuckArms(self, actuatorVal, tuck=True, initial=False):
-        """
-        This is an example of calling existing scripts with ROS
-
-        tuck (bool): True applies a tucking of the arms, False untucks them (default=True)
-        """
-        if initial:
-            pass
-        else:
-            if int(actuatorVal)==1:
-                if tuck:
-                    subprocess.Popen(['rosrun', 'pr2_tuckarm', 'tuck_arms.py','-r','t','-l','t'],stdout=subprocess.PIPE)
-                else:
-                    subprocess.Popen(['rosrun', 'pr2_tuckarm', 'tuck_arms.py','-r','u','-l','u'],stdout=subprocess.PIPE)
diff --git a/src/lib/handlers/ROS/RosDriveHandler.py b/src/lib/handlers/ROS/RosDriveHandler.py
index 033ca10c..ba60885a 100644
--- a/src/lib/handlers/ROS/RosDriveHandler.py
+++ b/src/lib/handlers/ROS/RosDriveHandler.py
@@ -9,6 +9,7 @@
 """
 
 from math import sin, cos
+import sys
 
 import lib.handlers.handlerTemplates as handlerTemplates
 
@@ -21,13 +22,14 @@ def __init__(self, executor, shared_data,d=0.6):
         """
 
         try:
-            self.loco = executor.hsub.getHandlerInstanceByType(handlerTemplates.LocomotionCommanHandler)
+            self.loco = executor.hsub.getHandlerInstanceByType(handlerTemplates.LocomotionCommandHandler)
             self.coordmap = executor.hsub.coordmap_lab2map
         except NameError:
-            print "(DRIVE) Locomotion Command Handler not found."
+            print >>sys.__stdout__, "(DRIVE) Locomotion Command Handler not found."
             exit(-1)
 
         self.d = d
+
     def setVelocity(self, x, y, theta=0, z = 0):
         #print "VEL:%f,%f" % tuple(self.coordmap([x, y]))
 
diff --git a/src/lib/handlers/ROS/RosInitHandler.py b/src/lib/handlers/ROS/RosInitHandler.py
index 52ba0d36..8f7ab127 100644
--- a/src/lib/handlers/ROS/RosInitHandler.py
+++ b/src/lib/handlers/ROS/RosInitHandler.py
@@ -6,17 +6,12 @@
 =================================================
 """
 import math
-import roslib; roslib.load_manifest('gazebo')
 import sys, subprocess, os, time, os, shutil, rospy
 import cairo, rsvg
 import re, Polygon, Polygon.IO
 import lib.regions as regions
 #import execute
 from numpy import *
-#from gazebo.srv import *
-from gazebo_msgs.srv import *
-from gazebo import gazebo_interface
-from gazebo_msgs.msg import ModelState
 from geometry_msgs.msg import Pose
 from geometry_msgs.msg import Twist
 import fileinput
@@ -24,7 +19,7 @@
 import lib.handlers.handlerTemplates as handlerTemplates
 
 class RosInitHandler(handlerTemplates.InitHandler):
-    def __init__(self, executor, init_region, worldFile='ltlmop_map.world', robotPixelWidth=200, robotPhysicalWidth=.5, robotPackage="pr2_gazebo", robotLaunchFile="pr2.launch", modelName = "pr2", calib=False):
+    def __init__(self, executor, init_region, worldFile='ltlmop_map.world', robotPixelWidth=2, robotPhysicalWidth=.2, robotPackage="turtlebot_bringup", robotLaunchFile="minimal.launch", modelName = "turtlebot", calib=False):
         """
         Initialization handler for ROS and gazebo
 
@@ -32,9 +27,10 @@ def __init__(self, executor, init_region, worldFile='ltlmop_map.world', robotPix
         worldFile (str): The alternate world launch file to be used (default="ltlmop_map.world")
         robotPixelWidth (int): The width of the robot in pixels in ltlmop (default=200)
         robotPhysicalWidth (float): The physical width of the robot in meters (default=.5)
-        robotPackage (str): The package where the robot is located (default="pr2_gazebo")
-        robotLaunchFile (str): The launch file name for the robot in the package (default="pr2.launch")
-        modelName(str): Name of the robot. Choices: pr2 and quadrotor for now(default="pr2")
+        robotPackage (str): The package where the robot is located (default="turlebot_bringup")
+        robotLaunchFile (str): The launch file name for the robot in the package (default="minimal.launch")
+        modelName(str): Name of the robot. Choices: turtlebot for now(default="turtlebot")
+        calib(bool): Something something.
         """
 
         #Set a blank offset for moving the map
@@ -45,7 +41,7 @@ def __init__(self, executor, init_region, worldFile='ltlmop_map.world', robotPix
         #The world file that is to be launched, see gazebo_worlds/worlds
         self.worldFile=worldFile
         #Map to real world scaling constant
-        self.ratio= robotPhysicalWidth/robotPixelWidth
+        self.ratio = robotPhysicalWidth/robotPixelWidth
         self.robotPhysicalWidth = robotPhysicalWidth
         self.modelName          = modelName
         self.coordmap_map2lab   = executor.hsub.coordmap_map2lab
@@ -53,8 +49,11 @@ def __init__(self, executor, init_region, worldFile='ltlmop_map.world', robotPix
 
         # change the starting pose of the box
         self.original_regions = executor.proj.loadRegionFile()
-        self.destination="/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds/worlds/"+self.worldFile
-        self.state      ="/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds/worlds/ltlmop_state.world"
+
+        self.path = os.path.dirname(os.path.realpath(__file__))
+
+        self.destination = self.path+ "/" + self.worldFile
+        self.state       = self.path+"/ltlmop_state.world"
         #clean the original file for world and state
         f = open(self.destination,"w")
         f.close()
@@ -62,17 +61,15 @@ def __init__(self, executor, init_region, worldFile='ltlmop_map.world', robotPix
         f.close()
 
         # start the world file with the necessities
-        source="/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds/worlds/ltlmop_essential_front.world"
+        source = self.path+"/ltlmop_essential_front.world"
         self.appendObject(source,self.destination)
 
         if self.worldFile=='ltlmop_map.world':
             #This creates a png copy of the regions to load into gazebo
             self.createRegionMap(executor.proj)
-        #Change robot and world file in gazebo:
-        self.changeRobotAndWorld(executor.proj)
 
         # close the world file with the necessities
-        source="/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds/worlds/ltlmop_essential_end.world"
+        source = self.path+"/ltlmop_essential_end.world"
         self.appendObject(source,self.destination)
 
         # Center the robot in the init region (not on calibration)
@@ -86,22 +83,21 @@ def __init__(self, executor, init_region, worldFile='ltlmop_map.world', robotPix
             f.close()
 
             # start the world file with the necessities
-            source="/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds/worlds/ltlmop_essential_front.world"
+            source = self.path+"/ltlmop_essential_front.world"
             self.appendObject(source,self.destination)
 
             if self.worldFile=='ltlmop_map.world':
                 #This creates a png copy of the regions to load into gazebo
                 self.createRegionMap(executor.proj)
-            #Change robot and world file in gazebo:
-            self.changeRobotAndWorld(executor.proj)
 
             # check if there are obstacles. If so, they will be added to the world
             for region in self.original_regions.regions:
                 if region.isObstacle is True:
+                    # TODO : add obstacles again
                     addObstacle = True
                     break
 
-            source="/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds/worlds/ltlmop_essential_state.world"
+            source = self.path + "/ltlmop_essential_state.world"
             self.appendObject(source,self.state)
 
             if addObstacle is False:
@@ -125,10 +121,10 @@ def __init__(self, executor, init_region, worldFile='ltlmop_map.world', robotPix
                     if region.isObstacle is True:
                         poly_region = self.createRegionPolygon(region)
                         center = poly_region.center()
-                        print >>sys.__stdout__,"center:" +str(center)
+                        print >>sys.__stdout__,"obstacle center:" +str(center)
                         pose = self.coordmap_map2lab(region.getCenter())
-                        print >>sys.__stdout__,"pose:" + str(pose)
                         pose = center
+                        print >>sys.__stdout__,"obstacle pose:" + str(pose)
                         height = region.height
                         if height == 0:
                             height = self.original_regions.getMaximumHeight()
@@ -170,6 +166,7 @@ def __init__(self, executor, init_region, worldFile='ltlmop_map.world', robotPix
                         else:
                             length= size[0]   #width in region.py = size[0]
                             depth= size[1]  #height in region.py =size[1]
+                            pose = (pose[0], pose[1])
                             print "INIT: pose "+str(pose)+" height: "+str(height)+" length: "+str(length)+" depth: "+str(depth)
                             self.addBox(i,length,depth,height,pose)
                         i += 1
@@ -178,11 +175,12 @@ def __init__(self, executor, init_region, worldFile='ltlmop_map.world', robotPix
             self.appendObject(self.state,self.destination)
 
             # close the world file with the necessities
-            source="/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds/worlds/ltlmop_essential_end.world"
+            source = self.path + "/ltlmop_essential_end.world"
             self.appendObject(source,self.destination)
 
         # set up a publisher to publish pose
-        self.pub = rospy.Publisher('/gazebo/set_model_state', ModelState)
+        # FIXME: We need to publish the pose somehow
+        # self.pub = rospy.Publisher('/gazebo/set_model_state', ModelState)
 
         # Create a subprocess for ROS
         self.rosSubProcess(executor.proj)
@@ -205,7 +203,7 @@ def createRegionPolygon(self,region,hole = None):
 
     def addBox(self,i,length,depth , height, pose):
         """
-        to add a cylinder into the world
+        to add a box into the world
         i = count for the name  (just to distinguish one model from another)
         length = length of the box (in x direction)
         depth  = depth of the box  (in y direction)
@@ -214,30 +212,26 @@ def addBox(self,i,length,depth , height, pose):
         """
         # for editing the starting pose of the cylinder (box works the same way)
         # change the name of the model
-        path="/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds/worlds/ltlmop_box.world"
+        path = self.path + "/ltlmop_box.world"
         searchExp='<model name='
-        replaceExp='<model name="box_model_'+str(i)+'" static="1">\n'
+        replaceExp='<model name="box_model_'+str(i)+'">\n'
         self.replaceAll(path,searchExp,replaceExp)
 
-        searchExp='<link name='
-        replaceExp='      <origin pose="0.000000 0.000000 '+str(float(height)/2)+' 0 -0.000000 0.000000"/>\n'
-        self.replaceNextLine(path,searchExp,replaceExp)
-
-        # change the pose of the model
-        searchExp='    </link>'
-        replaceExp='    <origin pose="'+str(pose[0])+' '+str(pose[1])+' 0 0 0 0 " />\n'
+        # searchExp='<link name='
+        searchExp='<model name='
+        replaceExp='      <pose>'+str(pose[0])+' '+ str(pose[1]) +' '+str(float(height)/2)+' 0 -0.000000 0.000000</pose>\n'
         self.replaceNextLine(path,searchExp,replaceExp)
 
         # change the radius and height of the model
-        searchExp='<box size='
-        replaceExp='       <box size="'+str(length)+' '+str(depth)+' '+str(height)+'"/>\n'
+        searchExp='<box><size>'
+        replaceExp='       <box><size>'+str(length)+' '+str(depth)+' '+str(height)+'</size></box>\n'
         self.replaceAll(path,searchExp,replaceExp)
 
         #append to the world file
         self.appendObject(path,self.destination)
 
         # change the state file for cylinder
-        path="/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds/worlds/ltlmop_state_cylinder.world"
+        path = self.path + "/ltlmop_state_cylinder.world"
         searchExp='    <model name='
         replaceExp='    <model name="box_model_'+str(i)+'">\n'
         self.replaceAll(path,searchExp,replaceExp)
@@ -263,26 +257,26 @@ def addCylinder(self,i,radius, height, pose):
         """
         # for editing the starting pose of the cylinder (box works the same way)
         # change the name of the model
-        path="/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds/worlds/ltlmop_cylinder.world"
+        path = self.path + "/ltlmop_cylinder.world"
         searchExp='<model name='
-        replaceExp='<model name="cylinder_'+str(i)+'" static="1">\n'
+        replaceExp='<model name="cylinder_'+str(i)+'"><static>1</static>\n'
         self.replaceAll(path,searchExp,replaceExp)
 
         # change the pose of the model
         searchExp='    </link>'
-        replaceExp='    <origin pose="'+str(pose[0])+' '+str(pose[1])+' 0 0 0 0 " />\n'
+        replaceExp='    <pose>'+str(pose[0])+' '+str(pose[1])+' 0 0 0 0 </pose> />\n'
         self.replaceNextLine(path,searchExp,replaceExp)
 
         # change the radius and height of the model
-        searchExp='<cylinder radius='
-        replaceExp='       <cylinder radius="'+ str(radius)+'" length="'+str(height)+'"/>\n'
+        searchExp='<cylinder><radius>'
+        replaceExp='       <cylinder><radius>'+ str(radius)+'</radius><length>'+str(height)+'</length></cylinder>\n'
         self.replaceAll(path,searchExp,replaceExp)
 
         #append to the world file
         self.appendObject(path,self.destination)
 
         # change the state file for cylinder
-        path="/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds/worlds/ltlmop_state_cylinder.world"
+        path= self.path + "/ltlmop_state_cylinder.world"
         searchExp='    <model name='
         replaceExp='    <model name="cylinder_'+str(i)+'">\n'
         self.replaceAll(path,searchExp,replaceExp)
@@ -385,7 +379,7 @@ def createRegionMap(self, proj):
         Gazebo Simulator.
         """
         #This block creates a copy and converts to svg
-        texture_dir =  '/opt/ros/fuerte/stacks/simulator_gazebo/gazebo/gazebo/share/gazebo-1.0.2/Media/materials/textures' #potentially dangerous as pathd in ROS change with updates
+        texture_dir = self.path
         ltlmop_path = proj.getFilenamePrefix()
         regionsFile = ltlmop_path+"_copy.regions"
         shutil.copy(proj.rfi.filename,regionsFile)
@@ -407,56 +401,45 @@ def createRegionMap(self, proj):
 
         # Change size of region map in gazebo
         # This is accomplished through edits of the world file before opening
-        path="/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds/worlds/"+self.worldFile # Potential problem when version changes >_<
-        #path="/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds/worlds/ltlmop_map.world" # Potential problem when version changes >_<
-        searchExp='<box size='
-        T=[self.ratio*self.imgWidth,self.ratio*self.imgHeight]
+        path=self.path + "/" + self.worldFile # Potential problem when version changes >_<
+        searchExp='<plane><size>'
+        T=[self.ratio * self.imgWidth,self.ratio * self.imgHeight]
         resizeX=T[0]
         resizeY=T[1]
-        replaceExp='            <box size="'+str(resizeX)+' '+str(resizeY)+' .1" />\n'
+        replaceExp='            <plane><size>'+str(resizeX)+' '+str(resizeY)+'</size><normal>0 0 1</normal></plane>\n'
         self.replaceAll(path,searchExp,replaceExp)
 
-    def changeRobotAndWorld(self, proj):
-        """
-        This changes the robot in the launch file
-        """
-        #Accomplish through edits in the launch file
-        path="/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds/launch/ltlmop.launch"
-        searchExp='<include file='
-        replaceExp='    <include file="$(find '+self.package+')/launch/'+self.launchFile+'" />\n'
+        searchExp='<uri>'
+        replaceExp='              <uri>'+self.path+'/ltlmop.material</uri>\n'
         self.replaceAll(path,searchExp,replaceExp)
 
-
-        searchExp='<node name="gazebo" pkg="gazebo"'
-        replaceExp='    <node name="gazebo" pkg="gazebo" type="gazebo" args=" $(find gazebo_worlds)/worlds/'+self.worldFile+'" respawn="false" output="screen"/>\n'
-        self.replaceAll(path,searchExp,replaceExp)
-
-
     def rosSubProcess(self, proj, worldFile='ltlmop_map.world'):
-        start = subprocess.Popen(['roslaunch gazebo_worlds ltlmop.launch'], shell = True, stdout=subprocess.PIPE)
-        start_output = start.stdout
-
+        start = subprocess.Popen(['stdbuf -oL roslaunch '+ self.path + '/ltlmop.launch world_file:='+self.path+'/'+worldFile], shell = True, stdout=subprocess.PIPE)
 
         # Wait for it to fully start up
         while 1:
-            input = start_output.readline()
-            print input, # Pass it on
+            input = start.stdout.readline()
+
+            print >>sys.__stdout__, input.rstrip() # Pass it on
             if input == '': # EOF
                 print "(INIT) WARNING:  Gazebo seems to have died!"
                 break
-            if "Successfully spawned" or "successfully spawned" in input:
-                #Successfully spawend is output from the creation of the PR2
+            # if "Successfully spawned" or "successfully spawned" in input:
+            # TODO only works for gazebo2?
+            if "started roslaunch server" in input:
+                #Successfully spawend is output from the creation of the robot
                 #It might get stuck waiting for another type of robot to spawn
+                print "Gazebo finished starting up"
                 time.sleep(5)
                 break
 
-    def centerTheRobot(self, proj, init_region):
+    def centerTheRobot(self, executor, init_region):
         # Start in the center of the defined initial region
 
         try: #Normal operation
-            initial_region = proj.rfiold.regions[proj.rfiold.indexOfRegionWithName(init_region)]
+            initial_region = executor.proj.rfiold.regions[executor.proj.rfiold.indexOfRegionWithName(init_region)]
         except: #Calibration
-            initial_region = proj.rfi.regions[proj.rfi.indexOfRegionWithName(init_region)]
+            initial_region = executor.proj.rfi.regions[executor.proj.rfi.indexOfRegionWithName(init_region)]
         center = initial_region.getCenter()
 
         # Load the map calibration data and the region file data to feed to the simulator
@@ -466,3 +449,12 @@ def centerTheRobot(self, proj, init_region):
         print "Initial region name: ", initial_region.name, " I think I am here: ", map2lab, " and center is: ", center
 
         os.environ['ROBOT_INITIAL_POSE']="-x "+str(map2lab[0])+" -y "+str(map2lab[1])
+
+# Needed because of errors due to control sequences?
+import unicodedata
+def remove_control_characters_unicode(s):
+    return "".join(ch for ch in s if unicodedata.category(ch)[0]!="C")
+
+def remove_control_characters(s):
+    mpa = dict.fromkeys(range(32))
+    return s.translate(mpa)
diff --git a/src/lib/handlers/ROS/RosLocomotionCommandHandler.py b/src/lib/handlers/ROS/RosLocomotionCommandHandler.py
index 43277471..b649f424 100644
--- a/src/lib/handlers/ROS/RosLocomotionCommandHandler.py
+++ b/src/lib/handlers/ROS/RosLocomotionCommandHandler.py
@@ -1,8 +1,5 @@
 #!/usr/bin/env python
-import roslib; roslib.load_manifest('gazebo')
-
 import rospy, math, subprocess, os, sys
-from gazebo.srv import *
 
 from geometry_msgs.msg import Twist
 """
@@ -14,19 +11,21 @@
 import lib.handlers.handlerTemplates as handlerTemplates
 
 class RosLocomotionCommandHandler(handlerTemplates.LocomotionCommandHandler):
-	def __init__(self, executor, shared_data, velocityTopic='/base_controller/command'):
+	def __init__(self, executor, shared_data, velocityTopic='/cmd_vel_mux/input/navi'):
 		"""
 		The ROS Locomotion Command Handler
 
-		velocityTopic (str): This is the topic which handles the movement commands (default='/base_controller/command')
+		velocityTopic (str): This is the topic which handles the movement commands (default='/cmd_vel_mux/input/navi')
 		"""
+
 		try:
 			#open a publisher for the base controller of the robot
-			self.pub = rospy.Publisher(velocityTopic, Twist)
+			# TODO: queue size okay?
+			self.pub = rospy.Publisher(velocityTopic, Twist, queue_size=10)
 			# for the pr2, use /base_controller/command
 			# the turtlebot takes /cmd_vel
 		except:
-			print 'Problem setting up Locomotion Command Node'
+			print >>sys.__stdout__, 'Problem setting up Locomotion Command Node'
 
 	def sendCommand(self, cmd):
 
diff --git a/src/lib/handlers/ROS/RosPoseHandler.py b/src/lib/handlers/ROS/RosPoseHandler.py
index d0780d0c..fa99a9fd 100644
--- a/src/lib/handlers/ROS/RosPoseHandler.py
+++ b/src/lib/handlers/ROS/RosPoseHandler.py
@@ -1,12 +1,10 @@
 #!/usr/bin/env python
-import roslib; roslib.load_manifest('gazebo')
-
 import rospy, math
-from gazebo.srv import *
 from numpy import *
 from std_msgs.msg import String
 from tf.transformations import euler_from_quaternion
-
+# TODO: don't rely on gazebo
+from gazebo_msgs import srv
 
 """
 =======================================
@@ -17,15 +15,15 @@
 import lib.handlers.handlerTemplates as handlerTemplates
 
 class RosPoseHandler(handlerTemplates.PoseHandler):
-	def __init__(self, executor, shared_data, modelName="pr2"):
+	def __init__(self, executor, shared_data, modelName="mobile_base"):
 		"""
 		Pose Handler for ROS and gazebo.
 
-		modelName (str): The model name of the robot in gazebo to get the pose information from (default="pr2")
+		modelName (str): The model name of the robot in gazebo to get the pose information from (default="turtlebot")
 		"""
 
 		#GetModelState expects the arguments model_name and relative_entity_name
-		#In this case it is pr2 and world respectively but can be changed for different robots and environments
+		#In this case it is turtlebot and world respectively but can be changed for different robots and environments
 		self.model_name = modelName
 		self.relative_entity_name = 'world' #implies the gazebo global coordinates
 		self.last_pose = None
@@ -38,7 +36,7 @@ def getPose(self,cached = False):
 			#This returns a GetModelStateResponse, which contains data on pose
 			rospy.wait_for_service('/gazebo/get_model_state')
 			try:
-				gms = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
+				gms = rospy.ServiceProxy('/gazebo/get_model_state', srv.GetModelState)
 				resp = gms(self.model_name,self.relative_entity_name)
 				#Cartesian Pose
 				self.pos_x = resp.pose.position.x
diff --git a/src/lib/handlers/ROS/RosSensorHandler.py b/src/lib/handlers/ROS/RosSensorHandler.py
old mode 100755
new mode 100644
index 22fde531..c0f8e5df
--- a/src/lib/handlers/ROS/RosSensorHandler.py
+++ b/src/lib/handlers/ROS/RosSensorHandler.py
@@ -5,7 +5,6 @@
 ====================================================
 """
 
-import roslib
 import rospy
 from sensor_msgs.msg import *
 
diff --git a/src/lib/handlers/ROS/install.sh b/src/lib/handlers/ROS/install.sh
deleted file mode 100755
index 0476af92..00000000
--- a/src/lib/handlers/ROS/install.sh
+++ /dev/null
@@ -1,65 +0,0 @@
-#! /bin/bash
-echo Installing ROS...
-sudo apt-get install python-wxglade
-# wget https://github.com/downloads/jraedler/Polygon2/Polygon-2.0.5.zip
-# unzip Polygon-2.0.5.zip
-# cd Polygon-2.0.5
-# sudo python setup.py install
-# cd ..
-sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list'
-wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-sudo apt-get update
-sudo apt-get install ros-fuerte-desktop-full
-sudo apt-get install ros-fuerte-hector-quadrotor
-echo "source /opt/ros/fuerte/setup.bash" >> ~/.bashrc
-. ~/.bashrc
-source /opt/ros/fuerte/setup.bash
-sudo apt-get install python-setuptools python-pip
-sudo pip install -U rosinstall vcstools rosdep
-sudo easy_install -U rosinstall vcstools rosdep
-sudo apt-get install ros-fuerte-pr2-desktop
-echo "source /opt/ros/fuerte/setup.bash" >> ~/.bashrc
-. ~/.bashrc
-source /opt/ros/fuerte/setup.bash
-sudo apt-get install python-rsvg
-PATHSM='/opt/ros/fuerte/stacks/simulator_gazebo/gazebo_worlds'
-sudo cp ltlmop_map.world $PATHSM/worlds/ltlmop_map.world
-sudo chmod 777 $PATHSM/worlds/ltlmop_map.world
-sudo cp ltlmop_state.world $PATHSM/worlds/ltlmop_state.world
-sudo chmod 777 $PATHSM/worlds/ltlmop_state.world
-sudo cp ltlmop_box.world $PATHSM/worlds/ltlmop_box.world
-sudo chmod 777 $PATHSM/worlds/ltlmop_box.world
-sudo cp ltlmop_cylinder.world $PATHSM/worlds/ltlmop_cylinder.world
-sudo chmod 777 $PATHSM/worlds/ltlmop_cylinder.world
-sudo cp ltlmop_essential_front.world $PATHSM/worlds/ltlmop_essential_front.world
-sudo chmod 777 $PATHSM/worlds/ltlmop_essential_front.world
-sudo cp ltlmop_essential_end.world $PATHSM/worlds/ltlmop_essential_end.world
-sudo chmod 777 $PATHSM/worlds/ltlmop_essential_end.world
-sudo cp ltlmop_essential_state.world $PATHSM/worlds/ltlmop_essential_state.world
-sudo chmod 777 $PATHSM/worlds/ltlmop_essential_state.world
-sudo cp ltlmop_state_cylinder.world $PATHSM/worlds/ltlmop_state_cylinder.world
-sudo chmod 777 $PATHSM/worlds/ltlmop_state_cylinder.world
-sudo chmod 777 $PATHSM/worlds
-sudo chmod 777 $PATHSM/launch
-sudo cp ltlmop.launch $PATHSM/launch/ltlmop.launch
-sudo chmod 777 $PATHSM/launch/ltlmop.launch
-
-PATHS='/opt/ros/fuerte/stacks/hector_quadrotor/hector_quadrotor_gazebo'
-sudo cp spawn_quadrotor.launch $PATHS/launch/spawn_quadrotor.launch
-sudo chmod 777 $PATHS/launch spawn_quadrotor.launch
-
-PATHLOC='/opt/ros/fuerte/stacks/simulator_gazebo/gazebo/gazebo/share/gazebo-1.0.2/Media/materials'
-sudo chmod 777 /opt/ros/fuerte/stacks/simulator_gazebo/gazebo/gazebo/share/gazebo-1.0.2/Media/materials/textures
-sudo chmod 777 $PATHLOC/scripts/Gazebo.material
-if ! grep -q ltlmop $PATHLOC/scripts/Gazebo.material 
-	then sudo cat ltlmop.material >> $PATHLOC/scripts/Gazebo.material 
-fi
-ROS_WORKSPACE=/opt/ros/fuerte
-sudo apt-get install openjdk-7-jre
-sudo apt-get install openjdk-7-jdk
-cd ../../../../etc/jtlv
-sh build.sh
-cd ../../lib/handlers/robots/ROS
-echo "source /opt/ros/fuerte/setup.bash" >> ~/.bashrc
-. ~/.bashrc
-source /opt/ros/fuerte/setup.bash
diff --git a/src/lib/handlers/ROS/ltlmop.launch b/src/lib/handlers/ROS/ltlmop.launch
old mode 100755
new mode 100644
index f8bc28df..38f8749e
--- a/src/lib/handlers/ROS/ltlmop.launch
+++ b/src/lib/handlers/ROS/ltlmop.launch
@@ -1,22 +1,37 @@
-<?xml version="1.0"?>
-
 <launch>
+  <arg name="world_file"  default="$(env TURTLEBOT_GAZEBO_WORLD_FILE)"/>
 
-  <arg name="use_sim_time" default="true"/>
+  <arg name="base"      value="$(optenv TURTLEBOT_BASE kobuki)"/> <!-- create, roomba -->
+  <arg name="battery"   value="$(optenv TURTLEBOT_BATTERY /proc/acpi/battery/BAT0)"/>  <!-- /proc/acpi/battery/BAT0 --> 
   <arg name="gui" default="true"/>
-  <arg name="withpr2" default="true"/>
+  <arg name="stacks"    value="$(optenv TURTLEBOT_STACKS hexagons)"/>  <!-- circles, hexagons --> 
+  <arg name="3d_sensor" value="$(optenv TURTLEBOT_3D_SENSOR kinect)"/>  <!-- kinect, asus_xtion_pro --> 
 
-  <group if="$(arg use_sim_time)">
-    <param name="/use_sim_time" value="true" />
-  </group>
-    
-  <node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/ltlmop_map.world" respawn="false" output="screen"/>
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="use_sim_time" value="true"/>
+    <arg name="debug" value="false"/>
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="world_name" value="$(arg world_file)"/>
+  </include>
   
-  <group if="$(arg gui)">
-    <node name="gazebo_gui" pkg="gazebo" type="gui" respawn="false" output="screen"/>
-  </group>
-
-  <group if="$(arg withpr2)">
-    <include file="$(find pr2_gazebo)/pr2.launch" />
-  </group>
+  <include file="$(find turtlebot_gazebo)/launch/includes/$(arg base).launch.xml">
+    <arg name="base" value="$(arg base)"/>
+    <arg name="stacks" value="$(arg stacks)"/>
+    <arg name="3d_sensor" value="$(arg 3d_sensor)"/>
+  </include>
+  
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+    <param name="publish_frequency" type="double" value="30.0" />
+  </node>
+  
+  <!-- Fake laser -->
+  <node pkg="nodelet" type="nodelet" name="laserscan_nodelet_manager" args="manager"/>
+  <node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan"
+        args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet laserscan_nodelet_manager">
+    <param name="scan_height" value="10"/>
+    <param name="output_frame_id" value="/camera_depth_frame"/>
+    <param name="range_min" value="0.45"/>
+    <remap from="image" to="/camera/depth/image_raw"/>
+    <remap from="scan" to="/scan"/>
+  </node>
 </launch>
diff --git a/src/lib/handlers/ROS/ltlmop.material b/src/lib/handlers/ROS/ltlmop.material
old mode 100755
new mode 100644
index d795b2e8..7671e57d
--- a/src/lib/handlers/ROS/ltlmop.material
+++ b/src/lib/handlers/ROS/ltlmop.material
@@ -4,9 +4,14 @@ material Gazebo/LTLMoP
     {
         pass
         {
+            ambient 0.5 0.5 0.5 1.0
+            diffuse 1.0 1.0 1.0 1.0
+            specular 0.0 0.0 0.0 1.0 0.5
+
             texture_unit
             {
                 texture ltlmop_map.png
+                filtering trilinear
             }
         }
     }
diff --git a/src/lib/handlers/ROS/ltlmop_box.world b/src/lib/handlers/ROS/ltlmop_box.world
index 51de151b..589dd4ca 100644
--- a/src/lib/handlers/ROS/ltlmop_box.world
+++ b/src/lib/handlers/ROS/ltlmop_box.world
@@ -1,44 +1,83 @@
-<?xml version ='1.0'?>
-
-<model name="box_model_8" static="1">
-    <link name='body' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 2.25 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='geom' laser_retro='0.000000'>
-        <geometry>
-       <box size="2.08451344688 2.2422551104 4.5"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual1' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <box size="2.08451344688 2.2422551104 4.5"/>
-        </geometry>
-        <material script='Gazebo/WoodPallet'/>
-      </visual>
-      <sensor name='cam' type='depth' always_on='1' update_rate='30.000000' visualize='0'>
-        <origin pose='0.023200 0.060000 0.098000 0.000000 -0.000000 0.000000'/>
-        <camera>
-          <horizontal_fov angle='0.785398'/>
-          <image width='320' height='240' format='L8'/>
-          <clip near='0.100000' far='100.000000'/>
-        </camera>
-      </sensor>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="2.25859798913 -6.12614325553 0 0 0 0 " />
-  </model>
+<model name="box_model_1">
+      <pose>-0.0119447248501 -0.0107272690917 0.5 0 -0.000000 0.000000</pose>
+  <static>1</static>
+  <link name="body">
+      <pose>0.0 0.0 0.0 0 -0.000000 0.000000</pose>
+    <inertial>
+      <mass>1.000000</mass>
+      <inertia>
+        <ixx>1.000000</ixx>
+        <ixy>0.000000</ixy>
+        <ixz>0.000000</ixz>
+        <iyy>1.000000</iyy>
+        <iyz>0.000000</iyz>
+        <izz>1.000000</izz>
+      </inertia>
+    </inertial>
+    <collision name="geom">
+      <laser_retro>0.000000</laser_retro>
+      <geometry>
+       <box><size>3.96523947409 3.8311014001 1.0</size></box>
+      </geometry>
+      <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+      <surface>
+        <bounce>
+          <restitution_coefficient>0.000000</restitution_coefficient>
+          <threshold>100000.000000</threshold>
+        </bounce>
+        <friction>
+          <ode>
+            <mu>-1.000000</mu>
+            <mu2>-1.000000</mu2>
+            <fdir1>0.000000 0.000000 0.000000</fdir1>
+            <slip1>0.000000</slip1>
+            <slip2>0.000000</slip2>
+          </ode>
+        </friction>
+        <contact>
+          <ode>
+            <soft_cfm>0.000000</soft_cfm>
+            <soft_erp>0.200000</soft_erp>
+            <kp>1000000000000.000000</kp>
+            <kd>1.000000</kd>
+            <max_vel>100.000000</max_vel>
+            <min_depth>0.001000</min_depth>
+          </ode>
+        </contact>
+      </surface>
+    </collision>
+    <visual name="visual1">
+      <cast_shadows>1</cast_shadows>
+      <laser_retro>0.000000</laser_retro>
+      <transparency>0.000000</transparency>
+      <geometry>
+       <box><size>3.96523947409 3.8311014001 1.0</size></box>
+      </geometry>
+      <material>
+        <script>Gazebo/WoodPallet</script>
+      </material>
+    </visual>
+    <sensor name="cam" type="depth">
+      <always_on>1</always_on>
+      <update_rate>30.000000</update_rate>
+      <visualize>0</visualize>
+      <pose>0.023200 0.060000 0.098000 0.000000 -0.000000 0.000000</pose>
+      <camera>
+        <horizontal_fov>0.785398</horizontal_fov>
+        <image>
+          <width>320</width>
+          <height>240</height>
+          <format>L8</format>
+        </image>
+        <clip>
+          <near>0.100000</near>
+          <far>100.000000</far>
+        </clip>
+      </camera>
+    </sensor>
+    <velocity_decay>
+      <linear>0.000000</linear>
+      <angular>0.000000</angular>
+    </velocity_decay>
+  </link>
+</model>
diff --git a/src/lib/handlers/ROS/ltlmop_cylinder.world b/src/lib/handlers/ROS/ltlmop_cylinder.world
index 0d8d66ec..6b73efe7 100644
--- a/src/lib/handlers/ROS/ltlmop_cylinder.world
+++ b/src/lib/handlers/ROS/ltlmop_cylinder.world
@@ -1,36 +1,64 @@
-<?xml version ='1.0'?>
-
-<model name="cylinder_14" static="1">
-    <link name='link' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 0.5 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
+<model name="cylinder_14"><static>1</static>
+  <link name="body">
+    <gravity>1</gravity>
+    <self_collide>0</self_collide>
+    <kinematic>0</kinematic>
+    <pose>0.000000 0.000000 0.5 0 -0.000000 0.000000</pose>
+    <inertial>
+      <mass>1.000000</mass>
+      <!-- <density>1.000000</density> -->
+      <inertia>
+        <ixx>1.000000</ixx>
+        <ixy>0.000000</ixy>
+        <ixz>0.000000</ixz>
+        <iyy>1.000000</iyy>
+        <iyz>0.000000</iyz>
+        <izz>1.000000</izz>
+      </inertia>
       </inertial>
-      <collision name='collision' laser_retro='0.000000'>
+      <collision name='collision'>
+        <laser_retro>0.000000</laser_retro>
         <geometry>
-       <cylinder radius="0.528049462913" length="3.0"/>
+          <cylinder><radius>0.528049462913</radius><length>3.0</length></cylinder>
         </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
+        <origin>
+          <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+        </origin>
         <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
+          <bounce>
+            <restitution_coefficient>0.000000</restitution_coefficient>
+            <threshold>100000.000000</threshold>
+          </bounce>
           <friction>
             <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
           </friction>
           <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
+            <ode>
+              <soft_cfm>0.000000</soft_cfm>
+              <soft_erp>0.200000</soft_erp>
+              <kp>1000000000000.000000</kp>
+              <kd>1.000000</kd>
+              <max_vel>100.000000</max_vel>
+              <min_depth>0.001000</min_depth>
+            </ode>
           </contact>
         </surface>
       </collision>
-      <visual name='visual' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
+      <visual name='visual'>
+        <cast_shadows>1</cast_shadows>
+        <laser_retro>0.000000</laser_retro>
+        <transparency>0.000000</transparency>
         <geometry>
-       <cylinder radius="0.528049462913" length="3.0"/>
+          <cylinder><radius>0.528049462913</radius><length>3.0</length></cylinder>
         </geometry>
-        <material script='Gazebo/Grey'/>
+        <material>
+          <script>Gazebo/Grey</script>
+        </material>
       </visual>
-      <damping>
+      <velocity_decay>
         <linear>0.000000</linear>
         <angular>0.000000</angular>
-      </damping>
+      </velocity_decay>
     </link>
-    <origin pose="6.73558649936 6.5990963012 0 0 0 0 " />
-  </model>
+      <pose>6.73558649936 6.5990963012 0 0 0 0 </pose>
+    </model>
diff --git a/src/lib/handlers/ROS/ltlmop_essential_end.world b/src/lib/handlers/ROS/ltlmop_essential_end.world
old mode 100755
new mode 100644
index ffb9cf28..e550716c
--- a/src/lib/handlers/ROS/ltlmop_essential_end.world
+++ b/src/lib/handlers/ROS/ltlmop_essential_end.world
@@ -1,3 +1,3 @@
     </state>
   </world>
-</gazebo>
+</sdf>
diff --git a/src/lib/handlers/ROS/ltlmop_essential_front.world b/src/lib/handlers/ROS/ltlmop_essential_front.world
old mode 100755
new mode 100644
index 6a947913..5d551a01
--- a/src/lib/handlers/ROS/ltlmop_essential_front.world
+++ b/src/lib/handlers/ROS/ltlmop_essential_front.world
@@ -1,53 +1,85 @@
-<?xml version="1.0"?> 
-<gazebo version="1.0">
+<?xml version="1.0" ?>
+<sdf version="1.4">
   <world name="default">
     <scene>
-      <ambient rgba="0.5 0.5 0.5 1"/>
-      <background rgba="0.5 0.5 0.5 1"/>
-      <shadows enabled="false"/>
+      <ambient>0.5 0.5 0.5 1</ambient>
+      <background>0.5 0.5 0.5 1</background>
+      <shadows>false</shadows>
     </scene>
-
-    <physics type="ode" update_rate="1000">
-      <gravity xyz="0 0 -9.8"/>
+    <physics type="ode">
+      <gravity>0 0 -9.8</gravity>
       <ode>
-        <solver type="quick" dt="0.001" iters="10" sor="1.3"/>
-        <constraints cfm="0.0" erp="0.2" contact_max_correcting_vel="100" contact_surface_layer="0.001"/>
+        <solver>
+          <type>quick</type>
+          <iters>10</iters>
+          <sor>1.3</sor>
+        </solver>
+        <constraints>
+          <cfm>0.0</cfm>
+          <erp>0.2</erp>
+          <contact_max_correcting_vel>100</contact_max_correcting_vel>
+          <contact_surface_layer>0.001</contact_surface_layer>
+        </constraints>
       </ode>
+      <real_time_update_rate>1000</real_time_update_rate>
+      <max_step_size>0.001</max_step_size>
     </physics>
-
     <!-- Ground Plane -->
-    <model name="plane1_model" static="true">
+    <model name="plane1_model">
       <link name="body">
         <collision name="geom_1">
           <geometry>
-            <plane normal="0 0 1"/>
+            <plane><size>10 10</size><normal>0 0 1</normal></plane>
           </geometry>
           <surface>
             <friction>
-              <ode mu="10.0" mu2="10.0" fdir1="0 0 0" slip1="0" slip2="0"/>
+              <ode>
+                <mu>10.0</mu>
+                <mu2>10.0</mu2>
+                <fdir1>0 0 0</fdir1>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
             </friction>
-            <bounce restitution_coefficient="0" threshold="1000000.0"/>
+            <bounce>
+              <restitution_coefficient>0</restitution_coefficient>
+              <threshold>1000000.0</threshold>
+            </bounce>
             <contact>
-              <ode soft_cfm="0" soft_erp="0.2" kp="1e10" kd="1" max_vel="100.0" min_depth="0.0001"/>
+              <ode>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <kp>1e10</kp>
+                <kd>1</kd>
+                <max_vel>100.0</max_vel>
+                <min_depth>0.0001</min_depth>
+              </ode>
             </contact>
           </surface>
         </collision>
-
-       <visual name="RegionsMap" cast_shadows="false">
+        <visual name="RegionsMap">
           <geometry>
-            <box size="18.28 12.38 .1" />
+            <plane><size>18.28 12.38</size><normal>0 0 1</normal></plane>
           </geometry>
-          <origin pose="0 0 -.1 0 0 0" /><!--1.5707963" />-->
-          <material script="Gazebo/LTLMoP"/>
+          <pose>0 0 -.1 0 0 0</pose>
+          <material>
+            <script>
+              <uri>DUMMYPATH</uri>
+              <name>Gazebo/LTLMoP</name>
+            </script>
+          </material>
+          <cast_shadows>false</cast_shadows>
         </visual>
       </link>
+      <static>true</static>
     </model>
-
-    <light type="directional" name="my_light" cast_shadows="false">
-      <origin pose="0 0 30 0 0 0"/>
-      <diffuse rgba=".9 .9 .9 1"/>
-      <specular rgba=".1 .1 .1 1"/>
-      <attenuation range="20"/>
-      <direction xyz="0 0 -1"/>
+    <light type="directional" name="my_light">
+      <pose>0 0 30 0 0 0</pose>
+      <diffuse>.9 .9 .9 1</diffuse>
+      <specular>.1 .1 .1 1</specular>
+      <attenuation>
+        <range>20</range>
+      </attenuation>
+      <direction>0 0 -1</direction>
+      <cast_shadows>false</cast_shadows>
     </light>
-
diff --git a/src/lib/handlers/ROS/ltlmop_essential_state.world b/src/lib/handlers/ROS/ltlmop_essential_state.world
old mode 100755
new mode 100644
index afa9385a..ef30128f
--- a/src/lib/handlers/ROS/ltlmop_essential_state.world
+++ b/src/lib/handlers/ROS/ltlmop_essential_state.world
@@ -1,8 +1,8 @@
-<state world_name='default' time='17 182000000'>
-    <model name='plane1_model'>
-      <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
-      <link name='body'>
+    <state world_name="default">
+      <model name="plane1_model">
         <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
+        <link name="body">
+          <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+          <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
+        </link>
+      </model>
\ No newline at end of file
diff --git a/src/lib/handlers/ROS/ltlmop_map.world b/src/lib/handlers/ROS/ltlmop_map.world
deleted file mode 100755
index f62b604e..00000000
--- a/src/lib/handlers/ROS/ltlmop_map.world
+++ /dev/null
@@ -1,781 +0,0 @@
-<?xml version="1.0"?> 
-<gazebo version="1.0">
-  <world name="default">
-    <scene>
-      <ambient rgba="0.5 0.5 0.5 1"/>
-      <background rgba="0.5 0.5 0.5 1"/>
-      <shadows enabled="false"/>
-    </scene>
-
-    <physics type="ode" update_rate="1000">
-      <gravity xyz="0 0 -9.8"/>
-      <ode>
-        <solver type="quick" dt="0.001" iters="10" sor="1.3"/>
-        <constraints cfm="0.0" erp="0.2" contact_max_correcting_vel="100" contact_surface_layer="0.001"/>
-      </ode>
-    </physics>
-
-    <!-- Ground Plane -->
-    <model name="plane1_model" static="true">
-      <link name="body">
-        <collision name="geom_1">
-          <geometry>
-            <plane normal="0 0 1"/>
-          </geometry>
-          <surface>
-            <friction>
-              <ode mu="10.0" mu2="10.0" fdir1="0 0 0" slip1="0" slip2="0"/>
-            </friction>
-            <bounce restitution_coefficient="0" threshold="1000000.0"/>
-            <contact>
-              <ode soft_cfm="0" soft_erp="0.2" kp="1e10" kd="1" max_vel="100.0" min_depth="0.0001"/>
-            </contact>
-          </surface>
-        </collision>
-
-       <visual name="RegionsMap" cast_shadows="false">
-          <geometry>
-            <box size="22.85 15.475 .1" />
-          </geometry>
-          <origin pose="0 0 -.1 0 0 0" /><!--1.5707963" />-->
-          <material script="Gazebo/LTLMoP"/>
-        </visual>
-      </link>
-    </model>
-
-    <light type="directional" name="my_light" cast_shadows="false">
-      <origin pose="0 0 30 0 0 0"/>
-      <diffuse rgba=".9 .9 .9 1"/>
-      <specular rgba=".1 .1 .1 1"/>
-      <attenuation range="20"/>
-      <direction xyz="0 0 -1"/>
-    </light>
-
-<?xml version ='1.0'?>
-
-<model name="box_model_0" static="1">
-    <link name='body' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 3.0 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='geom' laser_retro='0.000000'>
-        <geometry>
-       <box size="2.28716621621 1.47078100649 6.0"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual1' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <box size="2.28716621621 1.47078100649 6.0"/>
-        </geometry>
-        <material script='Gazebo/WoodPallet'/>
-      </visual>
-      <sensor name='cam' type='depth' always_on='1' update_rate='30.000000' visualize='0'>
-        <origin pose='0.023200 0.060000 0.098000 0.000000 -0.000000 0.000000'/>
-        <camera>
-          <horizontal_fov angle='0.785398'/>
-          <image width='320' height='240' format='L8'/>
-          <clip near='0.100000' far='100.000000'/>
-        </camera>
-      </sensor>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="1.65849964648 -2.61616875532 0 0 0 0 " />
-  </model>
-<?xml version ='1.0'?>
-
-<model name="box_model_1" static="1">
-    <link name='body' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 3.0 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='geom' laser_retro='0.000000'>
-        <geometry>
-       <box size="14.0205789069 1.35168289909 6.0"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual1' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <box size="14.0205789069 1.35168289909 6.0"/>
-        </geometry>
-        <material script='Gazebo/WoodPallet'/>
-      </visual>
-      <sensor name='cam' type='depth' always_on='1' update_rate='30.000000' visualize='0'>
-        <origin pose='0.023200 0.060000 0.098000 0.000000 -0.000000 0.000000'/>
-        <camera>
-          <horizontal_fov angle='0.785398'/>
-          <image width='320' height='240' format='L8'/>
-          <clip near='0.100000' far='100.000000'/>
-        </camera>
-      </sensor>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="-0.409779283522 -3.98561934142 0 0 0 0 " />
-  </model>
-<?xml version ='1.0'?>
-
-<model name="box_model_2" static="1">
-    <link name='body' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 3.0 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='geom' laser_retro='0.000000'>
-        <geometry>
-       <box size="3.94154034099 3.07915663532 6.0"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual1' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <box size="3.94154034099 3.07915663532 6.0"/>
-        </geometry>
-        <material script='Gazebo/WoodPallet'/>
-      </visual>
-      <sensor name='cam' type='depth' always_on='1' update_rate='30.000000' visualize='0'>
-        <origin pose='0.023200 0.060000 0.098000 0.000000 -0.000000 0.000000'/>
-        <camera>
-          <horizontal_fov angle='0.785398'/>
-          <image width='320' height='240' format='L8'/>
-          <clip near='0.100000' far='100.000000'/>
-        </camera>
-      </sensor>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="-9.28088428874 -3.13990336064 0 0 0 0 " />
-  </model>
-<?xml version ='1.0'?>
-
-<model name="box_model_3" static="1">
-    <link name='body' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 3.0 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='geom' laser_retro='0.000000'>
-        <geometry>
-       <box size="4.72625593929 1.09021243243 6.0"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual1' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <box size="4.72625593929 1.09021243243 6.0"/>
-        </geometry>
-        <material script='Gazebo/WoodPallet'/>
-      </visual>
-      <sensor name='cam' type='depth' always_on='1' update_rate='30.000000' visualize='0'>
-        <origin pose='0.023200 0.060000 0.098000 0.000000 -0.000000 0.000000'/>
-        <camera>
-          <horizontal_fov angle='0.785398'/>
-          <image width='320' height='240' format='L8'/>
-          <clip near='0.100000' far='100.000000'/>
-        </camera>
-      </sensor>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="-5.46789295333 0.638374339175 0 0 0 0 " />
-  </model>
-<?xml version ='1.0'?>
-
-<model name="box_model_4" static="1">
-    <link name='body' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 3.0 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='geom' laser_retro='0.000000'>
-        <geometry>
-       <box size="1.19397176461 4.69810843733 6.0"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual1' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <box size="1.19397176461 4.69810843733 6.0"/>
-        </geometry>
-        <material script='Gazebo/WoodPallet'/>
-      </visual>
-      <sensor name='cam' type='depth' always_on='1' update_rate='30.000000' visualize='0'>
-        <origin pose='0.023200 0.060000 0.098000 0.000000 -0.000000 0.000000'/>
-        <camera>
-          <horizontal_fov angle='0.785398'/>
-          <image width='320' height='240' format='L8'/>
-          <clip near='0.100000' far='100.000000'/>
-        </camera>
-      </sensor>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="-8.37241009903 2.44128363715 0 0 0 0 " />
-  </model>
-<?xml version ='1.0'?>
-
-<model name="box_model_5" static="1">
-    <link name='body' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 3.0 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='geom' laser_retro='0.000000'>
-        <geometry>
-       <box size="8.82845261847 1.12602490082 6.0"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual1' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <box size="8.82845261847 1.12602490082 6.0"/>
-        </geometry>
-        <material script='Gazebo/WoodPallet'/>
-      </visual>
-      <sensor name='cam' type='depth' always_on='1' update_rate='30.000000' visualize='0'>
-        <origin pose='0.023200 0.060000 0.098000 0.000000 -0.000000 0.000000'/>
-        <camera>
-          <horizontal_fov angle='0.785398'/>
-          <image width='320' height='240' format='L8'/>
-          <clip near='0.100000' far='100.000000'/>
-        </camera>
-      </sensor>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="3.23897459411 0.684788119363 0 0 0 0 " />
-  </model>
-<?xml version ='1.0'?>
-
-<model name="box_model_6" static="1">
-    <link name='body' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 3.0 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='geom' laser_retro='0.000000'>
-        <geometry>
-       <box size="2.01236789411 2.30789788466 6.0"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual1' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <box size="2.01236789411 2.30789788466 6.0"/>
-        </geometry>
-        <material script='Gazebo/WoodPallet'/>
-      </visual>
-      <sensor name='cam' type='depth' always_on='1' update_rate='30.000000' visualize='0'>
-        <origin pose='0.023200 0.060000 0.098000 0.000000 -0.000000 0.000000'/>
-        <camera>
-          <horizontal_fov angle='0.785398'/>
-          <image width='320' height='240' format='L8'/>
-          <clip near='0.100000' far='100.000000'/>
-        </camera>
-      </sensor>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="-2.12334946664 0.0221343694652 0 0 0 0 " />
-  </model>
-<?xml version ='1.0'?>
-
-<model name="box_model_7" static="1">
-    <link name='body' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 2.25 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='geom' laser_retro='0.000000'>
-        <geometry>
-       <box size="2.08451344688 2.2422551104 4.5"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual1' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <box size="2.08451344688 2.2422551104 4.5"/>
-        </geometry>
-        <material script='Gazebo/WoodPallet'/>
-      </visual>
-      <sensor name='cam' type='depth' always_on='1' update_rate='30.000000' visualize='0'>
-        <origin pose='0.023200 0.060000 0.098000 0.000000 -0.000000 0.000000'/>
-        <camera>
-          <horizontal_fov angle='0.785398'/>
-          <image width='320' height='240' format='L8'/>
-          <clip near='0.100000' far='100.000000'/>
-        </camera>
-      </sensor>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="-3.11377826481 -6.14149716563 0 0 0 0 " />
-  </model>
-<?xml version ='1.0'?>
-
-<model name="box_model_8" static="1">
-    <link name='body' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 2.25 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='geom' laser_retro='0.000000'>
-        <geometry>
-       <box size="2.08451344688 2.2422551104 4.5"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual1' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <box size="2.08451344688 2.2422551104 4.5"/>
-        </geometry>
-        <material script='Gazebo/WoodPallet'/>
-      </visual>
-      <sensor name='cam' type='depth' always_on='1' update_rate='30.000000' visualize='0'>
-        <origin pose='0.023200 0.060000 0.098000 0.000000 -0.000000 0.000000'/>
-        <camera>
-          <horizontal_fov angle='0.785398'/>
-          <image width='320' height='240' format='L8'/>
-          <clip near='0.100000' far='100.000000'/>
-        </camera>
-      </sensor>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="2.25859798913 -6.12614325553 0 0 0 0 " />
-  </model>
-<?xml version ='1.0'?>
-
-<model name="cylinder_9" static="1">
-    <link name='link' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 0.5 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='collision' laser_retro='0.000000'>
-        <geometry>
-       <cylinder radius="0.528049462913" length="3.0"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <cylinder radius="0.528049462913" length="3.0"/>
-        </geometry>
-        <material script='Gazebo/Grey'/>
-      </visual>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="9.45844221375 3.36539336654 0 0 0 0 " />
-  </model>
-<?xml version ='1.0'?>
-
-<model name="cylinder_10" static="1">
-    <link name='link' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 0.5 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='collision' laser_retro='0.000000'>
-        <geometry>
-       <cylinder radius="0.528049462913" length="3.0"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <cylinder radius="0.528049462913" length="3.0"/>
-        </geometry>
-        <material script='Gazebo/Grey'/>
-      </visual>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="2.386537473 2.32824812663 0 0 0 0 " />
-  </model>
-<?xml version ='1.0'?>
-
-<model name="cylinder_11" static="1">
-    <link name='link' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 0.5 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='collision' laser_retro='0.000000'>
-        <geometry>
-       <cylinder radius="0.528049462913" length="3.0"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <cylinder radius="0.528049462913" length="3.0"/>
-        </geometry>
-        <material script='Gazebo/Grey'/>
-      </visual>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="4.53909548089 3.91019913599 0 0 0 0 " />
-  </model>
-<?xml version ='1.0'?>
-
-<model name="cylinder_12" static="1">
-    <link name='link' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 0.5 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='collision' laser_retro='0.000000'>
-        <geometry>
-       <cylinder radius="0.528049462913" length="3.0"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <cylinder radius="0.528049462913" length="3.0"/>
-        </geometry>
-        <material script='Gazebo/Grey'/>
-      </visual>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="2.37962730467 6.58977228197 0 0 0 0 " />
-  </model>
-<?xml version ='1.0'?>
-
-<model name="cylinder_13" static="1">
-    <link name='link' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 0.5 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='collision' laser_retro='0.000000'>
-        <geometry>
-       <cylinder radius="0.528049462913" length="3.0"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <cylinder radius="0.528049462913" length="3.0"/>
-        </geometry>
-        <material script='Gazebo/Grey'/>
-      </visual>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="9.47234001915 5.38063452951 0 0 0 0 " />
-  </model>
-<?xml version ='1.0'?>
-
-<model name="cylinder_14" static="1">
-    <link name='link' gravity='1' self_collide='0' kinematic='0'>
-      <origin pose="0.000000 0.000000 0.5 0 -0.000000 0.000000"/>
-      <inertial mass='1.000000' density='1.000000'>
-        <inertia ixx='1.000000' ixy='0.000000' ixz='0.000000' iyy='1.000000' iyz='0.000000' izz='1.000000'/>
-      </inertial>
-      <collision name='collision' laser_retro='0.000000'>
-        <geometry>
-       <cylinder radius="0.528049462913" length="3.0"/>
-        </geometry>
-        <origin pose='0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000'/>
-        <surface>
-          <bounce restitution_coefficient='0.000000' threshold='100000.000000'/>
-          <friction>
-            <ode mu='-1.000000' mu2='-1.000000' fdir1='0.000000 0.000000 0.000000' slip1='0.000000' slip2='0.000000'/>
-          </friction>
-          <contact>
-            <ode soft_cfm='0.000000' soft_erp='0.200000' kp='1000000000000.000000' kd='1.000000' max_vel='100.000000' min_depth='0.001000'/>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='visual' cast_shadows='1' laser_retro='0.000000' transparency='0.000000'>
-        <geometry>
-       <cylinder radius="0.528049462913" length="3.0"/>
-        </geometry>
-        <material script='Gazebo/Grey'/>
-      </visual>
-      <damping>
-        <linear>0.000000</linear>
-        <angular>0.000000</angular>
-      </damping>
-    </link>
-    <origin pose="6.73558649936 6.5990963012 0 0 0 0 " />
-  </model>
-<state world_name='default' time='17 182000000'>
-    <model name='plane1_model'>
-      <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
-      <link name='body'>
-        <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_0">
-      <pose>1.65849964648 -2.61616875532 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_1">
-      <pose>-0.409779283522 -3.98561934142 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_2">
-      <pose>-9.28088428874 -3.13990336064 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_3">
-      <pose>-5.46789295333 0.638374339175 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_4">
-      <pose>-8.37241009903 2.44128363715 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_5">
-      <pose>3.23897459411 0.684788119363 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_6">
-      <pose>-2.12334946664 0.0221343694652 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_7">
-      <pose>-3.11377826481 -6.14149716563 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_8">
-      <pose>2.25859798913 -6.12614325553 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="cylinder_9">
-      <pose>9.45844221375 3.36539336654 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 1.5 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="cylinder_10">
-      <pose>2.386537473 2.32824812663 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 1.5 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="cylinder_11">
-      <pose>4.53909548089 3.91019913599 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 1.5 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="cylinder_12">
-      <pose>2.37962730467 6.58977228197 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 1.5 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="cylinder_13">
-      <pose>9.47234001915 5.38063452951 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 1.5 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="cylinder_14">
-      <pose>6.73558649936 6.5990963012 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 1.5 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    </state>
-  </world>
-</gazebo>
diff --git a/src/lib/handlers/ROS/ltlmop_state.world b/src/lib/handlers/ROS/ltlmop_state.world
old mode 100755
new mode 100644
index b8569777..68bb8a30
--- a/src/lib/handlers/ROS/ltlmop_state.world
+++ b/src/lib/handlers/ROS/ltlmop_state.world
@@ -1,113 +1,23 @@
-<state world_name='default' time='17 182000000'>
-    <model name='plane1_model'>
-      <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
-      <link name='body'>
+    <state world_name="default">
+      <model name="plane1_model">
         <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_0">
-      <pose>1.65849964648 -2.61616875532 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_1">
-      <pose>-0.409779283522 -3.98561934142 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_2">
-      <pose>-9.28088428874 -3.13990336064 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_3">
-      <pose>-5.46789295333 0.638374339175 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_4">
-      <pose>-8.37241009903 2.44128363715 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_5">
-      <pose>3.23897459411 0.684788119363 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_6">
-      <pose>-2.12334946664 0.0221343694652 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_7">
-      <pose>-3.11377826481 -6.14149716563 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="box_model_8">
-      <pose>2.25859798913 -6.12614325553 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 0 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="cylinder_9">
-      <pose>9.45844221375 3.36539336654 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 1.5 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="cylinder_10">
-      <pose>2.386537473 2.32824812663 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 1.5 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="cylinder_11">
-      <pose>4.53909548089 3.91019913599 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 1.5 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="cylinder_12">
-      <pose>2.37962730467 6.58977228197 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 1.5 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="cylinder_13">
-      <pose>9.47234001915 5.38063452951 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 1.5 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
-    <model name="cylinder_14">
-      <pose>6.73558649936 6.5990963012 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 1.5 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
+        <link name="body">
+          <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+          <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
+        </link>
+      </model><model name="box_model_1">
+  <pose>0.0 0.0 0 0 0 0 </pose>
+  <link name="body">
+      <pose>0 0 0 0 0 0 </pose>
+    <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
+  </link>
+</model>
+
+<model name="box_model_1">
+  <pose>0.0 0.0 0 0 0 0 </pose>
+  <link name="body">
+      <pose>0 0 0 0 0 0 </pose>
+    <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
+  </link>
+</model>
+
diff --git a/src/lib/handlers/ROS/ltlmop_state_cylinder.world b/src/lib/handlers/ROS/ltlmop_state_cylinder.world
old mode 100755
new mode 100644
index 9e3624ac..a8a94b6d
--- a/src/lib/handlers/ROS/ltlmop_state_cylinder.world
+++ b/src/lib/handlers/ROS/ltlmop_state_cylinder.world
@@ -1,7 +1,8 @@
-    <model name="cylinder_14">
-      <pose>6.73558649936 6.5990963012 0 0 0 0 </pose>
-      <link name='link'>
-      <pose>0 0 1.5 0 0 0 </pose>
-        <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
-      </link>
-    </model>
+<model name="box_model_1">
+  <pose>0.0 0.0 0 0 0 0 </pose>
+  <link name="body">
+      <pose>0 0 0 0 0 0 </pose>
+    <velocity>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</velocity>
+  </link>
+</model>
+
diff --git a/src/lib/handlers/ROS/ros.robot b/src/lib/handlers/ROS/ros.robot
index 0d32bbd1..ae55ba8d 100644
--- a/src/lib/handlers/ROS/ros.robot
+++ b/src/lib/handlers/ROS/ros.robot
@@ -5,22 +5,22 @@ Type: # Robot type
 ROS
 
 InitHandler: # Robot default init handler with default argument values
-ROS.RosInitHandler(worldFile="ltlmop_map.world", robotPixelWidth=200, robotPhysicalWidth=.5, package="pr2_gazebo", launchFile="pr2.launch")
+ROS.RosInitHandler(worldFile="ltlmop_map.world", robotPixelWidth=10, robotPhysicalWidth=.4, robotPackage="turtlebot_bringup", robotLaunchFile="minimal.launch")
 
 PoseHandler: # Robot default pose handler with default argument values
-ROS.RosPoseHandler(modelName="pr2")
+ROS.RosPoseHandler(modelName="mobile_base")
 
 SensorHandler: # Robot default sensors handler with default argument values
 ROS.RosSensorHandler()
 
-ActuatorHandler: # Robot default actuator handler wit hdefault argument values
-ROS.RosActuatorHandler()
+#ActuatorHandler: # Robot default actuator handler wit hdefault argument values
+#ROS.RosActuatorHandler()
 
 MotionControlHandler: # Robot default motion control handler with default argument values
-share.MotionControl.HeatControllerHandler()
+share.MotionControl.VectorControllerHandler()
 
-DriveHandler: # Robot default drive handler with deafult argument values
+DriveHandler: # Robot default drive handler with default argument values
 ROS.RosDriveHandler(d=.3)
 
 LocomotionCommandHandler: # Robot default locomotion command handler with default argument values
-ROS.RosLocomotionCommandHandler(velocityTopic='/base_controller/command')
+ROS.RosLocomotionCommandHandler(velocityTopic='/cmd_vel_mux/input/navi')
diff --git a/src/lib/handlers/ROS/spawn_quadrotor.launch b/src/lib/handlers/ROS/spawn_quadrotor.launch
deleted file mode 100755
index ef0af80b..00000000
--- a/src/lib/handlers/ROS/spawn_quadrotor.launch
+++ /dev/null
@@ -1,31 +0,0 @@
-<?xml version="1.0"?>
-
-
- <launch>
-   <arg name="model" default="$(find hector_quadrotor_urdf)/urdf/quadrotor.urdf.xacro"/>
-   <!-- send the robot XML to param server -->
-   <param name="robot_description" command="$(find xacro)/xacro.py '$(arg model)'" />
-   
-   <!-- push robot_description to factory and spawn robot in gazebo -->
-   <node name="spawn_robot" pkg="gazebo" type="spawn_model"
-   args="$(optenv ROBOT_INITIAL_POSE) -param robot_description
-   -urdf
-   -z 0.5
-   -model quadrotor"
-     respawn="false" output="screen"/>
-     
-   <!-- start robot state publisher -->
-   <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" output="screen" >
-     <param name="publish_frequency" type="double" value="50.0" />
-     <param name="tf_prefix" type="string" value="" />
-   </node>
-
-   <node name="ground_truth_to_tf" pkg="message_to_tf" type="message_to_tf" output="screen">
-     <param name="odometry_topic" value="ground_truth/state" />
-     <param name="frame_id" value="nav" />
-   </node>
-   <!-- node name="hector_pose_estimation" pkg="hector_pose_estimation" type="hector_pose_estimation_node" output="screen"/-->
-
-   <rosparam command="load" ns="gazebo" file="$(find hector_quadrotor_controller)/param/quadrotor_aerodynamics.yaml" />
- </launch>
-