diff --git a/description/bbox_camera.xacro b/description/bbox_camera.xacro
index 373fe1f..b620fdf 100644
--- a/description/bbox_camera.xacro
+++ b/description/bbox_camera.xacro
@@ -4,7 +4,7 @@
     <joint name="camera_joint" type="fixed">
         <parent link="chassis"/>
         <child link="camera_link"/>
-        <origin xyz="-0.996 0.0696 1.04" rpy="${11.8*deg_to_rad} ${20.2*deg_to_rad} ${22.5*deg_to_rad}"/>
+        <origin xyz="-1.0269530 0.0328365 0.9993990" rpy="0 ${9.0*deg_to_rad} ${-20.1931*deg_to_rad}"/>
     </joint>
 
     <link name="camera_link">
@@ -45,7 +45,7 @@
             </clip>
           </camera>
           <always_on>1</always_on>
-          <update_rate>20</update_rate>
+          <update_rate>3</update_rate>
           <visualize>false</visualize>
         </sensor>
         <sensor name="boundingbox_camera" type="boundingbox_camera">
@@ -65,7 +65,7 @@
             </clip>
           </camera>
           <always_on>1</always_on>
-          <update_rate>20</update_rate>
+          <update_rate>3</update_rate>
           <visualize>false</visualize>
         </sensor>
     </gazebo>
diff --git a/description/lidar.xacro b/description/lidar.xacro
index 800ba5c..c0b1ded 100644
--- a/description/lidar.xacro
+++ b/description/lidar.xacro
@@ -6,20 +6,20 @@
     <joint name="laser_joint" type="fixed">
         <parent link="chassis"/>
         <child link="laser_frame"/>
-        <origin xyz="-0.985 0.0 ${1.08-0.024604}" rpy="0 0 0"/>
+        <origin xyz="-0.994175 0.0 1.0387512" rpy="0 ${9.0*deg_to_rad} 0"/>
     </joint>
 
     
     <link name="laser_frame">
         <visual>
             <geometry>
-                <cylinder radius="0.05" length="0.04"/>
+                <box size="0.105 0.1316 0.065"/>
             </geometry>
             <material name="red"/>
         </visual>
         <collision>
             <geometry>
-                <cylinder radius="0.05" length="0.04"/>
+                <box size="0.105 0.1316 0.065"/>
             </geometry>
         </collision>
         <xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.05">
@@ -38,16 +38,16 @@
           <ray>
             <scan>
               <horizontal>
-                <samples>640</samples>
+                <samples>666</samples>
                 <resolution>1</resolution>
-                <min_angle>${-90*deg_to_rad}</min_angle>
-                <max_angle>${90*deg_to_rad}</max_angle>
+                <min_angle>${-60*deg_to_rad}</min_angle>
+                <max_angle>${60*deg_to_rad}</max_angle>
               </horizontal>
               <vertical>
-                <samples>64</samples>
+                <samples>108</samples>
                 <resolution>1</resolution>
-                <min_angle>${-45*deg_to_rad}</min_angle>
-                <max_angle>${0*deg_to_rad}</max_angle>
+                <min_angle>${-12.5*deg_to_rad}</min_angle>
+                <max_angle>${12.5*deg_to_rad}</max_angle>
               </vertical>
             </scan>
             <range>
@@ -63,5 +63,10 @@
           </ray>
           <alwaysOn>true</alwaysOn>
         </sensor>
+        <visual>
+          <plugin filename="ignition-gazebo-label-system" name="ignition::gazebo::systems::Label">
+            <label>30</label>
+          </plugin>
+        </visual>
     </gazebo>  
 </robot>
\ No newline at end of file
diff --git a/description/robot_core.xacro b/description/robot_core.xacro
index ccd4089..6b05fbe 100644
--- a/description/robot_core.xacro
+++ b/description/robot_core.xacro
@@ -30,6 +30,11 @@
         <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> 
     </inertial>
 
+    <gazebo>
+        <static>true</static>
+    </gazebo>
+
+
     </link>
 
     <gazebo reference="chassis">
diff --git a/src/extract_cone_pos_bbox.cpp b/src/extract_cone_pos_bbox.cpp
index 34cfe91..d2e99d3 100644
--- a/src/extract_cone_pos_bbox.cpp
+++ b/src/extract_cone_pos_bbox.cpp
@@ -12,45 +12,48 @@ const std::string world_name = "empty";
 const std::string model_name = "blue_cone";
 
 const int timeout_ms = 10;
-const double update_rate_hz = 100.0;
 
