Spawner API

.gitignore

@@ -2,3 +2,4 @@
 *.swp
 
 .gitman
+__pycache__

ros_packages/mrs_uav_gazebo_simulation/CMakeLists.txt

@@ -3,12 +3,16 @@ project(mrs_uav_gazebo_simulation)
 
 set(CATKIN_DEPENDENCIES
   cmake_modules
+  gazebo_ros
+  mavlink_sitl_gazebo
   mavros_msgs
   message_runtime
+  mrs_gazebo_common_resources
   mrs_msgs
+  mrs_uav_px4_api
+  px4
   rospy
   std_msgs
-  mrs_uav_testing
   )
 
 find_package(catkin REQUIRED COMPONENTS
@@ -18,12 +22,6 @@ find_package(catkin REQUIRED COMPONENTS
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
-if(COVERAGE)
-  message(WARNING "building with --coverage, the performance might be limited")
-  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} --coverage")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --coverage")
-endif()
-
 catkin_package(
   CATKIN_DEPENDS ${CATKIN_DEPENDENCIES}
   )
@@ -40,6 +38,9 @@ include_directories(
 
 if(CATKIN_ENABLE_TESTING)
 
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} --coverage")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --coverage")
+
   add_subdirectory(test)
 
 endif()

ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f330.yaml

@@ -27,6 +27,7 @@ model_params:
     # allocation motors -> torques
 
     # quadrotor X configuration
+    # hexarotor X configuration
     # [roll torque,      [            [RPM_1^2,
     #  pitch torque,  =    Alloc    *  RPM_2^2,
     #  yaw torque,         Matrix       ...

ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f450.yaml

@@ -27,6 +27,7 @@ model_params:
     # allocation motors -> torques
 
     # quadrotor X configuration
+    # hexarotor X configuration
     # [roll torque,      [            [RPM_1^2,
     #  pitch torque,  =    Alloc    *  RPM_2^2,
     #  yaw torque,         Matrix       ...

ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/f550.yaml

@@ -26,6 +26,7 @@ model_params:
 
     # allocation motors -> torques
 
+    # quadrotor X configuration
     # hexarotor X configuration
     # [roll torque,      [            [RPM_1^2,
     #  pitch torque,  =    Alloc    *  RPM_2^2,

ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/m690.yaml

@@ -27,6 +27,7 @@ model_params:
     # allocation motors -> torques
 
     # quadrotor X configuration
+    # hexarotor X configuration
     # [roll torque,      [            [RPM_1^2,
     #  pitch torque,  =    Alloc    *  RPM_2^2,
     #  yaw torque,         Matrix       ...

ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/t650.yaml

@@ -27,6 +27,7 @@ model_params:
     # allocation motors -> torques
 
     # quadrotor X configuration
+    # hexarotor X configuration
     # [roll torque,      [            [RPM_1^2,
     #  pitch torque,  =    Alloc    *  RPM_2^2,
     #  yaw torque,         Matrix       ...

ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/x500.yaml

@@ -27,6 +27,7 @@ model_params:
     # allocation motors -> torques
 
     # quadrotor X configuration
+    # hexarotor X configuration
     # [roll torque,      [            [RPM_1^2,
     #  pitch torque,  =    Alloc    *  RPM_2^2,
     #  yaw torque,         Matrix       ...

ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml

