diff --git a/cob_bringup_sim/config/benchmark_config.yaml b/cob_bringup_sim/config/benchmark_config.yaml
new file mode 100644
index 00000000..85c801e2
--- /dev/null
+++ b/cob_bringup_sim/config/benchmark_config.yaml
@@ -0,0 +1,232 @@
+# The inflation radius of the paths [float]
+inflation_radius: 1.0
+# The number of rooms [int]
+room_number: 4
+# The maximum number of objects to be spawned [float]
+max_num_objects: 10
+# The rate at which objects are spawned initially in hz [float]
+spawn_object_rate: 1
+# The rate at which objects are moved during runtime in hz [float]
+move_object_rate: 1
+# The number of objects that is supposed to be changed per time-step [int]
+num_changed_objects: 20
+# Specifies in which rooms moving objects will be spawned [list, int]
+spawn_moving_objects: [0, 1, 2, 3]
+
+# The paths to the urdf files for the models
+models:
+  box:
+    package: cob_gazebo_objects
+    path: objects/box.urdf
+  column:
+    package: cob_gazebo_objects
+    path: objects/column.urdf
+
+# The boundaries of each room
+boundaries:
+  - room: 0
+    start_x: -19.95
+    start_y: -10.95
+    end_x: 1.95
+    end_y: 12.95
+  - room: 1
+    start_x: 2.05
+    start_y: -10.95
+    end_x: 27.45
+    end_y: 12.95
+  - room: 2
+    start_x: 2.05
+    start_y: -34.95
+    end_x: 27.45
+    end_y: -11.05
+  - room: 3
+    start_x: -19.95
+    start_y: -34.95
+    end_x: 1.95
+    end_y: -11.05
+
+# The dynamic objects and their respective paths
+moving_objects:
+  - room_id: 0
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[-13.5, 4.5], [-4.5, 4.5], [-4.5, -4.5], [-13.5, -4.5], [-13.5, 4.5]]
+  - room_id: 0
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[-4.5, 4.5], [-4.5, -4.5], [-13.5, -4.5], [-13.5, 4.5], [-13.5, 4.5]]
+  - room_id: 0
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[-4.5, -4.5], [-13.5, -4.5], [-13.5, 4.5], [-13.5, 4.5], [-4.5, 4.5]]
+  - room_id: 0
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[-13.5, -4.5], [-13.5, 4.5], [-13.5, 4.5], [-4.5, 4.5], [-4.5, -4.5]]
+  - room_id: 1
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[8.5, 4.5], [21.0, 4.5], [21.0, 0.0], [8.5, 0.0], [8.5, 4.5]]
+  - room_id: 1
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[21.0, 4.5], [21.0, -4.5], [8.5, -4.5], [8.5, 4.5], [21.0, 4.5]]
+  - room_id: 1
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[21.0, -4.5], [8.5, -4.5], [8.5, 4.5], [21.0, 4.5], [21.0, 4.5]]
+  - room_id: 1
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[8.5, -4.5], [8.5, 4.5], [21.0, 4.5], [21.0, 4.5], [21.0, -4.5]]
+  - room_id: 2
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[21.0, -28.5], [8.5, -28.5], [8.5, -17.5], [21.0, -17.5], [21.0, -28.5]]
+  - room_id: 2
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[8.5, -28.5], [8.5, -17.5], [21.0, -17.5], [21.0, -28.5], [21.0, -28.5]]
+  - room_id: 2
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[8.5, -17.5], [21.0, -17.5], [21.0, -28.5], [21.0, -28.5], [8.5, -28.5]]
+  - room_id: 2
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[21.0, -17.5], [21.0, -28.5], [21.0, -28.5], [8.5, -28.5], [8.5, -17.5]]
+  - room_id: 3
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[-4.5, -17.5], [-4.5, -28.5], [-13.5, -28.5], [-13.5, -17.5], [-4.5, -17.5]]
+  - room_id: 3
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[-4.5, -28.5], [-13.5, -28.5], [-13.5, -17.5], [-4.5, -17.5],[-4.5, -17.5]]
+  - room_id: 3
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[-13.5, -28.5], [-13.5, -17.5], [-4.5, -17.5],[-4.5, -17.5], [-4.5, -28.5]]
+  - room_id: 3
+    type: column
+    velocity: 0.8
+    path_type: polygon
+    path: [[-13.5, -17.5], [-4.5, -17.5],[-4.5, -17.5], [-4.5, -28.5], [-13.5, -28.5]]
+
+# The paths the robot will travel on sorted by room
+paths:
+  room_0:
+    - start_x: -9.00
+      start_y: -14.50
+      end_x: -9.00
+      end_y: -7.50
+    - start_x: -16.5
+      start_y: -7.50
+      end_x: -1.50
+      end_y: -7.50
+    - start_x: -16.5
+      start_y: -7.5
+      end_x: -16.5
+      end_y: 7.5
+    - start_x: -16.5
+      start_y: 7.5
+      end_x: -1.5
+      end_y: 7.5
+    - start_x: -1.5
+      start_y: -7.5
+      end_x: -1.5
+      end_y: 7.5
+    - start_x: -1.5
+      start_y: 7.5
+      end_x: 5.5
+      end_y: 7.5
+  room_1:
+    - start_x: -1.5
+      start_y: 7.5
+      end_x: 5.5
+      end_y: 7.5
+    - start_x: 5.50
+      start_y: 7.50
+      end_x: 24.00
+      end_y: 7.50
+    - start_x: 24.00
+      start_y: -7.5
+      end_x: 24.00
+      end_y: 7.5
+    - start_x: 5.50
+      start_y: -7.50
+      end_x: 24.00
+      end_y: -7.50
+    - start_x: 5.50
+      start_y: -7.50
+      end_x: 5.50
+      end_y: 7.50
+    - start_x: 14.75
+      start_y: -14.50
+      end_x: 14.75
+      end_y: -7.50
+  room_2:
+    - start_x: 14.75
+      start_y: -14.50
+      end_x: 14.75
+      end_y: -7.50
+    - start_x: 5.50
+      start_y: -14.50
+      end_x: 24.00
+      end_y: -14.50
+    - start_x: 24.00
+      start_y: -31.50
+      end_x: 24.00
+      end_y: -14.50
+    - start_x: 5.50
+      start_y: -31.50
+      end_x: 24.00
+      end_y: -31.50
+    - start_x: 5.50
+      start_y: -31.50
+      end_x: 5.50
+      end_y: -14.50
+    - start_x: -1.50
+      start_y: -23.00
+      end_x: 5.50
+      end_y: -23.00
+  room_3:
+    - start_x: -1.50
+      start_y: -23.00
+      end_x: 5.50
+      end_y: -23.00
+    - start_x: -1.50
+      start_y: -31.50
+      end_x: -1.50
+      end_y: -14.50
+    - start_x: -16.50
+      start_y: -31.50
+      end_x: -1.50
+      end_y: -31.50
+    - start_x: -16.50
+      start_y: -31.50
+      end_x: -16.50
+      end_y: -14.50
+    - start_x: -16.50
+      start_y: -14.50
+      end_x: -1.50
+      end_y: -14.50
+    - start_x: -9.00
+      start_y: -14.50
+      end_x: -9.00
+      end_y: -7.50
\ No newline at end of file
diff --git a/cob_bringup_sim/launch/robot.launch b/cob_bringup_sim/launch/robot.launch
index 2650224a..db182a16 100644
--- a/cob_bringup_sim/launch/robot.launch
+++ b/cob_bringup_sim/launch/robot.launch
@@ -9,6 +9,7 @@
 	<arg name="pkg_gazebo_worlds" default="$(find cob_gazebo_worlds)"/>
 	<arg name="world_name" default="worlds/empty.world"/>
 	<arg name="initial_config" default=""/>
