diff --git a/configs/spotmicro_config/CMakeLists.txt b/configs/spotmicro_config/CMakeLists.txt
index 010f26d..ac96358 100644
--- a/configs/spotmicro_config/CMakeLists.txt
+++ b/configs/spotmicro_config/CMakeLists.txt
@@ -1,6 +1,11 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(spotmicro_config)
 
-find_package(catkin REQUIRED COMPONENTS)
+find_package(ament_cmake REQUIRED)
 
-catkin_package()
\ No newline at end of file
+install(
+  DIRECTORY launch config maps worlds
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
\ No newline at end of file
diff --git a/configs/spotmicro_config/README.md b/configs/spotmicro_config/README.md
index 32aa0b4..4216529 100644
--- a/configs/spotmicro_config/README.md
+++ b/configs/spotmicro_config/README.md
@@ -7,24 +7,20 @@ You don't need a physical robot to run the following demos.
 
 #### 1.1.1. Run the base driver:
 
-    roslaunch spotmicro_config bringup.launch rviz:=true
+    ros2 launch spotmicro_config bringup.launch.py rviz:=true
 
-#### 1.1.2. Run the teleop node:
-
-    roslaunch champ_teleop teleop.launch
 
-If you want to use a [joystick](https://www.logitechg.com/en-hk/products/gamepads/f710-wireless-gamepad.html) add joy:=true as an argument.
+#### 1.1.2. Run the teleop node:
 
+    ros2 launch champ_teleop teleop.launch.py
 
 ### 1.2. SLAM demo:
 
 #### 1.2.1. Run the Gazebo environment:
 
-    roslaunch spotmicro_config gazebo.launch 
+    ros2 launch spotmicro_config gazebo.launch.py 
 
-#### 1.2.2. Run gmapping package and move_base:
-
-    roslaunch spotmicro_config slam.launch rviz:=true
+#### 1.2.2 Run [Nav2](https://navigation.ros.org/)'s navigation and [slam_toolbox](https://github.com/SteveMacenski/slam_toolbox):
 
 To start mapping:
 
@@ -35,18 +31,21 @@ To start mapping:
 
 - Save the map by running:
 
-      roscd spotmicro_config/maps
-      rosrun map_server map_saver
+    cd spotmicro_config/maps
+    ros2 run nav2_map_server map_saver_cli -f new_map
+
+After this, you can use the new_map to do pure navigation.
+
 
 ### 1.3. Autonomous Navigation:
 
 #### 1.3.1. Run the Gazebo environment: 
 
-    roslaunch spotmicro_config gazebo.launch 
+    ros2 launch spotmicro_config gazebo.launch.py
 
-#### 1.3.2. Run amcl and move_base:
+#### 1.3.2 Run [Nav2](https://navigation.ros.org/):
 
-    roslaunch spotmicro_config navigate.launch rviz:=true
+    ros2 launch spotmicro_config navigate.launch.py rviz:=true
 
 To navigate:
 
diff --git a/configs/spotmicro_config/config/autonomy/navigation.yaml b/configs/spotmicro_config/config/autonomy/navigation.yaml
new file mode 100644
index 0000000..d5d81f6
--- /dev/null
+++ b/configs/spotmicro_config/config/autonomy/navigation.yaml
@@ -0,0 +1,383 @@
+amcl:
+  ros__parameters:
+    use_sim_time: True
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "nav2_amcl::DifferentialMotionModel"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan
+    set_initial_pose: True
+    initial_pose: 
+      x: 0.0
+      y: 0.0
+      yaw: 0.6
+
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: True
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    enable_groot_monitoring: True
+    groot_zmq_publisher_port: 1666
+    groot_zmq_server_port: 1667
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_single_trigger_bt_node
+    - nav2_is_battery_low_condition_bt_node
+    - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
+    - nav2_planner_selector_bt_node
+    - nav2_controller_selector_bt_node
+    - nav2_goal_checker_selector_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+controller_server:
+  ros__parameters:
+    use_sim_time: True
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+
+    # Goal checker parameters
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.4
+      max_vel_y: 0.0
+      max_vel_theta: 0.75
+      min_speed_xy: 0.0
+      max_speed_xy: 0.4
+      min_speed_theta: 0.0
+      # Add high threshold velocity for turtlebot 3 issue.
+      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2
+      decel_lim_x: -2.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -3.2
+      vx_samples: 20
+      vy_samples: 5
+      vtheta_samples: 20
+      sim_time: 1.7
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+      transform_tolerance: 0.2
+      xy_goal_tolerance: 0.25
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: True
+      stateful: True
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+      
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.22
+      plugins: ["voxel2d_layer", "voxel3d_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      voxel2d_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: False
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan base_scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+        base_scan:
+          topic: /base/scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+      voxel3d_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: False
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        mark_threshold: 1
+        observation_sources: realsense zed
+        min_obstacle_height: 0.00
+        max_obstacle_height: 2.0 
+        realsense:
+          topic: /camera/depth/color/points
+          raytrace_min_range: 0.5
+          min_obstacle_height: 0.0
+          max_obstacle_height: 2.0 
+          clearing: True
+          marking: True
+          data_type: "PointCloud2"
+        zed:
+          topic: /zed/point_cloud/cloud_registered
+          raytrace_min_range: 0.5
+          min_obstacle_height: 0.0
+          max_obstacle_height: 2.0 
+          clearing: True
+          marking: True
+          data_type: "PointCloud2"
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: False
+      robot_radius: 0.22
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "voxel2d_layer", "voxel3d_layer", "inflation_layer"]
+      voxel2d_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan base_scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+        base_scan:
+          topic: /base/scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+      voxel3d_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: False
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        mark_threshold: 1
+        observation_sources: realsense zed
+        min_obstacle_height: 0.00
+        max_obstacle_height: 2.0 
+        realsense:
+          topic: /camera/depth/color/points
+          raytrace_min_range: 0.5
+          min_obstacle_height: 0.0
+          max_obstacle_height: 2.0 
+          clearing: True
+          marking: True
+          data_type: "PointCloud2"
+        zed:
+          topic: /zed/point_cloud/cloud_registered
+          raytrace_min_range: 0.5
+          min_obstacle_height: 0.0
+          max_obstacle_height: 2.0 
+          clearing: True
+          marking: True
+          data_type: "PointCloud2"
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+map_server:
+  ros__parameters:
+    use_sim_time: True
+    yaml_filename: "map.yaml"
+
+map_saver:
+  ros__parameters:
+    use_sim_time: True
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: True
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+recoveries_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    recovery_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_recoveries/Spin"
+    backup:
+      plugin: "nav2_recoveries/BackUp"
+    wait:
+      plugin: "nav2_recoveries/Wait"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_timeout: 0.1
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"   
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
diff --git a/configs/spotmicro_config/config/autonomy/slam.yaml b/configs/spotmicro_config/config/autonomy/slam.yaml
new file mode 100644
index 0000000..d783cb6
--- /dev/null
+++ b/configs/spotmicro_config/config/autonomy/slam.yaml
@@ -0,0 +1,73 @@
+slam_toolbox:
+  ros__parameters:
+
+    # Plugin params
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # ROS Parameters
+    odom_frame: odom
+    map_frame: map
+    base_frame: base_footprint
+    scan_topic: /scan
+    mode: mapping #localization
+
+    # if you'd like to immediately start continuing a map at a given pose
+    # or at the dock, but they are mutually exclusive, if pose is given
+    # will use pose
+    #map_file_name: test_steve
+    # map_start_pose: [0.0, 0.0, 0.0]
+    #map_start_at_dock: true
+
+    debug_logging: true
+    throttle_scans: 1
+    transform_publish_period: 0.02 #if 0 never publishes odometry
+    map_update_interval: 5.0
+    resolution: 0.05
+    max_laser_range: 5.0 #for rastering images
+    minimum_time_interval: 0.5
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.
+    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+    enable_interactive_mode: true
+
+    # General Parameters
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.5
+    minimum_travel_heading: 0.5
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 10.0
+    link_match_minimum_response_fine: 0.1  
+    link_scan_maximum_distance: 1.5
+    loop_search_maximum_distance: 3.0
+    do_loop_closing: true 
+    loop_match_minimum_chain_size: 10           
+    loop_match_maximum_variance_coarse: 3.0  
+    loop_match_minimum_response_coarse: 0.35    
+    loop_match_minimum_response_fine: 0.45
+
+    # Correlation Parameters - Correlation Parameters
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1 
+
+    # Correlation Parameters - Loop Closure Parameters
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    # Scan Matcher Parameters
+    distance_variance_penalty: 0.5      
+    angle_variance_penalty: 1.0    
+
+    fine_search_angle_offset: 0.00349     
+    coarse_search_angle_offset: 0.349   
+    coarse_angle_resolution: 0.0349        
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true
diff --git a/configs/spotmicro_config/config/gait/gait.yaml b/configs/spotmicro_config/config/gait/gait.yaml
deleted file mode 100644
index 8bc0cc4..0000000
--- a/configs/spotmicro_config/config/gait/gait.yaml
+++ /dev/null
@@ -1,11 +0,0 @@
-knee_orientation : ">>"
-pantograph_leg : false
-odom_scaler: 1.0
-max_linear_velocity_x : 0.5
-max_linear_velocity_y : 0.25
-max_angular_velocity_z : 1.0
-com_x_translation : 0.0
-swing_height : 0.02
-stance_depth : 0.0
-stance_duration : 0.25
-nominal_height : 0.15
\ No newline at end of file
diff --git a/configs/spotmicro_config/config/joints/joints.yaml b/configs/spotmicro_config/config/joints/joints.yaml
deleted file mode 100644
index 3827519..0000000
--- a/configs/spotmicro_config/config/joints/joints.yaml
+++ /dev/null
@@ -1,23 +0,0 @@
-left_front:
-  - front_left_shoulder
-  - front_left_leg
-  - front_left_foot
-  - front_left_toe
-
-right_front:
-  - front_right_shoulder
-  - front_right_leg
-  - front_right_foot
-  - front_right_toe
-
-left_hind:
-  - rear_left_shoulder
-  - rear_left_leg
-  - rear_left_foot
-  - rear_left_toe
-
-right_hind:
-  - rear_right_shoulder
-  - rear_right_leg
-  - rear_right_foot
-  - rear_right_toe
\ No newline at end of file
diff --git a/configs/spotmicro_config/config/links/links.yaml b/configs/spotmicro_config/config/links/links.yaml
deleted file mode 100644
index aa82b04..0000000
--- a/configs/spotmicro_config/config/links/links.yaml
+++ /dev/null
@@ -1,24 +0,0 @@
-base: base_link
-left_front:
-  - front_left_shoulder_link
-  - front_left_leg_link
-  - front_left_foot_link
-  - front_left_toe_link
-
-right_front:
-  - front_right_shoulder_link
-  - front_right_leg_link
-  - front_right_foot_link
-  - front_right_toe_link
-
-left_hind:
-  - rear_left_shoulder_link
-  - rear_left_leg_link
-  - rear_left_foot_link
-  - rear_left_toe_link
-
-right_hind:
-  - rear_right_shoulder_link
-  - rear_right_leg_link
-  - rear_right_foot_link
-  - rear_right_toe_link
\ No newline at end of file
diff --git a/configs/spotmicro_config/config/move_base/base_local_planner_holonomic_params.yaml b/configs/spotmicro_config/config/move_base/base_local_planner_holonomic_params.yaml
deleted file mode 100644
index d05c9d3..0000000
--- a/configs/spotmicro_config/config/move_base/base_local_planner_holonomic_params.yaml
+++ /dev/null
@@ -1,46 +0,0 @@
-DWAPlannerROS:
-  #http://wiki.ros.org/dwa_local_planner
-
-  min_vel_trans: 0.01
-  max_vel_trans: 0.5
-
-  min_vel_x: -0.025
-  max_vel_x: 0.5
-
-  min_vel_y: 0.0
-  max_vel_y: 0.0
-
-  max_vel_rot: 1.0
-  min_vel_rot: -1.0
-
-  acc_lim_trans: 1.7
-  acc_lim_x: 1.7
-  acc_lim_y: 0.0
-  acc_lim_theta: 3
-
-  trans_stopped_vel: 0.1
-  theta_stopped_vel: 0.1
-
-  xy_goal_tolerance: 0.25
-  yaw_goal_tolerance: 0.34
-
-  sim_time: 3.5
-  sim_granularity: 0.1
-  vx_samples: 20
-  vy_samples: 0
-  vth_samples: 40
-
-  path_distance_bias: 34.0
-  goal_distance_bias: 24.0
-  occdist_scale: 0.05
-
-  forward_point_distance: 0.2
-  stop_time_buffer: 0.5
-  scaling_speed: 0.25
-  max_scaling_factor: 0.2
-
-  oscillation_reset_dist: 0.05
-
-  use_dwa: true
-
-  prune_plan: false
diff --git a/configs/spotmicro_config/config/move_base/costmap_common_params.yaml b/configs/spotmicro_config/config/move_base/costmap_common_params.yaml
deleted file mode 100644
index d24a93f..0000000
--- a/configs/spotmicro_config/config/move_base/costmap_common_params.yaml
+++ /dev/null
@@ -1,28 +0,0 @@
-obstacle_range: 2.5
-raytrace_range: 3.0
-footprint: [[-0.25, -0.145], [-0.25, 0.145], [0.25, 0.145], [0.25, -0.145]]
-
-transform_tolerance: 0.5
-resolution: 0.05 
-
-static_map_layer:
-    map_topic: map
-    subscribe_to_updates: true
-
-2d_obstacles_layer:
-  observation_sources: scan
-  scan: {data_type: LaserScan,
-    topic: scan,
-    marking: true,
-    clearing: true}
-
-3d_obstacles_layer: 
-  observation_sources: depth
-  depth: {data_type: PointCloud2,
-    topic: camera/depth/points,
-    min_obstacle_height: 0.1,
-    marking: true,
-    clearing: true}
-
-inflation_layer:
-    inflation_radius: 2.0
\ No newline at end of file
diff --git a/configs/spotmicro_config/config/move_base/global_costmap_params.yaml b/configs/spotmicro_config/config/move_base/global_costmap_params.yaml
deleted file mode 100644
index 107b26a..0000000
--- a/configs/spotmicro_config/config/move_base/global_costmap_params.yaml
+++ /dev/null
@@ -1,13 +0,0 @@
-global_costmap:
-  global_frame: map
-  robot_base_frame: base_footprint
-  update_frequency: 1.0 
-  publish_frequency: 0.5 
-  static_map: true
-  cost_scaling_factor: 10.0
-
-  plugins:
-    - {name: static_map_layer,    type: "costmap_2d::StaticLayer"}
-    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}
-    - {name: 2d_obstacles_layer,  type: "costmap_2d::ObstacleLayer"}
-    - {name: 3d_obstacles_layer,  type: "costmap_2d::VoxelLayer"}
diff --git a/configs/spotmicro_config/config/move_base/local_costmap_params.yaml b/configs/spotmicro_config/config/move_base/local_costmap_params.yaml
deleted file mode 100644
index 0be7cb3..0000000
--- a/configs/spotmicro_config/config/move_base/local_costmap_params.yaml
+++ /dev/null
@@ -1,16 +0,0 @@
-local_costmap:
-  global_frame: odom
-  robot_base_frame: base_footprint
-  update_frequency: 1.0 
-  publish_frequency: 2.0 
-
-  static_map: false
-  rolling_window: true
-  width: 2.5
-  height: 2.5
-  cost_scaling_factor: 5
-
-  plugins:
-    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}
-    - {name: 2d_obstacles_layer,  type: "costmap_2d::ObstacleLayer"}
-    - {name: 3d_obstacles_layer,  type: "costmap_2d::VoxelLayer"}
diff --git a/configs/spotmicro_config/config/move_base/move_base_params.yaml b/configs/spotmicro_config/config/move_base/move_base_params.yaml
deleted file mode 100644
index f975528..0000000
--- a/configs/spotmicro_config/config/move_base/move_base_params.yaml
+++ /dev/null
@@ -1,15 +0,0 @@
-base_global_planner: global_planner/GlobalPlanner
-base_local_planner: dwa_local_planner/DWAPlannerROS
-
-shutdown_costmaps: false
-
-controller_frequency: 5.0
-controller_patience: 3.0
-
-planner_frequency: 0.5
-planner_patience: 5.0
-
-oscillation_timeout: 10.0
-oscillation_distance: 0.2
-
-conservative_reset_dist: 0.1
\ No newline at end of file
diff --git a/configs/spotmicro_config/config/ros_control/ros_control.yaml b/configs/spotmicro_config/config/ros_control/ros_control.yaml
deleted file mode 100644
index 6b84d10..0000000
--- a/configs/spotmicro_config/config/ros_control/ros_control.yaml
+++ /dev/null
@@ -1,22 +0,0 @@
-"":
-  joint_states_controller:
-      type: joint_state_controller/JointStateController
-      publish_rate: 50
-
-  joint_group_position_controller:
-      type: position_controllers/JointTrajectoryController
-      joints:
-          - front_left_shoulder
-          - front_left_leg
-          - front_left_foot
-          - front_right_shoulder
-          - front_right_leg
-          - front_right_foot
-          - rear_left_shoulder
-          - rear_left_leg
-          - rear_left_foot
-          - rear_right_shoulder
-          - rear_right_leg
-          - rear_right_foot
-
-      # Servomotors have inner control loops so PIDs are not needed
diff --git a/configs/spotmicro_config/launch/bringup.launch b/configs/spotmicro_config/launch/bringup.launch
deleted file mode 100644
index e5b53d3..0000000
--- a/configs/spotmicro_config/launch/bringup.launch
+++ /dev/null
@@ -1,49 +0,0 @@
-<launch>
-    <arg name="robot_name"              default="/"/>  <!-- Change this for namespacing. -->
-    <arg name="base_frame"             default="base_link"/> <!-- Link name of floating base. Do not touch this. -->
-    <arg name="joints_map_file"        default="$(find spotmicro_config)/config/joints/joints.yaml"/> <!--Path to list of joint names. Do not touch this. -->
-    <arg name="links_map_file"         default="$(find spotmicro_config)/config/links/links.yaml"/> <!-- Path to list of link names. Do not touch this. -->
-    <arg name="gait_config_file"       default="$(find spotmicro_config)/config/gait/gait.yaml"/>  <!-- Path to gait parameters. Do not touch this. -->
-    <arg name="description_file"       default="$(find spotmicro_description)/urdf/spotmicroai.urdf"/> <!-- Path to URDF file Do not touch this. -->
-    <arg name="gazebo"                 default="false" /> <!-- Set to true during simulation. This is auto-set to true from gazebo.launch. -->
-    <arg name="rviz"                   default="false"/> <!-- Set to true to run rviz in parallel. -->
-    <arg name="rviz_ref_frame"         default="odom"/> <!-- Default RVIZ reference frame. -->
-    <arg name="has_imu"                default="true" /> <!-- Set to true if you want to visualize robot but there's no IMU. Only useful for microcontrollers. -->
-    <arg name="lite"                   default="false" /> <!-- Set to true if you're using CHAMP lite version. Only useful for microcontrollers. -->
-    <arg name="close_loop_odom"        default="false" /> <!-- Set to true if you want to calculate odometry using close loop. This is auto-set to true from gazebo.launch. -->
-    <arg name="publish_foot_contacts"  default="true" /> <!-- Set to true if you want the controller to publish the foot contact states. This is auto-set to false from gazebo.launch. -->
-    <arg name="publish_joint_control"  default="true" /> <!-- Set to true if you want the controller to publish the joint_states topic. This is auto-set to false from gazebo.launch. -->
-    <arg name="laser"                  default="sim"/> <!-- Set to the 2D LIDAR you're using. See https://github.com/chvmp/champ/tree/master/champ_bringup/launch/include/laser .-->
-    <arg name="joint_controller_topic" default="joint_group_position_controller/command" /> <!-- Change to remap command topic for actuator controller (ROS control).  -->
-    <arg name="hardware_connected"     default="false" /> <!-- Flag useful to launch hardware connected launch files. This auto disables publishing joint_states. -->
-
-    <arg if="$(eval arg('robot_name') == '/')"  name="frame_prefix" value="" />
-    <arg unless="$(eval arg('robot_name') == '/')" name="frame_prefix" value="$(arg robot_name)/" />
-
-    <include file="$(find champ_bringup)/launch/bringup.launch">
-        <arg name="robot_name"             value="$(arg robot_name)"/>
-        <arg name="base_frame"             value="$(arg base_frame)"/>
-        <arg name="joints_map_file"        value="$(arg joints_map_file)"/>
-        <arg name="links_map_file"         value="$(arg links_map_file)"/>
-        <arg name="gait_config_file"       value="$(arg gait_config_file)"/>
-        <arg name="description_file"       value="$(arg description_file)"/>
-        <arg name="has_imu"                value="$(arg has_imu)"/>
-        <arg name="gazebo"                 value="$(arg gazebo)"/>
-        <arg name="lite"                   value="$(arg lite)"/>
-        <arg name="laser"                  value="$(arg laser)"/>
-        <arg name="rviz"                   value="$(arg rviz)"/>
-        <arg name="rviz_ref_frame"         value="$(arg frame_prefix)$(arg rviz_ref_frame)"/>
-        <arg name="joint_controller_topic" value="$(arg joint_controller_topic)" />
-        <arg name="hardware_connected"     value="$(arg hardware_connected)" />
-        <arg name="publish_foot_contacts"  value="$(arg publish_foot_contacts)" />
-        <arg name="publish_joint_control"  value="$(arg publish_joint_control)" />
-        <arg name="close_loop_odom"        value="$(arg close_loop_odom)" />
-    </include>
-    
-    <group if="$(arg hardware_connected)">
-        <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_laser" args="0 0 0 0 0 0 $(arg frame_prefix)base_link $(arg frame_prefix)laser" />
-        <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_imu"   args="0 0 0 0 0 0 $(arg frame_prefix)base_link $(arg frame_prefix)imu_link" />
-
-        <include file="$(find spotmicro_config)/launch/position_controllers.launch"/>
-    </group>
-</launch>
diff --git a/configs/spotmicro_config/launch/bringup.launch.py b/configs/spotmicro_config/launch/bringup.launch.py
new file mode 100644
index 0000000..383d4b1
--- /dev/null
+++ b/configs/spotmicro_config/launch/bringup.launch.py
@@ -0,0 +1,83 @@
+# Copyright (c) 2021 Juan Miguel Jimeno
+#
+# 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.
+
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+
+def generate_launch_description():
+    description_package = FindPackageShare('spotmicro_description')
+
+    joints_config = PathJoinSubstitution(
+        [description_package, 'config', 'champ', 'joints.yaml']
+    )
+    links_config = PathJoinSubstitution(
+        [description_package, 'config', 'champ', 'links.yaml']
+    )
+    gait_config = PathJoinSubstitution(
+        [description_package, 'config', 'champ', 'gait.yaml']
+    )
+    description_path = PathJoinSubstitution(
+        [description_package, 'urdf', 'spotmicro_description.urdf.xacro']
+    )
+    bringup_launch_path = PathJoinSubstitution(
+        [FindPackageShare('champ_bringup'), 'launch', 'bringup.launch.py']
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            name='robot_name', 
+            default_value='',
+            description='Set robot name for multi robot'
+        ),
+
+        DeclareLaunchArgument(
+            name='sim', 
+            default_value='false',
+            description='Enable use_sime_time to true'
+        ),
+
+        DeclareLaunchArgument(
+            name='rviz', 
+            default_value='false',
+            description='Run rviz'
+        ),
+
+        DeclareLaunchArgument(
+            name='hardware_connected', 
+            default_value='false',
+            description='Set to true if connected to a physical robot'
+        ),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(bringup_launch_path),
+            launch_arguments={
+                "use_sim_time": LaunchConfiguration("sim"),
+                "robot_name": LaunchConfiguration("robot_name"),
+                "gazebo": LaunchConfiguration("sim"),
+                "rviz": LaunchConfiguration("rviz"),
+                "hardware_connected": LaunchConfiguration("hardware_connected"),
+                "publish_foot_contacts": "true",
+                "close_loop_odom": "true",
+                "joint_controller_topic": "joint_group_effort_controller/joint_trajectory",
+                "joints_map_path": joints_config,
+                "links_map_path": links_config,
+                "gait_config_path": gait_config,
+                "description_path": description_path
+            }.items(),
+        )
+    ])
diff --git a/configs/spotmicro_config/launch/gazebo.launch b/configs/spotmicro_config/launch/gazebo.launch
deleted file mode 100644
index 94f7363..0000000
--- a/configs/spotmicro_config/launch/gazebo.launch
+++ /dev/null
@@ -1,35 +0,0 @@
-<launch>
-    <arg name="robot_name"         default="/"/> <!-- Change this for namespacing. -->
-    <arg name="rviz"               default="false"/> <!-- Set to true to run rviz in parallel. -->
-    <arg name="lite"               default="false" />  <!-- Set to true if you're using CHAMP lite version. Only useful for microcontrollers. -->
-    <arg name="ros_control_file"   default="$(find spotmicro_config)/config/ros_control/ros_control.yaml" /> <!-- Path to ROS Control configurations. Do not touch. -->
-    <arg name="gazebo_world"       default="$(find spotmicro_config)/worlds/outdoor.world" /> <!-- Path to Gazebo world you want to load. -->
-    <arg name="gui"                default="true"/>
-    <arg name="world_init_x"       default="0.0" /> <!-- X Initial position of the robot in Gazebo World -->
-    <arg name="world_init_y"       default="0.0" /> <!-- Y Initial position of the robot in Gazebo World -->
-    <arg name="world_init_heading" default="0.0" /> <!-- Initial heading of the robot in Gazebo World -->
-
-    <param name="use_sim_time" value="true" />
-
-    <include file="$(find spotmicro_config)/launch/bringup.launch">
-        <arg name="robot_name"             value="$(arg robot_name)"/>
-        <arg name="gazebo"                 value="true"/>
-        <arg name="lite"                   value="$(arg lite)"/>
-        <arg name="rviz"                   value="$(arg rviz)"/>
-        <arg name="joint_controller_topic" value="joint_group_position_controller/command"/>
-        <arg name="hardware_connected"     value="false"/>
-        <arg name="publish_foot_contacts"  value="false"/>
-        <arg name="close_loop_odom"        value="true"/>
-    </include>
-
-    <include file="$(find champ_gazebo)/launch/gazebo.launch">
-        <arg name="robot_name"         value="$(arg robot_name)"/>
-        <arg name="lite"               value="$(arg lite)"/>
-        <arg name="ros_control_file"   value="$(arg ros_control_file)"/>
-        <arg name="gazebo_world"       value="$(arg gazebo_world)"/>
-        <arg name="world_init_x"       value="$(arg world_init_x)" />
-        <arg name="world_init_y"       value="$(arg world_init_y)" />
-        <arg name="world_init_heading" value="$(arg world_init_heading)" />
-        <arg name="gui"                value="$(arg gui)" />
-    </include>    
-</launch>
\ No newline at end of file
diff --git a/configs/spotmicro_config/launch/gazebo.launch.py b/configs/spotmicro_config/launch/gazebo.launch.py
new file mode 100644
index 0000000..762549f
--- /dev/null
+++ b/configs/spotmicro_config/launch/gazebo.launch.py
@@ -0,0 +1,125 @@
+import os
+
+import launch_ros
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import Node
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, LaunchConfiguration
+
+
+def generate_launch_description():
+
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    description_path = LaunchConfiguration("description_path")
+    base_frame = "base_link"
+
+    config_pkg_share = launch_ros.substitutions.FindPackageShare(
+        package="spotmicro_config"
+    ).find("spotmicro_config")
+    descr_pkg_share = launch_ros.substitutions.FindPackageShare(
+        package="spotmicro_description"
+    ).find("spotmicro_description")
+    joints_config = os.path.join(descr_pkg_share, "config", "champ", "joints.yaml")
+    gait_config = os.path.join(descr_pkg_share, "config", "champ", "gait.yaml")
+    links_config = os.path.join(descr_pkg_share, "config", "champ", "links.yaml")
+    default_model_path = os.path.join(descr_pkg_share, "urdf", "spotmicro_description.urdf.xacro")
+    default_world_path = os.path.join(config_pkg_share, "worlds", "outdoor.world")
+
+    declare_use_sim_time = DeclareLaunchArgument(
+        "use_sim_time",
+        default_value="true",
+        description="Use simulation (Gazebo) clock if true",
+    )
+    declare_rviz = DeclareLaunchArgument(
+        "rviz", default_value="false", description="Launch rviz"
+    )
+    declare_robot_name = DeclareLaunchArgument(
+        "robot_name", default_value="spotmicro", description="Robot name"
+    )
+    declare_lite = DeclareLaunchArgument(
+        "lite", default_value="false", description="Lite"
+    )
+
+    declare_gazebo_world = DeclareLaunchArgument(
+        "world", default_value=default_world_path, description="Gazebo world name"
+    )
+
+    declare_gui = DeclareLaunchArgument(
+        "gui", default_value="true", description="Use gui"
+    )
+    declare_world_init_x = DeclareLaunchArgument("world_init_x", default_value="0.0")
+    declare_world_init_y = DeclareLaunchArgument("world_init_y", default_value="0.0")
+    declare_world_init_heading = DeclareLaunchArgument(
+        "world_init_heading", default_value="0.6"
+    )
+
+    bringup_ld = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory("champ_bringup"),
+                "launch",
+                "bringup.launch.py",
+            )
+        ),
+        launch_arguments={
+            "description_path": default_model_path,
+            "joints_map_path": joints_config,
+            "links_map_path": links_config,
+            "gait_config_path": gait_config,
+            "use_sim_time": LaunchConfiguration("use_sim_time"),
+            "robot_name": LaunchConfiguration("robot_name"),
+            "gazebo": "true",
+            "lite": LaunchConfiguration("lite"),
+            "rviz": LaunchConfiguration("rviz"),
+            "joint_controller_topic": "joint_group_effort_controller/joint_trajectory",
+            "hardware_connected": "false",
+            "publish_foot_contacts": "false",
+            "close_loop_odom": "true",
+        }.items(),
+    )
+
+    gazebo_ld = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory("champ_gazebo"),
+                "launch",
+                "gazebo.launch.py",
+            )
+        ),
+        launch_arguments={
+            "use_sim_time": LaunchConfiguration("use_sim_time"),
+            "robot_name": LaunchConfiguration("robot_name"),
+            "world": LaunchConfiguration("world"),
+            "lite": LaunchConfiguration("lite"),
+            "world_init_x": LaunchConfiguration("world_init_x"),
+            "world_init_y": LaunchConfiguration("world_init_y"),
+            "world_init_heading": LaunchConfiguration("world_init_heading"),
+            "gui": LaunchConfiguration("gui"),
+            "close_loop_odom": "true",
+        }.items(),
+    )
+
+    return LaunchDescription(
+        [
+            declare_use_sim_time,
+            declare_rviz,
+            declare_robot_name,
+            declare_lite,
+            declare_gazebo_world,
+            declare_gui,
+            declare_world_init_x,
+            declare_world_init_y,
+            declare_world_init_heading,
+            bringup_ld,
+            gazebo_ld
+
+        ]
+    )
diff --git a/configs/spotmicro_config/launch/include/amcl.launch b/configs/spotmicro_config/launch/include/amcl.launch
deleted file mode 100644
index 635bd34..0000000
--- a/configs/spotmicro_config/launch/include/amcl.launch
+++ /dev/null
@@ -1,38 +0,0 @@
-<launch>
-    <arg name="frame_prefix" default=""/>
-
-    <node pkg="amcl" type="amcl" name="amcl" output="screen">
-        <param name="initial_pose_a" value="0.0"/>
-        <param name="use_map_topic" value="true"/>
-        <param name="base_frame_id" value="$(arg frame_prefix)base_footprint"/>
-        <param name="odom_frame_id" value="$(arg frame_prefix)odom"/>
-        <param name="global_frame_id" value="$(arg frame_prefix)map"/>
-        <param name="transform_broadcast" value="true"/>
-        <param name="gui_publish_rate" value="10.0"/> <!-- Maximum rate (Hz) at which scans and paths are published for visualization, -1.0 to disable. -->
-        <param name="kld_err" value="0.05"/>
-        <param name="kld_z" value="0.99"/>
-        <param name="laser_lambda_short" value="0.1"/>
-        <param name="laser_likelihood_max_dist" value="2.0"/>
-        <param name="laser_max_beams" value="60"/>	
-        <param name="laser_model_type" value="likelihood_field_prob"/>
-        <param name="laser_sigma_hit" value="0.2"/>
-        <param name="laser_z_hit" value="0.5"/>
-        <param name="laser_z_short" value="0.05"/>
-        <param name="laser_z_max" value="0.05"/>
-        <param name="laser_z_rand" value="0.5"/>
-        <param name="max_particles" value="2000"/>
-        <param name="min_particles" value="500"/>
-        <param name="odom_alpha1" value="0.5"/> <!-- Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion. -->
-        <param name="odom_alpha2" value="0.5"/> <!-- Specifies the expected noise in odometry's rotation estimate from translational component of the robot's motion. -->
-        <param name="odom_alpha3" value="0.5"/> <!-- Specifies the expected noise in odometry's translation estimate from the translational component of the robot's motion. -->
-        <param name="odom_alpha4" value="0.5"/> <!-- Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion. -->
-        <param name="odom_alpha5" value="0.5"/> <!-- Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion. -->
-        <param name="odom_model_type" value="omni"/>
-        <param name="recovery_alpha_slow" value="0.001"/> <!-- Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. -->
-        <param name="recovery_alpha_fast" value="0.1"/> <!-- Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. -->
-        <param name="resample_interval" value="1"/> <!-- Number of filter updates required before resampling. -->
-        <param name="transform_tolerance" value="1.25"/> <!-- Default 0.1; time with which to post-date the transform that is published, to indicate that this transform is valid into the future. -->
-        <param name="update_min_a" value="0.2"/> <!-- Rotational movement required before performing a filter update. 0.1 represents 5.7 degrees  -->
-        <param name="update_min_d" value="0.2"/> <!-- Translational movement required before performing a filter update. -->
-    </node>
-</launch>
diff --git a/configs/spotmicro_config/launch/include/gmapping.launch b/configs/spotmicro_config/launch/include/gmapping.launch
deleted file mode 100644
index 8bbf58b..0000000
--- a/configs/spotmicro_config/launch/include/gmapping.launch
+++ /dev/null
@@ -1,41 +0,0 @@
-<launch>
-    <arg name="frame_prefix" default=""/>
-
-    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
-        <param name="base_frame" value="$(arg frame_prefix)base_footprint" />
-        <param name="odom_frame" value="$(arg frame_prefix)odom" />
-        <param name="map_frame"  value="/$(arg frame_prefix)map" />
-
-        <param name="map_update_interval" value="15.0"/>
-        <param name="maxUrange" value="5.0"/>
-        <param name="minRange" value="-0.5"/>
-        <param name="sigma" value="0.05"/>
-        <param name="kernelSize" value="1"/>
-        <param name="lstep" value="0.05"/>
-        <param name="astep" value="0.05"/>
-        <param name="iterations" value="5"/>
-        <param name="lsigma" value="0.075"/>
-        <param name="ogain" value="3.0"/>
-        <param name="lskip" value="0"/>
-        <param name="minimumScore" value="100"/>
-        <param name="srr" value="0.01"/>
-        <param name="srt" value="0.02"/>
-        <param name="str" value="0.01"/>
-        <param name="stt" value="0.02"/>
-        <param name="linearUpdate" value="0.7"/>
-        <param name="angularUpdate" value="0.7"/>
-        <param name="temporalUpdate" value="-0.5"/>
-        <param name="resampleThreshold" value="0.5"/>
-        <param name="particles" value="50"/>
-        <param name="xmin" value="-50.0"/>
-        <param name="ymin" value="-50.0"/>
-        <param name="xmax" value="50.0"/>
-        <param name="ymax" value="50.0"/>
-        <param name="delta" value="0.05"/>
-        <param name="llsamplerange" value="0.05"/>
-        <param name="llsamplestep" value="0.05"/>
-        <param name="lasamplerange" value="0.005"/>
-        <param name="lasamplestep" value="0.005"/>
-        <param name="transform_publish_period" value="0.1"/>
-    </node>
-</launch>
\ No newline at end of file
diff --git a/configs/spotmicro_config/launch/include/move_base.launch b/configs/spotmicro_config/launch/include/move_base.launch
deleted file mode 100644
index 44235d0..0000000
--- a/configs/spotmicro_config/launch/include/move_base.launch
+++ /dev/null
@@ -1,23 +0,0 @@
-<launch>
-    <arg name="frame_prefix" default=""/>
-    <arg name="robot_name" default=""/>
-
-    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
-        <rosparam file="$(find spotmicro_config)/config/move_base/costmap_common_params.yaml" command="load" ns="global_costmap" />
-        <rosparam file="$(find spotmicro_config)/config/move_base/costmap_common_params.yaml" command="load" ns="local_costmap" />
-        <rosparam file="$(find spotmicro_config)/config/move_base/local_costmap_params.yaml" command="load" />
-        <rosparam file="$(find spotmicro_config)/config/move_base/global_costmap_params.yaml" command="load" />
-        <rosparam file="$(find spotmicro_config)/config/move_base/base_local_planner_holonomic_params.yaml" command="load" />
-        <rosparam file="$(find spotmicro_config)/config/move_base/move_base_params.yaml" command="load" />
-
-        <!-- explicitly define frame ids for movebase -->
-        <param name="global_costmap/global_frame"                   value="$(arg frame_prefix)map"/>
-        <param name="global_costmap/robot_base_frame"               value="$(arg frame_prefix)base_footprint"/>
-        <param name="global_costmap/2d_obstacles_layer/scan/topic"  value="$(arg robot_name)scan"/>
-        <param name="global_costmap/3d_obstacles_layer/depth/topic" value="$(arg robot_name)camera/depth/points"/>
-        <param name="local_costmap/global_frame"                    value="$(arg frame_prefix)odom"/>
-        <param name="local_costmap/robot_base_frame"                value="$(arg frame_prefix)base_footprint"/>
-        <param name="local_costmap/2d_obstacles_layer/scan/topic"   value="$(arg robot_name)scan"/>
-        <param name="local_costmap/3d_obstacles_layer/depth/topic"  value="$(arg robot_name)camera/depth/points"/>
-    </node>
-</launch>
\ No newline at end of file
diff --git a/configs/spotmicro_config/launch/navigate.launch b/configs/spotmicro_config/launch/navigate.launch
deleted file mode 100644
index 4e1fa3f..0000000
--- a/configs/spotmicro_config/launch/navigate.launch
+++ /dev/null
@@ -1,30 +0,0 @@
-<launch>
-    <arg name="robot_name" default="/"/>
-    <arg name="rviz"       default="false"/>
-
-    <arg if="$(eval arg('robot_name') == '/')"  name="frame_prefix" value="" />
-    <arg unless="$(eval arg('robot_name') == '/')" name="frame_prefix" value="$(arg robot_name)/" />
-
-    <group ns="$(arg robot_name)">
-        <!-- Map server -->
-        <arg name="map_file" default="$(find spotmicro_config)/maps/map.yaml"/>
-        <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)" >
-            <param name="frame_id" value="$(arg frame_prefix)map" />
-        </node>
-        <!-- AMCL used for localization -->
-        <include file="$(find spotmicro_config)/launch/include/amcl.launch">
-            <arg name="frame_prefix" value="$(arg frame_prefix)"/>
-        </include>
-
-        <!-- Calls navigation stack -->
-        <include file="$(find spotmicro_config)/launch/include/move_base.launch">
-            <arg name="frame_prefix" value="$(arg frame_prefix)"/>
-            <arg name="robot_name"   value="$(arg robot_name)"/>
-        </include>
-
-
-        <node if="$(arg rviz)" name="rviz" pkg="rviz" type="rviz"
-            args="-d $(find champ_navigation)/rviz/navigate.rviz -f $(arg frame_prefix)map"
-            output="screen"/>
-    </group>
-</launch>
\ No newline at end of file
diff --git a/configs/spotmicro_config/launch/navigate.launch.py b/configs/spotmicro_config/launch/navigate.launch.py
new file mode 100644
index 0000000..ef964bc
--- /dev/null
+++ b/configs/spotmicro_config/launch/navigate.launch.py
@@ -0,0 +1,72 @@
+# Copyright (c) 2021 Juan Miguel Jimeno
+#
+# 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.
+
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    this_package = FindPackageShare('spotmicro_config')
+
+    default_map_path = PathJoinSubstitution(
+        [this_package, 'maps', 'map.yaml']
+    )
+
+    default_params_file_path = PathJoinSubstitution(
+        [this_package, 'config', 'autonomy', 'navigation.yaml']
+    )
+
+    nav2_launch_path = PathJoinSubstitution(
+        [FindPackageShare('champ_navigation'), 'launch', 'navigate.launch.py']
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            name='map', 
+            default_value=default_map_path,
+            description='Navigation map path'
+        ),
+        
+        DeclareLaunchArgument(
+            name='params_file', 
+            default_value=default_params_file_path,
+            description='Navigation2 params file'
+        ),
+
+        DeclareLaunchArgument(
+            name='sim', 
+            default_value='false',
+            description='Enable use_sime_time to true'
+        ),
+
+        DeclareLaunchArgument(
+            name='rviz', 
+            default_value='false',
+            description='Run rviz'
+        ),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(nav2_launch_path),
+            launch_arguments={
+                'params_file': LaunchConfiguration("params_file"),
+                'map': LaunchConfiguration("map"),
+                'sim': LaunchConfiguration("sim"),
+                'rviz': LaunchConfiguration("rviz")
+            }.items()
+        )
+    ])
diff --git a/configs/spotmicro_config/launch/slam.launch b/configs/spotmicro_config/launch/slam.launch
deleted file mode 100644
index 4cc5ebd..0000000
--- a/configs/spotmicro_config/launch/slam.launch
+++ /dev/null
@@ -1,23 +0,0 @@
-<launch>
-    <arg name="robot_name" default="/"/>
-    <arg name="rviz"       default="false"/>
-    
-    <arg if="$(eval arg('robot_name') == '/')"  name="frame_prefix" value="" />
-    <arg unless="$(eval arg('robot_name') == '/')" name="frame_prefix" value="$(arg robot_name)/" />
-
-    <group ns="$(arg robot_name)">
-        <include file="$(find spotmicro_config)/launch/include/gmapping.launch">
-            <arg name="frame_prefix" value="$(arg frame_prefix)"/>
-        </include>
-
-        <!-- Calls navigation stack packages -->
-        <include file="$(find spotmicro_config)/launch/include/move_base.launch">
-            <arg name="frame_prefix" value="$(arg frame_prefix)"/>
-            <arg name="robot_name"   value="$(arg robot_name)"/>
-        </include>
-
-        <node if="$(arg rviz)" name="rviz" pkg="rviz" type="rviz"
-            args="-d $(find champ_navigation)/rviz/navigate.rviz -f $(arg frame_prefix)map"
-            output="screen"/>
-    </group>
-</launch>
\ No newline at end of file
diff --git a/configs/spotmicro_config/launch/slam.launch.py b/configs/spotmicro_config/launch/slam.launch.py
new file mode 100644
index 0000000..49be441
--- /dev/null
+++ b/configs/spotmicro_config/launch/slam.launch.py
@@ -0,0 +1,61 @@
+# Copyright (c) 2021 Juan Miguel Jimeno
+#
+# 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.
+
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    this_package = FindPackageShare('mini_pupper_config')
+
+    default_params_file_path = PathJoinSubstitution(
+        [this_package, 'config', 'autonomy', 'slam.yaml']
+    )
+
+    slam_launch_path = PathJoinSubstitution(
+        [FindPackageShare('champ_navigation'), 'launch', 'slam.launch.py']
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            name='slam_params_file',
+            default_value=default_params_file_path,
+            description='Navigation2 slam params file'
+        ),
+
+        DeclareLaunchArgument(
+            name='sim', 
+            default_value='false',
+            description='Enable use_sime_time to true'
+        ),
+
+        DeclareLaunchArgument(
+            name='rviz', 
+            default_value='false',
+            description='Run rviz'
+        ),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(slam_launch_path),
+            launch_arguments={
+                'slam_params_file': LaunchConfiguration("slam_params_file"),
+                'sim': LaunchConfiguration("sim"),
+                'rviz': LaunchConfiguration("rviz")
+            }.items()
+        )
+    ])
diff --git a/configs/spotmicro_config/launch/spawn_robot.launch b/configs/spotmicro_config/launch/spawn_robot.launch
deleted file mode 100644
index b2bb246..0000000
--- a/configs/spotmicro_config/launch/spawn_robot.launch
+++ /dev/null
@@ -1,32 +0,0 @@
-<launch>
-    <arg name="robot_name"         default="/"/> <!-- Change this for namespacing. -->
-    <arg name="rviz"               default="false"/> <!-- Set to true to run rviz in parallel. -->
-    <arg name="lite"               default="false" />  <!-- Set to true if you're using CHAMP lite version. Only useful for microcontrollers. -->
-    <arg name="ros_control_file"   default="$(find spotmicro_config)/config/ros_control/ros_control.yaml" /> <!-- Path to ROS Control configurations. Do not touch. -->
-    <arg name="gazebo_world"       default="$(find spotmicro_config)/worlds/outdoor.world" /> <!-- Path to Gazebo world you want to load. -->
-    <arg name="world_init_x"       default="0.0" /> <!-- X Initial position of the robot in Gazebo World -->
-    <arg name="world_init_y"       default="0.0" /> <!-- Y Initial position of the robot in Gazebo World -->
-    <arg name="world_init_heading" default="0.0" /> <!-- Initial heading of the robot in Gazebo World -->
-
-    <param name="use_sim_time" value="true" />
-
-    <include file="$(find spotmicro_config)/launch/bringup.launch">
-        <arg name="robot_name"             value="$(arg robot_name)"/>
-        <arg name="gazebo"                 value="true"/>
-        <arg name="lite"                   value="$(arg lite)"/>
-        <arg name="rviz"                   value="$(arg rviz)"/>
-        <arg name="joint_controller_topic" value="joint_group_position_controller/command"/>
-        <arg name="hardware_connected"     value="false"/>
-        <arg name="publish_foot_contacts"  value="false"/>
-        <arg name="close_loop_odom"        value="true"/>
-    </include>
-
-    <include file="$(find champ_gazebo)/launch/spawn_robot.launch">
-        <arg name="robot_name"         value="$(arg robot_name)"/>
-        <arg name="lite"               value="$(arg lite)"/>
-        <arg name="ros_control_file"   value="$(arg ros_control_file)"/>
-        <arg name="world_init_x"       value="$(arg world_init_x)" />
-        <arg name="world_init_y"       value="$(arg world_init_y)" />
-        <arg name="world_init_heading" value="$(arg world_init_heading)" />
-    </include>
-</launch>
\ No newline at end of file
diff --git a/configs/spotmicro_config/maps/playground.yaml b/configs/spotmicro_config/maps/playground.yaml
new file mode 100644
index 0000000..a0e63f9
--- /dev/null
+++ b/configs/spotmicro_config/maps/playground.yaml
@@ -0,0 +1,7 @@
+image: playground.pgm
+mode: trinary
+resolution: 0.05
+origin: [-10.3, -10.5, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25
diff --git a/configs/spotmicro_config/package.xml b/configs/spotmicro_config/package.xml
index 8badc9a..ff2a429 100644
--- a/configs/spotmicro_config/package.xml
+++ b/configs/spotmicro_config/package.xml
@@ -1,18 +1,22 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>spotmicro_config</name>
   <version>0.1.0</version>
-  <description>spotmicro Champ Config Package</description>
+  <description>CHAMP Config Package</description>
 
   <maintainer email="jimenojmm@gmail.com">Juan Miguel Jimeno</maintainer>
   <author>Juan Miguel Jimeno</author>
 
   <license>BSD</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
   <exec_depend>champ_base</exec_depend>
-  <exec_depend>ros_controllers</exec_depend>
-  <exec_depend>roslaunch</exec_depend>
-  <exec_depend>rviz</exec_depend>
-  
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>gazebo_plugins</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>
diff --git a/configs/spotmicro_config/worlds/playground.world b/configs/spotmicro_config/worlds/playground.world
new file mode 100644
index 0000000..d199a8c
--- /dev/null
+++ b/configs/spotmicro_config/worlds/playground.world
@@ -0,0 +1,1564 @@
+<sdf version='1.7'>
+  <world name='default'>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</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>
+      <spot>
+        <inner_angle>0</inner_angle>
+        <outer_angle>0</outer_angle>
+        <falloff>0</falloff>
+      </spot>
+    </light>
+    <model name='ground_plane'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+    </model>
+    <gravity>0 0 -9.8</gravity>
+    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
+    <atmosphere type='adiabatic'/>
+    <physics type='ode'>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>1</shadows>
+    </scene>
+    <wind/>
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+      <latitude_deg>0</latitude_deg>
+      <longitude_deg>0</longitude_deg>
+      <elevation>0</elevation>
+      <heading_deg>0</heading_deg>
+    </spherical_coordinates>
+    <model name='asphalt_plane'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>20 20 0.1</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <box>
+              <size>20 20 0.1</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://asphalt_plane/materials/scripts</uri>
+              <uri>model://asphalt_plane/materials/textures</uri>
+              <name>vrc/asphalt</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>-0.066287 0.086676 0 0 -0 0</pose>
+    </model>
+    <model name='playground'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <uri>model://playground/meshes/playground.dae</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <uri>model://playground/meshes/playground.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>0.556672 -0.365718 0 0 -0 0</pose>
+    </model>
+    <model name='pine_tree'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <uri>model://pine_tree/meshes/pine_tree.dae</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='branch'>
+          <geometry>
+            <mesh>
+              <uri>model://pine_tree/meshes/pine_tree.dae</uri>
+              <submesh>
+                <name>Branch</name>
+              </submesh>
+            </mesh>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://pine_tree/materials/scripts/</uri>
+              <uri>model://pine_tree/materials/textures/</uri>
+              <name>PineTree/Branch</name>
+            </script>
+          </material>
+        </visual>
+        <visual name='bark'>
+          <geometry>
+            <mesh>
+              <uri>model://pine_tree/meshes/pine_tree.dae</uri>
+              <submesh>
+                <name>Bark</name>
+              </submesh>
+            </mesh>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://pine_tree/materials/scripts/</uri>
+              <uri>model://pine_tree/materials/textures/</uri>
+              <name>PineTree/Bark</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>-8.74428 -9.11584 0 0 -0 0</pose>
+    </model>
+    <model name='pine_tree_0'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <uri>model://pine_tree/meshes/pine_tree.dae</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='branch'>
+          <geometry>
+            <mesh>
+              <uri>model://pine_tree/meshes/pine_tree.dae</uri>
+              <submesh>
+                <name>Branch</name>
+              </submesh>
+            </mesh>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://pine_tree/materials/scripts/</uri>
+              <uri>model://pine_tree/materials/textures/</uri>
+              <name>PineTree/Branch</name>
+            </script>
+          </material>
+        </visual>
+        <visual name='bark'>
+          <geometry>
+            <mesh>
+              <uri>model://pine_tree/meshes/pine_tree.dae</uri>
+              <submesh>
+                <name>Bark</name>
+              </submesh>
+            </mesh>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://pine_tree/materials/scripts/</uri>
+              <uri>model://pine_tree/materials/textures/</uri>
+              <name>PineTree/Bark</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>-9.17408 -6.9914 0 0 -0 0</pose>
+    </model>
+    <model name='pine_tree_1'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <uri>model://pine_tree/meshes/pine_tree.dae</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='branch'>
+          <geometry>
+            <mesh>
+              <uri>model://pine_tree/meshes/pine_tree.dae</uri>
+              <submesh>
+                <name>Branch</name>
+              </submesh>
+            </mesh>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://pine_tree/materials/scripts/</uri>
+              <uri>model://pine_tree/materials/textures/</uri>
+              <name>PineTree/Branch</name>
+            </script>
+          </material>
+        </visual>
+        <visual name='bark'>
+          <geometry>
+            <mesh>
+              <uri>model://pine_tree/meshes/pine_tree.dae</uri>
+              <submesh>
+                <name>Bark</name>
+              </submesh>
+            </mesh>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://pine_tree/materials/scripts/</uri>
+              <uri>model://pine_tree/materials/textures/</uri>
+              <name>PineTree/Bark</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>-5.79941 -9.32885 0 0 -0 0</pose>
+    </model>
+    <model name='Lamp Post'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <uri>model://lamp_post/meshes/lamp_post.dae</uri>
+              <scale>3 3 3</scale>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <uri>model://lamp_post/meshes/lamp_post.dae</uri>
+              <scale>3 3 3</scale>
+            </mesh>
+          </geometry>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>8.73716 -8.92816 0 0 -0 0</pose>
+    </model>
+    <model name='Lamp Post_clone'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <uri>model://lamp_post/meshes/lamp_post.dae</uri>
+              <scale>3 3 3</scale>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <uri>model://lamp_post/meshes/lamp_post.dae</uri>
+              <scale>3 3 3</scale>
+            </mesh>
+          </geometry>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>8.76856 8.53222 0 0 0 -2.10022</pose>
+    </model>
+    <model name='oak_tree'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <uri>model://oak_tree/meshes/oak_tree.dae</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='branch'>
+          <geometry>
+            <mesh>
+              <uri>model://oak_tree/meshes/oak_tree.dae</uri>
+              <submesh>
+                <name>Branch</name>
+              </submesh>
+            </mesh>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://oak_tree/materials/scripts/</uri>
+              <uri>model://oak_tree/materials/textures/</uri>
+              <name>OakTree/Branch</name>
+            </script>
+          </material>
+        </visual>
+        <visual name='bark'>
+          <geometry>
+            <mesh>
+              <uri>model://oak_tree/meshes/oak_tree.dae</uri>
+              <submesh>
+                <name>Bark</name>
+              </submesh>
+            </mesh>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://oak_tree/materials/scripts/</uri>
+              <uri>model://oak_tree/materials/textures/</uri>
+              <name>OakTree/Bark</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>-7.84166 8.24695 0 0 -0 0</pose>
+    </model>
+    <model name='jersey_barrier'>
+      <static>1</static>
+      <link name='link'>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <uri>model://jersey_barrier/meshes/jersey_barrier.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='upright'>
+          <pose>0 0 0.5715 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.3063 1.143</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='base'>
+          <pose>0 0 0.032258 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.8107 0.064516</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='base2'>
+          <pose>0 0 0.1 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.65 0.1</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='base3'>
+          <pose>0 0 0.2 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.5 0.1</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='left-angle'>
+          <pose>0 -0.224 0.2401 0.9 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.5 0.064516</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='right-angle'>
+          <pose>0 0.224 0.2401 -0.9 0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.5 0.064516</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>10.9243 2.40595 0 0 -0 0</pose>
+    </model>
+    <model name='jersey_barrier_clone'>
+      <static>1</static>
+      <link name='link'>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <uri>model://jersey_barrier/meshes/jersey_barrier.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='upright'>
+          <pose>0 0 0.5715 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.3063 1.143</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='base'>
+          <pose>0 0 0.032258 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.8107 0.064516</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='base2'>
+          <pose>0 0 0.1 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.65 0.1</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='base3'>
+          <pose>0 0 0.2 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.5 0.1</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='left-angle'>
+          <pose>0 -0.224 0.2401 0.9 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.5 0.064516</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='right-angle'>
+          <pose>0 0.224 0.2401 -0.9 0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.5 0.064516</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>10.0573 -0.353256 0 0 -0 1.54836</pose>
+    </model>
+    <model name='jersey_barrier_clone_clone'>
+      <static>1</static>
+      <link name='link'>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <uri>model://jersey_barrier/meshes/jersey_barrier.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='upright'>
+          <pose>0 0 0.5715 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.3063 1.143</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='base'>
+          <pose>0 0 0.032258 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.8107 0.064516</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='base2'>
+          <pose>0 0 0.1 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.65 0.1</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='base3'>
+          <pose>0 0 0.2 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.5 0.1</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='left-angle'>
+          <pose>0 -0.224 0.2401 0.9 -0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.5 0.064516</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <collision name='right-angle'>
+          <pose>0 0.224 0.2401 -0.9 0 0</pose>
+          <geometry>
+            <box>
+              <size>4.06542 0.5 0.064516</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>10.0327 -6.98111 0 0 -0 1.54836</pose>
+    </model>
+    <model name='fire_hydrant'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <uri>model://fire_hydrant/meshes/fire_hydrant.dae</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <uri>model://fire_hydrant/meshes/fire_hydrant.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>8.1439 -9.70032 0 0 -0 0</pose>
+    </model>
+    <model name='brick_box_3x1x3'>
+      <static>1</static>
+      <link name='chassis'>
+        <pose>0 0 1.5 0 -0 0</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>3 1 3</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <uri>model://brick_box_3x1x3/meshes/simple_box.dae</uri>
+              <scale>3 1 3</scale>
+            </mesh>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://brick_box_3x1x3/materials/scripts</uri>
+              <uri>model://brick_box_3x1x3/materials/textures</uri>
+              <name>BrickBox/Diffuse</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>-3.10709 9.8237 0 0 -0 0</pose>
+    </model>
+    <model name='brick_box_3x1x3_clone'>
+      <static>1</static>
+      <link name='chassis'>
+        <pose>0 0 1.5 0 -0 0</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>3 1 3</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <uri>model://brick_box_3x1x3/meshes/simple_box.dae</uri>
+              <scale>3 1 3</scale>
+            </mesh>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://brick_box_3x1x3/materials/scripts</uri>
+              <uri>model://brick_box_3x1x3/materials/textures</uri>
+              <name>BrickBox/Diffuse</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>3.64111 9.85455 0 0 -0 0</pose>
+    </model>
+    <state world_name='default'>
+      <sim_time>0</sim_time>
+      <real_time>0</real_time>
+      <wall_time>0</wall_time>
+      <iterations>0</iterations>
+      <model name='Construction Cone'>
+        <pose>4.68902 -9.79948 0.049992 0 -8e-06 0.014447</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>4.68902 -9.79948 0.049992 0 -8e-06 0.014447</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 -9.8 0 -0 0</acceleration>
+          <wrench>0 0 -9.8 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='Lamp Post'>
+        <pose>8.73716 -8.92816 0 0 0 -2.10022</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>8.73716 -8.92816 0 0 0 -2.10022</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='Lamp Post_clone'>
+        <pose>8.76856 8.53222 0 0 0 -1.04217</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>8.76856 8.53222 0 0 0 -1.04217</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='asphalt_plane'>
+        <pose>-0.089672 0.059574 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>-0.089672 0.059574 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='brick_box_3x1x3'>
+        <pose>-3.10709 9.8237 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='chassis'>
+          <pose>-3.10709 9.8237 1.5 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='brick_box_3x1x3_clone'>
+        <pose>3.64111 9.85455 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='chassis'>
+          <pose>3.64111 9.85455 1.5 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='drc_practice_organge_jersey_barrier'>
+        <pose>-1.1899 -9.96513 0 0 -0 1.35112</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>-1.1899 -9.96513 0 0 -0 1.35112</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='drc_practice_organge_jersey_barrier_clone'>
+        <pose>2.38207 -10.1405 0 0 -0 1.60373</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>2.38207 -10.1405 0 0 -0 1.60373</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='fire_hydrant'>
+        <pose>8.1439 -9.70032 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>8.1439 -9.70032 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='grey_wall'>
+        <pose>-10.1561 -5.99504 0 0 -0 1.58739</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>-10.1561 -5.99504 1.4 0 -0 1.58739</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='grey_wall_clone'>
+        <pose>-10.2657 1.37707 0 0 -0 1.58739</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>-10.2657 1.37707 1.4 0 -0 1.58739</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='grey_wall_clone_clone'>
+        <pose>-10.3624 6.45785 0 0 -0 1.58739</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>-10.3624 6.45785 1.4 0 -0 1.58739</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='ground_plane'>
+        <pose>0 0 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>0 0 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='jersey_barrier'>
+        <pose>10.0511 5.66451 0 0 -0 1.54836</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>10.0511 5.66451 0 0 -0 1.54836</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='jersey_barrier_clone'>
+        <pose>10.0573 -0.353256 0 0 -0 1.54836</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>10.0573 -0.353256 0 0 -0 1.54836</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='jersey_barrier_clone_clone'>
+        <pose>10.0327 -6.98111 0 0 -0 1.54836</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>10.0327 -6.98111 0 0 -0 1.54836</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='oak_tree'>
+        <pose>-7.84166 8.24695 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>-7.84166 8.24695 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='pine_tree'>
+        <pose>-8.74428 -9.11584 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>-8.74428 -9.11584 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='pine_tree_0'>
+        <pose>-9.17408 -6.9914 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>-9.17408 -6.9914 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='pine_tree_1'>
+        <pose>-5.79941 -9.32885 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>-5.79941 -9.32885 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='playground'>
+        <pose>0.556672 -0.365718 0 0 -0 0.836001</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>0.556672 -0.365718 0 0 -0 0.836001</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <light name='sun'>
+        <pose>0 0 10 0 -0 0</pose>
+      </light>
+      <light name='user_directional_light_0'>
+        <pose>-5.63054 -11.2657 1 0 -0 0</pose>
+      </light>
+      <light name='user_directional_light_1'>
+        <pose>10.4762 9.60881 1 0 -0 0</pose>
+      </light>
+    </state>
+    <model name='drc_practice_organge_jersey_barrier'>
+      <static>1</static>
+      <link name='link'>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <uri>model://drc_practice_orange_jersey_barrier/meshes/jersey_barrier.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <uri>model://drc_practice_orange_jersey_barrier/meshes/jersey_barrier.dae</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>-2.30016 -10.1505 0 0 -0 0</pose>
+    </model>
+    <model name='drc_practice_organge_jersey_barrier_clone'>
+      <static>1</static>
+      <link name='link'>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <uri>model://drc_practice_orange_jersey_barrier/meshes/jersey_barrier.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <uri>model://drc_practice_orange_jersey_barrier/meshes/jersey_barrier.dae</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>0.387818 -10.2088 0 0 -0 1.35112</pose>
+    </model>
+    <model name='Construction Cone'>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <scale>10 10 10</scale>
+              <uri>model://construction_cone/meshes/construction_cone.dae</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <scale>10 10 10</scale>
+              <uri>model://construction_cone/meshes/construction_cone.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <self_collide>0</self_collide>
+        <inertial>
+          <pose>0 0 0 0 -0 0</pose>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+          <mass>1</mass>
+        </inertial>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>4.68291 -9.79023 0 0 -0 0</pose>
+    </model>
+    <model name='grey_wall'>
+      <static>1</static>
+      <link name='link'>
+        <pose>0 0 1.4 0 -0 0</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://grey_wall/materials/scripts</uri>
+              <uri>model://grey_wall/materials/textures</uri>
+              <name>vrc/grey_wall</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>-17.584 -9.25495 0 0 -0 0</pose>
+    </model>
+    <model name='grey_wall_clone'>
+      <static>1</static>
+      <link name='link'>
+        <pose>0 0 1.4 0 -0 0</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://grey_wall/materials/scripts</uri>
+              <uri>model://grey_wall/materials/textures</uri>
+              <name>vrc/grey_wall</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>-10.3087 1.37636 0 0 -0 1.58739</pose>
+    </model>
+    <model name='grey_wall_clone_clone'>
+      <static>1</static>
+      <link name='link'>
+        <pose>0 0 1.4 0 -0 0</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <box>
+              <size>7.5 0.2 2.8</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <uri>model://grey_wall/materials/scripts</uri>
+              <uri>model://grey_wall/materials/textures</uri>
+              <name>vrc/grey_wall</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>-10.3624 6.45785 0 0 -0 1.58739</pose>
+    </model>
+    <light name='user_directional_light_0' type='directional'>
+      <pose>-5.63054 -11.2657 1 0 -0 0</pose>
+      <diffuse>0.5 0.5 0.5 1</diffuse>
+      <specular>0.1 0.1 0.1 1</specular>
+      <direction>0.1 0.1 -0.9</direction>
+      <attenuation>
+        <range>20</range>
+        <constant>0.5</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <cast_shadows>1</cast_shadows>
+    </light>
+    <light name='user_directional_light_1' type='directional'>
+      <pose>10.4762 9.60881 1 0 -0 0</pose>
+      <diffuse>0.5 0.5 0.5 1</diffuse>
+      <specular>0.1 0.1 0.1 1</specular>
+      <direction>0.1 0.1 -0.9</direction>
+      <attenuation>
+        <range>20</range>
+        <constant>0.5</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <cast_shadows>1</cast_shadows>
+    </light>
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose>18.2797 -9.50323 6.40337 0 0.311643 2.68019</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+  </world>
+</sdf>