Merge branch 'master' of github.com:ctu-mrs/mrs_uav_gazebo_simulation

README.md

@@ -19,14 +19,15 @@ Metapackage for the MRS UAV Gazebo simulation pipeline.
 
 ## Unmanned Aerial Vehicles
 
-| Model        | Spawn argument | Simulation                    |
-|--------------|----------------|-------------------------------|
-| DJI f330     | `--f330`       | ![](.fig/f330_simulation.jpg) |
-| DJI f450     | `--f450`       | ![](.fig/f450_simulation.jpg) |
-| Holybro x500 | `--x500`       | ![](.fig/x500_simulation.jpg) |
-| DJI f550     | `--f550`       | ![](.fig/f550_simulation.jpg) |
-| Tarot t650   | `--t650`       | ![](.fig/t650_simulation.jpg) |
-| NAKI II      | `--naki`       | ![](.fig/naki_simulation.jpg) |
+| Model         | Spawn argument | Simulation                    |
+|---------------|----------------|-------------------------------|
+| DJI f330      | `--f330`       | ![](.fig/f330_simulation.jpg) |
+| DJI f450      | `--f450`       | ![](.fig/f450_simulation.jpg) |
+| Holybro x500  | `--x500`       | ![](.fig/x500_simulation.jpg) |
+| DJI f550      | `--f550`       | ![](.fig/f550_simulation.jpg) |
+| Tarot t650    | `--t650`       | ![](.fig/t650_simulation.jpg) |
+| T-Drones m690 | `--m690`       | ![](.fig/m690_simulation.jpg) |
+| NAKI II       | `--naki`       | ![](.fig/naki_simulation.jpg) |
 
 ## Starting the simulation
 

ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/airframes/4001_m690

@@ -0,0 +1,20 @@
+#!/bin/sh
+#
+# @name Holybro X500 SITL
+#
+# @type Quadcopter x
+#
+# @maintainer Petr Stibinger <[email protected]>
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+param set-default MAV_TYPE 2
+
+param set-default BAT1_N_CELLS 6
+param set-default BAT1_CAPACITY 22000
+param set-default BAT1_V_EMPTY 3.6
+param set-default BAT1_V_CHARGED 4.2
+param set-default BAT1_V_LOAD_DROP 0.4
+
+set MIXER quad_x

ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/m690.yaml

@@ -0,0 +1,63 @@
+uav_mass: 4.65
+
+motor_params:
+  n_motors: 4
+  a:  0.19174
+  b: -0.15
+
+# these model parameters can be used when
+# - 'control group', and/or
+# - 'actuator control'
+# are accepted by the HW API.
+model_params:
+
+  arm_length: 0.345 # [m]
+  body_height: 0.20 # [m]
+
+  propulsion:
+
+    # force [N] = force_constant * rpm^2
+    force_constant: 0.00000096809
+
+    # torque [Nm] = torque_constant * force [N]
+    torque_constant: 0.07
+
+    prop_radius: 0.2286 # [m]
+
+    # allocation motors -> torques
+
+    # quadrotor X configuration
+    # hexarotor X configuration
+    # [roll torque,      [            [RPM_1^2,
+    #  pitch torque,  =    Alloc    *  RPM_2^2,
+    #  yaw torque,         Matrix       ...
+    #  thrust force]              ]    RPM_n_motors^2]
+    allocation_matrix: [
+      -0.707, 0.707, 0.707,  -0.707, # *= force_constant*arm_length
+      -0.707, 0.707, -0.707, 0.707,  # *= force_constant*arm_length
+      -1,     -1,    1,      1,      # *= torque_constant*force_constant
+      1,      1,     1,      1,      # *= force_constant
+    ]
+
+    # The UAV's inertia is approximated as a cilinder using the parameters above
+    # Alternatively, you can provide inertia matrix directly using the following parameter:
+    # inertia_matrix: []
+
+    rpm:
+      min: 801.55 # [revolutions/minute]
+      max: 5337.0 # [revolutions/minute]
+
+mrs_uav_managers:
+
+  constraint_manager:
+
+    default_constraints:
+      gps_garmin: "medium"
+      gps_baro: "medium"
+      rtk: "medium"
+      aloam: "slow"
+      hector_garmin: "slow"
+      vio: "slow"
+      optflow: "slow"
+      other: "slow"
+      ground_truth: "medium"

ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml

@@ -1,7 +1,7 @@
-enable_rangefinder: [False, 'Add a laser rangefinder (Garmin) pointing down', [f450, f550, t650, x500, f330, eaglemk2, brus, naki, big_dofec]]
+enable_rangefinder: [False, 'Add a laser rangefinder (Garmin) pointing down', [f450, f550, t650, x500, f330, eaglemk2, brus, naki, big_dofec, m690]]
 enable_rangefinder_up: [False, 'Add a laser rangefinder (Garmin) pointing up', [f450, f550, t650, naki]]
 enable_teraranger: [False, 'Add a laser rangefinder (Teraranger) to the vehicle', [f450, f550, t650]]
-enable_ground_truth: [False, 'Enable topic with ground truth odometry', [f450, f550, t650, x500, f330, eaglemk2, brus, naki, big_dofec, a300]]
+enable_ground_truth: [False, 'Enable topic with ground truth odometry', [f450, f550, t650, x500, f330, eaglemk2, brus, naki, big_dofec, a300, m690]]
 enable_mobius_camera_front: [False, 'Add mobius camera to the vehicle [1280x720 30hz], pointed to the front', [f450, f550, t650]]
 enable_mobius_camera_down: [False, 'Add mobius camera to the vehicle [1280x720 30hz], pointed to the ground', [f450, f550]]
 enable_mobius_camera_back_left: [False, 'Add mobius camera to the vehicle [1280x720 30hz], pointed to the back left', [f450, f550]]
@@ -13,8 +13,9 @@ enable_realsense_front: [False, 'Add Intel Realsense D435 depth camera to the ve
 enable_realsense_up: [False, 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 30hz], pointed upwards', [f450, f550, t650, x500]]
 use_realistic_realsense: [False, 'Enable a more realistic simulation of the Realsense D435 depth image (only takes effect if some Realsense is enabled)', [f330, f450, f550, t650, x500, brus, naki]]
 enable_whycon_box: [False, 'Add a Whycon box for relative localization', [f450, f550]]
-enable_bluefox_camera: [False, 'Add bluefox camera to the vehicle [752x480 50hz]', [f450, f550, t650, x500]]
+enable_bluefox_camera: [False, 'Add bluefox camera to the vehicle [752x480 50hz]', [f450, f550, t650, x500, eaglemk2]]
 enable_bluefox_camera_reverse: [False, 'Add bluefox camera to the vehicle [752x480 50hz], rotated by 180 deg', [f450, f550, t650, x500]]
+enable_basler_camera_down:  [False, 'Add basler dart camera pointing down [1920x1200]', [m690]]
 enable_fisheye_camera: [False, 'Add fisheye camera to the vehicle [752x480 180deg 45hz]', [f450]]
 enable_magnetic_gripper: [False, 'Add magnetic gripper', [f450, f550, t650]]
 enable_scanse_sweep: [False, 'Add Scanse Sweep laser scanner to the vehicle', [f450, f550, t650]]
@@ -23,15 +24,15 @@ enable_pendulum: [False, 'Add pendulum to the vehicle', [f450, f550, t650]]
 enable_teraranger_evo_tower: [False, 'Add the Teraranger Evo Tower laser scanner to the vehicle', [f450, f550, t650]]
 enable_timepix: [False, 'Add Timepix radiation detector to the vehicle', [f450, f550, t650]]
 enable_timepix3: [False, 'Add Timepix radiation detector to the vehicle', [f450, t650, x500]]
-enable_thermal_camera: [False, 'Add thermal camera to the vehicle', [t650]]
+enable_thermal_camera: [False, 'Add thermal camera to the vehicle', [t650, m690]]
 enable_velodyne: [False, 'Add Velodyne PUCK Lite laser scanner to the vehicle', [f450, f550, t650]]
-enable_ouster: [False, 'Add Ouster laser scanner to the vehicle', [f450, f550, t650, x500, eaglemk2, naki, big_dofec]]
-ouster_model: ['OS1-16', 'Choose the Ouster model. Options: OS0-32, OS0-64, OS0-128, OS1-16, OS1-32G1, OS1-32, OS1-64, OS1-128, OS2-32, OS2-64, OS2-128', [f450, f550, t650, x500, eaglemk2, naki, big_dofec]]
-use_gpu_ray: [False, 'Laser ray casting will be handled by GPU shaders', [f330, f450, f550, t650, x500, eaglemk2, naki, big_dofec]]
-horizontal_samples: [None, 'Override default number of horizontal samples for LiDARs', [f450, f550, t650, x500, naki]]
-rotation_freq: [None, 'Override default update rate for LiDARs', [f450, f550, t650, x500, naki]]
+enable_ouster: [False, 'Add Ouster laser scanner to the vehicle', [f450, f550, t650, x500, eaglemk2, naki, big_dofec, m690]]
+ouster_model: ['OS1-16', 'Choose the Ouster model. Options: OS0-32, OS0-64, OS0-128, OS1-16, OS1-32G1, OS1-32, OS1-64, OS1-128, OS2-32, OS2-64, OS2-128', [f450, f550, t650, x500, eaglemk2, naki, big_dofec, m690]]
+use_gpu_ray: [False, 'Laser ray casting will be handled by GPU shaders', [f330, f450, f550, t650, x500, eaglemk2, naki, big_dofec, m690]]
+horizontal_samples: [None, 'Override default number of horizontal samples for LiDARs', [f450, f550, t650, x500, eaglemk2, naki]]
+rotation_freq: [None, 'Override default update rate for LiDARs', [f450, f550, t650, x500, eaglemk2, naki]]
 enable_light: [False, 'Add light to the vehicle', [f450, f550, t650, x500, naki]]
-enable_servo_camera: [False, 'Add camera on virtual servo to the vehicle', [f450, f550, t650, naki]]
+enable_servo_camera: [False, 'Add camera on virtual servo to the vehicle', [f450, f550, t650, naki, m690]]
 enable_water_gun: [False, 'Add water gun for fire fighting', [t650]]
 enable_parachute: [False, 'Add parachute for emergency flight termination', [t650]]
 wall_challenge: [False, 'Configuration for the MBZIRC wall challenge (T650 with 2x bluefox down, realsense down pitched, rangefinder down)', [t650]]
@@ -49,11 +50,13 @@ 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]]
-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]]
+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]]
 enable_omni_ultrasounds: [False, 'Add omnidirectional ultrasonic sensors (up, down, 4 in horizontal plane)', [naki]]
 enable_safety_led: [False, 'Add a safety LED', [naki]]