-// Offsets
-double pose_offset_x = 0.0;
-double pose_offset_y = 0.0;
-double pose_offset_z = 0.17;
-
-double x = pose_offset_x;
-double y = pose_offset_y;
-double z = pose_offset_z;
+double x = 0;
+double y = 0;
+double z = 0;
 
 double roll = 0;
 double pitch = 0;
 double yaw = 0;
 
+// init semaphore
+pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER;
+pthread_cond_t bbox_cond = PTHREAD_COND_INITIALIZER;
+bool pos_changed = true;
+
 void callback_bbox(const ignition::msgs::AnnotatedAxisAligned2DBox_V &_msg)
 {
-    for (const auto & gz_box : _msg.annotated_box()) {
-        if (gz_box.label() == 5) {
-            auto box = gz_box.box();
-            auto min_corner = box.min_corner();
-            auto max_corner = box.max_corner();
-            // save bbox data and cone pose to csv file
-            std::cout << "Bounding box: " << min_corner.x() << ", " << min_corner.y() << ", " << max_corner.x() << ", " << max_corner.y() << std::endl;
+    if (pos_changed)
+    {
+        pthread_mutex_lock(&lock);
+        for (const auto & gz_box : _msg.annotated_box()) {
+            if (gz_box.label() == 5) {
+                auto box = gz_box.box();
+                auto min_corner = box.min_corner();
+                auto max_corner = box.max_corner();
 
-            // save cone pose to csv file
-            std::cout << "Cone pose: " << x << ", " << y << ", " << z << std::endl;
-
-            // write to csv file
-            std::ofstream outfile;
-            outfile.open("cone_pos_bbox.csv", std::ios_base::app);
-            outfile << x << "," << y << "," << z << "," << roll << "," << pitch << "," << yaw << "," << min_corner.x() << "," << min_corner.y() << "," << max_corner.x() << "," << max_corner.y() << "\n";
-            outfile.close();
-            
+                // write to csv file
+                std::ofstream outfile;
+                outfile.open("cone_pos_bbox.csv", std::ios_base::app);
+                outfile << x << "," << y << "," << z << "," << roll << "," << pitch << "," << yaw << "," << min_corner.x() << "," << min_corner.y() << "," << max_corner.x() << "," << max_corner.y() << "\n";
+                outfile.close();
+            }
         }
-  }
+        pos_changed = false;
+        pthread_cond_signal (&bbox_cond);
+        pthread_mutex_unlock(&lock);
+    }
+    
+  
 
 }
 
+
 int main()
 {
     // Connect to the Ignition Transport node
@@ -61,73 +64,65 @@ int main()
     const std::string reqtype = "ignition.msgs.Pose";
     const std::string reptype = "ignition.msgs.Boolean";
 
-    // Configure the update loop
-    auto start_time = std::chrono::steady_clock::now();
-    double update_period_s = 1.0 / update_rate_hz;
-    auto last_update_time = start_time;
-
-
-    // move cone in circles around the origin and rotate the cone randomly
-    // after setting the position, wait a short duration and listen for a bounding box message
-    // write current cone position and orientation and bounding box to csv file
-    // repeat
-
-    int circle_resolution = 100;
-    int circles = 100;
     int min_circle_radius = 1;
     int max_circle_radius = 40;
+    int min_angle = 60;
+    int max_angle = 210;
 
     // listen for bounding box message
     std::string bbox_topic = "/box";
     node.Subscribe(bbox_topic, callback_bbox); 
 
-    for (int i = 0; i < circles; i++)
-    {
-        for (int j = 0; j < circle_resolution; j++)
-        {
-            // move cone in rotation around origin with sin and cos
-            double cur_circle_radius = min_circle_radius + (max_circle_radius - min_circle_radius) * (double)i / (double)circles;
-            x = cur_circle_radius * cos(j * 2 * M_PI / circle_resolution);
-            y = cur_circle_radius * sin(j * 2 * M_PI / circle_resolution);
-            z = 0;
+    for (int i = 0; i < 1000; i++)
+    {   
+        std::cout << "Iteration: " << i << std::endl;
+        while (pos_changed)
+            pthread_cond_wait (&bbox_cond, &lock);
+        // random float between 1 and 40
+        float cur_circle_radius = min_circle_radius + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(max_circle_radius - min_circle_radius)));
+        // random float between min_angle and max_angle
+        float random_degree = min_angle + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(max_angle - min_angle)));
+        float random_radians = random_degree * M_PI / 180;
+        
+        x = cur_circle_radius * cos(random_radians);
+        y = cur_circle_radius * sin(random_radians);
+        z = 0;
 
-            
+        // rotate cone randomly around z axis
+        yaw = 0;
+        pitch = 0;
+        roll = (double)rand() / RAND_MAX * 2 * M_PI;
 
-            // rotate cone randomly around z axis
-            yaw = 0;
-            pitch = 0;
-            roll = (double)rand() / RAND_MAX * 2 * M_PI;
+        // create request message
+        ignition::msgs::Vector3d vector3d_msg;
+        vector3d_msg.set_x(x);
+        vector3d_msg.set_y(y);
+        vector3d_msg.set_z(z);
 
-            // create request message
-            ignition::msgs::Vector3d vector3d_msg;
-            vector3d_msg.set_x(x);
-            vector3d_msg.set_y(y);
-            vector3d_msg.set_z(z);
+        ignition::msgs::Quaternion quat_msg;
+        quat_msg.set_w(cos(yaw / 2) * cos(pitch / 2) * cos(roll / 2) + sin(yaw / 2) * sin(pitch / 2) * sin(roll / 2));
+        quat_msg.set_x(sin(yaw / 2) * cos(pitch / 2) * cos(roll / 2) - cos(yaw / 2) * sin(pitch / 2) * sin(roll / 2));
+        quat_msg.set_y(cos(yaw / 2) * sin(pitch / 2) * cos(roll / 2) + sin(yaw / 2) * cos(pitch / 2) * sin(roll / 2));
+        quat_msg.set_z(cos(yaw / 2) * cos(pitch / 2) * sin(roll / 2) - sin(yaw / 2) * sin(pitch / 2) * cos(roll / 2));
 
-            ignition::msgs::Quaternion quat_msg;
-            quat_msg.set_w(cos(yaw / 2) * cos(pitch / 2) * cos(roll / 2) + sin(yaw / 2) * sin(pitch / 2) * sin(roll / 2));
-            quat_msg.set_x(sin(yaw / 2) * cos(pitch / 2) * cos(roll / 2) - cos(yaw / 2) * sin(pitch / 2) * sin(roll / 2));
-            quat_msg.set_y(cos(yaw / 2) * sin(pitch / 2) * cos(roll / 2) + sin(yaw / 2) * cos(pitch / 2) * sin(roll / 2));
-            quat_msg.set_z(cos(yaw / 2) * cos(pitch / 2) * sin(roll / 2) - sin(yaw / 2) * sin(pitch / 2) * cos(roll / 2));
+        ignition::msgs::Pose pose_msg;
+        pose_msg.set_name(model_name);
+        pose_msg.mutable_position()->CopyFrom(vector3d_msg);
+        pose_msg.mutable_orientation()->CopyFrom(quat_msg);
+        bool result = false;
+        ignition::msgs::Boolean result_msg;
 
-            ignition::msgs::Pose pose_msg;
-            pose_msg.set_name(model_name);
-            pose_msg.mutable_position()->CopyFrom(vector3d_msg);
-            pose_msg.mutable_orientation()->CopyFrom(quat_msg);
-            bool result;
-            ignition::msgs::Boolean result_msg;
-
-            // submit request (blocking)
+        // submit request (blocking)
+        while(!result){
             result = node.Request(service, pose_msg, timeout_ms, result_msg, result);
 
-            if (!result)
-            {
-                std::cout << "Update failed" << std::endl;
-            }
-
-            // sleep for a short duration to avoid
-            std::this_thread::sleep_for(std::chrono::milliseconds(100));           
+        if (!result)
+        {
+            std::cout << "Update failed" << std::endl;
         }
+        }
+        pos_changed = true;
+        pthread_mutex_unlock(&lock);
     }
 
     
diff --git a/worlds/test_world.sdf b/worlds/test_world.sdf
index 3eb8a03..d7f19d4 100644
--- a/worlds/test_world.sdf
+++ b/worlds/test_world.sdf
@@ -188,6 +188,9 @@
             <diffuse>0.8 0.8 0.8 1</diffuse>
             <specular>0.8 0.8 0.8 1</specular>
           </material>
+          <plugin filename="ignition-gazebo-label-system" name="ignition::gazebo::systems::Label">
+            <label>40</label>
+          </plugin>
         </visual>
       </link>
     </model>
@@ -196,13 +199,6 @@
       <static>true</static>
       <link name="link">
         <pose>0 0 0 0 0 0</pose>
-        <collision name="collision">
-          <geometry>
-            <box>
-              <size>0.112 0.112 0.3</size>
-            </box>
-          </geometry>
-        </collision>
         <visual name="visual">
           <geometry>
             <mesh><uri>model://assets/blue_cone.dae</uri></mesh>