diff --git a/sensing/slam/laser_slam/script/laser_transfer.py b/sensing/slam/laser_slam/script/laser_transfer.py
index 73cb3ec..f824256 100755
--- a/sensing/slam/laser_slam/script/laser_transfer.py
+++ b/sensing/slam/laser_slam/script/laser_transfer.py
@@ -2,7 +2,7 @@ import rospy
 from gazebo_msgs.srv import GetModelState
 from geometry_msgs.msg import PoseStamped
 from nav_msgs.msg import Odometry
-import tf
+from tf2_ros import TransformListener, Buffer
 import sys
 
 vehicle_type = sys.argv[1]
@@ -10,6 +10,8 @@ vehicle_id = sys.argv[2]
 laser_slam_type = sys.argv[3]
 rospy.init_node(vehicle_type+vehicle_id+'_'+laser_slam_type+'_laser_transfer')
 pose_pub = rospy.Publisher(vehicle_type+'_'+ vehicle_id+"/mavros/vision_pose/pose", PoseStamped, queue_size=2)
+tfBuffer = Buffer()
+tflistener = TransformListener(tfBuffer)
 local_pose = PoseStamped()
 hector = PoseStamped()
 height = 0
@@ -17,11 +19,6 @@ height = 0
 def odm_groundtruth_callback(msg):
     global height
     height = msg.pose.pose.position.z
-    
-def odm_aloam_callback(msg):
-    global local_pose
-    local_pose.header.stamp = msg.header.stamp
-    local_pose.pose = msg.pose.pose
 
 def hector_callback(data):
     global hector
@@ -39,18 +36,24 @@ def hector_slam():
         
 def aloam():
     global local_pose
-    rate = rospy.Rate(100)
-    while True:
+    rate = rospy.Rate(30)
+    while not rospy.is_shutdown():
+        try:
+            tfstamped = tfBuffer.lookup_transform('camera_init', 'aft_mapped', rospy.Time(0))
+        except:
+            continue
+        local_pose.header.stamp = rospy.Time().now()
+        local_pose.header.frame_id = 'map'
+        local_pose.pose.position = tfstamped.transform.translation
+        local_pose.pose.orientation = tfstamped.transform.rotation
         pose_pub.publish(local_pose)
         rate.sleep()
     
 if __name__ == '__main__':
-    
     if laser_slam_type == '2d':
         odom_groundtruth_sub = rospy.Subscriber('/xtdrone/'+vehicle_type+'_'+ vehicle_id+'/ground_truth/odom', Odometry, odm_groundtruth_callback)
         hector_slam()
     elif laser_slam_type == '3d':
-        odom_aloam_sub = rospy.Subscriber('/laser_odom_to_init', Odometry, odm_aloam_callback)
         aloam()
     else:
         print('input error')
diff --git a/sensing/slam/laser_slam/script/laser_transfer_old.py b/sensing/slam/laser_slam/script/laser_transfer_old.py
index 5b46dfd..88f8174 100644
--- a/sensing/slam/laser_slam/script/laser_transfer_old.py
+++ b/sensing/slam/laser_slam/script/laser_transfer_old.py
@@ -2,7 +2,7 @@ import rospy
 from gazebo_msgs.srv import GetModelState
 from geometry_msgs.msg import PoseStamped, Pose2D
 from nav_msgs.msg import Odometry
-import tf
+from tf2_ros import TransformListener, Buffer
 import sys
 
 vehicle_type = sys.argv[1]
@@ -18,11 +18,6 @@ def odm_groundtruth_callback(msg):
     global local_pose
     local_pose.header.stamp = msg.header.stamp
     local_pose.pose.position.z  = msg.pose.pose.position.z
-    
-def odm_aloam_callback(msg):
-    global local_pose
-    local_pose.header.stamp = msg.header.stamp
-    local_pose.pose = msg.pose.pose
 
 def laser_scan_matcher_callback(data):
     global laser_scan
