diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt
index 11e7ef7e00..448b0f7572 100644
--- a/src/cmd/CMakeLists.txt
+++ b/src/cmd/CMakeLists.txt
@@ -63,7 +63,33 @@ configure_file(
 
 install(FILES ${model_configured} DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/ignition/)
 
+#===============================================================================
+# Used for the installed model command version.
+# Generate the ruby script that gets installed.
+# Note that the major version of the library is included in the name.
+set(cmd_model_script_generated "${CMAKE_CURRENT_BINARY_DIR}/cmdpkg_create${PROJECT_VERSION_MAJOR}.rb")
+set(cmd_model_script_configured "${cmd_model_script_generated}.configured")
 
+configure_file(
+  "cmdpkg_create.rb.in"
+  "${cmd_model_script_configured}"
+  @ONLY)
+file(GENERATE
+  OUTPUT "${cmd_model_script_generated}"
+  INPUT  "${cmd_model_script_configured}")
+
+install(FILES ${cmd_model_script_generated} DESTINATION lib/ruby/ignition)
+install(DIRECTORY resources
+  DESTINATION lib/ruby/ignition)
+# Used for the installed version.
+set(ign_model_ruby_path "${CMAKE_INSTALL_PREFIX}/lib/ruby/ignition/cmdpkg_create${PROJECT_VERSION_MAJOR}")
+
+set(model_configured "${CMAKE_CURRENT_BINARY_DIR}/pkg_create{PROJECT_VERSION_MAJOR}.yaml")
+configure_file(
+  "pkg_create.yaml.in"
+  ${model_configured})
+
+install(FILES ${model_configured} DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/ignition/)
 #===============================================================================
 # Generate the ruby script for internal testing.
 # Note that the major version of the library is included in the name.
diff --git a/src/cmd/cmdpkg_create.rb.in b/src/cmd/cmdpkg_create.rb.in
new file mode 100644
index 0000000000..df28889611
--- /dev/null
+++ b/src/cmd/cmdpkg_create.rb.in
@@ -0,0 +1,248 @@
+#!/usr/bin/ruby
+require 'erb'
+require 'open-uri'
+require 'yaml'
+require 'ostruct'
+
+# Copyright (C) 2022 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# We use 'dl' for Ruby <= 1.9.x and 'fiddle' for Ruby >= 2.0.x
+if RUBY_VERSION.split('.')[0] < '2'
+  require 'dl'
+  require 'dl/import'
+  include DL
+else
+  require 'fiddle'
+  require 'fiddle/import'
+  include Fiddle
+end
+
+require 'optparse'
+
+# Constants.
+LIBRARY_NAME = '@library_location@'
+LIBRARY_VERSION = '@PROJECT_VERSION_FULL@'
+
+COMMON_OPTIONS =
+               "  -h [--help]                 Print this help message.\n"\
+               "                                                    \n"        +
+               "  --force-version <VERSION>   Use a specific library version.\n"\
+               "                                                    \n"        +
+               '  --versions                  Show the available versions.'
+
+COMMANDS = { 'pkg_create' =>
+  "Create a new ignition package.\n"+
+  "                                                                        \n"\
+  "  ign pkg_create [options]                                              \n\n"\
+  "                                                                         \n"\
+  "Available Options:                                                       \n\n"\
+  "  -n [--name] arg                          Name of new package [Required]             \n\n"\
+  "  -i [--init-with-example-system]          Package with various example resources     \n\n"+
+  "  -b [--built_type] arg                    Build type between ament_cmake or cmake    \n\n"+
+  "  -e [--export_type] arg                   Export type between colcon or plain-camke] \n\n"+
+  "  -d [--ign_dependecies]                   Package depends on common ign packages   \n\n"+
+  "  -g [--gazebo_version] arg                Gazebo version name for dependencies       \n\n"+
+  
+  COMMON_OPTIONS
+}
+
+#
+# Class for the Gazebo pkg_create command line tools.
+#
+class Cmd
+
+  #
+  # Return a structure describing the options.
+  #
+  def parse(args)
+    options = {
+    }
+    usage = COMMANDS[args[0]]
+
+    opt_parser = OptionParser.new do |opts|
+      opts.banner = usage
+
+      opts.on('-h', '--help') do
+        puts usage
+        exit
+      end
+      opts.on('-n [arg]', '--name [arg]', String, 'Name of new package') do |n|
+        options['name'] = n
+      end
+      opts.on('-i','--init-with-example-system', String, 'Package with various example resources') do
+        options['init-with-example-system'] = 1
+      end
+      opts.on('-b [arg]', '--built_type [arg]', String, 'Build type between ament_cmake or cmake') do |b|
+        options['built_type'] = b
+      end
+      opts.on('-e [arg]', '--export_type [arg]', String, 'Export type between colcon or plain-camke') do |e|
+        options['export_type'] = e
+      end
+      opts.on('-d','--ign_dependecies', String, 'Package depends on common ign packages') do
+        options['ign_dependecies'] = 1
+      end
+      opts.on('-g [arg]', '--gazebo_version [arg]',String, 'Gazebo version name for dependencies') do |g|
+        options['gazebo_version'] = g
+      end
+      
+    end # opt_parser do
+    begin
+      opt_parser.parse!(args)
+    rescue
+      puts usage
+      exit(-1)
+    end
+
+    # Check if all options are correct
+
+    if !(options.key?('name') && options['name'].class()==String)
+     puts "ERROR: Please provide proper package name.\n"
+     puts usage
+     exit(-1)
+    end
+    if (options.key?('built_type') && !(['ament_cmake','cmake'].include? options["built_type"]))
+      puts "WARNING: Invalid Built-type,defaulting to cmake\n"
+      options['built_type'] = "cmake"
+    end
+    if (options.key?('export_type') && !(['plain_cmake','colcon'].include? options["export_type"]))
+      puts "WARNING: Invalid Export-type,defaulting to colcon\n"
+      options['export_type'] = "colcon"
+    end
+    if (options.key?('gazebo_version') && !(['acropolis','blueprint','citadel','dome','edifice','fortress','garden'].include? options["gazebo_version"]))
+      puts "WARNING: Invalid Gazebo Version,defaulting to fortress\n"
+      options['gazebo_version'] = "fortress"
+    end
+    #Giving default values
+
+    if !options.key?('init-with-example-system')
+      options['init-with-example-system'] = 0
+    end 
+    if !options.key?('built_type')
+      options['built_type'] = "cmake"
+    end 
+    if !options.key?('export_type')
+      options['export_type'] = "colcon"
+    end 
+    if !options.key?('ign_dependecies')
+      options['ign_dependecies'] = 0
+    end
+    if !options.key?('gazebo_version')
+      options['gazebo_version'] = "fortress"
+    end
+
+    options['command'] = args[0]
+
+    options
+  end # parse()
+
+  def execute(args)
+    options = parse(args)
+
+    # Read the plugin that handles the command.
+    if LIBRARY_NAME[0] == '/'
+      # If the first character is a slash, we'll assume that we've been given an
+      # absolute path to the library. This is only used during test mode.
+      plugin = LIBRARY_NAME
+    else
+      # We're assuming that the library path is relative to the current
+      # location of this script.
+      plugin = File.expand_path(File.join(File.dirname(__FILE__), LIBRARY_NAME))
+    end
+    conf_version = LIBRARY_VERSION
+
+    begin
+      Importer.dlload plugin
+    rescue DLError => e
+      puts "Library error for [#{plugin}]: #{e.to_s}"
+      exit(-1)
+    end
+
+    ##Code for package generation
+    if Dir.exists?(options['name'])
+      puts "ERROR:Folder with this name already exists."
+      exit(-1)
+    end
+
+    #Creates Package folder
+    Dir.mkdir(options['name'])
+
+    #Fetching dependencies versions for common dependencies
+    if options['ign_dependecies']==1
+      dependencies = URI.open('https://raw.githubusercontent.com/ignition-tooling/gazebodistro/master/collection-%s.yaml'% options['gazebo_version']){|f| f.read}
+      dependencies_yaml = YAML.load(dependencies)
+      dependencies_dict={}
+      for dependency in dependencies_yaml.fetch('repositories').keys
+        dependencies_dict[dependency]=dependencies_yaml['repositories'][dependency]['version'].scan( /\d+$/ ).first
+      end
+    end
+
+    resources_dir=__dir__+"/resources"
+
+    #Creates CMakeLists.txt using erb
+    cmakelists_render = ERB.new(File.read(resources_dir+"/CMakeLists.erb"),trim_mode: ">")
+    cmakelists_file = File.new(options['name']+"/CMakeLists.txt", "w")
+    cmakelists_file.puts(cmakelists_render.result(OpenStruct.new(options).instance_eval { binding }))
+    cmakelists_file.close
+    
+    #Creates package.xml for ament dependent
+    if options['built_type']=='ament_cmake'
+      package_xml_render = ERB.new(File.read(resources_dir+"/package_xml.erb"),trim_mode: ">")
+      package_xml_file = File.new(options['name']+"/package.xml", "w")
+      package_xml_file.puts(package_xml_render.result(OpenStruct.new(options).instance_eval { binding }))
+      package_xml_file.close
+    end
+
+    #Creates example resources for advance package
+    if options['init-with-example-system']==1
+      Dir.mkdir(options['name']+"/worlds")
+      Dir.mkdir(options['name']+"/models")
+      Dir.mkdir(options['name']+"/models/elevator")
+      Dir.mkdir(options['name']+"/plugins")
+      Dir.mkdir(options['name']+"/src")
+
+      executable_txt=File.read(resources_dir+"/executables/random_points.cc")
+      executable_file = File.new(options['name']+"/src/random_points.cc", "w")
+      executable_file.puts(executable_txt)
+      executable_file.close
+
+      plugin_txt=File.read(resources_dir+"/plugins/HelloWorld.cc")
+      plugin_file = File.new(options['name']+"/plugins/HelloWorld.cc", "w")
+      plugin_file.puts(plugin_txt)
+      plugin_file.close 
+
+      plugin_header_txt=File.read(resources_dir+"/plugins/HelloWorld.hh")
+      plugin_header_file = File.new(options['name']+"/plugins/HelloWorld.hh", "w")
+      plugin_header_file.puts(plugin_header_txt)
+      plugin_header_file.close
+
+      world_txt=File.read(resources_dir+"/worlds/empty_platform_with_elevator.sdf")
+      world_file = File.new(options['name']+"/worlds/empty_platform_with_elevator.sdf", "w")
+      world_file.puts(world_txt)
+      world_file.close
+
+      model_config_txt=File.read(resources_dir+"/models/elevator/model.config")
+      model_config_file = File.new(options['name']+"/models/elevator/model.config", "w")
+      model_config_file.puts(model_config_txt)
+      model_config_file.close
+
+      model_sdf_txt=File.read(resources_dir+"/models/elevator/model.sdf")
+      model_sdf_file = File.new(options['name']+"/models/elevator/model.sdf", "w")
+      model_sdf_file.puts(model_sdf_txt)
+      model_sdf_file.close
+    end
+  # # execute
+  end
+# class
+end
diff --git a/src/cmd/pkg_create.yaml.in b/src/cmd/pkg_create.yaml.in
new file mode 100644
index 0000000000..ed572c053e
--- /dev/null
+++ b/src/cmd/pkg_create.yaml.in
@@ -0,0 +1,8 @@
+--- # Model subcommand available inside ignition gazebo.
+format: 1.0.0
+library_name: ignition-gazebo-ign
+library_version: @PROJECT_VERSION_FULL@
+library_path: @ign_model_ruby_path@
+commands:
+    - pkg_create   : Create a new ignition package
+---
diff --git a/src/cmd/resources/CMakeLists.erb b/src/cmd/resources/CMakeLists.erb
new file mode 100644
index 0000000000..92cc91ea47
--- /dev/null
+++ b/src/cmd/resources/CMakeLists.erb
@@ -0,0 +1,80 @@
+#These lines will set minimum required version of cmake and declare the name of the project.
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
+project(<%= options['name'] %>)
+<% if options['ign_dependecies']==1 %>
+
+#This is a list of general packages on which your ignition package will depend,find_package function will find and source theses packages.
+#You can add or delete dependencies based on your requirement.
+<% if options['built_type']=='ament_cmake' %>
+#Since,the package is amnet dependent we will find and call ament_cmake.
+
+find_package(ament_cmake REQUIRED)
+<% end %>
+
+find_package(ignition-cmake<%= dependencies_dict["ign-cmake"]%> REQUIRED)
+
+find_package(ignition-gazebo<%= dependencies_dict["ign-gazebo"]%> REQUIRED COMPONENTS gui)
+set(IGN_GAZEBO_VER ${ignition-gazebo<%= dependencies_dict["ign-gazebo"]%>_VERSION_MAJOR})
+
+find_package(ignition-gui<%= dependencies_dict["ign-gui"]%> REQUIRED)
+set(IGN_GUI_VER ${ignition-gui<%= dependencies_dict["ign-gui"]%>_VERSION_MAJOR})
+
+find_package(ignition-rendering<%= dependencies_dict["ign-rendering"]%> REQUIRED)
+set(IGN_RENDERING_VER ${ignition-rendering<%= dependencies_dict["ign-rendering"]%>_VERSION_MAJOR})
+
+find_package(ignition-sensors<%= dependencies_dict["ign-sensors"]%> REQUIRED)
+set(IGN_SENSORS_VER ${ignition-sensors<%= dependencies_dict["ign-sensors"]%>_VERSION_MAJOR})
+
+find_package(ignition-msgs<%= dependencies_dict["ign-msgs"]%> REQUIRED)
+set(IGN_MSGS_VER ${ignition-msgs<%= dependencies_dict["ign-msgs"]%>_VERSION_MAJOR})
+
+find_package(ignition-plugin<%= dependencies_dict["ign-plugin"]%> REQUIRED COMPONENTS register)
+set(IGN_PLUGIN_VER ${ignition-plugin<%= dependencies_dict["ign-plugin"]%>_VERSION_MAJOR})
+
+find_package(ignition-utils<%= dependencies_dict["ign-utils"]%> REQUIRED)
+set(IGN_UTILS_VER ${ignition-utils<%= dependencies_dict["ign-utils"]%>_VERSION_MAJOR})
+
+find_package(ignition-common<%= dependencies_dict["ign-common"]%> REQUIRED COMPONENTS profiler)
+set(IGN_COMMON_VER ${ignition-common<%= dependencies_dict["ign-common"]%>_VERSION_MAJOR})
+<% end %>
+<% if options['init-with-example-system']==1 %>
+
+#These will install directories with various resources like worlds,plugins etc.
+ign_add_resources(worlds)
+ign_add_resources(models)
+ign_export_variable(LD_LIBRARY_PATH @CMAKE_INSTALL_PREFIX@/lib)
+ign_add_plugins(plugins COMMON_PUBLIC_LIBRARIES 
+ ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}
+ ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+ ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER})
+ign_add_executables(src COMMON_PUBLIC_LIBRARIES 
+ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER})
+
+#This line will export paths of all the above mentioned resources.
+<% if options['export_type']=="colcon"%>
+ign_environment_hook(colcon)
+<% end %>
+<% if options['export_type']=="plain_cmake"%>
+ign_environment_hook(plain-cmake)
+<% end %>
+<% end %>
+
+#These line will run tests for the package, if build testing is set to be TRUE
+#You can add your tests here
+
+if(BUILD_TESTING)
+
+<% if options['built_type']=='ament_cmake' %>
+find_package(ament_cmake_gtest REQUIRED)
+<% end %>
+set("PROJECT_BINARY_PATH" ${CMAKE_CURRENT_BINARY_DIR})
+set("PROJECT_SOURCE_PATH" ${CMAKE_CURRENT_SOURCE_DIR})
+
+endif()
+<% if options['built_type']=='ament_cmake' %>
+
+#The project setup is done by ament_package().
+#It installs the package.xml, registers the package with the ament index, and installs config (and possibly target) files
+#for CMake so that it can be found by other packages using find_package.
+ament_package()
+<% end %>
diff --git a/src/cmd/resources/executables/random_points.cc b/src/cmd/resources/executables/random_points.cc
new file mode 100644
index 0000000000..1bdf3e0ece
--- /dev/null
+++ b/src/cmd/resources/executables/random_points.cc
@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+//This is a sample code showing how an Gazebo simulator executable works
+#include <iostream>
+#include <ignition/msgs.hh>
+int main()
+{
+  ignition::msgs::Vector3d point1;
+  point1.set_x(std::rand());
+  point1.set_y(std::rand());
+  point1.set_z(std::rand());
+  ignition::msgs::Vector3d point2;
+  point2.set_x(std::rand());
+  point2.set_y(std::rand());
+  point2.set_z(std::rand());
+  std::cout << "Random_point1:\n" << point1.DebugString() << std::endl;
+  std::cout << "Random_point2:\n" << point2.DebugString() << std::endl;
+  return 0;
+}
\ No newline at end of file
diff --git a/src/cmd/resources/models/elevator/model.config b/src/cmd/resources/models/elevator/model.config
new file mode 100644
index 0000000000..03d08b5b9b
--- /dev/null
+++ b/src/cmd/resources/models/elevator/model.config
@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+  <name>Elevator</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+  <author>
+    <name>Nick Lamprianidis</name>
+    <email>nlamprian@gmail.com</email>
+  </author>
+  <description>An elevator with configurable number of floors.</description>
+</model>
diff --git a/src/cmd/resources/models/elevator/model.sdf b/src/cmd/resources/models/elevator/model.sdf
new file mode 100644
index 0000000000..7ec8b2e2d2
--- /dev/null
+++ b/src/cmd/resources/models/elevator/model.sdf
@@ -0,0 +1,1258 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <model name="elevator">
+    <pose>0 0 0.2 0 0 0</pose>
+    <!-- ========================= START OF WALLS ========================== -->
+    <link name="structure">
+      <kinematic>true</kinematic>
+      <pose>0.0 0.0 0.0 0 0 0</pose>
+      <inertial>
+        <mass>1000</mass>
+        <inertia>
+          <ixx>10974.166666666666</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>10854.166666666666</iyy>
+          <iyz>0</iyz>
+          <izz>546.6666666666667</izz>
+        </inertia>
+        <pose>0 0 3.2 0 0 0</pose>
+      </inertial>
+      <visual name="wall_front_bottom_visual_0">
+        <pose>0.875 0.0 -0.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_bottom_collision_0">
+        <pose>0.875 0.0 -0.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 0.2</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_top_visual_0">
+        <pose>0.875 0.0 2.5 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 1.0</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_top_collision_0">
+        <pose>0.875 0.0 2.5 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 1.0</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_right_visual_0">
+        <pose>0.875 -0.925 1.0 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_right_collision_0">
+        <pose>0.875 -0.925 1.0 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_left_visual_0">
+        <pose>0.875 0.925 1.0 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_left_collision_0">
+        <pose>0.875 0.925 1.0 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_right_visual_0">
+        <pose>0.0 -1.225 1.4 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 3.2</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_right_collision_0">
+        <pose>0.0 -1.225 1.4 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 3.2</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_left_visual_0">
+        <pose>0.0 1.225 1.4 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 3.2</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_left_collision_0">
+        <pose>0.0 1.225 1.4 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 3.2</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_rear_visual_0">
+        <pose>-1.025 0.0 1.4 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.6 3.2</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_rear_collision_0">
+        <pose>-1.025 0.0 1.4 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.6 3.2</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="entrance_visual_0">
+        <pose>1.95 0.0 -0.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>2.0 4.0 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="entrance_collision_0">
+        <pose>1.95 0.0 -0.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>2.0 4.0 0.2</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_bottom_visual_1">
+        <pose>0.875 0.0 3.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_bottom_collision_1">
+        <pose>0.875 0.0 3.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 0.2</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_top_visual_1">
+        <pose>0.875 0.0 5.45 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 0.5</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_top_collision_1">
+        <pose>0.875 0.0 5.45 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 0.5</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_right_visual_1">
+        <pose>0.875 -0.925 4.2 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_right_collision_1">
+        <pose>0.875 -0.925 4.2 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_left_visual_1">
+        <pose>0.875 0.925 4.2 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_left_collision_1">
+        <pose>0.875 0.925 4.2 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_right_visual_1">
+        <pose>0.0 -1.225 4.35 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 2.7</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_right_collision_1">
+        <pose>0.0 -1.225 4.35 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 2.7</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_left_visual_1">
+        <pose>0.0 1.225 4.35 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 2.7</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_left_collision_1">
+        <pose>0.0 1.225 4.35 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 2.7</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_rear_visual_1">
+        <pose>-1.025 0.0 4.35 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.6 2.7</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_rear_collision_1">
+        <pose>-1.025 0.0 4.35 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.6 2.7</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="entrance_visual_1">
+        <pose>1.95 0.0 3.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>2.0 4.0 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="entrance_collision_1">
+        <pose>1.95 0.0 3.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>2.0 4.0 0.2</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_bottom_visual_2">
+        <pose>0.875 0.0 5.8 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_bottom_collision_2">
+        <pose>0.875 0.0 5.8 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 0.2</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_top_visual_2">
+        <pose>0.875 0.0 8.15 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 0.5</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_top_collision_2">
+        <pose>0.875 0.0 8.15 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 0.5</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_right_visual_2">
+        <pose>0.875 -0.925 6.9 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_right_collision_2">
+        <pose>0.875 -0.925 6.9 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_left_visual_2">
+        <pose>0.875 0.925 6.9 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_left_collision_2">
+        <pose>0.875 0.925 6.9 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_right_visual_2">
+        <pose>0.0 -1.225 7.05 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 2.7</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_right_collision_2">
+        <pose>0.0 -1.225 7.05 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 2.7</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_left_visual_2">
+        <pose>0.0 1.225 7.05 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 2.7</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_left_collision_2">
+        <pose>0.0 1.225 7.05 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 2.7</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_rear_visual_2">
+        <pose>-1.025 0.0 7.05 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.6 2.7</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_rear_collision_2">
+        <pose>-1.025 0.0 7.05 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.6 2.7</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="entrance_visual_2">
+        <pose>1.95 0.0 5.8 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>2.0 4.0 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="entrance_collision_2">
+        <pose>1.95 0.0 5.8 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>2.0 4.0 0.2</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_bottom_visual_3">
+        <pose>0.875 0.0 8.5 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_bottom_collision_3">
+        <pose>0.875 0.0 8.5 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 0.2</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_top_visual_3">
+        <pose>0.875 0.0 10.85 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 0.5</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_top_collision_3">
+        <pose>0.875 0.0 10.85 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.3 0.5</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_right_visual_3">
+        <pose>0.875 -0.925 9.6 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_right_collision_3">
+        <pose>0.875 -0.925 9.6 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_front_left_visual_3">
+        <pose>0.875 0.925 9.6 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_front_left_collision_3">
+        <pose>0.875 0.925 9.6 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 0.45 2.0</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_right_visual_3">
+        <pose>0.0 -1.225 9.75 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 2.7</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_right_collision_3">
+        <pose>0.0 -1.225 9.75 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 2.7</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_left_visual_3">
+        <pose>0.0 1.225 9.75 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 2.7</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_left_collision_3">
+        <pose>0.0 1.225 9.75 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.9 0.15 2.7</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="wall_rear_visual_3">
+        <pose>-1.025 0.0 9.75 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.6 2.7</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="wall_rear_collision_3">
+        <pose>-1.025 0.0 9.75 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.15 2.6 2.7</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="entrance_visual_3">
+        <pose>1.95 0.0 8.5 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>2.0 4.0 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 1.0 1.0 1</ambient>
+          <diffuse>1.0 1.0 1.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="entrance_collision_3">
+        <pose>1.95 0.0 8.5 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>2.0 4.0 0.2</size>
+          </box>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="foundation" type="fixed">
+      <parent>world</parent>
+      <child>structure</child>
+    </joint>
+    <!-- ========================== END OF WALLS =========================== -->
+    <!-- ========================= START OF DOORS ========================== -->
+    <link name="door_panel_0">
+      <pose>0.85 0.0 1.0 0 0 0</pose>
+      <inertial>
+        <mass>20</mass>
+        <inertia>
+          <ixx>9.933333333333334</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>6.683333333333333</iyy>
+          <iyz>0</iyz>
+          <izz>3.2833333333333328</izz>
+        </inertia>
+        <pose>0 0 0 0 0 0</pose>
+      </inertial>
+      <visual name="door_visual_0">
+        <geometry>
+          <box>
+            <size>0.1 1.4 2.0</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 0.0 0.0 1</ambient>
+          <diffuse>1.0 0.0 0.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="door_collision_0">
+        <geometry>
+          <box>
+            <size>0.1 1.4 2.0</size>
+          </box>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="door_0" type="prismatic">
+      <parent>structure</parent>
+      <child>door_panel_0</child>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>0</lower>
+          <upper>1.4</upper>
+          <effort>5000</effort>
+          <velocity>0.1</velocity>
+        </limit>
+        <dynamics>
+          <damping>30</damping>
+        </dynamics>
+      </axis>
+    </joint>
+    <plugin filename="ignition-gazebo-joint-position-controller-system" name="ignition::gazebo::systems::JointPositionController">
+      <joint_name>door_0</joint_name>
+      <p_gain>60</p_gain>
+      <i_gain>0</i_gain>
+      <d_gain>40</d_gain>
+      <cmd_min>-20</cmd_min>
+      <cmd_max>20</cmd_max>
+    </plugin>
+    <link name="door_panel_1">
+      <pose>0.85 0.0 4.2 0 0 0</pose>
+      <inertial>
+        <mass>20</mass>
+        <inertia>
+          <ixx>9.933333333333334</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>6.683333333333333</iyy>
+          <iyz>0</iyz>
+          <izz>3.2833333333333328</izz>
+        </inertia>
+        <pose>0 0 0 0 0 0</pose>
+      </inertial>
+      <visual name="door_visual_1">
+        <geometry>
+          <box>
+            <size>0.1 1.4 2.0</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 0.0 0.0 1</ambient>
+          <diffuse>1.0 0.0 0.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="door_collision_1">
+        <geometry>
+          <box>
+            <size>0.1 1.4 2.0</size>
+          </box>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="door_1" type="prismatic">
+      <parent>structure</parent>
+      <child>door_panel_1</child>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>0</lower>
+          <upper>1.4</upper>
+          <effort>5000</effort>
+          <velocity>0.1</velocity>
+        </limit>
+        <dynamics>
+          <damping>30</damping>
+        </dynamics>
+      </axis>
+    </joint>
+    <plugin filename="ignition-gazebo-joint-position-controller-system" name="ignition::gazebo::systems::JointPositionController">
+      <joint_name>door_1</joint_name>
+      <p_gain>60</p_gain>
+      <i_gain>0</i_gain>
+      <d_gain>40</d_gain>
+      <cmd_min>-20</cmd_min>
+      <cmd_max>20</cmd_max>
+    </plugin>
+    <link name="door_panel_2">
+      <pose>0.85 0.0 6.9 0 0 0</pose>
+      <inertial>
+        <mass>20</mass>
+        <inertia>
+          <ixx>9.933333333333334</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>6.683333333333333</iyy>
+          <iyz>0</iyz>
+          <izz>3.2833333333333328</izz>
+        </inertia>
+        <pose>0 0 0 0 0 0</pose>
+      </inertial>
+      <visual name="door_visual_2">
+        <geometry>
+          <box>
+            <size>0.1 1.4 2.0</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 0.0 0.0 1</ambient>
+          <diffuse>1.0 0.0 0.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="door_collision_2">
+        <geometry>
+          <box>
+            <size>0.1 1.4 2.0</size>
+          </box>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="door_2" type="prismatic">
+      <parent>structure</parent>
+      <child>door_panel_2</child>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>0</lower>
+          <upper>1.4</upper>
+          <effort>5000</effort>
+          <velocity>0.1</velocity>
+        </limit>
+        <dynamics>
+          <damping>30</damping>
+        </dynamics>
+      </axis>
+    </joint>
+    <plugin filename="ignition-gazebo-joint-position-controller-system" name="ignition::gazebo::systems::JointPositionController">
+      <joint_name>door_2</joint_name>
+      <p_gain>60</p_gain>
+      <i_gain>0</i_gain>
+      <d_gain>40</d_gain>
+      <cmd_min>-20</cmd_min>
+      <cmd_max>20</cmd_max>
+    </plugin>
+    <link name="door_panel_3">
+      <pose>0.85 0.0 9.6 0 0 0</pose>
+      <inertial>
+        <mass>20</mass>
+        <inertia>
+          <ixx>9.933333333333334</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>6.683333333333333</iyy>
+          <iyz>0</iyz>
+          <izz>3.2833333333333328</izz>
+        </inertia>
+        <pose>0 0 0 0 0 0</pose>
+      </inertial>
+      <visual name="door_visual_3">
+        <geometry>
+          <box>
+            <size>0.1 1.4 2.0</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1.0 0.0 0.0 1</ambient>
+          <diffuse>1.0 0.0 0.0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="door_collision_3">
+        <geometry>
+          <box>
+            <size>0.1 1.4 2.0</size>
+          </box>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="door_3" type="prismatic">
+      <parent>structure</parent>
+      <child>door_panel_3</child>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>0</lower>
+          <upper>1.4</upper>
+          <effort>5000</effort>
+          <velocity>0.1</velocity>
+        </limit>
+        <dynamics>
+          <damping>30</damping>
+        </dynamics>
+      </axis>
+    </joint>
+    <plugin filename="ignition-gazebo-joint-position-controller-system" name="ignition::gazebo::systems::JointPositionController">
+      <joint_name>door_3</joint_name>
+      <p_gain>60</p_gain>
+      <i_gain>0</i_gain>
+      <d_gain>40</d_gain>
+      <cmd_min>-20</cmd_min>
+      <cmd_max>20</cmd_max>
+    </plugin>
+    <!-- ========================== END OF DOORS =========================== -->
+    <!-- ========================= START OF CABIN ========================== -->
+    <link name="cabin">
+      <pose>0.0 0.0 0.0 0 0 0</pose>
+      <inertial>
+        <mass>200</mass>
+        <inertia>
+          <ixx>147.33333333333334</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>123.33333333333336</iyy>
+          <iyz>0</iyz>
+          <izz>109.33333333333333</izz>
+        </inertia>
+        <pose>0 0 1.1 0 0 0</pose>
+      </inertial>
+      <light type="spot" name="spot_0">
+        <pose>0 -0.5 2.09 0 0 0</pose>
+        <diffuse>1 1 1 1</diffuse>
+        <specular>.2 .2 .2 1</specular>
+        <attenuation>
+          <range>2.2</range>
+          <linear>0.2</linear>
+          <constant>0.05</constant>
+          <quadratic>0.01</quadratic>
+        </attenuation>
+        <direction>0 0 1</direction>
+        <spot>
+          <inner_angle>0.1</inner_angle>
+          <outer_angle>1.0</outer_angle>
+          <falloff>0.8</falloff>
+        </spot>
+        <cast_shadows>true</cast_shadows>
+      </light>
+      <light type="spot" name="spot_1">
+        <pose>0 0.5 2.09 0 0 0</pose>
+        <diffuse>1 1 1 1</diffuse>
+        <specular>.2 .2 .2 1</specular>
+        <attenuation>
+          <range>2.2</range>
+          <linear>0.2</linear>
+          <constant>0.05</constant>
+          <quadratic>0.01</quadratic>
+        </attenuation>
+        <direction>0 0 1</direction>
+        <spot>
+          <inner_angle>0.1</inner_angle>
+          <outer_angle>1.0</outer_angle>
+          <falloff>0.8</falloff>
+        </spot>
+        <cast_shadows>true</cast_shadows>
+      </light>
+      <visual name="cabin_bottom_visual">
+        <pose>0 0 -0.05 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.6 2.0 0.1</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>0.175 0.175 0.175 1</ambient>
+          <diffuse>0.175 0.175 0.175 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="cabin_bottom_collision">
+        <pose>0 0 -0.05 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.6 2.0 0.1</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="cabin_top_visual">
+        <pose>0 0 2.25 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.6 2.0 0.1</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>0.175 0.175 0.175 1</ambient>
+          <diffuse>0.175 0.175 0.175 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="cabin_top_collision">
+        <pose>0 0 2.25 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.6 2.0 0.1</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="cabin_right_visual">
+        <pose>0 -1.05 1.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.6 0.1 2.4</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>0.175 0.175 0.175 1</ambient>
+          <diffuse>0.175 0.175 0.175 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="cabin_right_collision">
+        <pose>0 -1.05 1.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.6 0.1 2.4</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="cabin_left_visual">
+        <pose>0 1.05 1.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.6 0.1 2.4</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>0.175 0.175 0.175 1</ambient>
+          <diffuse>0.175 0.175 0.175 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="cabin_left_collision">
+        <pose>0 1.05 1.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>1.6 0.1 2.4</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="cabin_rear_visual">
+        <pose>-0.85 0 1.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.1 2.2 2.4</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>0.175 0.175 0.175 1</ambient>
+          <diffuse>0.175 0.175 0.175 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+      <collision name="cabin_rear_collision">
+        <pose>-0.85 0 1.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.1 2.2 2.4</size>
+          </box>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="lift" type="prismatic">
+      <parent>structure</parent>
+      <child>cabin</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>0</lower>
+          <upper>8.600000000000001</upper>
+          <effort>50000000</effort>
+          <velocity>0.5</velocity>
+        </limit>
+        <dynamics>
+          <damping>180000</damping>
+        </dynamics>
+      </axis>
+    </joint>
+    <plugin filename="ignition-gazebo-joint-position-controller-system" name="ignition::gazebo::systems::JointPositionController">
+      <joint_name>lift</joint_name>
+      <p_gain>4000000</p_gain>
+      <i_gain>2000</i_gain>
+      <d_gain>5000</d_gain>
+      <i_min>-2000</i_min>
+      <i_max>2000</i_max>
+      <cmd_min>-80000</cmd_min>
+      <cmd_max>120000</cmd_max>
+    </plugin>
+    <!-- ========================== END OF CABIN =========================== -->
+    <!-- =================== START OF FLOOR REFERENCES ===================== -->
+    <link name="floor_0">
+      <pose>0.0 0.0 0.0 0 0 0</pose>
+    </link>
+    <joint name="floor_0_joint" type="fixed">
+      <parent>structure</parent>
+      <child>floor_0</child>
+    </joint>
+    <link name="floor_1">
+      <pose>0.0 0.0 3.2 0 0 0</pose>
+    </link>
+    <joint name="floor_1_joint" type="fixed">
+      <parent>structure</parent>
+      <child>floor_1</child>
+    </joint>
+    <link name="floor_2">
+      <pose>0.0 0.0 5.9 0 0 0</pose>
+    </link>
+    <joint name="floor_2_joint" type="fixed">
+      <parent>structure</parent>
+      <child>floor_2</child>
+    </joint>
+    <link name="floor_3">
+      <pose>0.0 0.0 8.6 0 0 0</pose>
+    </link>
+    <joint name="floor_3_joint" type="fixed">
+      <parent>structure</parent>
+      <child>floor_3</child>
+    </joint>
+    <!-- ==================== END OF FLOOR REFERENCES ====================== -->
+    <!-- ===================== START OF DOOR LIDARS ======================== -->
+    <link name="door_lidar_0">
+      <pose>0.875 -0.7 0.2 1.5707963267948966 0 1.5707963267948966</pose>
+      <sensor name="sensor" type="gpu_lidar">
+        <topic>model/elevator/door_0/lidar</topic>
+        <update_rate>10</update_rate>
+        <lidar>
+          <scan>
+            <horizontal>
+              <samples>1</samples>
+              <resolution>1</resolution>
+              <min_angle>0</min_angle>
+              <max_angle>0</max_angle>
+            </horizontal>
+          </scan>
+          <range>
+            <min>0.001</min>
+            <max>1.395</max>
+            <resolution>0.01</resolution>
+          </range>
+        </lidar>
+      </sensor>
+    </link>
+    <joint name="door_lidar_0_joint" type="fixed">
+      <parent>structure</parent>
+      <child>door_lidar_0</child>
+    </joint>
+    <link name="door_lidar_1">
+      <pose>0.875 -0.7 3.4 1.5707963267948966 0 1.5707963267948966</pose>
+      <sensor name="sensor" type="gpu_lidar">
+        <topic>model/elevator/door_1/lidar</topic>
+        <update_rate>10</update_rate>
+        <lidar>
+          <scan>
+            <horizontal>
+              <samples>1</samples>
+              <resolution>1</resolution>
+              <min_angle>0</min_angle>
+              <max_angle>0</max_angle>
+            </horizontal>
+          </scan>
+          <range>
+            <min>0.001</min>
+            <max>1.395</max>
+            <resolution>0.01</resolution>
+          </range>
+        </lidar>
+      </sensor>
+    </link>
+    <joint name="door_lidar_1_joint" type="fixed">
+      <parent>structure</parent>
+      <child>door_lidar_1</child>
+    </joint>
+    <link name="door_lidar_2">
+      <pose>0.875 -0.7 6.1 1.5707963267948966 0 1.5707963267948966</pose>
+      <sensor name="sensor" type="gpu_lidar">
+        <topic>model/elevator/door_2/lidar</topic>
+        <update_rate>10</update_rate>
+        <lidar>
+          <scan>
+            <horizontal>
+              <samples>1</samples>
+              <resolution>1</resolution>
+              <min_angle>0</min_angle>
+              <max_angle>0</max_angle>
+            </horizontal>
+          </scan>
+          <range>
+            <min>0.001</min>
+            <max>1.395</max>
+            <resolution>0.01</resolution>
+          </range>
+        </lidar>
+      </sensor>
+    </link>
+    <joint name="door_lidar_2_joint" type="fixed">
+      <parent>structure</parent>
+      <child>door_lidar_2</child>
+    </joint>
+    <link name="door_lidar_3">
+      <pose>0.875 -0.7 8.8 1.5707963267948966 0 1.5707963267948966</pose>
+      <sensor name="sensor" type="gpu_lidar">
+        <topic>model/elevator/door_3/lidar</topic>
+        <update_rate>10</update_rate>
+        <lidar>
+          <scan>
+            <horizontal>
+              <samples>1</samples>
+              <resolution>1</resolution>
+              <min_angle>0</min_angle>
+              <max_angle>0</max_angle>
+            </horizontal>
+          </scan>
+          <range>
+            <min>0.001</min>
+            <max>1.395</max>
+            <resolution>0.01</resolution>
+          </range>
+        </lidar>
+      </sensor>
+    </link>
+    <joint name="door_lidar_3_joint" type="fixed">
+      <parent>structure</parent>
+      <child>door_lidar_3</child>
+    </joint>
+    <!-- ====================== END OF DOOR LIDARS ========================= -->
+    <!-- ====================== START OF CONTROLLER ======================== -->
+    <plugin filename="ignition-gazebo-elevator-system" name="ignition::gazebo::systems::Elevator">
+    </plugin>
+    <!-- ======================= END OF CONTROLLER ========================= -->
+  </model>
+</sdf>
diff --git a/src/cmd/resources/package_xml.erb b/src/cmd/resources/package_xml.erb
new file mode 100644
index 0000000000..75a6600362
--- /dev/null
+++ b/src/cmd/resources/package_xml.erb
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+<name> <%= options['name'] %> </name>
+<version>0.0.0</version>
+<description>Add a short description of your project</description>
+<maintainer email="youremail@domain.com">Your Name</maintainer>
+<license>Apache License 2.0</license>
+<author>Your Name</author>
+<buildtool_depend>ament_cmake</buildtool_depend>
+<exec_depend>ament_index_python</exec_depend>
+<exec_depend>launch</exec_depend>
+<exec_depend>launch_ros</exec_depend>
+<exec_depend>xacro</exec_depend>
+<test_depend>ament_cmake_gtest</test_depend>
+<test_depend>ament_lint_auto</test_depend>
+<test_depend>ament_lint_common</test_depend>
+<export>
+  <build_type>ament_cmake</build_type>
+</export>
+</package>
diff --git a/src/cmd/resources/plugins/HelloWorld.cc b/src/cmd/resources/plugins/HelloWorld.cc
new file mode 100644
index 0000000000..6d260437bc
--- /dev/null
+++ b/src/cmd/resources/plugins/HelloWorld.cc
@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+//This is a sample code showing how an Gazebo simulator plugins works
+// We'll use a string and the ignmsg command below for a brief example.
+// Remove these includes if your plugin doesn't need them.
+#include <string>
+#include <ignition/common/Console.hh>
+
+// This header is required to register plugins. It's good practice to place it
+// in the cc file, like it's done here.
+#include <ignition/plugin/Register.hh>
+
+// Don't forget to include the plugin's header.
+#include "HelloWorld.hh"
+
+// This is required to register the plugin. Make sure the interfaces match
+// what's in the header.
+IGNITION_ADD_PLUGIN(
+    hello_world::HelloWorld,
+    ignition::gazebo::System,
+    hello_world::HelloWorld::ISystemPostUpdate)
+
+using namespace hello_world;
+
+// Here we implement the PostUpdate function, which is called at every
+// iteration.
+void HelloWorld::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
+    const ignition::gazebo::EntityComponentManager &/*_ecm*/)
+{
+  // This is a simple example of how to get information from UpdateInfo.
+  std::string msg = "Hello, world! Simulation is ";
+  if (!_info.paused)
+    msg += "not ";
+  msg += "paused.";
+
+  // Messages printed with ignmsg only show when running with verbosity 3 or
+  // higher (i.e. ign gazebo -v 3)
+  ignmsg << msg << std::endl;
+}
+
+
diff --git a/src/cmd/resources/plugins/HelloWorld.hh b/src/cmd/resources/plugins/HelloWorld.hh
new file mode 100644
index 0000000000..ee93c28207
--- /dev/null
+++ b/src/cmd/resources/plugins/HelloWorld.hh
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef SYSTEM_PLUGIN_HELLOWORLD_HH_
+#define SYSTEM_PLUGIN_HELLOWORLD_HH_
+
+// The only required include in the header is this one.
+// All others will depend on what your plugin does.
+#include <ignition/gazebo/System.hh>
+
+// It's good practice to use a custom namespace for your project.
+namespace hello_world
+{
+  // This is the main plugin's class. It must inherit from System and at least
+  // one other interface.
+  // Here we use `ISystemPostUpdate`, which is used to get results after
+  // physics runs. The opposite of that, `ISystemPreUpdate`, would be used by
+  // plugins that want to send commands.
+  class HelloWorld:
+    public ignition::gazebo::System,
+    public ignition::gazebo::ISystemPostUpdate
+  {
+    // Plugins inheriting ISystemPostUpdate must implement the PostUpdate
+    // callback. This is called at every simulation iteration after the physics
+    // updates the world. The _info variable provides information such as time,
+    // while the _ecm provides an interface to all entities and components in
+    // simulation.
+    public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info,
+                const ignition::gazebo::EntityComponentManager &_ecm) override;
+  };
+}
+#endif
diff --git a/src/cmd/resources/worlds/empty_platform_with_elevator.sdf b/src/cmd/resources/worlds/empty_platform_with_elevator.sdf
new file mode 100644
index 0000000000..abd487508b
--- /dev/null
+++ b/src/cmd/resources/worlds/empty_platform_with_elevator.sdf
@@ -0,0 +1,102 @@
+<?xml version="1.0" ?>
+
+<sdf version="1.6">
+  <world name="empty_platform_with_elevator">
+
+    <physics name="4ms" type="dart">
+      <max_step_size>0.004</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-user-commands-system"
+      name="ignition::gazebo::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-sensors-system"
+      name="ignition::gazebo::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-air-pressure-system"
+      name="ignition::gazebo::systems::AirPressure">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-altimeter-system"
+      name="ignition::gazebo::systems::Altimeter">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-imu-system"
+      name="ignition::gazebo::systems::Imu">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-magnetometer-system"
+      name="ignition::gazebo::systems::Magnetometer">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-particle-emitter2-system"
+      name="ignition::gazebo::systems::ParticleEmitter2">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+
+    <scene>
+      <grid>false</grid>
+      <ambient>1.0 1.0 1.0</ambient>
+      <background>0.8 0.8 0.8</background>
+    </scene>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_platform">
+      <static>true</static>
+      <pose>0 0 -1.5 0 0 0</pose>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>20 20 3</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>20 20 3</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+<include>
+    <uri>
+      model://elevator
+    </uri>
+</include>
+  <plugin filename="HelloWorld" name="hello_world::HelloWorld">
+  </plugin>
+  </world>
+</sdf>
diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc
index 233ae638fa..9e2fa7c49f 100644
--- a/test/integration/log_system.cc
+++ b/test/integration/log_system.cc
@@ -1642,4 +1642,4 @@ TEST_F(LogSystemTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LogTopics))
   this->RemoveLogsDir();
   this->CreateLogsDir();
 #endif
-}
+}
\ No newline at end of file