+	<arg name="multi_robot" default="false"/>
 
 	<arg name="paused" default="false"/>
 	<arg name="use_sim_time" default="true"/>
@@ -26,12 +27,13 @@
 	</include>
 
 	<!-- startup simulated robot -->
-	<include file="$(find cob_gazebo)/launch/robot.launch" >
+	<include unless="$(arg multi_robot)" file="$(find cob_gazebo)/launch/robot.launch" >
 		<arg name="robot" value="$(arg robot)" />
 		<arg name="robot_env" value="$(arg robot_env)" />
 		<arg name="pkg_robot_config" value="$(arg pkg_robot_config)" />
 		<arg name="paused" value="$(arg paused)" />
 		<arg name="initial_config" value="$(arg initial_config)"/>
+		<arg name="multi_robot" value="$(arg multi_robot)"/>
 	</include>
 
 </launch>
diff --git a/cob_bringup_sim/scripts/benchmark_tool.py b/cob_bringup_sim/scripts/benchmark_tool.py
new file mode 100755
index 00000000..7a2aa044
--- /dev/null
+++ b/cob_bringup_sim/scripts/benchmark_tool.py
@@ -0,0 +1,530 @@
+#!/usr/bin/env python
+
+import rospy
+import rospkg
+from gazebo_ros.gazebo_interface import DeleteModelRequest, SpawnModelRequest, SpawnModel, DeleteModel, SetModelState, SetModelStateRequest
+
+from ipa_navigation_utils import log
+
+import sys
+import yaml
+import numpy as np
+import subprocess
+
+ros_packages = rospkg.RosPack()
+object_path = ros_packages.get_path('cob_gazebo_objects') + '/objects/'
+
+### PARAMETERS
+CONFIG_FILE = ros_packages.get_path('cob_bringup_sim') + '/config/benchmark_config.yaml'
+RADIUS = 0.71
+
+### GLOBAL VARIABLES
+objects_ = {}
+object_ids_ = {}
+moving_objects_ = []
+
+log.level = log.DEBUG
+
+
+class CONFIG:
+    """
+    Contains the configuration parameters for the benchmark-tool node.
+    """
+    inflation_radius = None
+    rooms = None
+    room_number = None
+    max_num_objects = None
+    move_object_rate = None
+    num_changed_objects = None
+    spawn_moving_objects = None
+    spawn_object_rate = None
+
+    models = None
+    room_boundaries = None
+    moving_objects = None
+    paths = None
+
+    @classmethod
+    def load(cls, file_path):
+        """
+        Loads all config parameters from the specified yaml-file.
+        :param file_path: The yaml-file containing the config.
+        :type file_path: str
+        :return:
+        :rtype: None
+        """
+        with open(file_path) as f:
+            data = yaml.safe_load(f)
+
+        cls.inflation_radius = data['inflation_radius']
+        cls.rooms = data['rooms']
+        cls.room_number = len(cls.rooms)
+        cls.max_num_objects = data['max_num_objects']
+        cls.move_object_rate = data['move_object_rate']
+        cls.num_changed_objects = data['num_changed_objects']
+        cls.spawn_moving_objects = data['spawn_moving_objects']
+        cls.spawn_object_rate = data['spawn_object_rate']
+
+        cls.models = data['models']
+        cls.room_boundaries = data['boundaries']
+        cls.moving_objects = data['moving_objects']
+        cls.paths = data['paths']
+
+        for model in cls.models.keys():
+            cls.models[model]['path'] = ros_packages.get_path(cls.models[model]['package']) + '/' + \
+                                        cls.models[model]['path']
+
+        for room in cls.paths:
+            for path in cls.paths[room]:
+                path['start_x'] -= cls.inflation_radius
+                path['start_y'] -= cls.inflation_radius
+                path['end_x'] += cls.inflation_radius
+                path['end_y'] += cls.inflation_radius
+
+    @classmethod
+    def set_inflation_radius(cls, inflation_radius):
+        """
+        Sets the inflation radius to the new value and updates all paths.
+        :param inflation_radius: The new inflation radius value.
+        :type inflation_radius: float
+        :return:
+        :rtype: None
+        """
+        diff = inflation_radius - cls.inflation_radius
+
+        for room in cls.paths:
+            for path in room[room.keys()[0]]:
+                path['start_x'] -= diff
+                path['start_y'] -= diff
+                path['end_x'] += diff
+                path['end_y'] += diff
+
+        cls.inflation_radius = inflation_radius
+
+
+class Object:
+    def __init__(self, x, y, room_id, radius=0.5):
+        self.pos = np.array([x, y])
+        self.room_id = room_id
+        self.radius = radius
+
+
+def spawn_moving_objects():
+    global moving_objects_
+
+    log.debug('Spawning moving objects')
+
+    moving_obj_id = 0
+    for moving_obj in CONFIG.moving_objects:
+        if moving_obj['room_id'] not in CONFIG.spawn_moving_objects:
+            continue
+
+        # Spawn model in Gazebo
+        model_name = 'moving_' + moving_obj['type'] + '_' + str(moving_obj_id)
+        obj_pos = np.array(moving_obj['path'][0])
+        spawn_model(model_name, CONFIG.models[moving_obj['type']]['path'], obj_pos)
+
+        # Run driver to move object around
+        new_obj_proc = subprocess.Popen(['rosrun', 'cob_bringup_sim', 'move_object.py',
+                                         '--mode='+moving_obj['path_type'],
+                                         '--name='+model_name,
+                                         '--velocity='+str(moving_obj['velocity']),
+                                         '--polygon='+str(moving_obj['path'])])
+        moving_objects_.append([model_name, new_obj_proc])
+
+        moving_obj_id += 1
+
+    log.info('Successfully spawned moving objects')
+
+
+def spawn_object_loop(radius, type=None):
+    """
+    Loops until node terminates and spawns objects dynamically at the given rate.
+    :param radius: The radius of the objects to spawn.
+    :type radius: float
+    :param type: The type of the objects to spawn.
+    :type type: str
+    :return:
+    :rtype: None
+    """
+    while not rospy.is_shutdown():
+        for i in range(CONFIG.num_changed_objects):
+            log.debug('Changing another spawned object')
+            # Select randomly a spawned object to delete
+            spawned_object_ids = np.where(object_ids_[type]>0)[0]
+            if spawned_object_ids.shape[0] <= 0:
+                log.error('No objects spawned yet, shutting down now')
+                return
+            obj_id = np.random.choice(spawned_object_ids, 1)[0]
+            model_name = type + '_' + str(obj_id)
+
+            # # Delete selected model
+            # delete_model(del_model_name)
+            #
+            # # Spawn a new object
+            # spawn_object(type, np.random.randint(CONFIG.room_number), radius)
+
+            room_id = CONFIG.rooms[np.random.randint(CONFIG.room_number)]
+            obj_pos = get_new_coordinates(room_id, radius)
+            if len(obj_pos) <= 0:
+                continue
+
+            move_model(model_name, obj_pos)
+
+            objects_[type][obj_id] = np.copy(obj_pos)
+
+        rospy.sleep(CONFIG.move_object_rate)
+
+
+def get_new_coordinates(room_id, radius, threshold=1000):
+    coord_check = False
+    threshold = 100
+    count = 0
+
+    while not coord_check:
+        obj_pos = generate_coordinates(room_id, radius)
+
+        coord_check = check_coordinates(obj_pos, room_id, radius)
+
+        count += 1
+        if count >= threshold:
+            log.warn('Could not find a valid position for model with type \'{}\' in room {}'.format(type, room_id))
+            log.warn('Aborting now')
+            return []
+
+    return obj_pos
+
+
+def spawn_object(type, room_id, radius, name=None):
+    """
+    Spawns an object with the given type in the specified room.
+    :param type: The type of the the object to spawn.
+    :type type: str
+    :param room_id: The room-id for the room the object should be spawned in.
+    :type room_id: int
+    :param radius: The radius of the object to spawn.
+    :type radius: float
+    :return:
+    :rtype: None
+    """
+    start_time = rospy.get_rostime()
+
+    global objects_
+
+    obj_pos = get_new_coordinates(room_id, radius)
+    if len(obj_pos) <= 0:
+        return
+
+    obj_id = np.argmax(object_ids_[type]==0)
+    model_name = type + '_' + str(obj_id)
+
+    # Set used object id to true (1)
+    object_ids_[type][obj_id] = 1
+
+    # Set coordinates for object
+    objects_[type][obj_id] = obj_pos
+
+    spawn_model(model_name, CONFIG.models[type]['path'], obj_pos)
+
+    # Control spawn rate
+    dur = rospy.get_rostime() - start_time
+    delta = rospy.Duration.from_sec(1/CONFIG.spawn_object_rate) - dur
+    if delta.to_sec() > 0:
+        rospy.sleep(delta)
+
+
+def check_coordinates(new_obj_pos, room_id, radius):
+    """
+    Performs a check on new object coordinates. The function verifies, that the position lies not within the boundaries
+    of another object or a robot path.
+    :param new_obj_pos: The position (x, y) of the new object.
+    :type new_obj_pos: ndarray
+    :param room_id: The id of the room, where the object is supposed to be spawned.
+    :type room_id: int
+    :param radius: The maximum radius of the object.
+    :type radius: float
+    :return: True if the object is collision-free, False otherwise.
+    :rtype: bool
+    """
+    # Check all objects within the room
+    for type in objects_:
+        for obj_pos in objects_[type]:
+            dist = np.linalg.norm(new_obj_pos - obj_pos)
+            if dist <= radius:
+                log.info('New Object didn\'t pass the object location check [room={}, pos=({}, {}), radius={}]'.format(
+                    room_id, new_obj_pos[0], new_obj_pos[1], radius))
+                return False
+
+    # Check for all paths in the room
+    for path in CONFIG.paths['room_'+str(room_id)]:
+        in_x = path['start_x']-radius <= new_obj_pos[0] <= path['end_x']+radius
+        in_y = path['start_y']-radius <= new_obj_pos[1] <= path['end_y']+radius
+
+        if in_x and in_y:
+            log.debug('New Object didn\'t pass the robot path check [room={}, pos=({}, {}), radius={}]'.format(
+                room_id, new_obj_pos[0], new_obj_pos[1], radius))
+            return False
+
+    # Check for moving obj paths
+    for moving_obj in CONFIG.moving_objects:
+        if moving_obj['room_id'] not in CONFIG.spawn_moving_objects:
+            continue
+
+        for i, start in enumerate(moving_obj['path'][:-1]):
+            end = moving_obj['path'][i+1]
+            if start[0] <= end[0]:
+                start_x = start[0]
+                end_x = end[0]
+            else:
+                start_x = end[0]
+                end_x = start[0]
+            if start[1] <= end[1]:
+                start_y = start[1]
+                end_y = end[1]
+            else:
+                start_y = end[1]
+                end_y = start[1]
+
+            in_x = start_x - radius*2 <= new_obj_pos[0] <= end_x + radius*2
+            in_y = start_y - radius*2 <= new_obj_pos[1] <= end_y + radius*2
+
+            if in_x and in_y:
+                log.debug('New Object didn\'t pass the moving object path check [room={}, pos=({}, {}), radius={}]'.format(
+                    room_id, new_obj_pos[0], new_obj_pos[1], radius))
+                return False
+
+    log.debug('New Object passed the check [room={}, pos=({}, {}), radius={}]'.format(
+        room_id, new_obj_pos[0], new_obj_pos[1], radius))
+
+    return True
+
+
+def generate_coordinates(room_id, radius):
+    """
+    Randomly generates a set of coordinates (x, y) that lie within the given rooms boundaries.
+    :param room_id: The id of the room where the coordinates are supposed to be in.
+    :type room_id: int
+    :param radius: The radius of the object, the coordinates are generated for.
+    :type radius: float
+    :return: The new coordinates (x, y).
+    :rtype: ndarray
+    """
+    x = np.random.uniform(CONFIG.room_boundaries[room_id]['start_x']+radius,
+                          CONFIG.room_boundaries[room_id]['end_x']-radius)
+    y = np.random.uniform(CONFIG.room_boundaries[room_id]['start_y']+radius,
+                          CONFIG.room_boundaries[room_id]['end_y']-radius)
+
+    return np.array([x, y])
+
+
+def spawn_init_objects(type, radius):
+    """
+    Spawns initially the number of objects specified in the config.
+    :param type: The type of the object to be spawned.
+    :type type: str
+    :param radius: The maximum radius of the object to be spawned.
+    :type radius: float
+    :return:
+    :rtype: None
+    """
+    log.debug('Spawning {} initial objects'.format(CONFIG.max_num_objects))
+    for i in range(CONFIG.max_num_objects):
+        spawn_object(type, CONFIG.rooms[np.random.randint(CONFIG.room_number)], radius)
+
+    log.info('Spawned {} initial objects'.format(CONFIG.max_num_objects))
+
+
+def delete_all_objects():
+    """
+    Deletes all objects that were created from this node.
+    :return:
+    :rtype: None
+    """
+    global moving_objects_
+
+    log.debug('Deleting all dynamic objects')
+    for dyn_obj in moving_objects_:
+        delete_model(dyn_obj[0])
+
+    log.info('Successfully deleted all dynamic objects')
+
+    log.debug('Deleting all static objects')
+    for obj_type in object_ids_:
+        for i in range(len(object_ids_[obj_type])):
+            if object_ids_[obj_type][i] > 0:
+                delete_model(obj_type + '_' + str(i))
+
+    log.info('Successfully deleted all static objects')
+
+
+def move_model(model_name, new_pos):
+    log.debug('Waiting for service: /gazebo/set_model_state')
+    rospy.wait_for_service('/gazebo/set_model_state', timeout=2.0)
+    try:
+        log.debug('Trying to move model \'{}\' to position {}'.format(model_name, new_pos))
+        req = SetModelStateRequest()
+        req.model_state.model_name = model_name
+        req.model_state.pose.position.x = new_pos[0]
+        req.model_state.pose.position.y = new_pos[1]
+        srv_mov_scanner = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
+        ret = srv_mov_scanner(req)
+        if not ret.success:
+            log.warn('Could not move model \'{}\' because an error occured during service call'.format(model_name))
+            return
+    except rospy.ServiceException, e:
+        log.warn("Service call failed: %s" % e)
+        return
+
+    log.info('Moved model \'{}\' to new position at [{:.2f}, {:.2f}]'.format(model_name, new_pos[0], new_pos[1]))
+
+    return
+
+
+def delete_model(model_name):
+    """
+    Deletes the given model from the current gazebo simulation.
+    :param model_name: The name of the model to be removed.
+    :type model_name: str
+    :return:
+    :rtype: None
+    """
+    log.debug('Waiting for service: /gazebo/delete_model')
+    rospy.wait_for_service('/gazebo/delete_model', timeout=2.0)
+    try:
+        log.debug('Trying to delete model: {}'.format(model_name))
+        req = DeleteModelRequest()
+        req.model_name = str(model_name)
+        srv_del_scanner = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
+        ret = srv_del_scanner(req)
+        if not ret.success:
+            log.warn('Could not delete model \'{}\' because an error occured during service call'.format(model_name))
+            return
+    except rospy.ServiceException, e:
+        log.warn("Service call failed: %s" % e)
+        return
+
+    log.info('Deleted existing model \'{}\''.format(model_name))
+
+    # Reset object id to 0
+    obj_id = int(model_name.split('_')[-1])
+    obj_name = model_name.split('_')[0]
+
+    if not 'moving' in model_name:
+        object_ids_[obj_name][obj_id] = 0
+
+    return
+
+
+def spawn_model(model_name, urdf_path, position=None):
+    """
+    Spawns the specified model in the current gazebo simulation.
+    :param model_name: The name of the model to be spawned.
+    :type model_name: str
+    :param urdf_path: The path of the model to be spawned.
+    :type urdf_path: str
+    :param position: The position (x, y) of the model to be spawned.
+    :type position: ndarray
+    :return:
+    :rtype: None
+    """
+    log.info('Spawning new model \'{}\' at position [{}, {}]'.format(model_name, position[0], position[1]))
+    rospy.wait_for_service('/gazebo/spawn_urdf_model', timeout=2.0)
+    try:
+        req = SpawnModelRequest()
+        req.model_name = model_name
+
+        # Load Model XML
+        f = open(urdf_path, 'r')
+        req.model_xml = f.read()
+        if req.model_xml == "":
+            rospy.logerr("Error: file is empty %s", urdf_path)
+            sys.exit(0)
+
+        if position is not None:
+            req.initial_pose.position.x = position[0]
+            req.initial_pose.position.y = position[1]
+
+        srv_del_scanner = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
+        ret = srv_del_scanner(req)
+        if ret.success:
+            log.info('Successfully spawned model \'{}\'.'.format(model_name))
+        else:
+            log.warn('Could not spawn model \'{}\'.'.format(model_name))
+    except rospy.ServiceException, e:
+        log.warn("Service call failed: %s" % e)
+
+
+def shutdown_moving_objects():
+    global moving_objects_
+
+    log.debug('Shutting down drivers for moving objects')
+
+    for moving_obj in moving_objects_:
+        moving_obj[1].terminate()
+        moving_obj[1].wait()
+        log.info('Terminated driver for moving object: '+moving_obj[0])
+
+    log.info('Successfully shut down all drivers for moving objects')
+
+
+def on_shutdown():
+    log.info('Shutting down now')
+
+    shutdown_moving_objects()
+
+    delete_all_objects()
+
+    log.info('Shutdown complete')
+
+
+def init_ros():
+    """
+    Initializes ros with subscribers, publishers, etc.
+    :return:
+    :rtype: None
+    """
+    rospy.init_node('benchmark_tool')
+
+    rospy.on_shutdown(on_shutdown)
+
+
+def initialize():
+    """
+    Initializes the node including ros and the config.
+    :return:
+    :rtype: None
+    """
+    global objects_, object_ids_
+
+    init_ros()
+
+    CONFIG.load(CONFIG_FILE)
+
+    # objects_ = [[] for i in range(CONFIG.room_number)
+    for obj_name in CONFIG.models.keys():
+        objects_[obj_name] = np.zeros((CONFIG.max_num_objects, 2))
+        object_ids_[obj_name] = np.zeros(CONFIG.max_num_objects)
+
+    log.info('Done initializing')
+
+
+def main():
+    """
+    The main function of the node. Contains the core functionality.
+    :return:
+    :rtype: None
+    """
+    initialize()
+
+    spawn_init_objects('box', RADIUS)
+
+    if len(CONFIG.spawn_moving_objects) > 0:
+        spawn_moving_objects()
+
+    spawn_object_loop(RADIUS, 'box')
+
+    delete_all_objects()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/cob_gazebo/launch/robot.launch b/cob_gazebo/launch/robot.launch
index 2e5ef77f..e3d9fb8e 100644
--- a/cob_gazebo/launch/robot.launch
+++ b/cob_gazebo/launch/robot.launch
@@ -7,10 +7,12 @@
 	<arg name="pkg_robot_config" default="$(find cob_default_robot_config)"/>
 	<arg name="paused" default="false"/>
 	<arg name="initial_config" default=""/>