@@ -46,8 +41,16 @@ def laser_scan_matcher():
         
 def aloam():
     global local_pose
-    rate = rospy.Rate(100)
-    while True:
+    rate = rospy.Rate(30)
+    while not rospy.is_shutdown():
+        try:
+            tfstamped = tfBuffer.lookup_transform('camera_init', 'aft_mapped', rospy.Time(0))
+        except:
+            continue
+        local_pose.header.stamp = rospy.Time().now()
+        local_pose.header.frame_id = 'map'
+        local_pose.pose.position = tfstamped.transform.translation
+        local_pose.pose.orientation = tfstamped.transform.rotation
         pose_pub.publish(local_pose)
         rate.sleep()
     
@@ -57,7 +60,6 @@ if __name__ == '__main__':
         odom_groundtruth_sub = rospy.Subscriber('/xtdrone/'+vehicle_type+'_'+ vehicle_id+'/ground_truth/odom', Odometry, odm_groundtruth_callback)
         laser_scan_matcher()
     elif laser_slam_type == '3d':
-        odom_aloam_sub = rospy.Subscriber('/laser_odom_to_init', Odometry, odm_aloam_callback)
         aloam()
     else:
         print('input error')
diff --git a/sitl_config/worlds/indoor1.world b/sitl_config/worlds/indoor1.world
index 7c16b7b..1402b6c 100644
--- a/sitl_config/worlds/indoor1.world
+++ b/sitl_config/worlds/indoor1.world
@@ -11,7 +11,7 @@
               <size>100 100</size>
             </plane>
           </geometry>
-           
+
             <friction>
               <ode>
                 <mu>100</mu>
@@ -25,7 +25,7 @@
               <ode/>
             </contact>
             <bounce/>
-           
+
           <max_contacts>10</max_contacts>
         </collision>
         <visual name='visual'>
@@ -95,7 +95,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -106,7 +106,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='wall_1'>
           <pose frame=''>-5.03 0.53 1.415 0 -0 0</pose>
@@ -116,7 +116,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -127,7 +127,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='wall_2'>
           <pose frame=''>4.24 -0.31 1.415 0 -0 0</pose>
@@ -137,7 +137,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -148,7 +148,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
 
         <collision name='wall_4'>
@@ -159,7 +159,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -170,7 +170,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='wall_5'>
           <pose frame=''>3.155 10.4 1.435 0 -0 0</pose>
@@ -180,7 +180,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -191,7 +191,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='wall_6'>
           <pose frame=''>0.615 -10.98 1.415 0 -0 0</pose>
@@ -201,7 +201,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -212,7 +212,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='wall_7'>
           <pose frame=''>-4.62 -10.98 1.415 0 -0 0</pose>
@@ -222,7 +222,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -233,7 +233,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='wall_8'>
           <pose frame=''>-3.61 -10.98 2.69 0 -0 0</pose>
@@ -243,7 +243,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -254,7 +254,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='coke'>
           <pose frame=''>-4.385 0.26 0.95 0 -0 0</pose>
@@ -264,7 +264,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -275,7 +275,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual'>
           <geometry>
@@ -320,7 +320,7 @@
           </geometry>
           <pose frame=''>-2.31327 0 1.425 0 -0 0</pose>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -331,7 +331,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='Wall_0_Visual_1'>
           <pose frame=''>1.18673 0 0.25 0 -0 0</pose>
@@ -359,7 +359,7 @@
           </geometry>
           <pose frame=''>1.18673 0 0.25 0 -0 0</pose>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -370,7 +370,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='Wall_0_Visual_2'>
           <pose frame=''>2.68673 0 1.675 0 -0 0</pose>
@@ -398,7 +398,7 @@
           </geometry>
           <pose frame=''>2.68673 0 1.675 0 -0 0</pose>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -409,7 +409,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='Wall_0_Visual_3'>
           <pose frame=''>0.37346 0 2.575 0 -0 0</pose>