@@ -1,62 +1,20 @@
-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, 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]]
-enable_mobius_camera_back_right: [False, 'Add mobius camera to the vehicle [1280x720 30hz], pointed to the back right', [f450, f550]]
-enable_realsense_front_pitched: [False, 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 30hz], pointed to the front pitched down by 10 up to 45 deg. Depends on the type of vehicle', [f450, f550]]
-enable_realsense_down: [False, 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 30hz], pointed down', [f450, f550, t650, x500]]
-enable_realsense_top: [False, 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 30hz], pointed forward placed on the aluminum frame', [f550, t650]]
-enable_realsense_front: [False, 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 30hz], pointed forward placed on the front holder between the legs', [f330, f450, f550, t650, x500, brus, naki]]
-enable_realsense_up: [False, 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 30hz], pointed upwards', [f450, f550, t650, x500]]
-use_realistic_realsense: [False, 'Enable a more realistic simulation of the Realsense D435 depth image (only takes effect if some Realsense is enabled)', [f330, f450, f550, t650, x500, brus, naki]]
-enable_whycon_box: [False, 'Add a Whycon box for relative localization', [f450, f550]]
-enable_bluefox_camera: [False, 'Add bluefox camera to the vehicle [752x480 50hz]', [f450, f550, t650, x500, eaglemk2]]
-enable_bluefox_camera_reverse: [False, 'Add bluefox camera to the vehicle [752x480 50hz], rotated by 180 deg', [f450, f550, t650, x500]]
-enable_basler_camera_down:  [False, 'Add basler dart camera pointing down [1920x1200]', [m690]]
-enable_fisheye_camera: [False, 'Add fisheye camera to the vehicle [752x480 180deg 45hz]', [f450]]
-enable_magnetic_gripper: [False, 'Add magnetic gripper', [f450, f550, t650]]
-enable_scanse_sweep: [False, 'Add Scanse Sweep laser scanner to the vehicle', [f450, f550, t650]]
-enable_rplidar: [False, 'Add the RPlidar A3 laser scanner to the vehicle', [f450, f550, t650, x500, naki]]
-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, 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, m690]]
-ouster_model: ['OS1-16', 'Choose the Ouster model. Options: OS0-32, OS0-64, OS0-128, OS1-16, OS1-32G1, OS1-32, OS1-64, OS1-128, OS2-32, OS2-64, OS2-128', [f450, f550, t650, x500, eaglemk2, naki, big_dofec, m690]]
-use_gpu_ray: [False, 'Laser ray casting will be handled by GPU shaders', [f330, f450, f550, t650, x500, eaglemk2, naki, big_dofec, m690]]
-horizontal_samples: [None, 'Override default number of horizontal samples for LiDARs', [f450, f550, t650, x500, eaglemk2, naki]]
-rotation_freq: [None, 'Override default update rate for LiDARs', [f450, f550, t650, x500, eaglemk2, naki]]
-enable_light: [False, 'Add light to the vehicle', [f450, f550, t650, x500, naki]]
-enable_servo_camera: [False, 'Add camera on virtual servo to the vehicle', [f450, f550, t650, naki, 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]]
-fire_challenge: [False, 'Configuration for the MBZIRC fire challenge (T650 with bluefox down, realsense forward, rp lidar, 3x thermal camera, water gun)', [t650]]
-fire_challenge_blanket: [False, 'Configuration for the MBZIRC fire challenge (blanket dropping) (T650 with bluefox down, realsense forward, rp lidar, 2x thermal camera, maybe blanket dropper)', [t650]]
-gps_indoor_jamming: [False, 'Enable GPS jamming when drone is indoors (NOTE: UNUSED PARAMETER!)', [f450, f550, t650]]
-enable_uv_leds: [False, 'Add UV LEDs to the vehicle', [f450, f550, x500]]
-uvled_s: [None, 'Specify UV LED signal ID set as space-separated numbers', [f450, f550, x500]]
-uvled_s_l: [None, 'Specify UV LED signal ID left (for backwards compatibility)', [f450, f550, x500]]
-uvled_s_r: [None, 'Specify UV LED signal ID right (for backwards compatibility)', [f450, f550, x500]]
-enable_uv_leds_beacon: [False, 'Add UV LED beacon to the top of the vehicle', [f450, x500]]
-uvled_beacon_s: [None, 'The signal of blinking of the UV Beacon', [f450, x500]]
-enable_downward_uv_camera: [False, 'Add UV camera on the vehicle', [t650]]
-enable_dual_uv_cameras: [False, 'Add right and left UV cameras on the vehicle', [f450, f550, x500]]
-enable_back_uv_camera: [False, 'Add back UV camera on the vehicle', [f450, x500]]
-uvcam_calib_file: [None, 'Specify UV camera calibration different than default one', [f450, f550, t650, x500]]
-uvcam_occlusions: [False, 'Enable occlusions for UVDAR simulation (NOTE: THIS REQUIRES A BEEFY COMPUTER)', [f450, f550, t650, x500]]
-enable_qorvo_dw1000: [False, 'Add Qorvo DW1000 UWB transceiver', [x500]]
-pos_file: [None, 'Load positions and ids from .csv file with format: [id, x, y, z, heading] or .yaml file with format: [uav_name: \n id: (int) \n x: (float) \n y: (float) \n z: (float) \n heading: (float)]', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec, m690]]
-uwb_id: [0, 'Set id for uwb ranging', [x500]]
-pos: [None, 'Specify spawn position of the first vehicle in format [x, y, z, heading]. Additional vehicles will spawn in line with 2m x-offset', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec]]
-enable_vio: [False, 'Add a forward-looking fisheye camera and a high frequency IMU', [f330, f450]]
-enable_vio_down: [False, 'Add a forward-looking fisheye camera and a high frequency IMU', [f330, f450]]
-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, m690]]
-disable_motor_crash: [False, 'Disables motor failure after crash with the environment', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec, a300]]
+mavlink_config:
+  vehicle_base_port: 14000
+  mavlink_tcp_base_port: 4560
+  mavlink_udp_base_port: 14560
+  qgc_udp_port: 14550
+  sdk_udp_port: 14540
+  send_vision_estimation: false
+  send_odometry: true
+  enable_lockstep: true
+  use_tcp: true
+
+gazebo_models:
+  spacing: 5.0 # for randomized spawn poses
+  default_robot_name: 'uav'
+
+jinja_templates:
+  save_rendered_sdf: true
+  suffix: '.sdf.jinja'
+
+extra_resource_paths: []