+	<arg name="multi_robot" default="false"/>
 
 	<!-- upload default configuration parameters -->
 	<include file="$(find cob_bringup)/robots/$(arg robot).xml" >
 		<arg name="sim" value="true" />
+		<arg name="multi_robot" value="$(arg multi_robot)" />
 	</include>
 
 	<!-- push robot_description to factory and spawn robot in gazebo -->
diff --git a/cob_gazebo_objects/launch/spawn_object_urdf.launch b/cob_gazebo_objects/launch/spawn_object_urdf.launch
index 1f1c3fbe..715db08f 100644
--- a/cob_gazebo_objects/launch/spawn_object_urdf.launch
+++ b/cob_gazebo_objects/launch/spawn_object_urdf.launch
@@ -3,9 +3,13 @@
 	<arg name="object"/>
 	<arg name="file_path_urdf" default="$(find cob_gazebo_objects)/objects/$(arg object).urdf"/>
 	<arg name="object_origin" default=""/>
+	<arg name="x" default="0.0"/>
+	<arg name="y" default="0.0"/>
+	<arg name="z" default="0.0"/>
+
 
 	<param name="object_description" textfile="$(arg file_path_urdf)" />
 
-	<node name="spawn_object_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -param object_description -model $(arg object) ${arg object_origin}" respawn="false" output="screen" />
+	<node name="spawn_object_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -param object_description -model $(arg object) -x $(arg x) -y $(arg y) -z $(arg z)" respawn="false" output="screen" />
 
 </launch>