@@ -437,7 +437,7 @@
           </geometry>
           <pose frame=''>0.37346 0 2.575 0 -0 0</pose>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -448,7 +448,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <self_collide>0</self_collide>
         <enable_wind>0</enable_wind>
@@ -482,7 +482,7 @@
           </geometry>
           <pose frame=''>-1.53914 0 1.425 0 -0 0</pose>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -493,7 +493,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='Wall_3_Visual_1'>
           <pose frame=''>1.69286 0 1.425 0 -0 0</pose>
@@ -521,7 +521,7 @@
           </geometry>
           <pose frame=''>1.69286 0 1.425 0 -0 0</pose>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -532,7 +532,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='Wall_3_Visual_2'>
           <pose frame=''>0.153711 0 0 0 -0 0</pose>
@@ -560,7 +560,7 @@
           </geometry>
           <pose frame=''>0.153711 0 0 0 -0 0</pose>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -571,7 +571,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='Wall_3_Visual_3'>
           <pose frame=''>0.153711 0 2.425 0 -0 0</pose>
@@ -599,7 +599,7 @@
           </geometry>
           <pose frame=''>0.153711 0 2.425 0 -0 0</pose>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -610,7 +610,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <self_collide>0</self_collide>
         <enable_wind>0</enable_wind>
@@ -773,27 +773,12 @@
       <light name='user_point_light_1'>
         <pose frame=''>0.766851 -12.0385 1 0 -0 0</pose>
       </light>
-      <light name='user_point_light_2'>
-        <pose frame=''>-2.25143 -7.57347 1 0 -0 0</pose>
-      </light>
       <light name='user_point_light_3'>
         <pose frame=''>2.80305 -7.33891 1.86283 -0.49865 0 0</pose>
       </light>
       <light name='user_point_light_4'>
         <pose frame=''>0.076075 -5.91752 1 0 -0 0</pose>
       </light>
-      <light name='user_point_light_5'>
-        <pose frame=''>-4.36942 -10.4942 1 0 -0 0</pose>
-      </light>
-      <light name='user_point_light_6'>
-        <pose frame=''>-3.92433 -9.26622 1 0 -0 0</pose>
-      </light>
-      <light name='user_point_light_7'>
-        <pose frame=''>-3.73468 -12.8798 1 0 -0 0</pose>
-      </light>
-      <light name='user_point_light_8'>
-        <pose frame=''>-3.8302 -8.15565 1 0 -0 0</pose>
-      </light>
     </state>
     <model name='cafe_table'>
       <static>1</static>
@@ -806,7 +791,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -817,7 +802,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='column'>
           <pose frame=''>0 0 0.37 0 -0 0</pose>
@@ -827,7 +812,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -838,7 +823,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='base'>
           <pose frame=''>0 0 0.02 0 -0 0</pose>
@@ -848,7 +833,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -859,7 +844,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual'>
           <geometry>
@@ -902,7 +887,7 @@
               <size>0.387354 0.387354 0.0254</size>
             </box>
           </geometry>
-           
+
             <contact>
               <ode>
                 <max_vel>0.1</max_vel>
@@ -919,7 +904,7 @@
               </torsional>
             </friction>
             <bounce/>
-           
+
           <max_contacts>10</max_contacts>
         </collision>
         <collision name='bottom'>
@@ -929,7 +914,7 @@
               <size>0.387354 0.387354 0.0254</size>
             </box>
           </geometry>
-           
+
             <contact>
               <ode>
                 <max_vel>0.1</max_vel>
@@ -946,7 +931,7 @@
               </torsional>
             </friction>
             <bounce/>
-           
+
           <max_contacts>10</max_contacts>
         </collision>
         <collision name='left'>
@@ -956,7 +941,7 @@
               <size>0.387354 0.020643 0.092075</size>
             </box>
           </geometry>
-           
+
             <contact>
               <ode>
                 <max_vel>0.1</max_vel>
@@ -973,7 +958,7 @@
               </torsional>
             </friction>
             <bounce/>