ros_packages/mrs_uav_gazebo_simulation/launch/mrs_drone_spawner.launch

@@ -1,8 +1,14 @@
 <launch>
+
+  <arg name="spawner_config" default="" />
 
-  <node name="mrs_drone_spawner" pkg="mrs_uav_gazebo_simulation" type="mrs_drone_spawner.py" output="screen" args="no_help verbose">
+  <node name="mrs_drone_spawner" pkg="mrs_uav_gazebo_simulation" type="mrs_drone_spawner.py" output="screen" args="verbose">
 
+    <rosparam file="$(find mrs_uav_gazebo_simulation)/config/spawner_params.yaml" />
+    <rosparam if="$(eval not arg('spawner_config') == '')" file="$(arg spawner_config)" />
+
     <remap from="~gazebo_model_states" to="/gazebo/model_states" />
+    <remap from="~gazebo_spawn_model" to="/gazebo/spawn_sdf_model" />
 
   </node>
 

ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch

@@ -1,34 +1,25 @@
 <?xml version="1.0"?>
 <launch>
   <!-- MAVROS posix SITL environment launch script -->
-  <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
-  <!-- <arg name="ID" default="1"/> -->
   <arg name="ID"/>
-  <arg name="vehicle"/>
-  <arg name="x"/>
-  <arg name="y"/>
-  <arg name="z"/>
-  <arg name="heading"/>
-
-  <arg name="roll" default="0"/>
-  <arg name="pitch" default="0"/>
-  <arg name="est" default="ekf2"/>
+  <arg name="PX4_SIM_MODEL"/>
+  <arg name="PX4_ESTIMATOR" default="ekf2"/>
 
   <!-- PX4 configs -->
   <arg name="interactive" default="false"/>
   <!-- PX4 SITL -->
-  <arg unless="$(arg interactive)" name="px4_command_arg1" value="-d"/>
-  <arg     if="$(arg interactive)" name="px4_command_arg1" value=""/>
+  <arg unless="$(arg interactive)" name="px4_interactive_mode" value="-d"/>
+  <arg     if="$(arg interactive)" name="px4_interactive_mode" value=""/>
 
   <!-- UAV namespace -->
   <group ns="uav$(arg ID)">
 
     <!-- PX4 params -->
-    <env name="PX4_SIM_MODEL" value="$(arg vehicle)" />
-    <env name="PX4_ESTIMATOR" value="$(arg est)" />
+    <env name="PX4_SIM_MODEL" value="$(arg PX4_SIM_MODEL)" />
+    <env name="PX4_ESTIMATOR" value="$(arg PX4_ESTIMATOR)" />
 
     <!-- PX4 SITL -->
-    <node name="sitl_uav$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find mrs_uav_gazebo_simulation)/ROMFS/px4fmu_common -s etc/init.d-posix/rcS $(arg px4_command_arg1) -i $(arg ID) -w sitl_uav$(arg ID)"/>
+    <node name="sitl_uav$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find mrs_uav_gazebo_simulation)/ROMFS/px4fmu_common -s etc/init.d-posix/rcS $(arg px4_interactive_mode) -i $(arg ID) -w sitl_uav$(arg ID)"/>
 
   </group>
 </launch>

ros_packages/mrs_uav_gazebo_simulation/launch/simulation.launch

@@ -9,7 +9,7 @@
   <arg name="debug" default="false" />
   <arg name="physics" default="ode" />
   <arg name="verbose" default="true" />
-  <arg name="reset" default="true" />
+  <arg name="spawner_config" default="" />
 
   <arg name="LOGGER_DEBUG" default="$(optenv LOGGER_DEBUG false)" />
 
@@ -39,13 +39,10 @@
   <arg unless="$(arg debug)" name="script_type" value="gzserver" />
   <arg     if="$(arg debug)" name="script_type" value="debug" />
 
-  <!-- kill previous sessions of mavros, px4 and gazebo -->
-  <group if="$(arg reset)">
-    <node pkg="mrs_uav_gazebo_simulation" type="kill_previous_session.sh" name="kill_previous_session" output="screen" />
-  </group>
-
-  <!-- start spawner -->
-  <include file="$(find mrs_uav_gazebo_simulation)/launch/mrs_drone_spawner.launch" />
+  <!-- start drone spawner -->
+  <include file="$(find mrs_uav_gazebo_simulation)/launch/mrs_drone_spawner.launch">
+    <arg name="spawner_config" value="$(arg spawner_config)"/>
+  </include>
 
   <!-- start gazebo server-->
   <node name="gazebo" pkg="gazebo_ros" type="$(arg script_type)" respawn="false" output="screen"