diff --git a/cob_gazebo_objects/launch/spawn_static_sensor.launch b/cob_gazebo_objects/launch/spawn_static_sensor.launch
new file mode 100644
index 00000000..8c44f660
--- /dev/null
+++ b/cob_gazebo_objects/launch/spawn_static_sensor.launch
@@ -0,0 +1,16 @@
+<launch>
+	<arg name="sensor_id" default="1"/>
+	<arg name="x" default="0.0"/>
+	<arg name="y" default="0.0"/>
+	<arg name="z" default="0.188"/>
+	<arg name="yaw" default="3.14"/>
+
+	<!-- Convert xacro and put on parameter server -->
+	<param name="sensor_description_$(arg sensor_id)" command="$(find xacro)/xacro --inorder '$(find cob_gazebo_objects)/objects/static_sensor_s300.urdf.xacro' id:=$(arg sensor_id) x:=$(arg x) y:=$(arg y) z:=$(arg z) yaw:=$(arg yaw)" />
+
+	<!-- Spawn sensor into Gazebo -->
+	<node name="spawn_sensor_$(arg sensor_id)" pkg="gazebo_ros" type="spawn_model" args="-param sensor_description_$(arg sensor_id) -urdf -model static_sensor_$(arg sensor_id) " />
+
+	<node pkg="tf" type="static_transform_publisher" name="static_laser_broadcaster_$(arg sensor_id)" args="$(arg x) $(arg y) $(arg z) $(arg yaw) 0 0 map static_laser_$(arg sensor_id)_link 100" />
+
+</launch>
\ No newline at end of file
diff --git a/cob_gazebo_objects/objects/column.urdf b/cob_gazebo_objects/objects/column.urdf
index 224e052a..95ca4adb 100644
--- a/cob_gazebo_objects/objects/column.urdf
+++ b/cob_gazebo_objects/objects/column.urdf
@@ -7,19 +7,19 @@
       <inertia ixx="0.4" ixy="0.0" ixz="0.0" iyy="0.4" iyz="0.0" izz="0.2"/>
     </inertial>
     <visual>
-      <origin xyz="0 0 2.5" rpy="0 0 0" />
+      <origin xyz="0 0 0.5" rpy="0 0 0" />
       <geometry>
-        <cylinder length="5.0" radius="0.5"/>
+        <cylinder length="1.0" radius="0.5"/>
       </geometry>
     </visual>
     <collision>
