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/m690.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/m690.yaml new file mode 100644 index 0000000..42839f9 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/m690.yaml @@ -0,0 +1,21 @@ +uav_mass: 3.5 + +motor_params: + n_motors: 4 + a: 0.1917 + b: -0.15 + +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/spawner_params.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml index 3206f51..0f9a4d5 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,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]] +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]] @@ -49,11 +50,11 @@ 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]] +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]] 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 9eddbfb..3863472 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( @@ -2221,7 +2235,6 @@ limitations under the License. 0 - @@ -2287,6 +2300,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/m690.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/m690.sdf.jinja new file mode 100644 index 0000000..53fe6d2 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/m690.sdf.jinja @@ -0,0 +1,696 @@ + + + + {%- 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 = 27.2 -%} {# 86% -> 271.19W [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/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():