Merge branch 'main' of https://git.fasttube.de/t.korjakow/sensor_sim

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>

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>
+                <min_angle>${-60*deg_to_rad}</min_angle>
-                <max_angle>${90*deg_to_rad}</max_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>
+                <min_angle>${-12.5*deg_to_rad}</min_angle>
-                <max_angle>${0*deg_to_rad}</max_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>

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">

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 x = 0;
-double pose_offset_x = 0.0;
+double y = 0;
-double pose_offset_y = 0.0;
+double z = 0;
-double pose_offset_z = 0.17;
-
-double x = pose_offset_x;
-double y = pose_offset_y;
-double z = pose_offset_z;
 
 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)
 {
+    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 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;
-
-            // 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();
+            }
+        }
+        pos_changed = false;
+        pthread_cond_signal (&bbox_cond);
+        pthread_mutex_unlock(&lock);
+    }
+    
   
-        }
-  }
 
 }
 
+
 int main()
 {
     // Connect to the Ignition Transport node
@@ -61,38 +64,30 @@ 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 i = 0; i < 1000; i++)
     {   
-        for (int j = 0; j < circle_resolution; j++)
+        std::cout << "Iteration: " << i << std::endl;
-        {
+        while (pos_changed)
-            // move cone in rotation around origin with sin and cos
+            pthread_cond_wait (&bbox_cond, &lock);
-            double cur_circle_radius = min_circle_radius + (max_circle_radius - min_circle_radius) * (double)i / (double)circles;
+        // random float between 1 and 40
-            x = cur_circle_radius * cos(j * 2 * M_PI / circle_resolution);
+        float cur_circle_radius = min_circle_radius + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(max_circle_radius - min_circle_radius)));
-            y = cur_circle_radius * sin(j * 2 * M_PI / circle_resolution);
+        // 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;
@@ -114,20 +109,20 @@ int main()
         pose_msg.set_name(model_name);
         pose_msg.mutable_position()->CopyFrom(vector3d_msg);
         pose_msg.mutable_orientation()->CopyFrom(quat_msg);
-            bool result;
+        bool result = false;
         ignition::msgs::Boolean result_msg;
 
         // 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));           
         }
+        pos_changed = true;
+        pthread_mutex_unlock(&lock);
     }
 
     

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>