-           
+
           <max_contacts>10</max_contacts>
         </collision>
         <collision name='right'>
@@ -983,7 +968,7 @@
               <size>0.387354 0.020643 0.092075</size>
             </box>
           </geometry>
-           
+
             <contact>
               <ode>
                 <max_vel>0.1</max_vel>
@@ -1000,7 +985,7 @@
               </torsional>
             </friction>
             <bounce/>
-           
+
           <max_contacts>10</max_contacts>
         </collision>
         <collision name='middle'>
@@ -1010,7 +995,7 @@
               <size>0.387354 0.0254 0.092075</size>
             </box>
           </geometry>
-           
+
             <contact>
               <ode>
                 <max_vel>0.1</max_vel>
@@ -1027,7 +1012,7 @@
               </torsional>
             </friction>
             <bounce/>
-           
+
           <max_contacts>10</max_contacts>
         </collision>
         <self_collide>0</self_collide>
@@ -1057,7 +1042,7 @@
               <uri>model://coke_can/meshes/coke_can.dae</uri>
             </mesh>
           </geometry>
-           
+
             <friction>
               <ode>
                 <mu>1</mu>
@@ -1076,7 +1061,7 @@
               </ode>
             </contact>
             <bounce/>
-           
+
           <max_contacts>10</max_contacts>
         </collision>
         <visual name='visual'>
@@ -1104,7 +1089,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1115,7 +1100,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='column'>
           <pose frame=''>0 0 0.37 0 -0 0</pose>
@@ -1125,7 +1110,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1136,7 +1121,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='base'>
           <pose frame=''>0 0 0.02 0 -0 0</pose>
@@ -1146,7 +1131,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1157,7 +1142,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual'>
           <geometry>
@@ -1194,7 +1179,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1205,7 +1190,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='lid_lip_1'>
           <pose frame=''>0 0.20063 0.3048 0 -0 0</pose>
@@ -1215,7 +1200,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1226,7 +1211,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='lid_lip_2'>
           <pose frame=''>0 -0.20063 0.3048 0 -0 0</pose>
@@ -1236,7 +1221,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1247,7 +1232,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='lid_lip_3'>
           <pose frame=''>0.32585 0 0.3048 0 -0 0</pose>
@@ -1257,7 +1242,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1268,7 +1253,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='lid_lip_4'>
           <pose frame=''>-0.32585 0 0.3048 0 -0 0</pose>
@@ -1278,7 +1263,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1289,7 +1274,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='side_wall_1'>
           <pose frame=''>0 -0.18 0.14155 0.061147 -0 0</pose>
@@ -1299,7 +1284,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1310,7 +1295,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='side_wall_2'>
           <pose frame=''>0 0.18 0.14155 -0.061147 0 0</pose>
@@ -1320,7 +1305,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1331,7 +1316,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='side_wall_3'>
           <pose frame=''>0.305 0 0.14155 0 0.061147 0</pose>
@@ -1341,7 +1326,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1352,7 +1337,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='side_wall_4'>
           <pose frame=''>-0.305 0 0.14155 0 -0.061147 0</pose>
@@ -1362,7 +1347,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1373,7 +1358,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='bottom'>
           <pose frame=''>0 0 0.01 0 -0 0</pose>
@@ -1383,7 +1368,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1394,7 +1379,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='corner_1'>
           <pose frame=''>0.30202 -0.175 0.14155 0.061147 0.061147 -0</pose>
@@ -1405,7 +1390,7 @@
             </cylinder>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1416,7 +1401,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='corner_2'>
           <pose frame=''>-0.30202 -0.175 0.14155 0.061147 -0.061147 0</pose>
@@ -1427,7 +1412,7 @@
             </cylinder>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1438,7 +1423,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='corner_3'>
           <pose frame=''>-0.30202 0.175 0.14155 -0.061147 -0.061147 -0</pose>