-      <origin xyz="0 0 2.5" rpy="0 0 0" />
+      <origin xyz="0 0 0.5" rpy="0 0 0" />
       <geometry>
-        <cylinder length="5.0" radius="0.5"/>
+        <cylinder length="1.0" radius="0.5"/>
       </geometry>
     </collision>
   </link>
   <gazebo>
-    <static>true</static>
+    <static>false</static>
   </gazebo>     
 </robot>
diff --git a/cob_gazebo_objects/objects/forklift.urdf b/cob_gazebo_objects/objects/forklift.urdf
index 690e277a..04b42988 100644
--- a/cob_gazebo_objects/objects/forklift.urdf
+++ b/cob_gazebo_objects/objects/forklift.urdf
@@ -7,12 +7,15 @@
     <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
   </inertial>
   <visual>
+   <laser_retro>1</laser_retro>
    <origin xyz="1.5 0.5 0.0" rpy="0 0 3.14" />
    <geometry>
      <mesh filename="package://cob_gazebo_objects/Media/models/forklift.dae" scale="0.25 0.25 0.25"/>
    </geometry>
+   <laser_retro>1</laser_retro>
   </visual>
-  <collision>
+  <collision name='base_link_collision'>
+   <laser_retro>1</laser_retro>
    <origin xyz="1.5 0.5 0.0" rpy="0 0 3.14" />
    <geometry>
       <mesh filename="package://cob_gazebo_objects/Media/models/forklift.dae" scale="0.25 0.25 0.25"/>
diff --git a/cob_gazebo_objects/objects/static_sensor_s300.urdf.xacro b/cob_gazebo_objects/objects/static_sensor_s300.urdf.xacro
new file mode 100644
index 00000000..81d9da24
--- /dev/null
+++ b/cob_gazebo_objects/objects/static_sensor_s300.urdf.xacro
@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
+       name="sensor">
+
+  <xacro:include filename="$(find cob_gazebo_worlds)/urdf/common.xacro" />
+  <!-- Include Sensor file -->
+  <xacro:include filename="$(find cob_description)/urdf/sensors/sick_s300_laser.urdf.xacro"/>
+
+  <xacro:arg name="id" default="1" />
+  <xacro:arg name="x" default="0.0"/>
+  <xacro:arg name="y" default="0.0"/>
+  <xacro:arg name="z" default="0.0"/>
+  <xacro:arg name="yaw" default="0.0"/>
+
+  <!-- world link -->
+  <link name="world" />
+
+  <!-- Spawn Sensors -->
+  <!-- sensor link -->
+  <link name="sensor_$(arg id)_link" />
+  
+  <!-- connect links -->
+  <joint name="static_sensor_$(arg id)_joint" type="fixed">
+    <parent link="world"/>
+    <child link="sensor_$(arg id)_link"/>
+  </joint>
+
+  <!-- Spawn Sensors - d=0.45 -->  
+  <xacro:sick_s300_laser_v0 name="static_laser_$(arg id)" parent="sensor_$(arg id)">
+    <origin xyz="$(arg x) $(arg y) $(arg z)" rpy="0 0 $(arg yaw)" />
+  </xacro:sick_s300_laser_v0>
+
+  <gazebo reference="sensor_$(arg id)_link">
+    <gravity>false</gravity>
+  </gazebo>
+
+</robot>
diff --git a/cob_gazebo_worlds/launch/ipa-benchmark.xml b/cob_gazebo_worlds/launch/ipa-benchmark.xml
new file mode 100644
index 00000000..6ff49752
--- /dev/null
+++ b/cob_gazebo_worlds/launch/ipa-benchmark.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<launch>
+
+	<arg name="robot_env" default="ipa-benchmark"/>
+	<arg name="publish_world_state" value="false"/>
+
+	<!-- spawn environment -->
+	<include file="$(find cob_gazebo_worlds)/launch/world_urdf.xml">
+		<arg name="robot_env" value="$(arg robot_env)"/>
+		<arg name="publish_world_state" value="$(arg publish_world_state)"/>
+	</include>
+
+	<arg name="static_laser_topic" default="/static_laser/scan_raw"/>
+
+	<!-- static tf for two sick sensors -->
+	<node pkg="tf" type="static_transform_publisher" name="static_laser_broadcaster_01" args="19.08 -1.5 0.1 3.14 0 0 map static_laser_01_link 100" />
+	<node pkg="tf" type="static_transform_publisher" name="static_laser_broadcaster_02" args="27.08 -1.5 0.1 3.14 0 0 map static_laser_02_link 100" />
+
+	<remap from="/static_laser_01/scan_raw" to="$(arg static_laser_topic)"/>
+	<remap from="/static_laser_02/scan_raw" to="$(arg static_laser_topic)"/>
+
+</launch>
diff --git a/cob_gazebo_worlds/launch/ipa-production-plant.xml b/cob_gazebo_worlds/launch/ipa-production-plant.xml
index 65c460b2..0956d4e9 100644
--- a/cob_gazebo_worlds/launch/ipa-production-plant.xml
+++ b/cob_gazebo_worlds/launch/ipa-production-plant.xml
@@ -19,4 +19,4 @@
 	<remap from="/static_laser_01/scan_raw" to="$(arg static_laser_topic)"/>
 	<remap from="/static_laser_02/scan_raw" to="$(arg static_laser_topic)"/>
 
