Merge pull request #2 from vitpetrik/Qorvo-dw1000-plugin-and-holder-f…

…or-x500 Qorvo dw1000 plugin and holder for x500
ctu-mrs · Nov 9, 2023 · e5a778d · e5a778d
2 parents ac1eaf7 + 63cb676
commit e5a778d
Show file tree

Hide file tree

Showing 4 changed files with 75 additions and 0 deletions.
diff --git a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml
@@ -50,7 +50,9 @@ enable_dual_uv_cameras: [False, 'Add right and left UV cameras on the vehicle',
 enable_back_uv_camera: [False, 'Add back UV camera on the vehicle', [f450, x500]]
 uvcam_calib_file: [None, 'Specify UV camera calibration different than default one', [f450, f550, t650, x500]]
 uvcam_occlusions: [False, 'Enable occlusions for UVDAR simulation (NOTE: THIS REQUIRES A BEEFY COMPUTER)', [f450, f550, t650, x500]]
+enable_qorvo_dw1000: [False, 'Add Qorvo DW1000 UWB transceiver', [x500]]
 pos_file: [None, 'Load positions and ids from .csv file with format: [id, x, y, z, heading] or .yaml file with format: [uav_name: \n id: (int) \n x: (float) \n y: (float) \n z: (float) \n heading: (float)]', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec, m690]]
+uwb_id: [0, 'Set id for uwb ranging', [x500]]
 pos: [None, 'Specify spawn position of the first vehicle in format [x, y, z, heading]. Additional vehicles will spawn in line with 2m x-offset', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec]]
 enable_vio: [False, 'Add a forward-looking fisheye camera and a high frequency IMU', [f330, f450]]
 enable_vio_down: [False, 'Add a forward-looking fisheye camera and a high frequency IMU', [f330, f450]]

diff --git a/...o_simulation/models/mrs_robots_description/meshes/x500v2/qorvo_dw1000_mount.stl b/...o_simulation/models/mrs_robots_description/meshes/x500v2/qorvo_dw1000_mount.stl
diff --git a/.../mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja b/.../mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja
@@ -1434,6 +1434,41 @@ limitations under the License.
 {%- endmacro -%}
 <!--}-->
 
+<!-- Macro to add a UWB Range{-->
+{%- macro uwb_range_macro(namespace, parent_link, uwb_id, uav_name, x, y, z, roll, pitch, yaw) -%}
+<link name="uwb_range_link">
+  <pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
+  {{ zero_inertial_macro() }}
+  <sensor name="uwb_range_sensor" type="wireless_transmitter">
+    <update_rate>10</update_rate>
+    <always_on>1</always_on>
+    <plugin name="uwb_range_plugin" filename="libuwb_range.so">
+      <uwb_id>{{uwb_id}}</uwb_id>
+      <publish_topic>/{{ uav_name }}/uwb_range/range</publish_topic>
+      <frame_name>{{ uav_name }}/uwb</frame_name>
+      <sensitivity>-150</sensitivity>
+      <stddev>0.1</stddev>
+    </plugin>
+    <transceiver>
+      <frequency>6489.6</frequency>
+      <min_frequency>6240.0</min_frequency>
+      <max_frequency>6739.2</max_frequency>
+      <power>14.5</power>
+      <gain>2.5</gain>
+    </transceiver>
+  </sensor>
+</link> 
+
+<joint name="uwb_range_joint" type="fixed">
+  <parent>{{ parent_link }}</parent>
+  <child>uwb_range_link</child>
+  <disableFixedJointLumping>true</disableFixedJointLumping>
+  <preserveFixedJoint>true</preserveFixedJoint>
+</joint>
+{%- endmacro -%}
+<!--}-->
+
+
 <!-- ========================== LIDAR sensors ========================= -->
 
 <!-- Macro to add a Scanse Sweeper{-->

diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/x500.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/x500.sdf.jinja
@@ -80,6 +80,7 @@
   {%- set realsense_mount_mesh_file = "model://mrs_robots_description/meshes/x500v2/realsense_mount_mesh_file.stl" -%}
   {%- set bluefox_mount_mesh_file = "model://mrs_robots_description/meshes/x500v2/bluefox_mount_mesh_file.stl" -%}
   {%- set uvdar_mount_mesh = "model://mrs_robots_description/meshes/dji/f450/dji_f450_uvdar_mount.dae" -%}
+  {%- set qorvo_dw1000_mount_file = "model://mrs_robots_description/meshes/x500v2/qorvo_dw1000_mount.stl" -%}
   {# <!--}--> #}
 
   {# Scales {--> #}
@@ -542,6 +543,24 @@
       <!--}-->
       {% endif %}
 
+
+      {% if enable_qorvo_dw1000 %}
+      <!-- Qorvo DW1000 mount {-->
+      {{components.visual_mesh_macro(
+        name = "qorvo_dw1000_mount",
+        mesh_file = qorvo_dw1000_mount_file,
+        mesh_scale = mesh_scale_milimeters,
+        color = "DarkGrey",
+        x = 0.0,
+        y = -0.01,
+        z = -0.078,
+        roll = 0,
+        pitch = 0,
+        yaw = 0)
+      }}
+      <!--}-->
+      {% endif %}
+
       <!--}-->
 
     </link>
@@ -1102,5 +1121,24 @@
     {% endif %}
     {# <!--}--> #}
 
+    {# Qorvo UWB {--> #}
+    {% if enable_qorvo_dw1000 %}
+    <!-- Qorvo DW1000 {-->
+        {{ components.uwb_range_macro(
+            parent_link = root,
+            uwb_id = uwb_id,
+            uav_name = namespace,
+            x = 0.0,
+            y = 0.08,
+            z = -0.10,
+            roll = 0.0,
+            pitch = 0.0,
+            yaw = 0.0)
+    }}
+    <!--}-->
+    {% endif %}
+    {# <!--}--> #}
+
+
   </model>
 </sdf>