Added cone simulation and bbox capturing

CMakeLists.txt

@@ -17,6 +17,13 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(ignition-cmake2 REQUIRED)
+find_package(ignition-msgs5 REQUIRED)
+find_package(ignition-transport8 REQUIRED)
+find_package(ignition-math6 REQUIRED)
+find_package(ignition-common3 REQUIRED)
+find_package(ignition-gazebo6 REQUIRED)
+
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -32,6 +39,12 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+add_executable(ExtractPos src/extract_cone_pos_bbox.cpp)
+
+target_link_libraries(ExtractPos ${IGNITION-MSGS_LIBRARIES} ${IGNITION-TRANSPORT_LIBRARIES} ${IGNITION-COMMON_LIBRARIES} ${IGNITION-MATH_LIBRARIES})
+
+install(TARGETS ExtractPos DESTINATION lib/${PROJECT_NAME})
+
 install(
   DIRECTORY description launch worlds src
   DESTINATION share/${PROJECT_NAME}

description/bbox_camera.xacro

@@ -2,15 +2,15 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
 
     <joint name="camera_joint" type="fixed">
-        <parent link="laser_frame"/>
+        <parent link="chassis"/>
         <child link="camera_link"/>
-        <origin xyz="0 0.05 -0.03" rpy="0 0 ${-20*deg_to_rad}"/>
+        <origin xyz="-0.996 0.0696 1.04" rpy="${11.8*deg_to_rad} ${20.2*deg_to_rad} ${22.5*deg_to_rad}"/>
     </joint>
 
     <link name="camera_link">
         <visual>
             <geometry>
-                <box size="0.025 0.09 0.025"/>
+                <box size="0.03 0.03 0.03"/>
             </geometry>
             <material name="black"/>
         </visual>
@@ -48,6 +48,26 @@
           <update_rate>20</update_rate>
           <visualize>false</visualize>
         </sensor>
+        <sensor name="boundingbox_camera" type="boundingbox_camera">
+          <ignition_frame_id>camera_link_optical</ignition_frame_id>
+          <topic>box</topic>
+          <box_type>2d</box_type>
+          <camera>
+            <horizontal_fov>${84.7*deg_to_rad}</horizontal_fov>
+            <vertical_fov>${66.1*deg_to_rad}</vertical_fov>
+            <image>
+              <width>640</width>
+              <height>480</height>
+            </image>
+            <clip>
+              <near>0.01</near>
+              <far>60</far>
+            </clip>
+          </camera>
+          <always_on>1</always_on>
+          <update_rate>20</update_rate>
+          <visualize>false</visualize>
+        </sensor>
     </gazebo>
 
 </robot>

description/lidar.xacro

@@ -6,7 +6,7 @@
     <joint name="laser_joint" type="fixed">
         <parent link="chassis"/>
         <child link="laser_frame"/>
-        <origin xyz="-1.1070259 0.0 1.0059250" rpy="0 0 0"/>
+        <origin xyz="-0.985 0.0 ${1.08-0.024604}" rpy="0 0 0"/>
     </joint>
 
     

description/robot_core.xacro

@@ -15,7 +15,7 @@
     <link name="chassis">
       <visual>
         <geometry>
-          <mesh filename="file://$(find sensor_sim)/description/assets/ft24_asm.dae"/>
+          <mesh filename="file://$(find sensor_sim)/description/assets/ft24_asm.obj" scale="0.001 0.001 0.001"/>
         </geometry>
         <material name="grey"/>
       </visual>

launch/launch_sim.py

@@ -46,7 +46,7 @@ def generate_launch_description():
         arguments=[
             '-topic', 'robot_description',
             '-name', 'spawn_robot',
-            '-z', '5',
+            '-z', '1',
             '-x', '0',
             ],
         output='screen'

src/extract_cone_pos_bbox.cpp

@@ -0,0 +1,136 @@
+#include <ignition/msgs.hh>
+#include <ignition/transport/Node.hh>
+#include <ignition/msgs/annotated_axis_aligned_2d_box_v.pb.h>
+
+#include <iostream>
+#include <cmath>
+#include <chrono>
+#include <thread>
+
+// Settings
+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 roll = 0;
+double pitch = 0;
+double yaw = 0;
+
+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;
+
+            // 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();
+            
+        }
+  }
+
+}
+
+int main()
+{
+    // Connect to the Ignition Transport node
+    ignition::transport::Node node;
+
+    // Set service details
+    const std::string service = "/world/" + world_name + "/set_pose";
+    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;
+
+    // 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;
+
+            
+
+            // 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);
+
+            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;
+            ignition::msgs::Boolean result_msg;
+
+            // submit request (blocking)
+            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));           
+        }
+    }
+
+    
+
+    return 0;
+}

worlds/test_world.sdf

@@ -138,11 +138,18 @@
       </plugin>
 
       <!-- Image Display Plugins for visualization -->
-      <plugin filename="ImageDisplay" name="Image Display">
+      <plugin filename="ImageDisplay" name="Segmentation Image Display">
         <ignition-gui>
         </ignition-gui>
         <topic>semantic/colored_map</topic>
       </plugin>
+
+      <!-- Image Display Plugins for visualization -->
+      <plugin filename="ImageDisplay" name="Boundingbox Image Display">
+        <ignition-gui>
+        </ignition-gui>
+        <topic>box_image</topic>
+      </plugin>
     </gui>
 
     <light type="directional" name="sun">
@@ -184,5 +191,30 @@
         </visual>
       </link>
     </model>
+
+    <model name="blue_cone">
+      <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>
+          </geometry>
+          <laser_retro>
+            100
+          </laser_retro>
+          <plugin filename="ignition-gazebo-label-system" name="ignition::gazebo::systems::Label">
+            <label>5</label>
+          </plugin>
+        </visual>
+      </link>
+    </model>
   </world>
 </sdf>