-</launch>
+</launch>
\ No newline at end of file
diff --git a/cob_gazebo_worlds/urdf/ipa-benchmark/ipa-benchmark.property.xacro b/cob_gazebo_worlds/urdf/ipa-benchmark/ipa-benchmark.property.xacro
new file mode 100644
index 00000000..21857115
--- /dev/null
+++ b/cob_gazebo_worlds/urdf/ipa-benchmark/ipa-benchmark.property.xacro
@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!--properties-->
+  <xacro:property name="wall_depth" value="0.1" />
+  <xacro:property name="wall_height" value="5" />
+  <xacro:property name="gate_size" value="3" />
+
+  <xacro:property name="room1_w1" value="9.5" />
+  <xacro:property name="room1_w2" value="9.5" />
+  <xacro:property name="room1_w3" value="24" />
+
+  <xacro:property name="room2_w1" value="22.5" />
+  <xacro:property name="room2_w2" value="21" />
+
+  <xacro:property name="room3_w1" value="24" />
+  <xacro:property name="room3_w2" value="22" />
+
+  <xacro:property name="room4_w1" value="36" />
+  <xacro:property name="room4_w2" value="21" /> 24
+  <xacro:property name="room4_w3" value="11.25" />
+  <xacro:property name="room4_w4" value="7.5" />
+
+  <xacro:property name="room1_room2_w1" value="10.5" />
+  <xacro:property name="room1_room3_w1" value="9.5" />
+  <xacro:property name="room2_room4_w1" value="11.25" />
+  <xacro:property name="room3_room4_w1" value="17" />
+  <xacro:property name="room3_room4_w2" value="4" />
+  
+
+  <xacro:property name="hallway_w1" value="26" />
+  <xacro:property name="hallway_w2" value="58" />
+  <xacro:property name="hallway_w3" value="5" />
+
+  <xacro:property name="hallway2_w1" value="3" />
+  <xacro:property name="hallway2_w2" value="53" />
+
+  <xacro:property name="position_offset_x" value="-20" />
+  <xacro:property name="position_offset_y" value="-35" />
+  <xacro:property name="position_offset_z" value="0" />
+
+</robot>
\ No newline at end of file
diff --git a/cob_gazebo_worlds/urdf/ipa-benchmark/ipa-benchmark.urdf.xacro b/cob_gazebo_worlds/urdf/ipa-benchmark/ipa-benchmark.urdf.xacro
new file mode 100644
index 00000000..f6eb8843
--- /dev/null
+++ b/cob_gazebo_worlds/urdf/ipa-benchmark/ipa-benchmark.urdf.xacro
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
+       name="ipa-benchmark">
+
+  <xacro:include filename="$(find cob_gazebo_worlds)/urdf/common.xacro" />
+  <xacro:include filename="$(find cob_gazebo_worlds)/urdf/ipa-benchmark/ipa-benchmark.property.xacro" />
+  
+  <!-- walls -->
+  <xacro:include filename="$(find cob_gazebo_worlds)/urdf/ipa-benchmark/walls.urdf.xacro" />
+
+  <!-- Include Sensor file -->
+  <!-- <xacro:include filename="$(find cob_gazebo_worlds)/urdf/ipa-benchmark/sensors.urdf.xacro" /> -->
+
+
+  <!-- world link -->
+  <link name="world" />
+
+  <!-- Spawn Walls -->
+  <!-- <xacro:sensors name="sensors"/> -->
+
+  <!-- Spawn Walls -->
+  <xacro:walls name="walls"/>
+
+</robot>
diff --git a/cob_gazebo_worlds/urdf/ipa-benchmark/sensors.urdf.xacro b/cob_gazebo_worlds/urdf/ipa-benchmark/sensors.urdf.xacro
new file mode 100644
index 00000000..ff3ceecd
--- /dev/null
+++ b/cob_gazebo_worlds/urdf/ipa-benchmark/sensors.urdf.xacro
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:include filename="$(find cob_description)/urdf/sensors/sick_s300_laser.urdf.xacro"/>
+
+  <xacro:macro name="sensors" params="name">
+
+    <!-- sensor link -->
+    <link name="sensors_link" />
+    
+    <!-- connect links -->
+    <joint name="static_sensors_joint" type="fixed">
+      <parent link="world"/>
+      <child link="sensors_link"/>
+    </joint>
+
+    <!-- Spawn Sensors -->  
+    <xacro:sick_s300_laser_v0 name="static_laser_01" parent="sensors">
+      <origin xyz="19.08 -1.50 0.2" rpy="0 0 3.14" />
+    </xacro:sick_s300_laser_v0>
+
+    <xacro:sick_s300_laser_v0 name="static_laser_02" parent="sensors">
+      <origin xyz="27.08 -1.50 0.2" rpy="0 0 3.14" />
+    </xacro:sick_s300_laser_v0>
+
+  </xacro:macro>
+
+</robot>
\ No newline at end of file
diff --git a/cob_gazebo_worlds/urdf/ipa-benchmark/walls.urdf.xacro b/cob_gazebo_worlds/urdf/ipa-benchmark/walls.urdf.xacro
new file mode 100644
index 00000000..2cee68ad
--- /dev/null
+++ b/cob_gazebo_worlds/urdf/ipa-benchmark/walls.urdf.xacro
@@ -0,0 +1,806 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="walls" params="name">
+
+  <!-- room1_w1 -->
+  <!-- wall_01 -->
+  <link name="wall_01">
+    <xacro:box_inertial x="${wall_depth}" y= "${room1_w1}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_w1} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_w1} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_01">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_01" type="fixed">
+    <origin xyz="${4.75+position_offset_x} ${position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 ${90*M_PI/180}" />
+    <parent link="world" />
+    <child link="wall_01" />
+  </joint>
+
+  <!-- room1_w2 -->
+  <!-- wall_02 -->
+  <link name="wall_02">
+    <xacro:box_inertial x="${wall_depth}" y="${room1_w2}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_w2} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_w2} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_02">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_02" type="fixed">
+    <origin xyz="${17.25+position_offset_x} ${position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 ${90*M_PI/180}" />
+    <parent link="world" />
+    <child link="wall_02" />
+  </joint>
+
+
+  <!-- room2_w1 -->
+  <!-- wall_02b -->
+  <link name="wall_02b">
+    <xacro:box_inertial x="${wall_depth}" y="${room2_w1}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room2_w1} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room2_w1} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_02b">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_02b" type="fixed">
+    <origin xyz="${33.25+position_offset_x} ${position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 ${90*M_PI/180}" />
+    <parent link="world" />
+    <child link="wall_02b" />
+  </joint>
+
+
+  <!-- room1_w3 -->
+  <!-- wall_03 -->
+  <link name="wall_03">
+    <xacro:box_inertial x="${wall_depth}" y="${room1_w3}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_w3} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_w3} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_03">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_03" type="fixed">
+    <origin xyz="${position_offset_x} ${12+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 0" />
+    <parent link="world" />
+    <child link="wall_03" />
+  </joint>
+
+
+  <!-- room1_room2_w1 -->
+  <!-- wall_04a -->
+  <link name="wall_04a">
+    <xacro:box_inertial x="${wall_depth}" y="${room1_room2_w1}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_room2_w1} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_room2_w1} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_04a">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_04a" type="fixed">
+    <origin xyz="${22+position_offset_x} ${18.75+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 0" />
+    <parent link="world" />
+    <child link="wall_04a" />
+  </joint>
+
+    <!-- room1_room2_w1 -->
+  <!-- wall_04b -->
+  <link name="wall_04b">
+    <xacro:box_inertial x="${wall_depth}" y="${room1_room2_w1}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_room2_w1} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_room2_w1} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_04b">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_04b" type="fixed">
+    <origin xyz="${22+position_offset_x} ${5.25+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 0" />
+    <parent link="world" />
+    <child link="wall_04b" />
+  </joint>
+
+
+  <!-- room1_room3_w1 -->
+  <!-- wall_06a -->
+  <link name="wall_06a">
+    <xacro:box_inertial x="${wall_depth}" y="${room1_room3_w1}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_room3_w1} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_room3_w1} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_06a">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_06a" type="fixed">
+    <origin xyz="${4.75+position_offset_x} ${24+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 ${90*M_PI/180}" />
+    <parent link="world" />
+    <child link="wall_06a" />
+  </joint>
+
+  
+
+  <!-- room1_room3_w1 -->
+  <!-- wall_06b -->
+  <link name="wall_06b">
+    <xacro:box_inertial x="${wall_depth}" y="${room1_room3_w1}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_room3_w1} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_room3_w1} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_06b">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_06b" type="fixed">
+    <origin xyz="${17.25+position_offset_x} ${24+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 ${90*M_PI/180}" />
+    <parent link="world" />
+    <child link="wall_06b" />
+  </joint>
+
+
+  <!-- room2_room4_w1 -->
+  <!-- wall_07 -->
+  <link name="wall_07">
+    <xacro:box_inertial x="${wall_depth}" y="${room2_room4_w1}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room2_room4_w1} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room2_room4_w1} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_07">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_07" type="fixed">
+    <origin xyz="${27.625+position_offset_x} ${24+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 ${90*M_PI/180}" />
+    <parent link="world" />
+    <child link="wall_07" />
+  </joint>
+
+
+  <!-- room4_w3 -->
+  <!-- wall_08 -->
+  <link name="wall_08">
+    <xacro:box_inertial x="${wall_depth}" y="${room4_w3}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room4_w3} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room4_w3} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_08">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_08" type="fixed">
+    <origin xyz="${41.875+position_offset_x} ${24+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 ${90*M_PI/180}" />
+    <parent link="world" />
+    <child link="wall_08" />
+  </joint>
+
+
+  <!-- room4_w4 -->
+  <!-- wall_09 -->
+  <link name="wall_09">
+    <xacro:box_inertial x="${wall_depth}" y="${room4_w4}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room4_w4} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room4_w4} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_09">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_09" type="fixed">
+    <origin xyz="${54.25+position_offset_x} ${24+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 ${90*M_PI/180}" />
+    <parent link="world" />
+    <child link="wall_09" />
+  </joint>
+
+
+  <!-- room3_w1 -->
+  <!-- wall_10 -->
+  <link name="wall_10">
+    <xacro:box_inertial x="${wall_depth}" y="${room3_w1}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room3_w1} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room3_w1} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_10">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_10" type="fixed">
+    <origin xyz="${position_offset_x} ${36+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 0" />
+    <parent link="world" />
+    <child link="wall_10" />
+  </joint>
+
+
+  <link name="wall_23">
+    <xacro:box_inertial x="${wall_depth}" y="${room3_w1}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room3_w1} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room3_w1} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_23">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_23" type="fixed">
+    <origin xyz="${47.5+position_offset_x} ${36+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 0" />
+    <parent link="world" />
+    <child link="wall_23" />
+  </joint>
+
+
+  <!-- room3_room4_w1 -->
+  <!-- wall_11 -->
+  <link name="wall_11">
+    <xacro:box_inertial x="${wall_depth}" y="${room3_room4_w1}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room3_room4_w1} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room3_room4_w1} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_11">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_11" type="fixed">
+    <origin xyz="${22+position_offset_x} ${32.5+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 0" />
+    <parent link="world" />
+    <child link="wall_11" />
+  </joint>
+
+
+  <!-- room4_w2 -->
+  <!-- wall_12 -->
+  <link name="wall_12">
+    <xacro:box_inertial x="${wall_depth}" y="${room4_w2}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room4_w2} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room4_w2} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_12">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_12" type="fixed">
+    <origin xyz="${58+position_offset_x} ${34.5+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 0" />
+    <parent link="world" />
+    <child link="wall_12" />
+  </joint>
+
+
+  <!-- room3_w2 -->
+  <!-- wall_13 -->
+  <link name="wall_13">
+    <xacro:box_inertial x="${wall_depth}" y="${room3_w2}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room3_w2} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room3_w2} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_13">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_13" type="fixed">
+    <origin xyz="${11+position_offset_x} ${48+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 ${90*M_PI/180}" />
+    <parent link="world" />
+    <child link="wall_13" />
+  </joint>
+
+
+  <!-- room4_w1 -->
+  <!-- wall_14 -->
+  <link name="wall_14">
+    <xacro:box_inertial x="${wall_depth}" y="${room4_w1}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room4_w1} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room4_w1} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_14">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_14" type="fixed">
+    <origin xyz="${40+position_offset_x} ${48+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 ${90*M_PI/180}" />
+    <parent link="world" />
+    <child link="wall_14" />
+  </joint>
+
+
+  <!-- hallway -->
+  <!-- wall_15 -->
+  <link name="wall_15">
+    <xacro:box_inertial x="${wall_depth}" y="${room1_w3}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_w3} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room1_w3} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_15">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_15" type="fixed">
+    <origin xyz="${47.5+position_offset_x} ${12+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 0" />
+    <parent link="world" />
+    <child link="wall_15" />
+  </joint>
+
+
+  <!-- hallway -->
+  <!-- wall_16 -->
+  <link name="wall_16">
+    <xacro:box_inertial x="${wall_depth}" y="${hallway_w1}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${hallway_w1} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${hallway_w1} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_16">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_16" type="fixed">
+    <origin xyz="${54.25+room4_w4/2+position_offset_x} ${11+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 0" />
+    <parent link="world" />
+    <child link="wall_16" />
+  </joint>
+
+
+  <!-- hallway -->
+  <!-- wall_17 -->
+  <link name="wall_17">
+    <xacro:box_inertial x="${wall_depth}" y="${hallway_w2}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${hallway_w2} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${hallway_w2} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_17">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_17" type="fixed">
+    <origin xyz="${hallway_w2/2+position_offset_x} ${-5+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 ${90*M_PI/180}" />
+    <parent link="world" />
+    <child link="wall_17" />
+  </joint>
+
+
+  <!-- hallway -->
+  <!-- wall_18 -->
+  <link name="wall_18">
+    <xacro:box_inertial x="${wall_depth}" y="${hallway_w3}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${hallway_w3} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${hallway_w3} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_18">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_18" type="fixed">
+    <origin xyz="${position_offset_x} ${-2.5+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 0" />
+    <parent link="world" />
+    <child link="wall_18" />
+  </joint>
+
+
+  <!-- room3_room4_w2 -->
+  <!-- wall_19 -->
+  <link name="wall_19">
+    <xacro:box_inertial x="${wall_depth}" y="${room3_room4_w2}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room3_room4_w2} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${room3_room4_w2} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_19">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_19" type="fixed">
+    <origin xyz="${22+position_offset_x} ${46+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 0" />
+    <parent link="world" />
+    <child link="wall_19" />
+  </joint>
+
+
+  <!-- hallway2 -->
+  <!-- wall_20 -->
+  <link name="wall_20">
+    <xacro:box_inertial x="${wall_depth}" y="${hallway2_w1}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${hallway2_w1} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${hallway2_w1} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_20">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_20" type="fixed">
+    <origin xyz="${hallway_w2+hallway2_w1/2+position_offset_x} ${-5+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 ${90*M_PI/180}" />
+    <parent link="world" />
+    <child link="wall_20" />
+  </joint>
+
+
+  <!-- hallway2 -->
+  <!-- wall_21 -->
+  <link name="wall_21">
+    <xacro:box_inertial x="${wall_depth}" y="${hallway2_w2}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${hallway2_w2} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${hallway2_w2} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_21">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_21" type="fixed">
+    <origin xyz="${hallway_w2+hallway2_w1+position_offset_x} ${hallway2_w2/2-5+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 0" />
+    <parent link="world" />
+    <child link="wall_21" />
+  </joint>
+
+
+  <!-- hallway2 -->
+  <!-- wall_22 -->
+  <link name="wall_22">
+    <xacro:box_inertial x="${wall_depth}" y="${hallway2_w1}" z="${wall_height}" mass="500.0">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:box_inertial>
+    <visual>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${hallway2_w1} ${wall_height}" />
+      </geometry>
+      <material name="IPA/Grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" />
+      <geometry>
+        <box size="${wall_depth} ${hallway2_w1} ${wall_height}" />
+      </geometry>
+    </collision>
+  </link>
+
+  <gazebo reference="wall_22">
+    <material>wall_floor_05</material>
+  </gazebo>
+
+  <joint name="joint_wall_22" type="fixed">
+    <origin xyz="${hallway_w2+hallway2_w1/2+position_offset_x} ${-5+hallway2_w2+position_offset_y} ${wall_height/2+position_offset_z}" rpy="0 0 ${90*M_PI/180}" />
+    <parent link="world" />
+    <child link="wall_22" />
+  </joint>
+
+
+
+  </xacro:macro>
+
+</robot>  
\ No newline at end of file
diff --git a/cob_gazebo_worlds/urdf/ipa-production-plant/ipa-production-plant.urdf.xacro b/cob_gazebo_worlds/urdf/ipa-production-plant/ipa-production-plant.urdf.xacro
index e245dc7e..c3015b84 100644
--- a/cob_gazebo_worlds/urdf/ipa-production-plant/ipa-production-plant.urdf.xacro
+++ b/cob_gazebo_worlds/urdf/ipa-production-plant/ipa-production-plant.urdf.xacro
@@ -9,14 +9,14 @@
   <xacro:include filename="$(find cob_gazebo_worlds)/urdf/ipa-production-plant/walls.urdf.xacro" />
 
   <!-- Include Sensor file -->