@@ -1449,7 +1434,7 @@
             </cylinder>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1460,7 +1445,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <collision name='corner_4'>
           <pose frame=''>0.30202 0.175 0.14155 -0.061147 0.061147 0</pose>
@@ -1471,7 +1456,7 @@
             </cylinder>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1482,7 +1467,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual'>
           <geometry>
@@ -1556,19 +1541,6 @@
       </attenuation>
       <cast_shadows>1</cast_shadows>
     </light>
-    <light name='user_point_light_2' type='point'>
-      <pose frame=''>-2.25143 -7.57347 1 0 -0 0</pose>
-      <diffuse>0.5 0.5 0.5 1</diffuse>
-      <specular>0.1 0.1 0.1 1</specular>
-      <attenuation>
-        <range>20</range>
-        <constant>0.5</constant>
-        <linear>0.01</linear>
-        <quadratic>0.001</quadratic>
-      </attenuation>
-      <cast_shadows>0</cast_shadows>
-      <direction>0 0 -1</direction>
-    </light>
     <light name='user_point_light_3' type='point'>
       <pose frame=''>2.80305 -7.84683 1 0 -0 0</pose>
       <diffuse>0.5 0.5 0.5 1</diffuse>
@@ -1621,58 +1593,6 @@
       <cast_shadows>0</cast_shadows>
       <direction>0 0 -1</direction>
     </light>
-    <light name='user_point_light_5' type='point'>
-      <pose frame=''>-4.36942 -10.4942 1 0 -0 0</pose>
-      <diffuse>0.5 0.5 0.5 1</diffuse>
-      <specular>0.1 0.1 0.1 1</specular>
-      <attenuation>
-        <range>20</range>
-        <constant>0.5</constant>
-        <linear>0.01</linear>
-        <quadratic>0.001</quadratic>
-      </attenuation>
-      <cast_shadows>0</cast_shadows>
-      <direction>0 0 -1</direction>
-    </light>
-    <light name='user_point_light_6' type='point'>
-      <pose frame=''>-3.92433 -9.26622 1 0 -0 0</pose>
-      <diffuse>0.5 0.5 0.5 1</diffuse>
-      <specular>0.1 0.1 0.1 1</specular>
-      <attenuation>
-        <range>20</range>
-        <constant>0.5</constant>
-        <linear>0.01</linear>
-        <quadratic>0.001</quadratic>
-      </attenuation>
-      <cast_shadows>0</cast_shadows>
-      <direction>0 0 -1</direction>
-    </light>
-    <light name='user_point_light_7' type='point'>
-      <pose frame=''>-3.73468 -12.8798 1 0 -0 0</pose>
-      <diffuse>0.5 0.5 0.5 1</diffuse>
-      <specular>0.1 0.1 0.1 1</specular>
-      <attenuation>
-        <range>20</range>
-        <constant>0.5</constant>
-        <linear>0.01</linear>
-        <quadratic>0.001</quadratic>
-      </attenuation>
-      <cast_shadows>0</cast_shadows>
-      <direction>0 0 -1</direction>
-    </light>
-    <light name='user_point_light_8' type='point'>
-      <pose frame=''>-3.8302 -8.15565 1 0 -0 0</pose>
-      <diffuse>0.5 0.5 0.5 1</diffuse>
-      <specular>0.1 0.1 0.1 1</specular>
-      <attenuation>
-        <range>20</range>
-        <constant>0.5</constant>
-        <linear>0.01</linear>
-        <quadratic>0.001</quadratic>
-      </attenuation>
-      <cast_shadows>0</cast_shadows>
-      <direction>0 0 -1</direction>
-    </light>
     <model name='bookshelf'>
       <static>1</static>
       <link name='link'>
@@ -1687,7 +1607,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1698,7 +1618,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual1'>
           <pose frame=''>0 0.005 0.6 0 -0 0</pose>
@@ -1722,7 +1642,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1733,7 +1653,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual2'>
           <pose frame=''>0.45 -0.195 0.6 0 -0 0</pose>
