diff --git a/.fig/m690_simulation.jpg b/.fig/m690_simulation.jpg new file mode 100644 index 0000000..4456ecd Binary files /dev/null and b/.fig/m690_simulation.jpg differ diff --git a/.github/workflows/ros_build_test.yml b/.github/workflows/ros_build_test.yml index 28768ce..2709510 100644 --- a/.github/workflows/ros_build_test.yml +++ b/.github/workflows/ros_build_test.yml @@ -8,8 +8,8 @@ on: paths-ignore: - '**/README.md' - pull_request: - branches: [ master ] + pull_request: + branches: [ master ] workflow_dispatch: diff --git a/README.md b/README.md index e40660a..f3ca0ed 100644 --- a/README.md +++ b/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 diff --git a/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/airframes/4001_m690 b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/airframes/4001_m690 new file mode 100644 index 0000000..4f8b01f --- /dev/null +++ b/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 +# + +. ${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 diff --git a/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f330.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f330.yaml index e3c7c86..d08b5eb 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f330.yaml +++ b/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f330.yaml @@ -6,9 +6,9 @@ motor_params: b: -0.17647 # these model parameters can be used when -# - 'attitude rate', and/or +# - 'control group', and/or # - 'actuator control' -# are done by the MRS system. +# are accepted by the HW API. model_params: arm_length: 0.165 # [m] diff --git a/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f450.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f450.yaml index 785577c..56178be 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f450.yaml +++ b/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f450.yaml @@ -6,9 +6,9 @@ motor_params: b: -0.17647 # these model parameters can be used when -# - 'attitude rate', and/or +# - 'control group', and/or # - 'actuator control' -# are done by the MRS system. +# are accepted by the HW API. model_params: arm_length: 0.225 # [m] diff --git a/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f550.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f550.yaml index 7cd7f0b..5a65d37 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f550.yaml +++ b/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f550.yaml @@ -6,9 +6,9 @@ motor_params: b: -0.17647 # these model parameters can be used when -# - 'attitude rate', and/or +# - 'control group', and/or # - 'actuator control' -# are done by the MRS system. +# are accepted by the HW API. model_params: arm_length: 0.27 # [m] diff --git a/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/m690.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/m690.yaml new file mode 100644 index 0000000..bc3a4fb --- /dev/null +++ b/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" diff --git a/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/t650.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/t650.yaml index 9918ae6..76f5029 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/t650.yaml +++ b/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/t650.yaml @@ -6,9 +6,9 @@ motor_params: b: -0.17647 # these model parameters can be used when -# - 'attitude rate', and/or +# - 'control group', and/or # - 'actuator control' -# are done by the MRS system. +# are accepted by the HW API. model_params: arm_length: 0.325 # [m] diff --git a/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/x500.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/x500.yaml index 965828c..bd23e81 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/x500.yaml +++ b/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/x500.yaml @@ -6,9 +6,9 @@ motor_params: b: -0.17647 # these model parameters can be used when -# - 'attitude rate', and/or +# - 'control group', and/or # - 'actuator control' -# are done by the MRS system. +# are accepted by the HW API. model_params: arm_length: 0.25 # [m] diff --git a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml index 902c80f..1cdca72 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml +++ b/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]] @@ -15,6 +15,7 @@ use_realistic_realsense: [False, 'Enable a more realistic simulation of the Real 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_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,13 +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]] +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, naki]] +rotation_freq: [None, 'Override default update rate for LiDARs', [f450, f550, t650, x500, 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]] @@ -47,13 +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]] diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/sensors/basler_dart.stl b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/sensors/basler_dart.stl new file mode 100644 index 0000000..ad15973 Binary files /dev/null and b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/sensors/basler_dart.stl differ diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/m690.blend b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/m690.blend new file mode 100644 index 0000000..ff0c022 Binary files /dev/null and b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/m690.blend differ diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_arm.stl b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_arm.stl new file mode 100644 index 0000000..4879ca3 Binary files /dev/null and b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_arm.stl differ diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_battery.stl b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_battery.stl new file mode 100644 index 0000000..0461cdd Binary files /dev/null and b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_battery.stl differ diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_central_body.stl b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_central_body.stl new file mode 100644 index 0000000..ff5e907 Binary files /dev/null and b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_central_body.stl differ diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_legs.stl b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_legs.stl new file mode 100644 index 0000000..35ed027 Binary files /dev/null and b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_legs.stl differ diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_motor_top_part.stl b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_motor_top_part.stl new file mode 100644 index 0000000..56701a9 Binary files /dev/null and b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_motor_top_part.stl differ diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_propeller.stl b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_propeller.stl new file mode 100644 index 0000000..92986a5 Binary files /dev/null and b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_propeller.stl differ diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja index 3524996..d4a8d1a 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja @@ -261,6 +261,20 @@ limitations under the License. {%- endmacro -%} + +{%- macro collision_cylinder_macro(name, collision_length, collision_radius, x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + + {{ collision_length }} + {{ collision_radius }} + + + +{%- endmacro -%} + + {%- macro leg_macro(name, mesh_file, mesh_scale, color, parent, x, y, z, roll, pitch, yaw, collision_height, collision_radius) -%} {{ visual_mesh_macro( @@ -1536,7 +1550,20 @@ limitations under the License. -{%- macro rplidar_macro(namespace, parent_link, x, y, z, roll, pitch, yaw) -%} +{%- macro rplidar_macro(namespace, parent_link, horizontal_samples, rotation_freq, x, y, z, roll, pitch, yaw) -%} + +{% if horizontal_samples == 'None' %} + {%- set samples = 1600 -%} +{% else %} + {%- set samples = horizontal_samples | int -%} +{% endif %} + +{% if rotation_freq == 'None' %} + {%- set update_rate = 20 -%} +{% else %} + {%- set update_rate = rotation_freq | int -%} +{% endif %} + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} {{ zero_inertial_macro() }} @@ -1544,8 +1571,7 @@ limitations under the License. 0 0 -0.029 0 0 {{ rad180 }} - model://mrs_robots_description/meshes/seMrsGazeboCommonResources_3DLidarGpuPlugiMrsGazeboCommonResources_3DLidarGpuPlugii - .stl + model://mrs_robots_description/meshes/sensors/rplidar.stl 0.001 0.001 0.001 @@ -1573,11 +1599,11 @@ limitations under the License. false - 20 + {{ update_rate }} - {{ 14400/20 }} + {{ samples }} 1 -3.1241390751 3.1241390751 @@ -1594,7 +1620,7 @@ limitations under the License. 0.01 - + rplidar/scan {{ namespace }}/rplidar {{ namespace }}/fcu @@ -1616,7 +1642,7 @@ limitations under the License. -{%- macro velodyne_macro(namespace, parent_link, sensor_name, rot_freq, lasers, max_range, noise, vfov_angle, enable_gpu_ray, x, y, z, roll, pitch, yaw) -%} +{%- macro velodyne_macro(namespace, parent_link, sensor_name, rotation_freq, lasers, max_range, noise, vfov_angle, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%} {# The real ouster is transforming lidar data from lidar_frame to sensor_frame directly for user. #} {# For simplicity, we are placing sensor_frame to the same place as the lidar_frame is. #} @@ -1648,6 +1674,18 @@ limitations under the License. {%- set sensor_type = "ray" -%} {% endif %} +{% if horizontal_samples == 'None' %} + {%- set samples = 3600 -%} +{% else %} + {%- set samples = horizontal_samples | int -%} +{% endif %} + +{% if rotation_freq == 'None' %} + {%- set update_rate = 20 -%} +{% else %} + {%- set update_rate = rotation_freq | int -%} +{% endif %} + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} {{ zero_inertial_macro() }} @@ -1700,11 +1738,11 @@ limitations under the License. {{ lidar_x }} {{ lidar_y }} {{ lidar_z }} {{ lidar_roll }} {{ lidar_pitch }} {{ lidar_yaw }} {# 0 0 0 0 0 0 #} false - {{ rot_freq }} + {{ update_rate }} - {{ 291200/(rot_freq*lasers) }} + {{ samples }} 1 {{ -rad180 }} {{ rad180 }} @@ -1758,7 +1796,7 @@ limitations under the License. -{%- macro generic_ouster_macro(namespace, parent_link, sensor_name, rot_freq, lasers, max_range, noise, vfov_angle, enable_gpu_ray, x, y, z, roll, pitch, yaw) -%} +{%- macro generic_ouster_macro(namespace, parent_link, sensor_name, rotation_freq, lasers, max_range, noise, vfov_angle, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%} {# The real ouster is transforming lidar data from lidar_frame to sensor_frame directly for user. #} {# For simplicity, we are placing sensor_frame to the same place as the lidar_frame is. #} @@ -1801,6 +1839,18 @@ limitations under the License. {%- set sensor_type = "ray" -%} {% endif %} +{% if horizontal_samples == 'None' %} + {%- set samples = 2048 -%} +{% else %} + {%- set samples = horizontal_samples | int -%} +{% endif %} + +{% if rotation_freq == 'None' %} + {%- set update_rate = 10 -%} +{% else %} + {%- set update_rate = rotation_freq | int -%} +{% endif %} + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} {{ zero_inertial_macro() }} @@ -1834,7 +1884,7 @@ limitations under the License. - + {# IMU #} {{ imu_x }} {{ imu_y }} {{ imu_z }} {{ imu_roll }} {{ imu_pitch }} {{ imu_yaw }} @@ -1865,17 +1915,17 @@ limitations under the License. - + {# LIDAR #} {{ lidar_x }} {{ lidar_y }} {{ lidar_z }} {{ lidar_roll }} {{ lidar_pitch }} {{ lidar_yaw }} {# 0 0 0 0 0 0 #} false - {{ rot_freq }} + {{ update_rate }} - {{ 20480/rot_freq }} + {{ samples }} 1 0 {{ 2*pi }} @@ -1938,7 +1988,7 @@ limitations under the License. {%- endmacro -%} -{%- macro ouster_macro(namespace, parent_link, sensor_name, ouster_model, rot_freq, noise, enable_gpu_ray, x, y, z, roll, pitch, yaw) -%} +{%- macro ouster_macro(namespace, parent_link, sensor_name, ouster_model, rotation_freq, noise, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%} {# default: OS1-16 Generation 1 - is specified in the config file #} @@ -2029,12 +2079,13 @@ limitations under the License. namespace = namespace, parent_link = parent_link, sensor_name = sensor_name, - rot_freq = rot_freq, + rotation_freq = rotation_freq, lasers = lasers, vfov_angle = vfov_angle, max_range = range, noise = noise, enable_gpu_ray = enable_gpu_ray, + horizontal_samples = horizontal_samples, x = x, y = y, z = z, @@ -2219,7 +2270,6 @@ limitations under the License. 0 - @@ -2285,6 +2335,41 @@ limitations under the License. {%- endmacro -%} + +{%- 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 fisheye_camera_macro(namespace, camera_name, topic_ns_prefix, parent_link, frame_rate, noise, x, y, z, roll, pitch, yaw) -%} {{ fisheye_macro( diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f450.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f450.sdf.jinja index e596537..cd5796f 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f450.sdf.jinja +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f450.sdf.jinja @@ -857,6 +857,8 @@ {{ components.rplidar_macro( namespace = namespace, parent_link = root, + horizontal_samples = horizontal_samples, + rotation_freq = rotation_freq, x = 0.0, y = 0.0, z = 0.1, @@ -876,12 +878,13 @@ namespace = namespace, parent_link = root, sensor_name = "velodyne", - rot_freq = 20, + rotation_freq = rotation_freq, lasers = 16, vfov_angle = 30, max_range = 100, noise = 0.01, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.066, @@ -901,9 +904,10 @@ parent_link = root, sensor_name = "os", ouster_model = ouster_model, - rot_freq = 10, + rotation_freq = rotation_freq, noise = 0.03, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.066, diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f550.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f550.sdf.jinja index abe26c5..3d6c0f5 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f550.sdf.jinja +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f550.sdf.jinja @@ -1006,6 +1006,8 @@ {{ components.rplidar_macro( namespace = namespace, parent_link = root, + horizontal_samples = horizontal_samples, + rotation_freq = rotation_freq, x = 0.0, y = 0.0, z = 0.1, @@ -1025,12 +1027,13 @@ namespace = namespace, parent_link = root, sensor_name = "velodyne", - rot_freq = 20, + rotation_freq = rotation_freq, lasers = 16, vfov_angle = 30, max_range = 100, noise = 0.01, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.066, @@ -1050,9 +1053,10 @@ parent_link = root, sensor_name = "os", ouster_model = ouster_model, - rot_freq = 10, + rotation_freq = rotation_freq, noise = 0.03, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.066, diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/m690.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/m690.sdf.jinja new file mode 100644 index 0000000..dfae09b --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/m690.sdf.jinja @@ -0,0 +1,695 @@ + + + + {%- import "mrs_robots_description/sdf/component_snippets.sdf.jinja" as components -%} + + {# ================================================================== #} + {# || parameters definition || #} + {# ================================================================== #} + + {# Robot parameters and arguments {--> #} + {%- set mass = 4.65 -%} {# [kg] #} + {%- set body_radius = 0.1 -%} {# [m] #} + {%- set body_height = 0.1 -%} {# [m] #} + {%- set mass_prop = 0.005 -%} {# [kg] #} + {%- set prop_radius = 0.2286 -%} {# [m] #} + + {%- set motor_mesh_z_offset = -0.012 -%} {# [m] #} + {%- set rotor_x_offset = 0.245 -%} {# [m] #} + {%- set rotor_y_offset = 0.26 -%} {# [m] #} + {%- set rotor_z_offset = 0.052 -%} {# [m] #} + + {%- set use_battery_mount = true -%} {# [bool] #} + {%- set root = "base_link" -%} + + {%- set enable_motor_crash = true -%} + {% if disable_motor_crash %} + {%- set enable_motor_crash = false -%} + {% endif %} + + {# #} + + {# Motor constants {--> #} + {%- set rotor_velocity_slowdown_sim = 0.0159236 -%} + {%- set motor_constant = 29.371 -%} {# [kg.m/s^2] #} + {%- set moment_constant = 0.016 -%} {# [m] #} + {%- set time_constant_up = 1.0 / 80.0 -%} {# [s] #} + {%- set time_constant_down = 1.0 / 40.0 -%} {# [s] #} + {%- set max_rot_velocity = 1 -%} {# [rad/s] #} + {%- set rotor_drag_coefficient = 0.1 -%} {# orig 8.06428e-04 #} + {%- set rolling_moment_coefficient = "1.0e-6" -%} + {# #} + + {# Inertia constants {--> #} + {%- set inertia_body_radius = 0.25 -%} {# [m] #} + {%- set inertia_body_height = 0.05 -%} {# [m] #} + {# #} + + {# Meshes {--> #} + + {# Frame parts {--> #} + {%- set central_body_mesh_file = "model://mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_central_body.stl" -%} + {%- set arm_mesh_file = "model://mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_arm.stl" -%} + {%- set legs_mesh_file = "model://mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_legs.stl" -%} + {# #} + + {# Motors and props {--> #} + {%- set motor_top_mesh_file = "model://mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_motor_top_part.stl" -%} + {%- set prop_mesh_file = "model://mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_propeller.stl" -%} + {# #} + + {# Sensors and computers {--> #} + {# #} + + {# Mounts {--> #} + {%- set battery_mesh_file = "model://mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_battery.stl" -%} + {# #} + + {# Scales {--> #} + {%- set mesh_scale = "1 1 1" -%} + {%- set mirrored_mesh_scale = "1 -1 1" -%} + {%- set mesh_scale_prop_ccw = "1 1 1" -%} + {%- set mesh_scale_prop_cw = "-1 1 1" -%} + {%- set mesh_scale_milimeters = "0.001 0.001 0.001" -%} + {# #} + + {# #} + + {# Inertias {--> #} + {%- set body_ixx = mass * (3 * inertia_body_radius * inertia_body_radius + inertia_body_height * inertia_body_height) / 12 -%} + {%- set body_ixy = 0 -%} + {%- set body_ixz = 0 -%} + {%- set body_iyy = mass * (3 * inertia_body_radius * inertia_body_radius + inertia_body_height * inertia_body_height) / 12 -%} + {%- set body_iyz = 0 -%} + {%- set body_izz = (mass * inertia_body_radius * inertia_body_radius) / 2 -%} + + {%- set prop_ixx = 0.0001 -%} + {%- set prop_ixy = 0 -%} + {%- set prop_ixz = 0 -%} + {%- set prop_iyy = 0.0001 -%} + {%- set prop_iyz = 0 -%} + {%- set prop_izz = 0.0001 -%} + {# #} + + + + + + + + + + + {{ components.multirotor_physics_macro( + mass = mass, + body_radius = body_radius, + body_height = body_height, + rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, + ixx = body_ixx, + ixy = body_ixy, + ixz = body_ixz, + iyy = body_iyy, + iyz = body_iyz, + izz = body_izz) + }} + + + + + + {{ components.visual_mesh_macro( + name = "central_board", + mesh_file = central_body_mesh_file, + mesh_scale = mesh_scale, + color = "Grey", + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + + + {{ components.visual_mesh_macro( + name = "arm_back_right", + mesh_file = arm_mesh_file, + mesh_scale = mesh_scale, + color = "Grey", + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + {{ components.visual_mesh_macro( + name = "arm_back_left", + mesh_file = arm_mesh_file, + mesh_scale = mirrored_mesh_scale, + color = "Grey", + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + {{ components.visual_mesh_macro( + name = "arm_front_left", + mesh_file = arm_mesh_file, + mesh_scale = mesh_scale, + color = "Grey", + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = components.rad180) + }} + + {{ components.visual_mesh_macro( + name = "arm_front_right", + mesh_file = arm_mesh_file, + mesh_scale = mirrored_mesh_scale, + color = "Grey", + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = components.rad180) + }} + + + + {{ components.visual_mesh_macro( + name = "legs", + mesh_file = legs_mesh_file, + mesh_scale = mesh_scale, + color = "Grey", + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + {{ components.collision_cylinder_macro( + name = "leg_right", + collision_length = 0.3, + collision_radius = 0.01, + x = 0, + y = 0.165, + z = -0.258, + roll = 0, + pitch = -components.rad90, + yaw = 0) + }} + + {{ components.collision_cylinder_macro( + name = "leg_left", + collision_length = 0.3, + collision_radius = 0.01, + x = 0, + y = -0.165, + z = -0.258, + roll = 0, + pitch = -components.rad90, + yaw = 0) + }} + + + + + {{ components.visual_mesh_macro( + name = "battery", + mesh_file = battery_mesh_file, + mesh_scale = mesh_scale, + color = "Grey", + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + + + + + + + {{ components.prop_macro_2_meshes( + direction = "ccw", + rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, + motor_constant = motor_constant, + moment_constant = moment_constant, + parent = root, + mass = mass_prop, + radius = prop_radius, + time_constant_up = time_constant_up, + time_constant_down = time_constant_down, + max_rot_velocity = max_rot_velocity, + motor_number = 0, + rotor_drag_coefficient = rotor_drag_coefficient, + rolling_moment_coefficient = rolling_moment_coefficient, + enable_motor_crash = enable_motor_crash, + color = "Grey", + mesh_file_1 = prop_mesh_file, + mesh_file_2 = motor_top_mesh_file, + meshes_z_offset = motor_mesh_z_offset, + mesh_scale = mesh_scale_prop_ccw, + x = rotor_x_offset, + y = -rotor_y_offset, + z = rotor_z_offset, + roll = 0, + pitch = 0, + yaw = 0, + ixx = prop_ixx, + ixy = prop_ixy, + ixz = prop_ixz, + iyy = prop_iyy, + iyz = prop_iyz, + izz = prop_izz) + }} + + {{ components.prop_macro_2_meshes( + direction = "ccw", + rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, + motor_constant = motor_constant, + moment_constant = moment_constant, + parent = root, + mass = mass_prop, + radius = prop_radius, + time_constant_up = time_constant_up, + time_constant_down = time_constant_down, + max_rot_velocity = max_rot_velocity, + motor_number = 1, + rotor_drag_coefficient = rotor_drag_coefficient, + rolling_moment_coefficient = rolling_moment_coefficient, + enable_motor_crash = enable_motor_crash, + color = "Grey", + mesh_file_1 = prop_mesh_file, + mesh_file_2 = motor_top_mesh_file, + meshes_z_offset = motor_mesh_z_offset, + mesh_scale = mesh_scale_prop_ccw, + x = -rotor_x_offset, + y = rotor_y_offset, + z = rotor_z_offset, + roll = 0, + pitch = 0, + yaw = 0, + ixx = prop_ixx, + ixy = prop_ixy, + ixz = prop_ixz, + iyy = prop_iyy, + iyz = prop_iyz, + izz = prop_izz) + }} + + {{ components.prop_macro_2_meshes( + direction = "cw", + rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, + motor_constant = motor_constant, + moment_constant = moment_constant, + parent = root, + mass = mass_prop, + radius = prop_radius, + time_constant_up = time_constant_up, + time_constant_down = time_constant_down, + max_rot_velocity = max_rot_velocity, + motor_number = 2, + rotor_drag_coefficient = rotor_drag_coefficient, + rolling_moment_coefficient = rolling_moment_coefficient, + enable_motor_crash = enable_motor_crash, + color = "Grey", + mesh_file_1 = prop_mesh_file, + mesh_file_2 = motor_top_mesh_file, + meshes_z_offset = motor_mesh_z_offset, + mesh_scale = mesh_scale_prop_cw, + x = rotor_x_offset, + y = rotor_y_offset, + z = rotor_z_offset, + roll = 0, + pitch = 0, + yaw = 0, + ixx = prop_ixx, + ixy = prop_ixy, + ixz = prop_ixz, + iyy = prop_iyy, + iyz = prop_iyz, + izz = prop_izz) + }} + + {{ components.prop_macro_2_meshes( + direction = "cw", + rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, + motor_constant = motor_constant, + moment_constant = moment_constant, + parent = root, + mass = mass_prop, + radius = prop_radius, + time_constant_up = time_constant_up, + time_constant_down = time_constant_down, + max_rot_velocity = max_rot_velocity, + motor_number = 3, + rotor_drag_coefficient = rotor_drag_coefficient, + rolling_moment_coefficient = rolling_moment_coefficient, + enable_motor_crash = enable_motor_crash, + color = "Grey", + mesh_file_1 = prop_mesh_file, + mesh_file_2 = motor_top_mesh_file, + meshes_z_offset = motor_mesh_z_offset, + mesh_scale = mesh_scale_prop_cw, + x = -rotor_x_offset, + y = -rotor_y_offset, + z = rotor_z_offset, + roll = 0, + pitch = 0, + yaw = 0, + ixx = prop_ixx, + ixy = prop_ixy, + ixz = prop_ixz, + iyy = prop_iyy, + iyz = prop_iyz, + izz = prop_izz) + }} + + + + + + + + {{ components.mavlink_interface_macro( + mavlink_addr = mavlink_addr, + mavlink_udp_port = mavlink_udp_port, + mavlink_tcp_port = mavlink_tcp_port, + serial_enabled = serial_enabled, + serial_device = serial_device, + baudrate = serial_baudrate, + qgc_addr = qgc_addr, + qgc_udp_port = qgc_udp_port, + sdk_addr = sdk_addr, + sdk_udp_port =sdk_udp_port, + hil_mode = hil_mode, + hil_state_level = hil_state_level, + send_vision_estimation = send_vision_estimation, + send_odometry = send_odometry, + enable_lockstep = use_lockstep, + use_tcp = use_tcp) + }} + + + + {{ components.gps_macro( + gps_name = "gps0", + parent_link = root, + update_rate = 10, + gps_noise = true, + gps_xy_random_walk = 2.0, + gps_z_random_walk = 4.0, + gps_xy_noise_density = "2.0e-4", + gps_z_noise_density = "4.0e-4", + gps_vxy_noise_density = 0.2, + gps_vz_noise_density = 0.4, + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + + + {{ components.magnetometer_plugin_macro( + pub_rate = 100, + noise_density = 0.0004, + random_walk = 0.0000064, + bias_correlation_time = 600, + mag_topic = "/mag") + }} + + + + {{ components.gps_groundtruth_plugin_macro( + home_latitude = 0, + home_longitude = 0, + home_altitude = 0) + }} + + + + {{ components.barometer_plugin_macro( + baro_topic = "/baro", + pub_rate = 50, + baro_drift_pa_per_sec = 0) + }} + + + + {{ components.imu_plugin_macro( + imu_name = "imu", + parent_link = root, + imu_topic = "/imu", + gyroscope_noise_density = 0.00018665, + gyroscope_random_walk = 0.000038785, + gyroscope_bias_correlation_time = 1000.0, + gyroscope_turn_on_bias_sigma = 0.0087, + accelerometer_noise_density = 0.00186, + accelerometer_random_walk = 0.006, + accelerometer_bias_correlation_time = 300.0, + accelerometer_turn_on_bias_sigma = 0.1960, + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + + + + + + {# Ground truth {--> #} + {% if enable_ground_truth %} + + {{ components.odometry_plugin_macro( + odometry_sensor_name = "ground_truth", + parent_link = root, + topic_name = "ground_truth", + noise = "0", + frame_name = "world", + frame_rate = "150", + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + {% endif %} + {# #} + + + + {# Garmin {--> #} + {% if enable_rangefinder %} + + {# {{ components.garmin_macro("lidar0", root, -0.077, 0, -0.069, 0, components.rad(90), 0) }} - uncomment when simulation will work with pixgarm #} + {{ components.external_garmin_macro( + namespace = namespace, + parent_link = root, + orientation = "", + x = -0.05, + y = 0.0, + z = -0.055, + roll = 0, + pitch = components.rad(90), + yaw = 0) + }} + + {% endif %} + {# #} + + + + {# Ouster {--> #} + {% if enable_ouster %} + + {{ components.ouster_macro( + namespace = namespace, + parent_link = root, + sensor_name = "os", + ouster_model = ouster_model, + rotation_freq = rotation_freq, + noise = 0.03, + enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, + x = 0.0, + y = 0.0, + z = 0.107, + roll = 0, + pitch = 0, + yaw = 0) + }} + + {% endif %} + {# #} + + + + {% if enable_realsense_down %} + + {{ components.realsense_macro( + namespace = namespace, + camera_name = "rgbd", + camera_suffix="_down", + parent_link = root, + enable_realistic_realsense = enable_realistic_realsense, + x = 0.09, + y = 0, + z = -0.04, + roll = 0, + pitch = components.rad90, + yaw = 0) + }} + + {% endif %} + + {% if enable_basler_camera_down %} + + {{ components.basler_camera_macro( + namespace = namespace, + camera_name = "basler_down", + parent_link = root, + frame_rate = 30.0, + hfov = 1.990252, + noise = 0.007, + x = 0, + y = 0.1, + z = -0.22, + roll = components.rad90, + pitch = components.rad90, + yaw = 0) + }} + + {% endif %} + 623.037145, 0.000000, 325.528122, 0.000000, 623.843810, 248.324351, 0.0, 0.0, 1.0 + + {# Servo camera {--> #} + {% if enable_servo_camera %} + + {{ components.servo_camera_macro( + parent = root, + parent_frame_name = namespace + "/fcu", + camera_frame_name = namespace + "/servo_camera_optical", + sensor_base_frame_name = namespace + "/servo_camera", + offset_pitch_link_x = 0.0, + offset_pitch_link_y = 0.0, + offset_pitch_link_z = 0.0, + offset_pitch_link_roll = 0.0, + offset_pitch_link_yaw = 0.0, + offset_pitch_link_pitch = 0.0, + offset_roll_link_x = 0.2, + offset_roll_link_y = 0.0, + offset_roll_link_z = -0.1, + offset_roll_link_roll = 0.0, + offset_roll_link_yaw = 0.0, + offset_roll_link_pitch = 0.0, + tilt_update_rate = 5.0, + min_pitch = -1.57, + max_pitch = 1.57, + min_roll = -0.6, + max_roll = 0.6, + max_pitch_rate = 2.0, + max_roll_rate = 2.0, + camera_update_rate = 30, + horizontal_fov = 1.92, + img_width = 1280, + img_height = 720, + compensate_tilt_roll = true, + compensate_tilt_pitch = true, + pitch_link_mesh_file = "", + roll_link_mesh_file = "", + mesh_scale = "") + }} + a + {% endif %} + {# #} + + {# Thermal cameras {--> #} + {% if enable_thermal_camera %} + + {{ components.thermal_camera_macro( + camera_name = "thermal_top", + camera_topic_name = "/" + namespace + "/thermal/top", + parent_frame_name = namespace + "/fcu", + camera_frame_name = "thermal/top_optical", + sensor_base_frame_name = namespace + "/thermal/top", + parent_link = root, + frame_rate = 14.0, + hfov = 0.575959, + image_width = 32, + image_height = 32, + x = 0.15, + y = 0.06, + z = -0.025, + roll = 0, + pitch = components.rad(-30), + yaw = 0) + }} + + {{ components.thermal_camera_macro( + camera_name = "thermal_middle", + camera_topic_name = "/" + namespace + "/thermal/middle", + parent_frame_name = namespace + "/fcu", + camera_frame_name = "thermal/middle_optical", + sensor_base_frame_name = namespace + "/thermal/middle", + parent_link = root, + frame_rate = 14.0, + hfov = 0.575959, + image_width = 32, + image_height = 32, + x = 0.16, + y = 0.06, + z = -0.055, + roll = 0, + pitch = 0, + yaw = 0) + }} + + {{ components.thermal_camera_macro( + camera_name = "thermal_bottom", + camera_topic_name = "/" + namespace + "/thermal/bottom", + parent_frame_name = namespace + "/fcu", + camera_frame_name = "thermal/bottom_optical", + sensor_base_frame_name = namespace + "/thermal/bottom", + parent_link = root, + frame_rate = 14.0, + hfov = 0.575959, + image_width = 32, + image_height = 32, + x = 0.15, + y = 0.06, + z = -0.085, + roll = 0, + pitch = components.rad(30), + yaw = 0) + }} + + {% endif %} + {# #} + + + diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/naki.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/naki.sdf.jinja index b82c923..d3be1fe 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/naki.sdf.jinja +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/naki.sdf.jinja @@ -782,6 +782,8 @@ {{ components.rplidar_macro( namespace = namespace, parent_link = root, + horizontal_samples = horizontal_samples, + rotation_freq = rotation_freq, x = 0.0, y = 0.0, z = 0.09, @@ -801,9 +803,10 @@ parent_link = root, sensor_name = "os", ouster_model = ouster_model, - rot_freq = 10, + rotation_freq = rotation_freq, noise = 0.03, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.0611, diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/t650.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/t650.sdf.jinja index ba35a1b..fe80036 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/t650.sdf.jinja +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/t650.sdf.jinja @@ -213,7 +213,7 @@ yaw = 0) }} - + {{ components.visual_mesh_macro( name = "pixhawk", @@ -392,7 +392,7 @@ }} {% endif %} - + {% if enable_rplidar or fire_challenge_blanket %} {{ components.visual_mesh_macro( @@ -409,7 +409,7 @@ }} {% endif %} - + {% if enable_realsense_front %} {{ components.visual_mesh_macro( @@ -426,7 +426,7 @@ }} {% endif %} - + {% if enable_ouster or enable_velodyne %} {{ components.visual_mesh_macro( @@ -443,7 +443,7 @@ }} {% endif %} - + @@ -760,6 +760,8 @@ {{ components.rplidar_macro( namespace = namespace, parent_link = root, + horizontal_samples = horizontal_samples, + rotation_freq = rotation_freq, x = 0.0, y = 0.0, z = 0.107, @@ -779,12 +781,13 @@ namespace = namespace, parent_link = root, sensor_name = "velodyne", - rot_freq = 20, + rotation_freq = rotation_freq, lasers = 16, vfov_angle = 30, max_range = 100, noise = 0.01, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.073, @@ -804,9 +807,10 @@ parent_link = root, sensor_name = "os", ouster_model = ouster_model, - rot_freq = 10, + rotation_freq = rotation_freq, noise = 0.03, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.073, @@ -817,7 +821,7 @@ {% endif %} {# #} - + @@ -1269,10 +1273,10 @@ yaw=-2.5665787) }} - + {% endif %} - + {% if fire_challenge_blanket %} @@ -1289,7 +1293,7 @@ yaw = 0) }} - + {{ components.rplidar_macro( namespace = namespace, @@ -1344,7 +1348,7 @@ yaw = 0) }} - + {% endif %} {# #} 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 index 2f1df56..d7d6943 100644 --- 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 @@ -851,6 +851,8 @@ {{ components.rplidar_macro( namespace = namespace, parent_link = root, + horizontal_samples = horizontal_samples, + rotation_freq = rotation_freq, x = 0.0, y = 0.0, z = 0.136, @@ -870,9 +872,10 @@ parent_link = root, sensor_name = "os", ouster_model = ouster_model, - rot_freq = 10, + rotation_freq = rotation_freq, noise = 0.03, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.107, diff --git a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py index c13654f..7d13172 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py +++ b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py @@ -26,7 +26,7 @@ MAVLINK_UDP_BASE_PORT = 14560 LAUNCH_BASE_PORT = 14900 DEFAULT_VEHICLE_TYPE = 't650' -VEHICLE_TYPES = ['a300', 'f450', 'f550', 't650', 'x500', 'eaglemk2', 'f330', 'brus', 'naki', 'big_dofec'] +VEHICLE_TYPES = ['a300', 'f450', 'f550', 't650', 'x500', 'eaglemk2', 'f330', 'brus', 'naki', 'big_dofec', 'm690'] SPAWNING_DELAY_SECONDS = 6 class MrsDroneSpawner(): diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone/config/custom_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone/config/custom_config.yaml index 249e548..9fce17b 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone/config/custom_config.yaml +++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone/config/custom_config.yaml @@ -6,7 +6,6 @@ mrs_uav_managers: state_estimators: [ "gps_garmin", "gps_baro", - "ground_truth", ] initial_state_estimator: "gps_baro" # will be used as the first state estimator diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone/config/world_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone/config/world_config.yaml index 6d89f64..9b55067 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone/config/world_config.yaml +++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone/config/world_config.yaml @@ -1,46 +1,34 @@ -# -## DEFINITION OF THE ARENA -# +world_origin: -world_origin_units: "LATLON" # {"UTM, "LATLON"} + units: "LATLON" # {"UTM, "LATLON"} -world_origin_x: 47.397743 -world_origin_y: 8.545594 + origin_x: 47.397743 + origin_y: 8.545594 safety_area: - use_safety_area: true - - frame_name: "world_origin" - - # convex polygon CCW - safety_area: [ - -50, -50, - 50, -50, - 50, 50, - -50, 50 - ] - - max_z: 15.0 - min_z: 0.5 - - polygon_obstacles: - # loaded as a vector of matrices - # each matrix has polygon vertices in columns - # [[M1], [M2]] - enabled: false - data: [8.6, 18.61, 20.35, 6.82, -3.14, 10.7, - -22.5, -22.71, 1.02, 17.68, 11.1, -4.1] - rows: 2 # each matrix has two rows - cols: [6] # nums of cols of each matrix - - point_obstacles: - # loaded as a vector of matrices - # x, y, radius - enabled: false - # [x1, y1, radisu1, height1, - # x2, y2, radius2, height2] - data: [-5.0, -5.0, 2, 2, - -10.0, -10.0, 4, 2] - rows: 1 # each matrix has a single row - cols: [4, 4] # nums of cols of each matrix + enabled: true + + horizontal: + + # the frame of reference in which the points are expressed + frame_name: "world_origin" + + # polygon + # + # x, y [m] for any frame_name except latlon_origin + # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" + points: [ + -50, -50, + 50, -50, + 50, 50, + -50, 50, + ] + + vertical: + + # the frame of reference in which the max&min z is expressed + frame_name: "world_origin" + + max_z: 15.0 + min_z: 0.5 diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/custom_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/custom_config.yaml new file mode 100644 index 0000000..249e548 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/custom_config.yaml @@ -0,0 +1,26 @@ +mrs_uav_managers: + + estimation_manager: + + # loaded state estimator plugins + state_estimators: [ + "gps_garmin", + "gps_baro", + "ground_truth", + ] + + initial_state_estimator: "gps_baro" # will be used as the first state estimator + agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) + + uav_manager: + + takeoff: + + during_takeoff: + controller: "MpcController" + tracker: "LandoffTracker" + + after_takeoff: + controller: "MpcController" + # controller: "Se3Controller" + tracker: "MpcTracker" diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/network_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/network_config.yaml new file mode 100644 index 0000000..08f370d --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/network_config.yaml @@ -0,0 +1,15 @@ +# 1. This list is used by NimbroNetwork for mutual communication of the UAVs +# The names of the robots have to match hostnames described in /etc/hosts. +# +# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. +# The names should match the true "UAV_NAMES" (the topic prefixes). +# +# network_config:=~/config/network_config.yaml +# +# to the core.launch and nimbro.launch. + +network: + + robot_names: [ + uav1, + ] diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/world_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/world_config.yaml new file mode 100644 index 0000000..9b55067 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/config/world_config.yaml @@ -0,0 +1,34 @@ +world_origin: + + units: "LATLON" # {"UTM, "LATLON"} + + origin_x: 47.397743 + origin_y: 8.545594 + +safety_area: + + enabled: true + + horizontal: + + # the frame of reference in which the points are expressed + frame_name: "world_origin" + + # polygon + # + # x, y [m] for any frame_name except latlon_origin + # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" + points: [ + -50, -50, + 50, -50, + 50, 50, + -50, 50, + ] + + vertical: + + # the frame of reference in which the max&min z is expressed + frame_name: "world_origin" + + max_z: 15.0 + min_z: 0.5 diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/kill.sh b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/kill.sh new file mode 100755 index 0000000..28d50b0 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/kill.sh @@ -0,0 +1,14 @@ +#!/bin/bash + +# Absolute path to this script. /home/user/bin/foo.sh +SCRIPT=$(readlink -f $0) +# Absolute path this script is in. /home/user/bin +SCRIPTPATH=`dirname $SCRIPT` +cd "$SCRIPTPATH" + +export TMUX_SESSION_NAME=simulation +export TMUX_SOCKET_NAME=mrs + +# just attach to the session +tmux -L $TMUX_SOCKET_NAME split-window -t $TMUX_SESSION_NAME +tmux -L $TMUX_SOCKET_NAME send-keys -t $TMUX_SESSION_NAME "sleep 1; tmux list-panes -s -F \"#{pane_pid} #{pane_current_command}\" | grep -v tmux | cut -d\" \" -f1 | while read in; do killp \$in; done; exit" ENTER diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/layout.json b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/layout.json new file mode 100644 index 0000000..3b10030 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/layout.json @@ -0,0 +1,105 @@ +[ + { + "border": "normal", + "floating": "auto_off", + "fullscreen_mode": 0, + "layout": "splith", + "percent": 0.5, + "type": "con", + "nodes": [ + { + "border": "normal", + "floating": "auto_off", + "layout": "splith", + "percent": 1, + "type": "con", + "nodes": [ + { + "border": "normal", + "floating": "auto_off", + "layout": "splitv", + "percent": 1, + "type": "con", + "nodes": [ + { + "border": "normal", + "floating": "auto_off", + "layout": "splith", + "percent": 0.5, + "type": "con", + "nodes": [ + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 460, + "width": 724, + "x": 0, + "y": 0 + }, + "name": "Gazebo", + "percent": 0.5, + "swallows": [ + { + "instance": "^gazebo$" + } + ], + "type": "con" + }, + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 460, + "width": 724, + "x": 0, + "y": 0 + }, + "name": "default_simulation.rviz* - RViz", + "percent": 0.5, + "swallows": [ + { + "instance": "^rviz$" + } + ], + "type": "con" + } + ] + }, + { + "border": "normal", + "floating": "auto_off", + "layout": "splith", + "percent": 0.5, + "type": "con", + "nodes": [ + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 460, + "width": 724, + "x": 0, + "y": 0 + }, + "name": "./start.sh", + "percent": 1, + "swallows": [ + { + "instance": "^urxvt$" + } + ], + "type": "con" + } + ] + } + ] + } + ] + } + ] + } +] diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/session.yml b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/session.yml new file mode 100644 index 0000000..68f2571 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/session.yml @@ -0,0 +1,58 @@ +# do not modify these +root: ./ +name: simulation +socket_name: mrs +attach: false +tmux_options: -f /etc/ctu-mrs/tmux.conf +# you can modify these +pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500 +startup_window: status +windows: + - roscore: + layout: tiled + panes: + - roscore + - gazebo: + layout: tiled + panes: + - waitForRos; roslaunch mrs_uav_gazebo_simulation simulation.launch world_name:=forest gui:=true + - waitForControl; gz camera -c gzclient_camera -f $UAV_NAME; history -s gz camera -c gzclient_camera -f $UAV_NAME + - status: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_status status.launch + - spawn: + layout: tiled + panes: + - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 $UAV_TYPE --enable-rangefinder --enable-ground-truth --enable-ouster --ouster-model OS0-128 --use-gpu-ray --horizontal-samples 128 --rotation-freq 10" + - hw_api: + layout: tiled + panes: + - waitForTime; roslaunch mrs_uav_px4_api api.launch + - core: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_core core.launch + platform_config:=`rospack find mrs_uav_gazebo_simulation`/config/mrs_uav_system/$UAV_TYPE.yaml + custom_config:=./config/custom_config.yaml + world_config:=./config/world_config.yaml + network_config:=./config/network_config.yaml + - takeoff: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_autostart automatic_start.launch + - 'waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard' + - goto: + layout: tiled + panes: + - 'history -s rosservice call /$UAV_NAME/control_manager/goto \"goal: \[0.0, 10.0, 1.5, 0.0\]\"' + - rviz: + layout: tiled + panes: + - waitForControl; roslaunch mrs_uav_core rviz.launch + - waitForControl; roslaunch mrs_rviz_plugins load_robot.launch + - waitForTime; waitForControl; roslaunch mrs_rviz_plugins rviz_interface.launch + - layout: + layout: tiled + panes: + - waitForControl; sleep 3; ~/.i3/layout_manager.sh ./layout.json diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/start.sh b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/start.sh new file mode 100755 index 0000000..5bb2275 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_3dlidar/start.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +# Absolute path to this script. /home/user/bin/foo.sh +SCRIPT=$(readlink -f $0) +# Absolute path this script is in. /home/user/bin +SCRIPTPATH=`dirname $SCRIPT` +cd "$SCRIPTPATH" + +export TMUX_SESSION_NAME=simulation +export TMUX_SOCKET_NAME=mrs + +# start tmuxinator +tmuxinator start -p ./session.yml + +# if we are not in tmux +if [ -z $TMUX ]; then + + # just attach to the session + tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME + +# if we are in tmux +else + + # switch to the newly-started session + tmux detach-client -E "tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME" + +fi