-  <xacro:include filename="$(find cob_gazebo_worlds)/urdf/ipa-production-plant/sensors.urdf.xacro" />
+  <!-- <xacro:include filename="$(find cob_gazebo_worlds)/urdf/ipa-production-plant/sensors.urdf.xacro" /> -->
 
 
   <!-- world link -->
   <link name="world" />
 
   <!-- Spawn Walls -->
-  <xacro:sensors name="sensors"/>
+  <!-- <xacro:sensors name="sensors"/> -->
 
   <!-- Spawn Walls -->
   <xacro:walls name="walls"/>
diff --git a/cob_gazebo_worlds/urdf/ipa-production-plant/sensors.urdf.xacro b/cob_gazebo_worlds/urdf/ipa-production-plant/sensors.urdf.xacro
index 149bd03c..ff3ceecd 100644
--- a/cob_gazebo_worlds/urdf/ipa-production-plant/sensors.urdf.xacro
+++ b/cob_gazebo_worlds/urdf/ipa-production-plant/sensors.urdf.xacro
@@ -14,14 +14,14 @@
     </joint>
 
     <!-- Spawn Sensors -->  
-    <xacro:sick_s300_laser_v0 name="static_laser_01" parent="sensors_link">
-      <origin xyz="19.08 -1.50 0.1" rpy="0 0 3.14" />
+    <xacro:sick_s300_laser_v0 name="static_laser_01" parent="sensors">
+      <origin xyz="19.08 -1.50 0.2" rpy="0 0 3.14" />
     </xacro:sick_s300_laser_v0>
 
