diff --git a/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/airframes/4001_a300 b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/airframes/4001_a300 new file mode 100644 index 0000000..5305429 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/airframes/4001_a300 @@ -0,0 +1,21 @@ +#!/bin/sh +# +# @name DJI F330 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 8000 +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/spawner_params.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml index d878d3f..10883ed 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_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]] +enable_ground_truth: [False, 'Enable topic with ground truth odometry', [f450, f550, t650, x500, f330, eaglemk2, brus, naki, big_dofec, a300]] 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]] @@ -53,5 +53,5 @@ enable_vio: [False, 'Add a forward-looking fisheye camera and a high frequency I 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]] -disable_motor_crash: [False, 'Disables motor failure after crash with the environment', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec]] +model_package: ["mrs_uav_gazebo_simulation", 'package name for the UAV models', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec, a300]] +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/sdf/component_snippets.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja index 8b883ee..a7225ba 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 @@ -439,7 +439,7 @@ limitations under the License. {{ enable_motor_crash }} - + /motor_speed/{{ motor_number }} @@ -539,7 +539,107 @@ limitations under the License. {{ enable_motor_crash }} - + + + /motor_speed/{{ motor_number }} + + + + {{ parent }} + prop_{{ motor_number }}_link + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + 1 + + +{%- endmacro -%} + + + +{%- macro prop_macro_2_meshes_mrs_motor_model(direction, rotor_velocity_slowdown_sim, motor_constant, moment_constant, parent, mass, radius, time_constant_up, time_constant_down, max_rot_velocity, motor_number, rotor_drag_coefficient, rolling_moment_coefficient, enable_motor_crash, color, mesh_file_1, mesh_file_2, meshes_z_offset, mesh_scale, x, y, z, roll, pitch, yaw, ixx, ixy, ixz, iyy, iyz, izz) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + {{ mass }} + + {{ ixx }} + {{ ixy }} + {{ ixz }} + {{ iyy }} + {{ iyz }} + {{ izz }} + + + + + + {{ mesh_file_1 }} + {{ mesh_scale }} + + + + + {{ 0 }} {{ 0 }} {{ meshes_z_offset }} {{ 0 }} {{ 0 }} {{ 0 }} + + + {{ mesh_file_2 }} + {{ mesh_scale }} + + + + + + + + 0 0 0 0 {{ rad90 }} 0 + + + {{ 2*radius }} + 0.01 + + + + + + + + + + + + + + + + prop_{{ motor_number }}_joint + prop_{{ motor_number }}_link + {{ direction }} + {{ time_constant_up }} + {{ time_constant_down }} + {{ max_rot_velocity }} + {{ motor_constant }} + {{ moment_constant }} + /gazebo/command/motor_speed + {{ motor_number }} + {{ rotor_drag_coefficient }} + {{ rolling_moment_coefficient }} + /motor_speed/{{ motor_number }} + {{ rotor_velocity_slowdown_sim }} + {{ enable_motor_crash }} + + + /motor_speed/{{ motor_number }} @@ -610,6 +710,22 @@ limitations under the License. {{ rotor_velocity_slowdown_sim }} {%- endmacro -%} + + + +{%- macro fluid_resistance_plugin_macro(verbose, model_mass, parent_link, update_rate, uav_body_resistance_x, uav_body_resistance_y, uav_body_resistance_z) -%} + + + {{ model_mass }} + /fluid_resistance + {{ parent_link }} + {{ update_rate }} + {{ uav_body_resistance_x }} + {{ uav_body_resistance_y }} + {{ uav_body_resistance_z }} + {{ verbose }} + +{%- endmacro -%} 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 14b63f5..566a2bb 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 = ['f450', 'f550', 't650', 'x500', 'eaglemk2', 'f330', 'brus', 'naki', 'big_dofec'] +VEHICLE_TYPES = ['a300', 'f450', 'f550', 't650', 'x500', 'eaglemk2', 'f330', 'brus', 'naki', 'big_dofec'] SPAWNING_DELAY_SECONDS = 6 class MrsDroneSpawner():