@@ -1757,7 +1677,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1768,7 +1688,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual3'>
           <pose frame=''>-0.45 -0.195 0.6 0 -0 0</pose>
@@ -1792,7 +1712,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1803,7 +1723,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual4'>
           <pose frame=''>0 -0.195 0.03 0 -0 0</pose>
@@ -1827,7 +1747,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1838,7 +1758,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual5'>
           <pose frame=''>0 -0.195 1.19 0 -0 0</pose>
@@ -1862,7 +1782,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1873,7 +1793,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual6'>
           <pose frame=''>0 -0.195 0.43 0 -0 0</pose>
@@ -1897,7 +1817,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1908,7 +1828,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual7'>
           <pose frame=''>0 -0.195 0.8 0 -0 0</pose>
@@ -1944,7 +1864,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1955,7 +1875,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual1'>
           <pose frame=''>0 0.005 0.6 0 -0 0</pose>
@@ -1979,7 +1899,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -1990,7 +1910,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual2'>
           <pose frame=''>0.45 -0.195 0.6 0 -0 0</pose>
@@ -2014,7 +1934,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -2025,7 +1945,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual3'>
           <pose frame=''>-0.45 -0.195 0.6 0 -0 0</pose>
@@ -2049,7 +1969,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -2060,7 +1980,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual4'>
           <pose frame=''>0 -0.195 0.03 0 -0 0</pose>
@@ -2084,7 +2004,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -2095,7 +2015,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual5'>
           <pose frame=''>0 -0.195 1.19 0 -0 0</pose>
@@ -2119,7 +2039,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -2130,7 +2050,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual6'>
           <pose frame=''>0 -0.195 0.43 0 -0 0</pose>
@@ -2154,7 +2074,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -2165,7 +2085,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='visual7'>
           <pose frame=''>0 -0.195 0.8 0 -0 0</pose>
@@ -2210,7 +2130,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -2221,7 +2141,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='cabinet_bottom_plate_geom_visual'>
           <pose frame=''>0 0 0.01 0 -0 0</pose>
@@ -2245,7 +2165,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -2256,7 +2176,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='cabinet_bottom_plate_geom_cabinet_back_plate_visual'>
           <pose frame=''>0.235 0 0.51 0 -0 0</pose>
@@ -2280,7 +2200,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -2291,7 +2211,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='cabinet_bottom_plate_geom_cabinet_left_plate_visual'>
           <pose frame=''>0 0.235 0.51 0 -0 0</pose>
@@ -2315,7 +2235,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -2326,7 +2246,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='cabinet_bottom_plate_geom_cabinet_middle_plate_visual'>
           <pose frame=''>0 0 0.51 0 -0 0</pose>
@@ -2350,7 +2270,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -2361,7 +2281,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='cabinet_bottom_plate_geom_cabinet_right_plate_visual'>
           <pose frame=''>0 -0.235 0.51 0 -0 0</pose>
@@ -2385,7 +2305,7 @@
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -2396,7 +2316,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='cabinet_bottom_plate_geom_cabinet_top_plate_visual'>
           <pose frame=''>0 0 1.01 0 -0 0</pose>
@@ -2429,7 +2349,7 @@
           </geometry>
           <pose frame=''>0 0 1.25 0 -0 0</pose>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -2440,7 +2360,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='Wall_10_Visual'>
           <pose frame=''>0 0 1.25 0 -0 0</pose>
@@ -2474,7 +2394,7 @@
           </geometry>
           <pose frame=''>0 0 1.25 0 -0 0</pose>
           <max_contacts>10</max_contacts>
-           
+
             <contact>
               <ode/>
             </contact>
@@ -2485,7 +2405,7 @@
               </torsional>
               <ode/>
             </friction>
-           
+
         </collision>
         <visual name='Wall_4_Visual'>
           <pose frame=''>0 0 1.25 0 -0 0</pose>