-    <xacro:sick_s300_laser_v0 name="static_laser_02" parent="sensors_link">
-      <origin xyz="27.08 -1.50 0.1" rpy="0 0 3.14" />
+    <xacro:sick_s300_laser_v0 name="static_laser_02" parent="sensors">
+      <origin xyz="27.08 -1.50 0.2" rpy="0 0 3.14" />
     </xacro:sick_s300_laser_v0>
 
   </xacro:macro>
 
-</robot>
+</robot>
\ No newline at end of file
diff --git a/cob_gazebo_worlds/urdf/ipa-production-plant/walls.urdf.xacro b/cob_gazebo_worlds/urdf/ipa-production-plant/walls.urdf.xacro
index 868f1cbf..c0c82f23 100644
--- a/cob_gazebo_worlds/urdf/ipa-production-plant/walls.urdf.xacro
+++ b/cob_gazebo_worlds/urdf/ipa-production-plant/walls.urdf.xacro
@@ -6,7 +6,7 @@
   <!-- room1_w1 -->
   <!-- wall_01 -->
   <link name="wall_01">
-    <xacro:box_inertial x="${wall_depth}" y= "${room1_w1}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y= "${room1_w1}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -37,7 +37,7 @@
   <!-- room1_w2 -->
   <!-- wall_02 -->
   <link name="wall_02">
-    <xacro:box_inertial x="${wall_depth}" y="${room1_w2}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room1_w2}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -69,7 +69,7 @@
   <!-- room2_w1 -->
   <!-- wall_02b -->
   <link name="wall_02b">
-    <xacro:box_inertial x="${wall_depth}" y="${room2_w1}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room2_w1}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -101,7 +101,7 @@
   <!-- room1_w3 -->
   <!-- wall_03 -->
   <link name="wall_03">
-    <xacro:box_inertial x="${wall_depth}" y="${room1_w3}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room1_w3}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -133,7 +133,7 @@
   <!-- room1_room2_w1 -->
   <!-- wall_04a -->
   <link name="wall_04a">
-    <xacro:box_inertial x="${wall_depth}" y="${room1_room2_w1}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room1_room2_w1}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -164,7 +164,7 @@
     <!-- room1_room2_w1 -->
   <!-- wall_04b -->
   <link name="wall_04b">
-    <xacro:box_inertial x="${wall_depth}" y="${room1_room2_w1}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room1_room2_w1}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -196,7 +196,7 @@
   <!-- room1_room3_w1 -->
   <!-- wall_06a -->
   <link name="wall_06a">
-    <xacro:box_inertial x="${wall_depth}" y="${room1_room3_w1}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room1_room3_w1}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -229,7 +229,7 @@
   <!-- room1_room3_w1 -->
   <!-- wall_06b -->
   <link name="wall_06b">
-    <xacro:box_inertial x="${wall_depth}" y="${room1_room3_w1}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room1_room3_w1}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -261,7 +261,7 @@
   <!-- room2_room4_w1 -->
   <!-- wall_07 -->
   <link name="wall_07">
-    <xacro:box_inertial x="${wall_depth}" y="${room2_room4_w1}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room2_room4_w1}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -293,7 +293,7 @@
   <!-- room4_w3 -->
   <!-- wall_08 -->
   <link name="wall_08">
-    <xacro:box_inertial x="${wall_depth}" y="${room4_w3}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room4_w3}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -325,7 +325,7 @@
   <!-- room4_w4 -->
   <!-- wall_09 -->
   <link name="wall_09">
-    <xacro:box_inertial x="${wall_depth}" y="${room4_w4}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room4_w4}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -357,7 +357,7 @@
   <!-- room3_w1 -->
   <!-- wall_10 -->
   <link name="wall_10">
-    <xacro:box_inertial x="${wall_depth}" y="${room3_w1}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room3_w1}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -389,7 +389,7 @@
   <!-- room3_room4_w1 -->
   <!-- wall_11 -->
   <link name="wall_11">
-    <xacro:box_inertial x="${wall_depth}" y="${room3_room4_w1}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room3_room4_w1}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -421,7 +421,7 @@
   <!-- room4_w2 -->
   <!-- wall_12 -->
   <link name="wall_12">
-    <xacro:box_inertial x="${wall_depth}" y="${room4_w2}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room4_w2}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -453,7 +453,7 @@
   <!-- room3_w2 -->
   <!-- wall_13 -->
   <link name="wall_13">
-    <xacro:box_inertial x="${wall_depth}" y="${room3_w2}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room3_w2}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -485,7 +485,7 @@
   <!-- room4_w1 -->
   <!-- wall_14 -->
   <link name="wall_14">
-    <xacro:box_inertial x="${wall_depth}" y="${room4_w1}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room4_w1}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -517,7 +517,7 @@
   <!-- hallway -->
   <!-- wall_15 -->
   <link name="wall_15">
-    <xacro:box_inertial x="${wall_depth}" y="${room1_w3}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room1_w3}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -549,7 +549,7 @@
   <!-- hallway -->
   <!-- wall_16 -->
   <link name="wall_16">
-    <xacro:box_inertial x="${wall_depth}" y="${hallway_w1}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${hallway_w1}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -581,7 +581,7 @@
   <!-- hallway -->
   <!-- wall_17 -->
   <link name="wall_17">
-    <xacro:box_inertial x="${wall_depth}" y="${hallway_w2}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${hallway_w2}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -613,7 +613,7 @@
   <!-- hallway -->
   <!-- wall_18 -->
   <link name="wall_18">
-    <xacro:box_inertial x="${wall_depth}" y="${hallway_w3}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${hallway_w3}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -645,7 +645,7 @@
   <!-- room3_room4_w2 -->
   <!-- wall_19 -->
   <link name="wall_19">
-    <xacro:box_inertial x="${wall_depth}" y="${room3_room4_w2}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${room3_room4_w2}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -677,7 +677,7 @@
   <!-- hallway2 -->
   <!-- wall_20 -->
   <link name="wall_20">
-    <xacro:box_inertial x="${wall_depth}" y="${hallway2_w1}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${hallway2_w1}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -709,7 +709,7 @@
   <!-- hallway2 -->
   <!-- wall_21 -->
   <link name="wall_21">
-    <xacro:box_inertial x="${wall_depth}" y="${hallway2_w2}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${hallway2_w2}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>
@@ -741,7 +741,7 @@
   <!-- hallway2 -->
   <!-- wall_22 -->
   <link name="wall_22">
-    <xacro:box_inertial x="${wall_depth}" y="${hallway2_w1}" z="${wall_height}" mass="5.0">
+    <xacro:box_inertial x="${wall_depth}" y="${hallway2_w1}" z="${wall_height}" mass="500.0">
       <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:box_inertial>
     <visual>