-model_package: ["mrs_uav_gazebo_simulation", 'package name for the UAV models', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec, a300]]
+model_package: ["mrs_uav_gazebo_simulation", 'package name for the UAV models', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec, a300, m690]]
 disable_motor_crash: [False, 'Disables motor failure after crash with the environment', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec, a300]]

ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja

@@ -261,6 +261,20 @@ limitations under the License.
 {%- endmacro -%}
 <!--}-->
 
+<!-- Collision cylinder macro {-->
+{%- macro collision_cylinder_macro(name, collision_length, collision_radius, x, y, z, roll, pitch, yaw) -%}
+<collision name="{{ name }}_collision">
+  <pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
+  <geometry>
+    <cylinder>
+      <length>{{ collision_length }}</length>
+      <radius>{{ collision_radius }}</radius>
+    </cylinder>
+  </geometry>
+</collision>
+{%- endmacro -%}
+<!--}-->
+
 <!-- Leg macro {-->
 {%- macro leg_macro(name, mesh_file, mesh_scale, color, parent, x, y, z, roll, pitch, yaw, collision_height, collision_radius) -%}
 {{ visual_mesh_macro(
@@ -830,29 +844,27 @@ limitations under the License.
 
 <!-- Macro to add the gps_plugin {-->
 {%- macro gps_macro(gps_name, parent_link, update_rate, gps_noise, gps_xy_random_walk, gps_z_random_walk, gps_xy_noise_density, gps_z_noise_density, gps_vxy_noise_density, gps_vz_noise_density, x, y, z, roll, pitch, yaw) -%}
-<model name="{{ gps_name }}">
-  <link name="link">
-    <pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
-    {{ zero_inertial_macro() }}
-    <sensor name="gps_sensor" type="gps">
-      <plugin name="gps_plugin" filename="libgazebo_gps_plugin.so">
-        <robotNamespace/>
-        <update_rate>{{ update_rate }}</update_rate>
-        <gpsNoise>{{ gps_noise }}</gpsNoise>
-        <gpsXYRandomWalk>{{ gps_xy_random_walk }}</gpsXYRandomWalk>
-        <gpsZRandomWalk>{{ gps_z_random_walk }}</gpsZRandomWalk>
-        <gpsXYNoiseDensity>{{ gps_xy_noise_density }}</gpsXYNoiseDensity>
-        <gpsZNoiseDensity>{{ gps_z_noise_density }}</gpsZNoiseDensity>
-        <gpsVXYNoiseDensity>{{ gps_vxy_noise_density }}</gpsVXYNoiseDensity>
-        <gpsVZNoiseDensity>{{ gps_vz_noise_density }}</gpsVZNoiseDensity>
-        <topic>{{ gps_name }}</topic>
-      </plugin>
-    </sensor>
-  </link>
-</model>
+<link name="{{ gps_name }}_link">
+  <pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
+  {{ zero_inertial_macro() }}
+  <sensor name="gps_sensor" type="gps">
+    <plugin name="gps_plugin" filename="libgazebo_gps_plugin.so">
+      <robotNamespace/>
+      <update_rate>{{ update_rate }}</update_rate>
+      <gpsNoise>{{ gps_noise }}</gpsNoise>
+      <gpsXYRandomWalk>{{ gps_xy_random_walk }}</gpsXYRandomWalk>
+      <gpsZRandomWalk>{{ gps_z_random_walk }}</gpsZRandomWalk>
+      <gpsXYNoiseDensity>{{ gps_xy_noise_density }}</gpsXYNoiseDensity>
+      <gpsZNoiseDensity>{{ gps_z_noise_density }}</gpsZNoiseDensity>
+      <gpsVXYNoiseDensity>{{ gps_vxy_noise_density }}</gpsVXYNoiseDensity>
+      <gpsVZNoiseDensity>{{ gps_vz_noise_density }}</gpsVZNoiseDensity>
+      <topic>{{ gps_name }}</topic>
+    </plugin>
+  </sensor>
+</link>
 
 <joint name='{{ gps_name }}_joint' type='fixed'>
-  <child>{{ gps_name }}::link</child>
+  <child>{{ gps_name }}_link</child>
   <parent>{{ parent_link }}</parent>
 </joint>
 
@@ -1224,7 +1236,7 @@ limitations under the License.
 
 <!-- Macro to add a garmin (connected to the pixhawk) {-->
 {%- macro garmin_macro(sensor_name, parent_link, x, y, z, roll, pitch, yaw) -%}
-<link name="{{ sensor_name }}::link">
+<link name="{{ sensor_name }}_link">
   <pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
   {{ zero_inertial_macro() }}
   <visual name="visual">
@@ -1276,7 +1288,7 @@ limitations under the License.
 
 <joint name="{{ sensor_name }}_joint" type="fixed">
   <parent>{{ parent_link }}</parent>
-  <child>{{ sensor_name }}::link</child>
+  <child>{{ sensor_name }}_link</child>
 </joint>
 {%- endmacro -%}
 <!--}-->
@@ -1420,6 +1432,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{-->
@@ -2221,7 +2268,6 @@ limitations under the License.
     <visualize>0</visualize>
   </sensor>
   <!--}-->
-  <!--}-->
 
 </link>
 
@@ -2287,6 +2333,41 @@ limitations under the License.
 {%- endmacro -%}
 <!--}-->
 
+<!-- Macro to add a Basler Dart camera {-->
+{%- macro basler_camera_macro(namespace, camera_name, parent_link, frame_rate, hfov, noise, x, y, z, roll, pitch, yaw) -%}
+{{ camera_macro(
+  parent_link = parent_link,
+  camera_name = camera_name,
+  parent_frame_name = namespace + "/fcu",
+  camera_frame_name = namespace + "/" + camera_name + "_optical",
+  sensor_base_frame_name = namespace  + "/" + camera_name,
+  frame_rate = frame_rate,
+  horizontal_fov = hfov,
+  image_width = 1920,
+  image_height = 1200,
+  min_distance = 0.02,
+  max_distance = 80,
+  noise_mean = 0.0,
+  noise_stddev = noise,
+  mesh_file = "model://mrs_robots_description/meshes/sensors/basler_dart.stl",
+  mesh_scale = "0.001 0.001 0.001",
+  color = "DarkGrey",
+  visual_x = 0,
+  visual_y = 0,
+  visual_z = 0,
+  visual_roll = 0,
+  visual_pitch = 0,
+  visual_yaw = 0,
+  x = x,
+  y = y,
+  z = z,
+  roll = roll,
+  pitch = pitch,
+  yaw = yaw)
+}}
+{%- endmacro -%}
+<!--}-->
+
 <!-- Macro to add a Fisheye camera {-->
 {%- macro fisheye_camera_macro(namespace, camera_name, topic_ns_prefix, parent_link, frame_rate, noise, x, y, z, roll, pitch, yaw) -%}
 {{ fisheye_macro(