diff --git a/.gitignore b/.gitignore index 1f35b0a..29d079b 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ *.swp .gitman +__pycache__ diff --git a/ros_packages/mrs_uav_gazebo_simulation/CMakeLists.txt b/ros_packages/mrs_uav_gazebo_simulation/CMakeLists.txt index 6e04e3c..a745ad2 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/CMakeLists.txt +++ b/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() 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 efe8e57..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 @@ -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 ... 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 f5c19be..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 @@ -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 ... 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 8d093c1..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 @@ -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, 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 index bce50a6..bc3a4fb 100644 --- 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 @@ -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 ... diff --git a/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/naki.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/naki.yaml deleted file mode 100644 index 548ba28..0000000 --- a/ros_packages/mrs_uav_gazebo_simulation/config/mrs_uav_system/naki.yaml +++ /dev/null @@ -1,62 +0,0 @@ -uav_mass: 7.5 - -motor_params: - n_motors: 8 - a: 0.24300 - b: -0.17647 - -# these model parameters can be used when -# - 'control group', and/or -# - 'actuator control' -# are accepted by the HW API. -model_params: - - arm_length: 0.20 # [m] - body_height: 0.2 # [m] - - propulsion: - - # force [N] = force_constant * rpm^2 - force_constant: 0.00000057658 - - # torque [Nm] = torque_constant * force [N] - torque_constant: 0.07 - - prop_radius: 0.13 # [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, 0.707, -0.707, -0.707, 0.707, # *= force_constant*arm_length - -0.707, 0.707, -0.707, 0.707, 0.707, -0.707, 0.707, -0.707, # *= force_constant*arm_length - -1, -1, 1, 1, 1, 1, -1, -1, # *= moment_constant*force_constant - 1, 1, 1, 1, 1, 1, 1, 1, # *= force_constant - ] - - rpm: - min: 956 # [revolutions/minute] - max: 6376 # [revolutions/minute] - -mrs_uav_managers: - - constraint_manager: - - # list of allowed constraints per odometry mode - allowed_constraints: - gps_garmin: ["slow", "medium"] - gps_baro: ["slow", "medium"] - rtk: ["slow", "medium"] - rtk_garmin: ["slow", "medium"] - aloam: ["slow"] - hector_garmin: ["slow"] - vio: ["slow"] - optflow: ["slow"] - other: ["slow"] - ground_truth: ["slow", "medium"] - passthrough: ["slow", "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 c4b1015..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 @@ -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 ... 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 da05623..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 @@ -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 ... 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 6696dbd..d7c9c18 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml +++ b/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: [] diff --git a/ros_packages/mrs_uav_gazebo_simulation/launch/mrs_drone_spawner.launch b/ros_packages/mrs_uav_gazebo_simulation/launch/mrs_drone_spawner.launch index 7036504..a3a263f 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/launch/mrs_drone_spawner.launch +++ b/ros_packages/mrs_uav_gazebo_simulation/launch/mrs_drone_spawner.launch @@ -1,8 +1,14 @@ + + - + + + + + diff --git a/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch b/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch index faffa4c..620f295 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch +++ b/ros_packages/mrs_uav_gazebo_simulation/launch/run_simulation_firmware.launch @@ -1,34 +1,25 @@ - - - - - - - - - - - + + - - + + - - + + - + diff --git a/ros_packages/mrs_uav_gazebo_simulation/launch/simulation.launch b/ros_packages/mrs_uav_gazebo_simulation/launch/simulation.launch index 7666c9c..e31ae19 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/launch/simulation.launch +++ b/ros_packages/mrs_uav_gazebo_simulation/launch/simulation.launch @@ -9,7 +9,7 @@ - + @@ -39,13 +39,10 @@ - - - - - - - + + + + + + - - - - +{%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} + +{# !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! #} +{# !! THIS DOCUMENT CONTAINS ONLY COMPLEX COMPONENT MACROS DESIGNED !! #} +{# !! FOR USE WITH THE MRS DRONE SPAWNER AND USE THE SPAWNER API !! #} +{# !! GENERIC SENSORS, VISUAL BLOCKS, PLUGINS ETC BELONG TO GENERIC !! #} +{# !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! #} - - +{# ================================================================== #} +{# || Components using spawner API || #} +{# ================================================================== #} + +{# ground_truth_macro {--> #} +{%- macro ground_truth_macro(parent_link, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-ground-truth' -%} + {%- set spawner_description = 'Enable ROS topic with ground truth odometry published under model namespace' -%} + {%- set spawner_default_args = {'topic_name': 'ground_truth', 'frame_name': 'world', 'update_rate': 150} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + {{ generic.odometry_sensor_macro( + odometry_sensor_name = 'ground_truth', + parent_link = parent_link, + topic_name = spawner_args[spawner_keyword]['topic_name'], + noise = 0, + frame_name = spawner_args[spawner_keyword]['frame_name'], + update_rate = spawner_args[spawner_keyword]['update_rate'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + -{%- set pi = 3.14159265359 -%} + {%- endif -%} -{%- macro rad(deg) -%} -{{ deg * pi / 180 }} {%- endmacro -%} +{# #} -{%- set rad0 = 0.0 -%} -{%- set rad10 = 10 * pi / 180 -%} -{%- set rad20 = 20 * pi / 180 -%} -{%- set rad30 = 30 * pi / 180 -%} -{%- set rad45 = 45 * pi / 180 -%} -{%- set rad60 = 60 * pi / 180 -%} -{%- set rad65 = 65 * pi / 180 -%} -{%- set rad70 = 70 * pi / 180 -%} -{%- set rad85 = 85 * pi / 180 -%} -{%- set rad90 = 90 * pi / 180 -%} -{%- set rad95 = 95 * pi / 180 -%} -{%- set rad115 = 115 * pi / 180 -%} -{%- set rad120 = 120 * pi / 180 -%} -{%- set rad135 = 135 * pi / 180 -%} -{%- set rad150 = 150 * pi / 180 -%} -{%- set rad180 = 180 * pi / 180 -%} -{%- set rad210 = 210 * pi / 180 -%} -{%- set rad270 = 270 * pi / 180 -%} -{%- set rad330 = 330 * pi / 180 -%} - -{%- set sin30 = 0.5 -%} -{%- set sin45 = 0.70710678118 -%} -{%- set sin60 = 0.86602540378 -%} -{%- set sin90 = 1.0 -%} - -{%- set cos30 = 0.86602540378 -%} -{%- set cos45 = 0.70710678118 -%} -{%- set cos60 = 0.5 -%} -{%- set cos90 = 0.0 -%} - -{%- set sqrt2 = 1.41421356237 -%} - +{# propellers_macro {--> #} +{%- macro propellers_macro(prop_list, rotor_velocity_slowdown_sim, motor_constant, moment_constant, parent, mass, radius, time_constant_up, time_constant_down, max_rot_velocity, rotor_drag_coefficient, rolling_moment_coefficient, meshes_z_offset, prop_ixx, prop_ixy, prop_ixz, prop_iyy, prop_iyz, prop_izz, spawner_args) -%} + + {%- set spawner_keyword = 'disable-motor-crash' -%} + {%- set spawner_description = 'Disables motor failure after a crash with the environment.' -%} + {%- set spawner_default_args = none -%} + + {%- set enable_motor_crash = spawner_args[spawner_keyword] is undefined -%} + + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + {%- for prop_params in prop_list -%} + + {%- if prop_params['mesh_files'] | length == 1 -%} + + {{ generic.prop_macro( + direction = prop_params['direction'], + rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, + motor_constant = motor_constant, + moment_constant = moment_constant, + parent = parent, + mass = mass, + radius = radius, + time_constant_up = time_constant_up, + time_constant_down = time_constant_down, + max_rot_velocity = max_rot_velocity, + motor_number = prop_params['motor_number'], + rotor_drag_coefficient = rotor_drag_coefficient, + rolling_moment_coefficient = rolling_moment_coefficient, + enable_motor_crash = enable_motor_crash, + color = prop_params['color'], + mesh_file = prop_params['mesh_files'][0], + mesh_scale = prop_params['mesh_scale'], + x = prop_params['x'], + y = prop_params['y'], + z = prop_params['z'], + roll = 0, + pitch = 0, + yaw = 0, + ixx = prop_ixx, + ixy = prop_ixy, + ixz = prop_ixz, + iyy = prop_iyy, + iyz = prop_iyz, + izz = prop_izz) + }} + + {%- elif prop_params['mesh_files'] | length == 2 -%} + + {{ generic.prop_macro_2_meshes( + direction = prop_params['direction'], + rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, + motor_constant = motor_constant, + moment_constant = moment_constant, + parent = parent, + mass = mass, + radius = radius, + time_constant_up = time_constant_up, + time_constant_down = time_constant_down, + max_rot_velocity = max_rot_velocity, + motor_number = prop_params['motor_number'], + rotor_drag_coefficient = rotor_drag_coefficient, + rolling_moment_coefficient = rolling_moment_coefficient, + enable_motor_crash = enable_motor_crash, + color = prop_params['color'], + mesh_file_1 = prop_params['mesh_files'][0], + mesh_file_2 = prop_params['mesh_files'][1], + meshes_z_offset = meshes_z_offset, + mesh_scale = prop_params['mesh_scale'], + x = prop_params['x'], + y = prop_params['y'], + z = prop_params['z'], + roll = 0, + pitch = 0, + yaw = 0, + ixx = prop_ixx, + ixy = prop_ixy, + ixz = prop_ixz, + iyy = prop_iyy, + iyz = prop_iyz, + izz = prop_izz) + }} + + {%- endif -%} - - - - - -{# macro to place the lowest mass and intertia that gazebo allows #} -{%- macro zero_inertial_macro() -%} - - 0.0001 - - 1e-7 - 0 - 0 - 1e-7 - 0 - 1e-7 - - -{%- endmacro -%} - + {%- endfor -%} + - -{%- macro cylinder_inertia(m, r, h) -%} - - {{ m * ( 3 * r * r + h * h ) / 12 }} - 0 - 0 - {{ m * ( 3 * r * r + h * h ) / 12 }} - 0 - {{ m * r * r / 2 }} - {%- endmacro -%} - +{# #} - - - - - -{%- macro visual_colored_box_macro(name, size_x, size_y, size_z, color, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - - - {{ size_x }} {{ size_y }} {{ size_z }} - - - - - - +{# ======================= rangefinder sensors ====================== #} + +{# garmin_down_macro (connected through pixhawk) {--> #} +{%- macro garmin_down_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-rangefinder' -%} + {%- set spawner_description = 'Add a laser rangefinder (Garmin LIDAR-Lite v3) pointing down. Creates a Gazebo publisher for gazebo_mavlink_interface. ROS topic must be created by mavros. Do not set range outside <0.06 - 40.0> (unrealistic)' -%} + {%- set spawner_default_args = {'update_rate': 100, 'min_range': 0.1, 'max_range': 36.0, 'noise': 0.01} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- set sensor_name = 'lidar0' -%} + + + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + {{ generic.zero_inertial_macro() }} + + {{ generic.visual_mesh_macro( + name = sensor_name, + mesh_file = 'model://mrs_robots_description/meshes/sensors/garmin_lidar_v3.stl', + mesh_scale = '0.001 0.001 0.001', + color = 'DarkGrey', + x = 0.015, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + {{ generic.gazebo_rangefinder_sensor_macro( + name = sensor_name, + parent_frame_name = spawner_args['name'] + '/fcu', + sensor_frame_name = spawner_args['name'] + '/garmin', + topic = sensor_name, + update_rate = spawner_args[spawner_keyword]['update_rate'], + samples = 1, + fov = 0.03, + min_distance = spawner_args[spawner_keyword]['min_range'], + max_distance = spawner_args[spawner_keyword]['max_range'], + resolution = 0.005, + noise = spawner_args[spawner_keyword]['noise'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + + + {{ sensor_name }}_link + {{ parent_link }} + + + + + {{ mount }} + + + + + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- macro visual_colored_box_with_collision_macro(name, size_x, size_y, size_z, color, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - - - {{ size_x }} {{ size_y }} {{ size_z }} - - - - - - - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - - - {{ size_x }} {{ size_y }} {{ size_z }} - - - +{# garmin_down_external_macro (external -> connected to computer) {--> #} +{%- macro garmin_down_external_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-rangefinder-external' -%} + {%- set spawner_description = 'Add a laser rangefinder (Garmin LIDAR-Lite v3) pointing down. Creates a ROS topic /robot_name/garmin/range. Do not set range outside <0.06 - 40.0> (unrealistic)' -%} + {%- set spawner_default_args = {'update_rate': 100, 'min_range': 0.1, 'max_range': 36.0, 'noise': 0.01} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- set sensor_name = 'garmin' -%} + + + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + {{ generic.zero_inertial_macro() }} + + {{ generic.visual_mesh_macro( + name = sensor_name, + mesh_file = 'model://mrs_robots_description/meshes/sensors/garmin_lidar_v3.stl', + mesh_scale = '0.001 0.001 0.001', + color = 'DarkGrey', + x = 0.015, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + {{ generic.rangefinder_sensor_macro( + name = sensor_name, + parent_frame_name = spawner_args['name'] + '/fcu', + sensor_frame_name = spawner_args['name'] + '/' + sensor_name, + topic = sensor_name + '/range', + update_rate = spawner_args[spawner_keyword]['update_rate'], + samples = 1, + fov = 0.03, + min_distance = spawner_args[spawner_keyword]['min_range'], + max_distance = spawner_args[spawner_keyword]['max_range'], + resolution = 0.005, + noise = spawner_args[spawner_keyword]['noise'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + + + {{ sensor_name }}_link + {{ parent_link }} + + + + + {{ mount }} + + + + + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- macro visual_mesh_macro(name, mesh_file, mesh_scale, color, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - - - {{ mesh_file }} - {{ mesh_scale }} - - - - - - +{# garmin_up_external_macro (external -> connected to computer) {--> #} +{%- macro garmin_up_external_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-rangefinder-up' -%} + {%- set spawner_description = 'Add a laser rangefinder (Garmin LIDAR-Lite v3) pointing up. Creates a ROS topic /robot_name/garmin/range. Do not set range outside <0.06 - 40.0> (unrealistic)' -%} + {%- set spawner_default_args = {'update_rate': 100, 'min_range': 0.1, 'max_range': 36.0, 'noise': 0.01} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- set sensor_name = 'garmin_up' -%} + + + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + {{ generic.zero_inertial_macro() }} + + {{ generic.visual_mesh_macro( + name = sensor_name, + mesh_file = 'model://mrs_robots_description/meshes/sensors/garmin_lidar_v3.stl', + mesh_scale = '0.001 0.001 0.001', + color = 'DarkGrey', + x = 0.015, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + {{ generic.rangefinder_sensor_macro( + name = sensor_name, + parent_frame_name = spawner_args['name'] + '/fcu', + sensor_frame_name = spawner_args['name'] + '/' + sensor_name, + topic = sensor_name + '/range', + update_rate = spawner_args[spawner_keyword]['update_rate'], + samples = 1, + fov = 0.03, + min_distance = spawner_args[spawner_keyword]['min_range'], + max_distance = spawner_args[spawner_keyword]['max_range'], + resolution = 0.005, + noise = spawner_args[spawner_keyword]['noise'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + + + {{ sensor_name }}_link + {{ parent_link }} + + + + + {{ mount }} + + + + + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- macro visual_mesh_mrs_material_macro(name, mesh_file, mesh_scale, color, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - - - {{ mesh_file }} - {{ mesh_scale }} - - - - - - +{# teraranger_macro {--> #} +{%- macro teraranger_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-teraranger' -%} + {%- set spawner_description = 'Add a laser rangefinder (Teraranger One) pointing down' -%} + {%- set spawner_default_args = {'update_rate': 100, 'min_range': 0.1, 'max_range': 14.0, 'noise': 0.04} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- set sensor_name = 'teraranger' -%} + + + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_colored_box_macro(sensor_name, 0.015, 0.027, 0.033, 'Yellow', 0, 0, 0, 0, 0, 0) }} + {{ generic.rangefinder_sensor_macro( + name = sensor_name, + parent_frame_name = spawner_args['name'] + '/fcu', + sensor_frame_name = spawner_args['name'] + '/' + sensor_name, + topic = sensor_name + '/range', + update_rate = spawner_args[spawner_keyword]['update_rate'], + samples = 1, + fov = 0.03, + min_distance = spawner_args[spawner_keyword]['min_range'], + max_distance = spawner_args[spawner_keyword]['max_range'], + resolution = 0.005, + noise = spawner_args[spawner_keyword]['noise'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ parent_link }} + {{ sensor_name}}_link + + + + + {{ mount }} + + + + + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- macro visual_mesh_with_collision_macro(name, mesh_file, mesh_scale, color, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - - - {{ mesh_file }} - {{ mesh_scale }} - - - - - - - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - - - {{ mesh_file }} - {{ mesh_scale }} - - - +{# teraranger_evo_tower_macro {--> #} +{%- macro teraranger_evo_tower_macro(parent_link, visualize, frame_name, parent_frame_name, gaussian_noise, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-teraranger-evo-tower' -%} + {%- set spawner_description = 'Add the Teraranger Evo Tower laser scanner to the vehicle' -%} + {%- set spawner_default_args = none -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {# -- parameters of sensors -- (id, x, y, z, roll, pitch, yaw) #} + {%- set sensor_parameters = [(0, 0.046, 0.0, 0.001, 0.0, 0.0, 0.0), + (1, 0.032, 0.032, 0.001, 0.0, 0.0, math.radians(45)), + (2, 0.000, 0.046, 0.001, 0.0, 0.0, math.radians(90)), + (3, -0.032, 0.032, 0.001, 0.0, 0.0, math.radians(135)), + (4, -0.046, 0.000, 0.001, 0.0, 0.0, math.radians(180)), + (5, -0.032, -0.032, 0.001, 0.0, 0.0, -math.radians(135)), + (6, 0.000, -0.046, 0.001, 0.0, 0.0, -math.radians(90)), + (7, 0.032, -0.032, 0.001, 0.0, 0.0, -math.radians(45))] -%} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + 0 0 -0.014 0 0 0 + + + 0.001 + 0.06 + + + + + + + + {%- for id_, x_, y_, z_, roll_, pitch_, yaw_ in sensor_parameters -%} + + {{ generic.teraranger_evo_macro( + parent_link = parent_link, + id = id_, + visualize = visualize, + frame_name = frame_name, + parent_frame_name = parent_frame_name, + gaussian_noise = gaussian_noise, + x = x_, + y = y_, + z = z_, + roll = roll_, + pitch = pitch_, + yaw = yaw_) + }} + + {%- endfor -%} + + + + {{ parent_link }} + teraranger_evo_tower_link + + + + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- macro visual_mesh_textured_macro(name, mesh_file, mesh_scale, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - - - {{ mesh_file }} - {{ mesh_scale }} - - - -{%- endmacro -%} - +{# ========================== LIDAR sensors ========================= #} + +{# rplidar_macro {--> #} +{%- macro rplidar_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-rplidar' -%} + {%- set spawner_description = 'Add the RPlidar A3 laser scanner. Do not set range outside <0.10 - 15.0> (unrealistic)' -%} + {%- set spawner_default_args = {'horizontal_samples': 1600, 'update_rate': 20.0, 'min_range': 0.15, 'max_range': 14.0, 'noise': 0.01} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- set sensor_name = 'rplidar' -%} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + + {{ generic.visual_mesh_macro( + name = 'base_visual', + mesh_file = 'model://mrs_robots_description/meshes/sensors/rplidar.stl', + mesh_scale = '0.001 0.001 0.001', + color = 'FlatBlack', + x = 0, + y = 0, + z = -0.029, + roll = 0, + pitch = 0, + yaw = math.radians(180)) + }} + + 0 0 -0.011 0 0 0 + + + 0.001 + 0.038 + + + + + + + - -{%- macro visual_mesh_textured_with_collision_macro(name, mesh_file, mesh_scale, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - - - {{ mesh_file }} - {{ mesh_scale }} - - - - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - - - {{ mesh_file }} - {{ mesh_scale }} - - - -{%- endmacro -%} - + + + false + {{ spawner_args[spawner_keyword]['update_rate'] }} + + + + {{ spawner_args[spawner_keyword]['horizontal_samples'] | int }} + 1 + -3.1241390751 + 3.1241390751 + + + + {{ spawner_args[spawner_keyword]['min_range'] }} + {{ spawner_args[spawner_keyword]['max_range'] }} + 0.01 + + + gaussian + 0.0 + {{ spawner_args[spawner_keyword]['noise'] }} + + + + {{ sensor_name }}/scan + {{ spawner_args['name'] }}/{{ sensor_name }} + {{ spawner_args['name'] }}/fcu + {{ x }} + {{ y }} + {{ z }} + {{ roll }} + {{ pitch }} + {{ yaw }} + + + - -{%- 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( - name = name, - mesh_file = mesh_file, - mesh_scale = mesh_scale, - color = color, - x = x, - y = y, - z = z, - roll = roll, - pitch = pitch, - yaw = yaw) -}} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - - - {{ collision_height }} - {{ collision_radius }} - - - - - - 0.001 - 0.0 - - - - - - - -{%- endmacro -%} - + + {{ parent_link }} + {{ sensor_name }}_link + - -{%- macro leg_offset_macro(name, mesh_file, mesh_scale, color, x, y, z, roll, pitch, yaw, collision_height, collision_radius, offset_x, offset_y, offset_z) -%} -{{ visual_mesh_macro( - name = name, - mesh_file = mesh_file, - mesh_scale = mesh_scale, - color = color, - x = x, - y = y, - z = z, - roll = roll, - pitch = pitch, - yaw = yaw) -}} - - {{ x + offset_x }} {{ y + offset_y }} {{ z + offset_z }} {{ roll }} {{ pitch }} {{ yaw }} - - - {{ collision_height }} - {{ collision_radius }} - - - - - - 0.001 - 0.0 - - - - - - - -{%- endmacro -%} - + + {{ mount }} + - -{%- macro leg_macro_collision_offset(name, mesh_file, mesh_scale, color, parent, x, y, z, roll, pitch, yaw, collision_height, collision_radius, collision_offset) -%} -{{ visual_mesh_macro( - name = name, - mesh_file = mesh_file, - mesh_scale = mesh_scale, - color = color, - x = x, - y = y, - z = z, - roll = roll, - pitch = pitch, - yaw = yaw) -}} - - {{ x }} {{ y }} {{ z + collision_offset }} {{ roll }} {{ pitch }} {{ yaw }} - - - {{ collision_height }} - {{ collision_radius }} - - - - - - 0.001 - 0.0 - - - - - - - + + + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- macro prop_macro(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, 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 }} - {{ mesh_scale }} - - - - - - - - 0 0 0 0 {{ rad90 }} 0 - - - {{ 2*radius }} - 0.01 - - - - - - - - - - - - +{# scanse_sweep_macro {--> #} +{%- macro scanse_sweep_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-scanse-sweep' -%} + {%- set spawner_description = 'Add the Scanse Sweep laser scanner' -%} + {%- set spawner_default_args = {'horizontal_samples': 500, 'update_rate': 10.0, 'min_range': 0.45, 'max_range': 10.0, 'noise': 0.01} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + + + 0 0 -0.031 0 0 0 + + + 0.0255 + 0.0234 + + + + + + + + + + 0.0385 + 0.0325 + + + + + + + - - - 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 }} - - - - {{ parent }} - prop_{{ motor_number }}_link - - 0 0 1 - - -1e+16 - 1e+16 - - - 0 - 0 - - 1 - - -{%- endmacro -%} - + + + false + {{ spawner_args[spawner_keyword]['update_rate'] }} + + + + {{ spawner_args[spawner_keyword]['horizontal_samples'] | int }} + 1 + 0 + 6.283185 + + + + {{ spawner_args[spawner_keyword]['min_range'] }} + {{ spawner_args[spawner_keyword]['max_range'] }} + 0.01 + + + gaussian + 0.0 + {{ spawner_args[spawner_keyword]['noise'] }} + + + + scanse_sweep/range + {{ spawner_args['name'] }}/scanse_sweep + {{ spawner_args['name'] }}/fcu + {{ x }} + {{ y }} + {{ z }} + {{ roll }} + {{ pitch }} + {{ yaw }} + + + - -{%- macro prop_macro_2_meshes(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 }} - - - - {{ parent }} - prop_{{ motor_number }}_link - - 0 0 1 - - -1e+16 - 1e+16 - - - 0 - 0 - - 1 - - -{%- endmacro -%} - + + {{ parent_link }} + sweeper_link + - -{%- 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 - - - - - - - - - - - - + + {{ mount }} + - - - 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 }} - - - - {{ parent }} - prop_{{ motor_number }}_link - - 0 0 1 - - -1e+16 - 1e+16 - - - 0 - 0 - - 1 - - -{%- endmacro -%} - + - -{%- macro multirotor_physics_macro(mass, body_radius, body_height, rotor_velocity_slowdown_sim, ixx, ixy, ixz, iyy, iyz, izz) -%} - - {{ mass }} - - {{ ixx }} - {{ ixy }} - {{ ixz }} - {{ iyy }} - {{ iyz }} - {{ izz }} - - - - - 0 - 0 - {{ - body_height / 2 }} - 0 - 0 - 0 - - - - {{ body_height }} - {{ body_radius }} - - - - - - 0.001 - 0.0 - - - - - - - - - - - base_link - {{ rotor_velocity_slowdown_sim }} - + {%- endif -%} {%- 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 -%} - +{# ouster_macro {--> #} +{%- macro ouster_macro(parent_link, sensor_name, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-ouster' -%} + {%- set spawner_description = 'Add Ouster laser scanner to the vehicle. Select a model to automatically set number of lines, vertical FOV and range. Available models: OS0-32, OS0-64, OS0-128, OS1-16, OS1-32G1, OS1-32, OS1-64, OS1-128, OS2-32, OS2-64, OS2-128' -%} + {%- set spawner_default_args = {'model': 'OS1-16', 'horizontal_samples': 2048, 'update_rate': 10, 'noise': 0.03, 'use_gpu': false} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {# model selection {--> #} + {%- set ouster_model = spawner_args[spawner_keyword]['model'] -%} + + {# OS0 {--> #} + + {# OS0-32 #} + {%- if ouster_model == 'OS0-32' -%} + {%- set lasers = 32 -%} + {%- set vfov_angle = 90 -%} + {%- set range = 55 -%} + {%- endif -%} + + {# OS0-64 #} + {%- if ouster_model == 'OS0-64' -%} + {%- set lasers = 64 -%} + {%- set vfov_angle = 90 -%} + {%- set range = 55 -%} + {%- endif -%} + + {# OS0-128 #} + {%- if ouster_model == 'OS0-128' -%} + {%- set lasers = 128 -%} + {%- set vfov_angle = 90 -%} + {%- set range = 55 -%} + {%- endif -%} + + {# #} + + {# OS1 Generation 1 {--> #} + {# #} + {%- if ouster_model == 'OS1-16' -%} + {%- set lasers = 16 -%} + {%- set vfov_angle = 33.2 -%} + {%- set range = 120 -%} + {%- endif -%} + {# #} + + {# OS1 Generation 2 {--> #} + + {# OS1-32 Generation 2 #} + {%- if ouster_model == 'OS1-32' -%} + {%- set lasers = 32 -%} + {%- set vfov_angle = 45 -%} + {%- set range = 120 -%} + {%- endif -%} + + {# OS1-64 Generation 2 #} + {%- if ouster_model == 'OS1-64' -%} + {%- set lasers = 64 -%} + {%- set vfov_angle = 45 -%} + {%- set range = 120 -%} + {%- endif -%} + + {# OS1-128 Generation 2 #} + {%- if ouster_model == 'OS1-128' -%} + {%- set lasers = 128 -%} + {%- set vfov_angle = 45 -%} + {%- set range = 120 -%} + {%- endif -%} + + {# #} + + {# OS2 {--> #} + + {# OS2-32 #} + {%- if ouster_model == 'OS2-32' -%} + {%- set lasers = 32 -%} + {%- set vfov_angle = 22.5 -%} + {%- set range = 240 -%} + {%- endif -%} + + {# OS2-64 #} + {%- if ouster_model == 'OS2-64' -%} + {%- set lasers = 64 -%} + {%- set vfov_angle = 22.5 -%} + {%- set range = 240 -%} + {%- endif -%} + + {# OS2-128 #} + {%- if ouster_model == 'OS2-128' -%} + {%- set lasers = 128 -%} + {%- set vfov_angle = 22.5 -%} + {%- set range = 240 -%} + {%- endif -%} + + {# #} + + {# #} + + {# 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. #} + + {# setup local variables {--> #} + + {# -- gazebo links -- #} + {%- set sensor_link = sensor_name + '_sensor_link' -%} + + {# -- frame names -- #} + {%- set frame_fcu = spawner_args['name'] + '/fcu' -%} + {%- set frame_sensor = spawner_args['name'] + '/' + sensor_name + '_sensor' -%} + {%- set frame_lidar = spawner_args['name'] + '/' + sensor_name + '_lidar' -%} + {%- set frame_imu = spawner_args['name'] + '/' + sensor_name + '_imu' -%} + + {# -- topics -- #} + {%- set topic_lidar = '/' + spawner_args['name'] + '/' + sensor_name + '_cloud_nodelet/points' -%} + {%- set topic_imu = '/' + spawner_args['name'] + '/' + sensor_name + '_cloud_nodelet/imu' -%} + {%- set topic_diag = '/' + spawner_args['name'] + '/' + sensor_name + '_cloud_nodelet/is_alive' -%} + + {# -- tf from sensor to lidar -- #} + {%- set lidar_x = 0 -%} + {%- set lidar_y = 0 -%} + {%- set lidar_z = 0.0344 -%} + {%- set lidar_roll = 0 -%} + {%- set lidar_pitch = 0 -%} + {%- set lidar_yaw = 0 -%} + + {# -- tf from sensor to imu -- #} + {%- set imu_x = 0.006253 -%} + {%- set imu_y = -0.011775 -%} + {%- set imu_z = 0.007645 -%} + {%- set imu_roll = 0 -%} + {%- set imu_pitch = 0 -%} + {%- set imu_yaw = 0 -%} + + {%- if spawner_args[spawner_keyword]['use_gpu'] -%} + {%- set ouster_plugin_filename ='libMrsGazeboCommonResources_3DLidarGpuPlugin.so' -%} + {%- set sensor_type = 'gpu_ray' -%} + {%- else -%} + {%- set ouster_plugin_filename ='libMrsGazeboCommonResources_3DLidarPlugin.so' -%} + {%- set sensor_type = 'ray' -%} + {%- endif -%} + + {# #} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + + {{ generic.visual_mesh_macro( + name = 'base_visual', + mesh_file = 'model://mrs_robots_description/meshes/sensors/os1_64.dae', + mesh_scale = '1 1 1', + color = 'White', + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = math.radians(90)) + }} + + 0 0 {{ lidar_z }} 0 0 + + + 0.035 + 0.038 + + + + + + + - - - - - - - -{%- macro mavlink_interface_macro(mavlink_addr, mavlink_udp_port, mavlink_tcp_port, serial_enabled, serial_device, baudrate, qgc_addr, qgc_udp_port, sdk_addr, sdk_udp_port, hil_mode, hil_state_level, send_vision_estimation, send_odometry, enable_lockstep, use_tcp) -%} - - - /imu - /mag - /baro - {{ mavlink_addr }} - {{ mavlink_udp_port }} - {{ mavlink_tcp_port }} - {{ serial_enabled }} - {{ serial_device }} - {{ baudrate }} - {{ qgc_addr }} - {{ qgc_udp_port }} - {{ sdk_addr }} - {{ sdk_udp_port }} - {{ hil_mode }} - {{ hil_state_level }} - {{ send_vision_estimation }} - {{ send_odometry }} - {{ enable_lockstep }} - {{ use_tcp }} - /gazebo/command/motor_speed - - - 0 - 0.0 - 0.85 - 0 - 0.15 - velocity - - - 1 - 0.0 - 0.85 - 0 - 0.15 - velocity - - - 2 - 0.0 - 0.85 - 0 - 0.15 - velocity - - - 3 - 0.0 - 0.85 - 0 - 0.15 - velocity - - - 4 - 0.0 - 0.85 - 0 - 0.15 - velocity - - - 5 - 0.0 - 0.85 - 0 - 0.15 - velocity - - - 6 - 0.0 - 0.85 - 0 - 0.15 - velocity - - - 7 - 0.0 - 0.85 - 0 - 0.15 - velocity - - - -{%- endmacro -%} - + + + {{ imu_x }} {{ imu_y }} {{ imu_z }} {{ imu_roll }} {{ imu_pitch }} {{ imu_yaw }} + true + true + 100 + false + __default_topic__ + + + {{ topic_imu }} + {{ sensor_link }} + 100 + 0.005 + 0 0 0 + 0 0 0 + {{ frame_imu }} + + + - -{%- macro gps_macro(gps_name, parent_link, update_rate, gps_noise, gps_xy_random_walk, gps_z_random_walk, gps_xy_noise_density, gps_z_noise_density, gps_vxy_noise_density, gps_vz_noise_density, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - - - {{ update_rate }} - {{ gps_noise }} - {{ gps_xy_random_walk }} - {{ gps_z_random_walk }} - {{ gps_xy_noise_density }} - {{ gps_z_noise_density }} - {{ gps_vxy_noise_density }} - {{ gps_vz_noise_density }} - {{ gps_name }} - - - + + + {{ lidar_x }} {{ lidar_y }} {{ lidar_z }} {{ lidar_roll }} {{ lidar_pitch }} {{ lidar_yaw }} + false + {{ spawner_args[spawner_keyword]['update_rate'] }} + + + + {{ spawner_args[spawner_keyword]['horizontal_samples'] }} + 1 + 0 + {{ 2*math.pi }} + + + {{ lasers }} + 1 + {{ -vfov_angle/2*math.radians(180)/180.0 }} + {{ vfov_angle/2*math.radians(180)/180.0 }} + + + + 0.1 + {{ range }} + 0.03 + + + + {{ frame_fcu }} + {{ frame_sensor }} + {{ x + lidar_x }} + {{ y + lidar_y }} + {{ z + lidar_z }} + {{ roll + lidar_roll }} + {{ pitch + lidar_pitch }} + {{ yaw + lidar_yaw }} + + + {{ frame_lidar }} + 0 + 0 + 0 + 0 + 0 + {{ math.radians(180) }} + {{ topic_lidar }} + {{ topic_diag }} + 0.1 + {{ range }} + true + {{ spawner_args[spawner_keyword]['noise'] }} + + + true + {{ frame_imu }} + {{ imu_x - lidar_x }} + {{ imu_y - lidar_y }} + {{ imu_z - lidar_z }} + {{ imu_roll - lidar_roll }} + {{ imu_pitch - lidar_pitch }} + {{ imu_yaw - lidar_yaw }} + + + - - {{ gps_name }}_link - {{ parent_link }} - + -{%- endmacro -%} - + + {{ parent_link }} + {{ sensor_link }} + - -{%- macro magnetometer_plugin_macro(pub_rate, noise_density, random_walk, bias_correlation_time, mag_topic) -%} - - - {{ pub_rate }} - {{ noise_density }} - {{ random_walk }} - {{ bias_correlation_time }} - {{ mag_topic }} - -{%- endmacro -%} - + + {{ mount }} + - -{%- macro gps_groundtruth_plugin_macro(home_latitude, home_longitude, home_altitude) -%} - - - {{ home_latitude }} - {{ home_longitude }} - {{ home_altitude }} - -{%- endmacro -%} - + - -{%- macro barometer_plugin_macro(baro_topic, pub_rate, baro_drift_pa_per_sec) -%} - - - {{ pub_rate }} - {{ baro_topic }} - {{ baro_drift_pa_per_sec }} - + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- macro imu_plugin_macro(imu_name, parent_link, imu_topic, gyroscope_noise_density, gyroscope_random_walk, gyroscope_bias_correlation_time, gyroscope_turn_on_bias_sigma, accelerometer_noise_density, accelerometer_random_walk, accelerometer_bias_correlation_time, accelerometer_turn_on_bias_sigma, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - - {{ parent_link }} - {{ imu_name }}_link - - 0 0 1 - - 0 - 0 - 0 - 0 - - 1 - - - - - - {{ imu_name }}_link - {{ imu_topic }} - {{ gyroscope_noise_density }} - {{ gyroscope_random_walk }} - {{ gyroscope_bias_correlation_time }} - {{ gyroscope_turn_on_bias_sigma }} - {{ accelerometer_noise_density }} - {{ accelerometer_random_walk }} - {{ accelerometer_bias_correlation_time }} - {{ accelerometer_turn_on_bias_sigma }} - +{# velodyne_macro {--> #} +{%- macro velodyne_macro(parent_link, sensor_name, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-velodyne' -%} + {%- set spawner_description = 'Add Velodyne PUCK laser scanner to the vehicle' -%} + {%- set spawner_default_args = {'horizontal_samples': 3600, 'lasers': 16, 'noise': 0.01, 'range': 100, 'vfov': 30, 'update_rate': 20, 'use_gpu': false} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {# setup local variables {--> #} + + {# 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. #} + {# Velodyne macro is using the same plugin as ouster macro. Therefore we need to render data in the same way. #} + + {# -- frame names -- #} + {%- set frame_fcu = spawner_args['name'] + '/fcu' -%} + {%- set frame_sensor = spawner_args['name'] + '/' + sensor_name + '_sensor' -%} + {%- set frame_lidar = spawner_args['name'] + '/' + sensor_name -%} + + {# -- topics -- #} + {%- set topic_lidar = '/' + spawner_args['name'] + '/' + sensor_name + '/scan' -%} + {%- set topic_diag = '/' + spawner_args['name'] + '/' + sensor_name + '/is_alive' -%} + + {# -- tf from sensor to lidar -- #} + {# The laser rays should be coming approximately from the half of sensor height #} + {%- set lidar_x = 0 -%} + {%- set lidar_y = 0 -%} + {%- set lidar_z = 0.037725 -%} + {%- set lidar_roll = 0 -%} + {%- set lidar_pitch = 0 -%} + {%- set lidar_yaw = 0 -%} + + {%- if spawner_args[spawner_keyword]['use_gpu'] -%} + {%- set velodyne_plugin_filename ='libMrsGazeboCommonResources_3DLidarGpuPlugin.so' -%} + {%- set sensor_type = 'gpu_ray' -%} + {%- else -%} + {%- set velodyne_plugin_filename ='libMrsGazeboCommonResources_3DLidarPlugin.so' -%} + {%- set sensor_type = 'ray' -%} + {%- endif -%} + {# #} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + + + 0 0 0.0094 0 0 0 + + + 0.0188 + 0.062 + + + + + + + + 0 0 0.0643 0 0 0 + + + 0.0148 + 0.062 + + + + + + + + 0 0 0.03785 0 0 0 + + + 0.0381 + 0.058 + + + + + + + + + + + {{ lidar_x }} {{ lidar_y }} {{ lidar_z }} {{ lidar_roll }} {{ lidar_pitch }} {{ lidar_yaw }} + {# 0 0 0 0 0 0 #} + false + {{ spawner_args[spawner_keyword]['update_rate'] }} + + + + {{ spawner_args[spawner_keyword]['horizontal_samples'] }} + 1 + {{ -math.radians(180) }} + {{ math.radians(180) }} + + + {{ spawner_args[spawner_keyword]['lasers'] }} + 1 + {{ -spawner_args[spawner_keyword]['vfov']/2*math.radians(180)/180.0 }} + {{ spawner_args[spawner_keyword]['vfov']/2*math.radians(180)/180.0 }} + + + + 0.1 + {{ spawner_args[spawner_keyword]['range'] }} + 0.03 + + + + {{ frame_fcu }} + {{ frame_sensor }} + {{ x + lidar_x }} + {{ y + lidar_y }} + {{ z + lidar_z }} + {{ roll }} + {{ pitch }} + {{ yaw }} + 0 + 0 + 0 + {{ lidar_roll }} + {{ lidar_pitch }} + {{ lidar_yaw }} + {{ topic_lidar }} + {{ topic_diag }} + {{ frame_lidar }} + 0.1 + {{ spawner_args[spawner_keyword]['range'] }} + {{ spawner_args[spawner_keyword]['noise'] }} + false + + + + + + + + {{ parent_link }} + velodyne_link + + + + {{ mount }} + + + + + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- macro custom_imu_macro(sensor_name, parent_link, update_rate, topic_name, frame_name, noise_mean, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - true - true - {{ update_rate }} - false - __default_topic__ - - - {{ topic_name }} - {{ update_rate }} - {{ noise_mean }} - 0 0 0 - 0 0 0 - {{ frame_name }} - - - +{# ============================= cameras ============================ #} + +{# realsense_top_macro {--> #} +{%- macro realsense_top_macro(camera_name, camera_suffix, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-realsense-top' -%} + {%- set spawner_description = 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 @ 30hz], pointed forward, placed on an aluminum frame above the body' -%} + {%- set spawner_default_args = {'realistic': False, 'update_rate': 30} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.realsense_d435_macro( + namespace = spawner_args['name'], + camera_name = camera_name, + camera_suffix = camera_suffix, + parent_link = parent_link, + realistic = spawner_args[spawner_keyword]['realistic'], + update_rate = spawner_args[spawner_keyword]['update_rate'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ mount }} + + + + + {%- endif -%} - - {{ parent_link }} - {{ sensor_name }}_link - {%- endmacro -%} - +{# #} + +{# realsense_front_macro {--> #} +{%- macro realsense_front_macro(camera_name, camera_suffix, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-realsense-front' -%} + {%- set spawner_description = 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 @ 30hz], pointed forward placed on the front holder between the legs' -%} + {%- set spawner_default_args = {'realistic': False, 'update_rate': 30} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.realsense_d435_macro( + namespace = spawner_args['name'], + camera_name = camera_name, + camera_suffix = camera_suffix, + parent_link = parent_link, + realistic = spawner_args[spawner_keyword]['realistic'], + update_rate = spawner_args[spawner_keyword]['update_rate'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ mount }} + + + + + {%- endif -%} - -{%- macro wind_plugin_macro(xyz_offset, wind_direction, wind_force_mean, wind_gust_direction, wind_gust_duration, wind_gust_start, wind_gust_force_mean) -%} - - base_link - base_link - - {{ xyz_offset }} - {{ wind_direction }} - {{ wind_force_mean }} - {{ wind_gust_direction }} - {{ wind_gust_duration }} - {{ wind_gust_start }} - {{ wind_gust_force_mean }} - {%- endmacro -%} - +{# #} + +{# realsense_front_pitched_macro {--> #} +{%- macro realsense_front_pitched_macro(camera_name, camera_suffix, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-realsense-front-pitched' -%} + {%- set spawner_description = 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 @ 30hz], pointed forward pitched down by 10-45 deg (angle depends on vehicle model)' -%} + {%- set spawner_default_args = {'realistic': False, 'update_rate': 30} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.realsense_d435_macro( + namespace = spawner_args['name'], + camera_name = camera_name, + camera_suffix = camera_suffix, + parent_link = parent_link, + realistic = spawner_args[spawner_keyword]['realistic'], + update_rate = spawner_args[spawner_keyword]['update_rate'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ mount }} + + + + + {%- endif -%} - -{%- macro odometry_plugin_macro(odometry_sensor_name, parent_link, topic_name, noise, frame_name, frame_rate, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - - {{ parent_link }} - {{ odometry_sensor_name }}_link - - 0 0 1 - - 0 - 0 - 0 - 0 - - 1 - - - - true - {{ frame_rate }} - {{ odometry_sensor_name }}_link - {{ topic_name }} - {{ noise }} - {{ frame_name }} - {{ x }} {{ y }} {{ z }} - {{ roll }} {{ pitch }} {{ yaw }} - {%- endmacro -%} - +{# #} + +{# realsense_up_macro {--> #} +{%- macro realsense_up_macro(camera_name, camera_suffix, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-realsense-up' -%} + {%- set spawner_description = 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 @ 30hz], pointed upwards' -%} + {%- set spawner_default_args = {'realistic': False, 'update_rate': 30} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.realsense_d435_macro( + namespace = spawner_args['name'], + camera_name = camera_name, + camera_suffix = camera_suffix, + parent_link = parent_link, + realistic = spawner_args[spawner_keyword]['realistic'], + update_rate = spawner_args[spawner_keyword]['update_rate'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ mount }} + + + + + {%- endif -%} - -{%- macro rangefinder_sensor_macro(name, parent_frame_name, rangefinder_frame_name, topic, frame_rate, fov, min_distance, max_distance, resolution, noise, x, y, z, roll, pitch, yaw) -%} - - {{ frame_rate }} - - - - 5 - 1 - -{{ fov/2 }} - {{ fov/2 }} - - - 5 - 1 - -{{ fov/2 }} - {{ fov/2 }} - - - - {{ min_distance }} - {{ max_distance }} - {{ resolution }} - - - - {{ noise }} - true - {{ frame_rate }} - {{ topic }} - {{ rangefinder_frame_name }} - {{ fov }} - radiation - {{ parent_frame_name }} - {{ x }} - {{ y }} - {{ z }} - {{ roll }} - {{ pitch }} - {{ yaw }} - - {%- endmacro -%} - +{# #} - -{%- macro camera_macro(parent_link, camera_name, camera_frame_name, sensor_base_frame_name, frame_rate, parent_frame_name, horizontal_fov, image_width, image_height, min_distance, max_distance, noise_mean, noise_stddev, mesh_file, mesh_scale, color, visual_x, visual_y, visual_z, visual_roll, visual_pitch, visual_yaw, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - {{ visual_mesh_macro( - name = "", - mesh_file = mesh_file, - mesh_scale = mesh_scale, - color = color, - x = visual_x, - y = visual_y, - z = visual_z, - roll = visual_roll, - pitch = visual_pitch, - yaw = visual_yaw) - }} - - {{ frame_rate }} - - {{ horizontal_fov }} - - {{ image_width }} - {{ image_height }} - - - {{ min_distance }} - {{ max_distance }} - - - gaussian - - {{ noise_mean }} - {{ noise_stddev }} - - - - true - {{ frame_rate }} - {{ camera_name }} - image_raw - camera_info - /{{ camera_frame_name }} - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - {{ parent_frame_name }} - {{ sensor_base_frame_name }} - {{ x }} - {{ y }} - {{ z }} - {{ roll }} - {{ pitch }} - {{ yaw }} - - - +{# realsense_down_macro {--> #} +{%- macro realsense_down_macro(camera_name, camera_suffix, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-realsense-down' -%} + {%- set spawner_description = 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 @ 30hz], pointed down' -%} + {%- set spawner_default_args = {'realistic': False, 'update_rate': 30} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.realsense_d435_macro( + namespace = spawner_args['name'], + camera_name = camera_name, + camera_suffix = camera_suffix, + parent_link = parent_link, + realistic = spawner_args[spawner_keyword]['realistic'], + update_rate = spawner_args[spawner_keyword]['update_rate'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + - - {{ parent_link }} - {{ camera_name }}_link - + + {{ mount }} + + + + + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- macro fisheye_macro(parent_link, camera_name, topic_ns_prefix, camera_frame_name, sensor_base_frame_name, frame_rate, parent_frame_name, horizontal_fov, image_width, image_height, min_distance, max_distance, lens_type, lens_c1, lens_c2, lens_f, lens_fun, lens_scale, lens_cutoff_angle, lens_texture_size, noise_mean, noise_stddev, mesh_file, mesh_scale, color, visual_x, visual_y, visual_z, visual_roll, visual_pitch, visual_yaw, x, y, z, roll, pitch, yaw) -%} -{# -- topics -- #} -{%- set topic_image = topic_ns_prefix + "image_raw" -%} -{%- set topic_camera_ifo = topic_ns_prefix + "camera_info" -%} - - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - {{ visual_mesh_macro( - name = "", - mesh_file = mesh_file, - mesh_scale = mesh_scale, - color = color, - x = visual_x, - y = visual_y, - z = visual_z, - roll = visual_roll, - pitch = visual_pitch, - yaw = visual_yaw) - }} - - {{ frame_rate }} - - {{ horizontal_fov }} - - {{ image_width }} - {{ image_height }} - - - {{ min_distance }} - {{ max_distance }} - - - gaussian - - {{ noise_mean }} - {{ noise_stddev }} - - - {{ lens_type }} - - {{ lens_c1 }} - {{ lens_c2 }} - {{ lens_f }} - {{ lens_fun }} - - {{ lens_scale }} - {{ lens_cutoff_angle }} - {{ lens_texture_size }} - - - - true - {{ frame_rate }} - {{ camera_name }} - {{ topic_image }} - {{ topic_camera_ifo }} - /{{ camera_frame_name }} - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - {{ parent_frame_name }} - {{ sensor_base_frame_name }} - {{ x }} - {{ y }} - {{ z }} - {{ roll }} - {{ pitch }} - {{ yaw }} - - - +{# bluefox_camera_macro {--> #} +{%- macro bluefox_camera_macro(camera_name, parent_link, x, y, z, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-bluefox-camera' -%} + {%- set spawner_description = 'Add bluefox camera to the vehicle [752x480 @ 50hz], pointing down' -%} + {%- set spawner_default_args = {'update_rate': 100, 'noise': 0.0} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + frame_rate = spawner_args[spawner_keyword]['update_rate'], + horizontal_fov = 2.1, + image_width = 752, + image_height = 480, + min_distance = 0.02, + max_distance = 80, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = 0, + visual_y = 0, + visual_z = 0, + visual_roll = math.radians(90), + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = 0, + pitch = math.radians(90), + yaw = 0) + }} + - - {{ parent_link }} - {{ camera_name }}_link - + + {{ mount }} + + + + + {%- endif -%} {%- endmacro -%} - +{# #} - +{# bluefox_camera_reverse_macro {--> #} +{%- macro bluefox_camera_reverse_macro(camera_name, parent_link, x, y, z, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-bluefox-camera-reverse' -%} + {%- set spawner_description = 'Add bluefox camera to the vehicle [752x480 @ 50hz], pointing down but image is rotated by 180 deg' -%} + {%- set spawner_default_args = {'update_rate': 100, 'noise': 0.0} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + frame_rate = spawner_args[spawner_keyword]['update_rate'], + horizontal_fov = 2.1, + image_width = 752, + image_height = 480, + min_distance = 0.02, + max_distance = 80, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = 0, + visual_y = 0, + visual_z = 0, + visual_roll = math.radians(90), + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = 0, + pitch = math.radians(90), + yaw = math.radians(180)) + }} + - - - - - - - -{%- macro garmin_macro(sensor_name, parent_link, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - - - - model://mrs_robots_description/meshes/sensors/garmin_lidar_v3.stl - 0.001 0.001 0.001 - - - - - - - - true - 100 - false - - - - 1 - 1 - -0 - 0 - - - - 0.06 - 35 - 0.01 - - - gaussian - 0.0 - 0.01 - - - - - 0.1 - 35.0 - - - + + {{ mount }} + - - {{ parent_link }} - {{ sensor_name }}_link - + + + {%- endif -%} {%- endmacro -%} - +{# #} - - -{%- macro external_garmin_macro(namespace, parent_link, orientation, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - - - - model://mrs_robots_description/meshes/sensors/garmin_lidar_v3.stl - 0.001 0.001 0.001 - - - - - - - {{ rangefinder_sensor_macro( - name = "garmin" + orientation, - parent_frame_name = namespace + "/fcu", - rangefinder_frame_name = namespace + "/garmin" + orientation, - topic = "garmin" + orientation + "/range", - frame_rate = 100, - fov = 0.03, - min_distance = 0.1, - max_distance = 40.0, - resolution = 0.005, - noise = 0.01, - x = x, - y = y, - z = z, - roll = roll, - pitch = pitch, - yaw = yaw) - }} - +{# bluefox_camera_front_macro {--> #} +{%- macro bluefox_camera_front_macro(camera_name, parent_link, x, y, z, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-bluefox-camera-front' -%} + {%- set spawner_description = 'Add bluefox camera to the vehicle [752x480 @ 50hz], pointing forward' -%} + {%- set spawner_default_args = {'update_rate': 100, 'noise': 0.0} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + frame_rate = spawner_args[spawner_keyword]['update_rate'], + horizontal_fov = 2.1, + image_width = 752, + image_height = 480, + min_distance = 0.02, + max_distance = 80, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = 0, + visual_y = 0, + visual_z = 0, + visual_roll = math.radians(90), + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = 0, + pitch = 0, + yaw = 0) + }} + - - {{ parent_link }} - garmin{{ orientation }}_link - + + {{ mount }} + + + + + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- macro teraranger_macro(namespace, parent_link, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - - - 0.015 0.027 0.033 - - - - - - - {{ rangefinder_sensor_macro( - name = "teraranger", - parent_frame_name = namespace + "/fcu", - rangefinder_frame_name = namespace + "/teraranger", - topic = "teraranger/range", - frame_rate = 100, - fov = 0.03, - min_distance = 0.1, - max_distance = 14, - resolution = 0.005, - noise = 0.04, - x = x, - y = y, - z = z, - roll = roll, - pitch = pitch, - yaw = yaw) - }} - +{# mobius_down_macro {-->#} +{%- macro mobius_down_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-mobius-camera-down' -%} + {%- set spawner_description = 'Add mobius camera to the vehicle [1280x720 @ 30hz], pointing down' -%} + {%- set spawner_default_args = {'update_rate': 30, 'noise': 0.007} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + frame_rate = spawner_args[spawner_keyword]['update_rate'], + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + horizontal_fov = 2.28638, + image_width = 1280, + image_height = 720, + min_distance = 0.02, + max_distance = 40, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/mobius.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = -0.004, + visual_y = 0.0045, + visual_z = 0, + visual_roll = 0, + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + - - {{ parent_link }} - teraranger_link - + + {{ mount }} + + + + + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- macro ultrasonic_sensor_macro(namespace, parent_link, suffix, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - - - model://mrs_robots_description/meshes/sensors/ultrasonic_URM37.dae - 1 1 1 - - - - - - - {{ rangefinder_sensor_macro( - name = "ultrasound" + suffix, - parent_frame_name = namespace + "/fcu", - rangefinder_frame_name = namespace + "/ultrasound" + suffix, - topic = "ultrasound" + suffix + "/range", - frame_rate = 40, - fov = 0.698131701, - min_distance = 0.04, - max_distance = 5, - resolution = 0.01, - noise = 0.04, - x = x, - y = y, - z = z, - roll = roll, - pitch = pitch, - yaw = yaw) - }} - +{# mobius_front_macro {-->#} +{%- macro mobius_front_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-mobius-camera-front' -%} + {%- set spawner_description = 'Add mobius camera to the vehicle [1280x720 @ 30hz], pointing forward' -%} + {%- set spawner_default_args = {'update_rate': 30, 'noise': 0.007} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + frame_rate = spawner_args[spawner_keyword]['update_rate'], + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + horizontal_fov = 2.28638, + image_width = 1280, + image_height = 720, + min_distance = 0.02, + max_distance = 40, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/mobius.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = -0.004, + visual_y = 0.0045, + visual_z = 0, + visual_roll = 0, + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + - - {{ parent_link }} - ultrasound{{ suffix }}_link - + + {{ mount }} + + + + + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- macro uwb_range_macro(namespace, parent_link, uwb_id, uav_name, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - 10 - 1 - - {{uwb_id}} - /{{ uav_name }}/uwb_range/range - {{ uav_name }}/uwb - -150 - 0.1 - - - 6489.6 - 6240.0 - 6739.2 - 14.5 - 2.5 - - - +{# mobius_back_left_macro {-->#} +{%- macro mobius_back_left_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-mobius-camera-back-left' -%} + {%- set spawner_description = 'Add mobius camera to the vehicle [1280x720 @ 30hz], pointing to the back left' -%} + {%- set spawner_default_args = {'update_rate': 30, 'noise': 0.007} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + frame_rate = spawner_args[spawner_keyword]['update_rate'], + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + horizontal_fov = 2.28638, + image_width = 1280, + image_height = 720, + min_distance = 0.02, + max_distance = 40, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/mobius.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = -0.004, + visual_y = 0.0045, + visual_z = 0, + visual_roll = 0, + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + - - {{ parent_link }} - uwb_range_link - true - true - + + {{ mount }} + + + + + {%- endif -%} {%- endmacro -%} - +{# #} +{# mobius_back_right_macro {-->#} +{%- macro mobius_back_right_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-mobius-camera-back-right' -%} + {%- set spawner_description = 'Add mobius camera to the vehicle [1280x720 @ 30hz], pointing to the back right' -%} + {%- set spawner_default_args = {'update_rate': 30, 'noise': 0.007} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + frame_rate = spawner_args[spawner_keyword]['update_rate'], + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + horizontal_fov = 2.28638, + image_width = 1280, + image_height = 720, + min_distance = 0.02, + max_distance = 40, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/mobius.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = -0.004, + visual_y = 0.0045, + visual_z = 0, + visual_roll = 0, + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + - - - -{%- macro scanse_sweep_macro(namespace, parent_link, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - 0 0 -0.031 0 0 0 - - - 0.0255 - 0.0234 - - - - - - - - - - 0.0385 - 0.0325 - - - - - - - - false - 10 - - - - 500 - 1 - 0 - 6.283185 - - - - 0.45 - 10 - 0.01 - - - gaussian - 0.0 - 0.01 - - - - scanse_sweep/range - {{ namespace }}/scanse_sweep - {{ namespace}}/fcu - {{ x }} - {{ y }} - {{ z }} - {{ roll }} - {{ pitch }} - {{ yaw }} - - - + + {{ mount }} + - - {{ parent_link }} - sweeper_link - + + + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- 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() }} - - 0 0 -0.029 0 0 {{ rad180 }} - - - model://mrs_robots_description/meshes/sensors/rplidar.stl - 0.001 0.001 0.001 - - - - - - - - 0 0 -0.011 0 0 0 - - - 0.001 - 0.038 - - - - - - - - false - {{ update_rate }} - - - - {{ samples }} - 1 - -3.1241390751 - 3.1241390751 - - - - 0.15 - 14 - 0.01 - - - gaussian - 0.0 - 0.01 - - - - rplidar/scan - {{ namespace }}/rplidar - {{ namespace }}/fcu - {{ x }} - {{ y }} - {{ z }} - {{ roll }} - {{ pitch }} - {{ yaw }} - - - +{# fisheye_bluefox_macro {--> #} +{%- macro fisheye_bluefox_macro(camera_name, topic_ns_prefix, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-fisheye-camera' -%} + {%- set spawner_description = 'Add a fisheye bluefox camera [752x480 @ 45hz, 180 deg] to the vehicle' -%} + {%- set spawner_default_args = {'update_rate': 45, 'noise': 0.0} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.fisheye_camera_macro( + camera_name = camera_name, + topic_ns_prefix = topic_ns_prefix, + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + parent_link = parent_link, + frame_rate = spawner_args[spawner_keyword]['update_rate'], + horizontal_fov = math.radians(180), + image_width = 752, + image_height = 480, + lens_type = 'custom', + lens_c1 = 1.05, + lens_c2 = 4, + lens_f = 1.0, + lens_fun = 'tan', + lens_scale = True, + lens_cutoff_angle = math.radians(90), + lens_texture_size = 512, + min_distance = 0.02, + max_distance = 40, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = 0, + visual_y = 0, + visual_z = 0, + visual_roll = math.radians(90), + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + - - {{ parent_link }} - rplidar_link - + + {{ mount }} + + + + + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- 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. #} -{# Velodyne macro is using the same plugin as ouster macro. Therefore we need to render data in the same way. #} - -{# -- frame names -- #} -{%- set frame_fcu = namespace + "/fcu" -%} -{%- set frame_sensor = namespace + "/" + sensor_name + "_sensor" -%} -{%- set frame_lidar = namespace + "/" + sensor_name -%} - -{# -- topics -- #} -{%- set topic_lidar = "/" + namespace + "/" + sensor_name + "/scan" -%} -{%- set topic_diag = "/" + namespace + "/" + sensor_name + "/is_alive" -%} - -{# -- tf from sensor to lidar -- #} -{# The laser rays should be coming approximately from the half of sensor height #} -{%- set lidar_x = 0 -%} -{%- set lidar_y = 0 -%} -{%- set lidar_z = 0.037725 -%} -{%- set lidar_roll = 0 -%} -{%- set lidar_pitch = 0 -%} -{%- set lidar_yaw = 0 -%} - -{% if enable_gpu_ray %} - {%- set velodyne_plugin_filename ="libMrsGazeboCommonResources_3DLidarGpuPlugin.so" -%} - {%- set sensor_type = "gpu_ray" -%} -{% else %} - {%- set velodyne_plugin_filename ="libMrsGazeboCommonResources_3DLidarPlugin.so" -%} - {%- 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() }} - - 0 0 0.0094 0 0 0 - - - 0.0188 - 0.062 - - - - - - - - 0 0 0.0643 0 0 0 - - - 0.0148 - 0.062 - - - - - - - - 0 0 0.03785 0 0 0 - - - 0.0381 - 0.058 - - - - - - - - {{ lidar_x }} {{ lidar_y }} {{ lidar_z }} {{ lidar_roll }} {{ lidar_pitch }} {{ lidar_yaw }} - {# 0 0 0 0 0 0 #} - false - {{ update_rate }} - - - - {{ samples }} - 1 - {{ -rad180 }} - {{ rad180 }} - - - {{ lasers }} - 1 - {{ -vfov_angle/2*rad180/180.0 }} - {{ vfov_angle/2*rad180/180.0 }} - - - - 0.1 - {{ max_range }} - 0.03 - - - - {{ frame_fcu }} - {{ frame_sensor }} - {{ x + lidar_x }} - {{ y + lidar_y }} - {{ z + lidar_z }} - {{ roll }} - {{ pitch }} - {{ yaw }} - 0 - 0 - 0 - {{ lidar_roll }} - {{ lidar_pitch }} - {{ lidar_yaw }} - {{ topic_lidar }} - {{ topic_diag }} - {{ frame_lidar }} - 0.1 - {{ max_range }} - {{ noise }} - false +{# servo_camera_macro {--> #} +{%- macro servo_camera_macro(parent, offset_pitch_link_x, offset_pitch_link_y, offset_pitch_link_z, offset_pitch_link_yaw, offset_pitch_link_roll, offset_pitch_link_pitch, offset_roll_link_x, offset_roll_link_y, offset_roll_link_z, offset_roll_link_yaw, offset_roll_link_roll, offset_roll_link_pitch, tilt_update_rate, max_pitch, min_pitch, max_pitch_rate, max_roll, min_roll, max_roll_rate, compensate_tilt_roll, compensate_tilt_pitch, parent_frame_name, sensor_base_frame_name, camera_frame_name, camera_update_rate, horizontal_fov, img_width, img_height, roll_link_mesh_file, pitch_link_mesh_file, mesh_scale, spawner_args) -%} + + {%- set spawner_keyword = 'enable-servo-camera' -%} + {%- set spawner_description = 'Add a camera on virtual servo to the vehicle' -%} + {%- set spawner_default_args = none -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + + 0 0 0 0 0 0 + {{ generic.zero_inertial_macro() }} + + 0 0 0 0 0 0 + + {%- if roll_link_mesh_file == '' -%} + + 0.01 0.01 0.01 + + {%- else -%} + + {{ roll_link_mesh_file }} + {{ mesh_scale }} + + {%- endif -%} + + + + + + + + + + + 0 0 0 0 0 0 + {{ generic.zero_inertial_macro() }} + + 0 0 0 0 0 0 + + {%- if pitch_link_mesh_file == '' -%} + + 0.01 0.01 0.01 + + {%- else -%} + + {{ pitch_link_mesh_file }} + {{ mesh_scale }} + + {%- endif -%} + + + + + + + {{ camera_update_rate }} + + {{ horizontal_fov }} + + {{ img_width }} + {{ img_height }} + + + 0.25 + 100 + + + gaussian + + 0.0 + 0.01 + + + + true + {{ camera_update_rate }} + servo_camera + image_raw + camera_info + {{ camera_frame_name }} + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + {{ parent_frame_name }} + {{ sensor_base_frame_name }} + + + + + + {{ tilt_update_rate }} + {{ max_pitch_rate }} + {{ max_pitch }} + {{ min_pitch }} + {{ max_roll_rate }} + {{ max_roll }} + {{ min_roll }} + servo_camera_pitch_joint + servo_camera_roll_joint + servo_camera_gimbal_link + {{ parent }} + {{ compensate_tilt_roll }} + {{ compensate_tilt_pitch }} - - + + + + + {{ parent }} + servo_camera_gimbal_link + {{ offset_roll_link_x }} {{ offset_roll_link_y }} {{ offset_roll_link_z }} {{ offset_roll_link_roll }} {{ offset_roll_link_pitch }} {{ offset_roll_link_yaw }} + + 1 0 0 + + -0.5 + 0.5 + 1 + 10 + + + 0.0 + 0.0 + + + + + + servo_camera_gimbal_link + servo_camera_link + {{ offset_pitch_link_x }} {{ offset_pitch_link_y }} {{ offset_pitch_link_z }} {{ offset_pitch_link_roll }} {{ offset_pitch_link_pitch }} {{ offset_pitch_link_yaw }} + + 0 1 0 + + -1.57 + 1.57 + 1 + 10 + + + 0.0 + 0.0 + + + + + + + + {%- endif -%} +{%- endmacro -%} +{# #} - - {{ parent_link }} - velodyne_link - +{# TODO: Thermal camera plugin is not core #} +{# thermal_cameras_macro {--> #} +{%- macro thermal_cameras_macro(cameras_list, parent_link, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-thermal-cameras' -%} + {%- set spawner_description = 'Add several thermal cameras to the vehicle' -%} + {%- set spawner_default_args = {'image_width': 32, 'image_height': 32, 'update_rate': 14} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- if cameras_list | length > 0 -%} + + {%- for camera in cameras_list -%} + + {{ generic.thermal_camera_macro( + camera_name = camera['name'], + camera_topic_name = '/' + spawner_args['name'] + '/thermal/' + camera['name'], + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = 'thermal/' + camera['name'] + '_optical', + sensor_base_frame_name = spawner_args['name'] + '/thermal/' + camera['name'], + parent_link = parent_link, + frame_rate = spawner_args[spawner_keyword]['update_rate'], + hfov = 0.575959, + image_width = spawner_args[spawner_keyword]['image_width'] | int, + image_height = spawner_args[spawner_keyword]['image_height'] | int, + x = camera['x'], + y = camera['y'], + z = camera['z'], + roll = camera['roll'], + pitch = camera['pitch'], + yaw = camera['yaw']) + }} + + {%- endfor -%} + + + {{ mount }} + + + + {%- endif -%} + + {%- endif -%} {%- endmacro -%} - +{# #} - - - -{%- 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. #} - -{# -- gazebo links -- #} -{%- set sensor_link = sensor_name + "_sensor_link" -%} - -{# -- frame names -- #} -{%- set frame_fcu = namespace + "/fcu" -%} -{%- set frame_sensor = namespace + "/" + sensor_name + "_sensor" -%} -{%- set frame_lidar = namespace + "/" + sensor_name + "_lidar" -%} -{%- set frame_imu = namespace + "/" + sensor_name + "_imu" -%} - -{# -- topics -- #} -{%- set topic_lidar = "/" + namespace + "/" + sensor_name + "_cloud_nodelet/points" -%} -{%- set topic_imu = "/" + namespace + "/" + sensor_name + "_cloud_nodelet/imu" -%} -{%- set topic_diag = "/" + namespace + "/" + sensor_name + "_cloud_nodelet/is_alive" -%} - -{# -- tf from sensor to lidar -- #} -{%- set lidar_x = 0 -%} -{%- set lidar_y = 0 -%} -{%- set lidar_z = 0.0344 -%} -{%- set lidar_roll = 0 -%} -{%- set lidar_pitch = 0 -%} -{%- set lidar_yaw = 0 -%} - -{# -- tf from sensor to imu -- #} -{%- set imu_x = 0.006253 -%} -{%- set imu_y = -0.011775 -%} -{%- set imu_z = 0.007645 -%} -{%- set imu_roll = 0 -%} -{%- set imu_pitch = 0 -%} -{%- set imu_yaw = 0 -%} - -{% if enable_gpu_ray %} - {%- set ouster_plugin_filename ="libMrsGazeboCommonResources_3DLidarGpuPlugin.so" -%} - {%- set sensor_type = "gpu_ray" -%} -{% else %} - {%- set ouster_plugin_filename ="libMrsGazeboCommonResources_3DLidarPlugin.so" -%} - {%- 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() }} - - 0 0 0 0 0 {{ rad90 }} - - - model://mrs_robots_description/meshes/sensors/os1_64.dae - 1 1 1 - - - - - - - - 0 0 {{ lidar_z }} 0 0 - - - 0.035 - 0.038 - - - - - - - - {# IMU #} - - {{ imu_x }} {{ imu_y }} {{ imu_z }} {{ imu_roll }} {{ imu_pitch }} {{ imu_yaw }} - true - true - 100 - false - __default_topic__ - - - {{ topic_imu }} - {{ sensor_link }} - 100 - 0.005 - 0 0 0 - 0 0 0 - {{ frame_imu }} - - - - - - - - - - - - - +{# basler_camera_down_macro {--> #} +{%- macro basler_camera_down_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-basler-camera-down' -%} + {%- set spawner_description = 'Add a Basler Dart camera pointing down' -%} + {%- set spawner_default_args = {'image_width': 1920, 'image_height': 1080, 'update_rate': 30, 'hfov': 1.990252, 'noise': 0.007} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + frame_rate = spawner_args[spawner_keyword]['update_rate'], + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + horizontal_fov = spawner_args[spawner_keyword]['hfov'], + image_width = spawner_args[spawner_keyword]['image_width'], + image_height = spawner_args[spawner_keyword]['image_height'], + min_distance = 0.02, + max_distance = 80, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['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) + }} + - {# LIDAR #} - - {{ lidar_x }} {{ lidar_y }} {{ lidar_z }} {{ lidar_roll }} {{ lidar_pitch }} {{ lidar_yaw }} - {# 0 0 0 0 0 0 #} - false - {{ update_rate }} - - - - {{ samples }} - 1 - 0 - {{ 2*pi }} - - - {{ lasers }} - 1 - {{ -vfov_angle/2*rad180/180.0 }} - {{ vfov_angle/2*rad180/180.0 }} - - - - 0.1 - {{ max_range }} - 0.03 - - - - {{ frame_fcu }} - {{ frame_sensor }} - {{ x + lidar_x }} - {{ y + lidar_y }} - {{ z + lidar_z }} - {{ roll + lidar_roll }} - {{ pitch + lidar_pitch }} - {{ yaw + lidar_yaw }} - - - {{ frame_lidar }} - 0 - 0 - 0 - 0 - 0 - {{ rad180 }} - {{ topic_lidar }} - {{ topic_diag }} - 0.1 - {{ max_range }} - true - {{ noise }} - - - true - {{ frame_imu }} - {{ imu_x - lidar_x }} - {{ imu_y - lidar_y }} - {{ imu_z - lidar_z }} - {{ imu_roll - lidar_roll }} - {{ imu_pitch - lidar_pitch }} - {{ imu_yaw - lidar_yaw }} - - - + + {{ mount }} + - - {{ parent_link }} - {{ sensor_link }} - + + + {%- endif -%} {%- endmacro -%} - +{# #} + +{# ========================= UVDAR components ======================= #} + +{# dual_uv_cameras_macro {--> #} +{%- macro dual_uv_cameras_macro(parent_link, x1, y1, z1, roll1, pitch1, yaw1, x2, y2, z2, roll2, pitch2, yaw2, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-dual-uv-cameras' -%} + {%- set spawner_description = 'Add right and left UV cameras to the vehicle. Requires param "calib_file" to be set. Setting param "occlusions" to True requires a BEEFY computer.' -%} + {%- set spawner_default_args = {'occlusions': False, 'calib_file': none, 'update_rate': 60} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- if spawner_args[spawner_keyword]['calib_file'] is not none -%} + + {{ generic.uvcam_macro( + parent_link = parent_link, + camera_publish_topic = '/' + spawner_args['name'] + '/uvdar_bluefox_left/image_raw', + calibration_file = spawner_args[spawner_keyword]['calib_file'], + occlusions = spawner_args[spawner_keyword]['occlusions'], + frame_rate = spawner_args[spawner_keyword]['update_rate'], + device_id = spawner_args['name'] + '_1', + x = x1, + y = y1, + z = z1, + roll = roll1, + pitch = pitch1, + yaw = yaw1) + }} + + {{ generic.uvcam_macro( + parent_link = parent_link, + camera_publish_topic = '/' + spawner_args['name'] + '/uvdar_bluefox_right/image_raw', + calibration_file = spawner_args[spawner_keyword]['calib_file'], + occlusions = spawner_args[spawner_keyword]['occlusions'], + frame_rate = spawner_args[spawner_keyword]['update_rate'], + device_id = spawner_args['name'] + '_2', + x = x2, + y = y2, + z = z2, + roll = roll2, + pitch = pitch2, + yaw = yaw2) + }} + + + {{ mount }} + + + + {%- endif -%} -{%- 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 #} - -{# #} -{# #} -{% if ouster_model == 'OS0-32' %} - {%- set lasers = 32 -%} - {%- set vfov_angle = 90 -%} - {%- set range = 55 -%} -{% endif %} - -{# #} -{% if ouster_model == 'OS0-64' %} - {%- set lasers = 64 -%} - {%- set vfov_angle = 90 -%} - {%- set range = 55 -%} -{% endif %} - -{# #} -{% if ouster_model == 'OS0-128' %} - {%- set lasers = 128 -%} - {%- set vfov_angle = 90 -%} - {%- set range = 55 -%} -{% endif %} + {%- endif -%} +{%- endmacro -%} {# #} -{# #} -{# #} -{% if ouster_model == 'OS1-16' %} - {%- set lasers = 16 -%} - {%- set vfov_angle = 33.2 -%} - {%- set range = 120 -%} -{% endif %} +{# back_uv_camera_macro {--> #} +{%- macro back_uv_camera_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-back-uv-camera' -%} + {%- set spawner_description = 'Add a back UV camera to the vehicle. Requires param "calib_file" to be set' -%} + {%- set spawner_default_args = {'occlusions': False, 'calib_file': none, 'update_rate': 60} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + {{ generic.uvcam_macro( + parent_link = parent_link, + camera_publish_topic = '/' + spawner_args['name'] + '/uvdar_bluefox_back/image_raw', + calibration_file = spawner_args[spawner_keyword]['calib_file'], + occlusions = spawner_args[spawner_keyword]['occlusions'], + frame_rate = spawner_args[spawner_keyword]['update_rate'], + device_id = spawner_args['name'] + '_3', + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + {{ mount }} + + + + {%- endif -%} +{%- endmacro -%} {# #} -{# #} -{# #} -{% if ouster_model == 'OS1-32' %} - {%- set lasers = 32 -%} - {%- set vfov_angle = 45 -%} - {%- set range = 120 -%} -{% endif %} - -{# #} -{% if ouster_model == 'OS1-64' %} - {%- set lasers = 64 -%} - {%- set vfov_angle = 45 -%} - {%- set range = 120 -%} -{% endif %} - -{# #} -{% if ouster_model == 'OS1-128' %} - {%- set lasers = 128 -%} - {%- set vfov_angle = 45 -%} - {%- set range = 120 -%} -{% endif %} +{# downward_uv_camera_macro {--> #} +{%- macro downward_uv_camera_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-downward-uv-camera' -%} + {%- set spawner_description = 'Add a UV camera to the vehicle pointed down. Requires param "calib_file" to be set' -%} + {%- set spawner_default_args = {'occlusions': False, 'calib_file': none, 'update_rate': 60} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + {{ generic.uvcam_macro( + parent_link = parent_link, + camera_publish_topic = '/' + spawner_args['name'] + '/uvdar_bluefox_bottom/image_raw', + calibration_file = spawner_args[spawner_keyword]['calib_file'], + occlusions = spawner_args[spawner_keyword]['occlusions'], + frame_rate = spawner_args[spawner_keyword]['update_rate'], + device_id = spawner_args['name'] + '_3', + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + {{ mount }} + + + + {%- endif -%} +{%- endmacro -%} {# #} - - -{% if ouster_model == 'OS2-32' %} - {%- set lasers = 32 -%} - {%- set vfov_angle = 22.5 -%} - {%- set range = 240 -%} -{% endif %} - - -{% if ouster_model == 'OS2-64' %} - {%- set lasers = 64 -%} - {%- set vfov_angle = 22.5 -%} - {%- set range = 240 -%} -{% endif %} - - -{% if ouster_model == 'OS2-128' %} - {%- set lasers = 128 -%} - {%- set vfov_angle = 22.5 -%} - {%- set range = 240 -%} -{% endif %} +{# uv_leds_macro {--> #} +{%- macro uv_leds_macro(parent_link, x1, x2, y1, y2, z, spawner_args) -%} + + {%- set spawner_keyword = 'enable-uv-leds' -%} + {%- set spawner_description = 'Add UV LEDs to the vehicle. Requires param "signal_id" to be set. Legacy params "left_id" and "right_id" may replace "signal_id" (for backwards compatibility)' -%} + {%- set spawner_default_args = {'signal_id': none, 'left_id': none, 'right_id': none} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {# setup led signals {--> #} + {%- if spawner_args[spawner_keyword]['signal_id'] is not none and spawner_args[spawner_keyword]['signal_id'] | length > 3 -%} + + {%- set led1 = spawner_args[spawner_keyword]['signal_id'][0] -%} + {%- set led2 = spawner_args[spawner_keyword]['signal_id'][1] -%} + {%- set led4 = spawner_args[spawner_keyword]['signal_id'][3] -%} + {%- set led3 = spawner_args[spawner_keyword]['signal_id'][2] -%} + + {%- elif spawner_args[spawner_keyword]['left_id'] is not none and spawner_args[spawner_keyword]['right_id'] is not none -%} + + {%- set led1 = spawner_args[spawner_keyword]['left_id'] -%} + {%- set led2 = spawner_args[spawner_keyword]['right_id'] -%} + {%- set led3 = spawner_args[spawner_keyword]['left_id'] -%} + {%- set led4 = spawner_args[spawner_keyword]['right_id'] -%} + + {%- endif -%} + {# #} + + {%- if led1 is not none and led2 is not none and led3 is not none and led4 is not none -%} + + {# -- leds configuration -- (id, signal_id, x, y, z, roll, pitch, yaw) #} + {%- set uv_leds_macro_parameters = [(1, led1, x1, y1, z, 0.0, math.radians(90), 0.0), + (2, led1, x2, y2, z, 0.0, math.radians(90), math.radians(90)), + (3, led2, x2, -y2, z, 0.0, math.radians(90), -math.radians(90)), + (4, led2, x1, -y1, z, 0.0, math.radians(90), 0.0), + (5, led3, -x2, y2, z, 0.0, math.radians(90), math.radians(90)), + (6, led3, -x1, y1, z, 0.0, math.radians(90), math.radians(180)), + (7, led4, -x1, -y1, z, 0.0, math.radians(90), -math.radians(180)), + (8, led4, -x2, -y2, z, 0.0, math.radians(90), -math.radians(90))] -%} + + + {%- for id_, signal_id_, x_, y_, z_, roll_, pitch_, yaw_ in uv_leds_macro_parameters -%} + + {{ generic.uvled_macro( + parent_link = parent_link, + device_id = spawner_args['name'] + '_' + id_ | string(), + signal_id = signal_id_, + x = x_, + y = y_, + z = z_, + roll = roll_, + pitch = pitch_, + yaw = yaw_) + }} + + {%- endfor -%} + - - + {%- endif -%} -{{ generic_ouster_macro( - namespace = namespace, - parent_link = parent_link, - sensor_name = sensor_name, - 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, - roll = roll, - pitch = pitch, - yaw = yaw) -}} + {%- endif -%} {%- endmacro -%} +{# #} - +{# uv_leds_beacon_macro {--> #} +{%- macro uv_leds_beacon_macro(parent_link, x1, x2, y1, y2, z, spawner_args) -%} - - - -{%- macro realsense_macro(namespace, camera_name, camera_suffix, parent_link, enable_realistic_realsense, x, y, z, roll, pitch, yaw) -%} -{# -- frame names -- #} -{%- set frame_fcu = namespace + "/fcu" -%} - - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - 0 0 0 {{ -rad90 }} 0 {{ -rad90 }} - - - model://mrs_robots_description/meshes/sensors/realsense_body.stl - 0.001 0.001 0.001 - - - - - - - - 0 0 0 {{ -rad90 }} 0 {{ -rad90 }} - - - model://mrs_robots_description/meshes/sensors/realsense_glass.stl - 0.001 0.001 0.001 - - - - - - - - - - - {# 0 -0.046 0.004 0 0 0 #} - 0 0 -0.0115 0 0 0 - - {# 1.211259 #} - 1.211259 - - 1280 - 720 - RGB_INT8 - - - 0.1 - 100 - - - gaussian - 0.0 - 0.007 - - - 1 - 60 - 0 - - + {%- set spawner_keyword = 'enable-uv-leds-beacon' -%} + {%- set spawner_description = 'Add UV LED beacon to the top of the vehicle. Requires param "signal_id" to be set.' -%} + {%- set spawner_default_args = {'signal_id': none} -%} - - - 1 - 30 - 0 - - {# 1.7523106 #} - 1.211259 - - 640 - 360 - L_INT8 - - - 0.1 - 50 - - - gaussian - 0.0 - 0.005 - - - - 0 0.05 0 0 0 0 - {# 1.7523106 #} - 1.211259 - - 640 - 360 - L_INT8 - - - 0.1 - 50 - - - gaussian - 0.0 - 0.005 - - - - + {%- if spawner_keyword in spawner_args.keys() -%} - - - - - - - - - - - - - - - - - - - - - - - - - - - - + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} - - - 0 -0.0115 0.0 0 0 0 - - 1.211259 - - {% if enable_realistic_realsense %} - 320 - 180 - {% else %} - 1280 - 720 - {% endif %} - - - 0.3 - 12 - - - gaussian - 0.0 - 5000.0 - - - 1 - 60 - 0 - - + {%- if spawner_args[spawner_keyword]['signal_id'] is not none -%} - + {# setup led signals {--> #} + {# -- leds configuration -- (id, signal_id, x, y, z, roll, pitch, yaw) #} + {%- set uv_leds_macro_parameters = [('b1', spawner_args[spawner_keyword]['signal_id'], x1, y1, z, 0.0, math.radians(90), 0.0), + ('b2', spawner_args[spawner_keyword]['signal_id'], x2, y2, z, 0.0, math.radians(90), math.radians(90)), + ('b3', spawner_args[spawner_keyword]['signal_id'], -x1, -y1, z, 0.0, math.radians(90), math.radians(180)), + ('b4', spawner_args[spawner_keyword]['signal_id'], -x2, -y2, z, 0.0, math.radians(90), math.radians(270))] -%} - - {{ camera_name }} - {{ camera_suffix }} - {{ enable_realistic_realsense }} - 0.2 - 4.0 - 0.8 - 0.2 - 4 - 15 - 5 - {{ frame_fcu }} - {{ x }} - {{ y }} - {{ z }} - {{ roll }} - {{ pitch }} - {{ yaw }} - - - - {{ parent_link }} - {{ camera_name }}_link - + {# #} + + {%- for id_, signal_id_, x_, y_, z_, roll_, pitch_, yaw_ in uv_leds_macro_parameters -%} + + {{ generic.uvled_macro( + parent_link = parent_link, + device_id = spawner_args['name'] + '_' + id_ | string(), + signal_id = signal_id_, + x = x_, + y = y_, + z = z_, + roll = roll_, + pitch = pitch_, + yaw = yaw_) + }} + + {%- endfor -%} + + {%- endif -%} + + {%- endif -%} {%- endmacro -%} - +{# #} + - -{%- macro bluefox_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 = 752, - image_height = 480, - min_distance = 0.02, - max_distance = 80, - noise_mean = 0.0, - noise_stddev = noise, - mesh_file = "model://mrs_robots_description/meshes/sensors/bluefox.dae", - mesh_scale = "1 1 1", - color = "DarkGrey", - visual_x = 0, - visual_y = 0, - visual_z = 0, - visual_roll = rad90, - visual_pitch = 0, - visual_yaw = 0, - x = x, - y = y, - z = z, - roll = roll, - pitch = pitch, - yaw = yaw) -}} +{# ========================== VIO components ======================== #} + +{# vio_macro {--> #} +{%- macro vio_macro(sensor_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-vio' -%} + {%- set spawner_description = 'Add a forward-looking fisheye camera and a high-frequency IMU' -%} + {%- set spawner_default_args = {'camera_rate': 45, 'camera_noise': 0.0, 'imu_rate': 100, 'imu_noise': 0.0} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.fisheye_camera_macro( + camera_name = sensor_name, + topic_ns_prefix = 'camera/', + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/' + sensor_name + '_optical', + sensor_base_frame_name = spawner_args['name'] + '/' + sensor_name, + parent_link = parent_link, + frame_rate = spawner_args[spawner_keyword]['camera_rate'], + horizontal_fov = math.radians(180), + image_width = 752, + image_height = 480, + lens_type = 'custom', + lens_c1 = 1.05, + lens_c2 = 4, + lens_f = 1.0, + lens_fun = 'tan', + lens_scale = True, + lens_cutoff_angle = math.radians(90), + lens_texture_size = 512, + min_distance = 0.02, + max_distance = 40, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['camera_noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = 0, + visual_y = 0, + visual_z = 0, + visual_roll = math.radians(90), + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ generic.imu_sensor_macro( + sensor_name = sensor_name + '_imu', + parent_link = parent_link, + update_rate = spawner_args[spawner_keyword]['imu_rate'], + topic_name = '/' + spawner_args['name'] + '/' + sensor_name + '/imu', + frame_name = spawner_args['name'] + '/' + sensor_name, + noise_mean = spawner_args[spawner_keyword]['imu_noise'], + x = 0, + y = 0, + z = 0, + roll = -math.radians(90), + pitch = 0, + yaw = -math.radians(90)) + }} + + + + {{ mount }} + + + + + {%- endif -%} {%- endmacro -%} - +{# #} + +{# vio_down_macro {--> #} +{%- macro vio_down_macro(sensor_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-vio-down' -%} + {%- set spawner_description = 'Add a downward-looking fisheye camera and a high-frequency IMU' -%} + {%- set spawner_default_args = {'camera_rate': 45, 'camera_noise': 0.0, 'imu_rate': 100, 'imu_noise': 0.0} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.fisheye_camera_macro( + camera_name = sensor_name, + topic_ns_prefix = 'camera/', + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/' + sensor_name + '_optical', + sensor_base_frame_name = spawner_args['name'] + '/' + sensor_name, + parent_link = parent_link, + frame_rate = spawner_args[spawner_keyword]['camera_rate'], + horizontal_fov = math.radians(180), + image_width = 752, + image_height = 480, + lens_type = 'custom', + lens_c1 = 1.05, + lens_c2 = 4, + lens_f = 1.0, + lens_fun = 'tan', + lens_scale = True, + lens_cutoff_angle = math.radians(90), + lens_texture_size = 512, + min_distance = 0.02, + max_distance = 40, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['camera_noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = 0, + visual_y = 0, + visual_z = 0, + visual_roll = math.radians(90), + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ generic.imu_sensor_macro( + sensor_name = sensor_name + '_imu', + parent_link = parent_link, + update_rate = spawner_args[spawner_keyword]['imu_rate'], + topic_name = '/' + spawner_args['name'] + '/' + sensor_name + '/imu', + frame_name = spawner_args['name'] + '/' + sensor_name, + noise_mean = spawner_args[spawner_keyword]['imu_noise'], + x = 0, + y = 0, + z = 0, + roll = -math.radians(90), + pitch = 0, + yaw = -math.radians(90)) + }} + + + + {{ mount }} + - -{%- 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) -}} + + + {%- endif -%} {%- endmacro -%} - +{# #} + +{# ==================== miscellaneous components ==================== #} + +{# light_macro {--> #} +{%- macro light_macro(parent_link, max_pitch_rate, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-light' -%} + {%- set spawner_description = 'Add a light on a gimbal to the vehicle. Requires a Gazebo world with enabled shadows' -%} + {%- set spawner_default_args = {'update_rate': 30, 'compensate_tilt': True, 'initial_on': True} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_colored_box_macro('light', 0.01, 0.01, 0.01, 'Yellow', 0, 0, 0, 0, 0, 0) }} + + + + {{ x }} + {{ y }} + {{ z }} + {{ roll }} + {{ pitch }} + {{ yaw }} + {{ parent_link }} + {{ spawner_args[spawner_keyword]['update_rate'] }} + {{ max_pitch_rate }} + {{ spawner_args[spawner_keyword]['initial_on'] }} + {{ spawner_args[spawner_keyword]['compensate_tilt'] }} + - -{%- macro fisheye_camera_macro(namespace, camera_name, topic_ns_prefix, parent_link, frame_rate, noise, x, y, z, roll, pitch, yaw) -%} -{{ fisheye_macro( - camera_name = camera_name, - topic_ns_prefix = topic_ns_prefix, - parent_frame_name = namespace + "/fcu", - camera_frame_name = namespace + "/" + camera_name + "_optical", - sensor_base_frame_name = namespace + "/" + camera_name, - parent_link = parent_link, - frame_rate = frame_rate, - horizontal_fov = rad180, - image_width = 752, - image_height = 480, - lens_type = "custom", - lens_c1 = 1.05, - lens_c2 = 4, - lens_f = 1.0, - lens_fun = "tan", - lens_scale = True, - lens_cutoff_angle = rad90, - lens_texture_size = 512, - min_distance = 0.02, - max_distance = 40, - noise_mean = 0.0, - noise_stddev = noise, - mesh_file = "model://mrs_robots_description/meshes/sensors/bluefox.dae", - mesh_scale = "1 1 1", - color = "DarkGrey", - visual_x = 0, - visual_y = 0, - visual_z = 0, - visual_roll = rad90, - visual_pitch = 0, - visual_yaw = 0, - x = x, - y = y, - z = z, - roll = roll, - pitch = pitch, - yaw = yaw) -}} + + {{ parent_link }} + light_macro_link + + 0 1 0 + + -1.57 + 1.57 + 1 + 10 + + + 0.0 + 0.0 + + + + + + {%- endif -%} +{%- endmacro -%} + + +{# magnet_gripper_visualization_macro {--> #} +{%- macro magnet_gripper_visualization_macro(parent_link, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-magnetic-gripper' -%} + {%- set spawner_description = 'Add a magnetic gripper visualization' -%} + {%- set spawner_default_args = none %} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_colored_box_macro('front', 0.005, 0.05, 0.06, 'Black', -0.09, 0, 0.03, 0, 0, 0) }} + {{ generic.visual_colored_box_macro('back', 0.005, 0.05, 0.06, 'Black', 0.09, 0, 0.03, 0, 0, 0) }} + {{ generic.visual_colored_box_macro('down', 0.18, 0.05, 0.005, 'Black', 0, 0, 0, 0, 0, 0) }} + {{ generic.visual_colored_box_macro('up', 0.18, 0.05, 0.005, 'Black', 0, 0, 0.06, 0, 0, 0) }} + + 0 0 0 0 0 0 + + + 0.18 0.05 0.005 + + + + + + + {{ parent_link }} + magnet_gripper_visualization_link + + + + {%- endif -%} {%- endmacro -%} - +{# #} + +{# omni_ultrasounds_macro {--> #} +{%- macro omni_ultrasounds_macro(sensors_list, parent_link, spawner_args) -%} + + {%- set spawner_keyword = 'enable-omni-ultrasounds' -%} + {%- set spawner_description = 'Add omnidirectional ultrasonic sensors (up, down, 4 in horizontal plane)' -%} + {%- set spawner_default_args = none -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {%- for sensor in sensors_list -%} + + + {{ generic.ultrasonic_sensor_macro( + namespace = spawner_args['name'], + parent_link = parent_link, + suffix = sensor['suffix'], + x = sensor['x'], + y = sensor['y'], + z = sensor['z'], + roll = sensor['roll'], + pitch = sensor['pitch'], + yaw = sensor['yaw']) + }} + - -{%- macro mobius_camera_macro(namespace, camera_name, parent_link, frame_rate, 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 = 2.28638, - image_width = 1280, - image_height = 720, - min_distance = 0.02, - max_distance = 40, - noise_mean = 0.0, - noise_stddev = 0.007, - mesh_file = "model://mrs_robots_description/meshes/sensors/mobius.dae", - mesh_scale = "1 1 1", - color = "DarkGrey", - visual_x = -0.004, - visual_y = 0.0045, - visual_z = 0, - visual_roll = 0, - visual_pitch = 0, - visual_yaw = 0, - x = x, - y = y, - z = z, - roll = roll, - pitch = pitch, - yaw = yaw) -}} + {%- endfor -%} + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# pendulum_macro {--> #} +{%- macro pendulum_macro(parent_link, x, y, z, spawner_args) -%} + + {%- set spawner_keyword = 'enable-pendulum' -%} + {%- set spawner_description = 'Add a pendulum to the vehicle. Specify the length and mass of the whole chain. Length and mass is uniformly distributed between the links' -%} + {%- set spawner_default_args = {'chain_length': 3.0, 'chain_mass': 0.5, 'num_links': 30, 'radius': 0.01} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ generic.single_chain_pendulum_macro( + parent_link = parent_link, + id = 0, + link_mass = spawner_args[spawner_keyword]['chain_mass'] / spawner_args[spawner_keyword]['num_links'], + link_radius = spawner_args[spawner_keyword]['radius'], + link_length = spawner_args[spawner_keyword]['chain_length'] / spawner_args[spawner_keyword]['num_links'], + joint_offset_x = x, + joint_offset_y = y, + joint_offset_z = z) + }} + + + {%- for id_ in range(1, spawner_args[spawner_keyword]['num_links'] | int) -%} + + + {{ generic.single_chain_pendulum_macro( + parent_link = 'pendulum_chain_' + (id_ - 1)| string() + '_link', + id = id_, + link_mass = spawner_args[spawner_keyword]['chain_mass'] / spawner_args[spawner_keyword]['num_links'], + link_radius = spawner_args[spawner_keyword]['radius'], + link_length = spawner_args[spawner_keyword]['chain_length'] / spawner_args[spawner_keyword]['num_links'], + joint_offset_x = 0, + joint_offset_y = 0, + joint_offset_z = -0.5 * (spawner_args[spawner_keyword]['chain_length'] / spawner_args[spawner_keyword]['num_links'])) + }} + + + {%- endfor -%} + + + {%- endif -%} {%- endmacro -%} - -{%- macro thermal_camera_macro(camera_name, camera_topic_name, parent_frame_name, camera_frame_name, sensor_base_frame_name, parent_link, frame_rate, hfov, image_width, image_height, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - 0 0 0 0 0 0 - - - mrs_robots_description/meshes/sensors/teraranger_evo_thermal_33.dae - 0.001 0.001 0.001 - - - - - - - - {{ frame_rate }} - - {{ hfov }} - - R8G8B8 - {{ 3 * image_width }} - {{ 3 * image_height }} - - - 0.1 - 300 - - - gaussian - 0.0 - 0.0 - - - - true - {{ camera_name }} - {{ frame_rate }} - {{ camera_topic_name }}/rgb_image - {{ camera_topic_name }}/camera_info - {{ camera_topic_name }}/raw_temp_array - 20 - 150 - 0.2 - 4.0 - 20.0 - {{ camera_frame_name }} - {{ parent_frame_name }} - {{ sensor_base_frame_name }} +{# safety_led_macro {--> #} +{%- macro safety_led_macro(parent_link, failure_duration_threshold, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-safety-led' -%} + {%- set spawner_description = 'Add a safety LED' -%} + {%- set spawner_default_args = none -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + + + safety_led + {{ failure_duration_threshold }} + 5.0 {{ x }} {{ y }} - {{ z }} + {{ z }}} {{ roll }} {{ pitch }} {{ yaw }} - - - - {{ parent_link }} - {{ camera_name }}_link - + + {{ parent_link }} + safety_led_link + + + + {%- endif -%} {%- endmacro -%} - +{# #} + +{# TODO: Timepix is not core #} +{# Timepix detector {--> #} +{%- macro timepix_macro(parent_link, sensor_name, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-timepix' -%} + {%- set spawner_description = 'Add a Timepix radiation detector to the vehicle' -%} + {%- set spawner_default_args = {'material': 'si', 'exposition_time': 0.1, 'thickness': 0.0003} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_colored_box_macro(sensor_name, 0.02, 0.09, 0.02, 'Green', 0, 0.05, 0, 0, 0, 0) }} + - -{%- macro uvcam_macro(parent_link, calibration_file, occlusions, frame_rate, device_id, camera_publish_topic, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - 0 0 0 {{ rad90 }} 0 0 - - - model://mrs_robots_description/meshes/sensors/bluefox.dae - 1 1 1 - - - - - - - - 1 - - false - {{ frame_rate }} - {{ occlusions }} - {{ frame_rate }} - {{ calibration_file }} - {{ device_id }} - {{ camera_publish_topic }} + + {{ spawner_args[spawner_keyword]['exposition_time'] }} + {{ spawner_args[spawner_keyword]['material'] }} + {{ spawner_args[spawner_keyword]['thickness'] }} 0.01408 0.01408 - - - - {{ parent_link }} - uvcam_{{ device_id }}_link - + + {{ parent_link }} + {{ sensor_name }}_link + + + + {%- endif -%} + {%- endmacro -%} - -{%- macro servo_camera_macro(parent, offset_pitch_link_x, offset_pitch_link_y, offset_pitch_link_z, offset_pitch_link_yaw, offset_pitch_link_roll, offset_pitch_link_pitch, offset_roll_link_x, offset_roll_link_y, offset_roll_link_z, offset_roll_link_yaw, offset_roll_link_roll, offset_roll_link_pitch, tilt_update_rate, max_pitch, min_pitch, max_pitch_rate, max_roll, min_roll, max_roll_rate, compensate_tilt_roll, compensate_tilt_pitch, parent_frame_name, sensor_base_frame_name, camera_frame_name, camera_update_rate, horizontal_fov, img_width, img_height, roll_link_mesh_file, pitch_link_mesh_file, mesh_scale) -%} - - - 0 0 0 0 0 0 - {{ zero_inertial_macro() }} - - 0 0 0 0 0 0 - - {% if roll_link_mesh_file == '' %} - - 0.01 0.01 0.01 - - {% else %} - - {{ roll_link_mesh_file }} - {{ mesh_scale }} - - {% endif %} - - - - - - +{# TODO: Timepix3 is not core #} +{# Timepix3 detector {--> #} +{%- macro timepix3_macro(parent_link, sensor_name, sensor_suffix, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-timepix3' -%} + {%- set spawner_description = 'Add a Timepix3 gamma event camera to the vehicle' -%} + {%- set spawner_default_args = {'material': 'cdte', 'max_message_window': 1.0, 'thickness': 0.002} %} - - 0 0 0 0 0 0 - {{ zero_inertial_macro() }} - - 0 0 0 0 0 0 - - {% if pitch_link_mesh_file == '' %} - - 0.01 0.01 0.01 - - {% else %} - - {{ pitch_link_mesh_file }} - {{ mesh_scale }} - - {% endif %} - - - - - - - {{ camera_update_rate }} - - {{ horizontal_fov }} - - {{ img_width }} - {{ img_height }} - - - 0.25 - 100 - - - gaussian - - 0.0 - 0.01 - - - - true - {{ camera_update_rate }} - servo_camera - image_raw - camera_info - {{ camera_frame_name }} - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - {{ parent_frame_name }} - {{ sensor_base_frame_name }} + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_colored_box_macro(sensor_name, 0.02, 0.09, 0.02, 'Green', 0.05, 0, 0, 0, 0, 0) }} + + + + {{ material }} + {{ spawner_args[spawner_keyword]['thickness'] }} 0.01408 0.01408 + {{ spawner_args[spawner_keyword]['max_message_window'] }} + {{ sensor_suffix }} - - - - {{ tilt_update_rate }} - {{ max_pitch_rate }} - {{ max_pitch }} - {{ min_pitch }} - {{ max_roll_rate }} - {{ max_roll }} - {{ min_roll }} - servo_camera_pitch_joint - servo_camera_roll_joint - servo_camera_gimbal_link - {{ parent }} - {{ compensate_tilt_roll }} - {{ compensate_tilt_pitch }} - - - - {{ parent }} - servo_camera_gimbal_link - {{ offset_roll_link_x }} {{ offset_roll_link_y }} {{ offset_roll_link_z }} {{ offset_roll_link_roll }} {{ offset_roll_link_pitch }} {{ offset_roll_link_yaw }} - - 1 0 0 - - -0.5 - 0.5 - 1 - 10 - - - 0.0 - 0.0 - - - + + {{ parent_link }} + {{ sensor_name }}_link + + + + {%- endif -%} + +{%- endmacro -%} + + +{# TODO: UWB plugin is not core #} +{# uwb_range_macro {--> #} +{%- macro uwb_range_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-uwb-range' -%} + {%- set spawner_description = 'Add Qorvo DW1000 UWB transceiver. Requires param "signal_id" to be set.' -%} + {%- set spawner_default_args = {'signal_id': none, 'update_rate': 10} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- if spawner_args[spawner_keyword]['signal_id'] is not none -%} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + {{ spawner_args[spawner_keyword]['update_rate'] }} + 1 + + {{ spawner_args[spawner_keyword]['signal_id'] }} + /{{ spawner_args['name'] }}/uwb_range/range + {{ spawner_args['name'] }}/uwb + -150 + 0.1 + + + 6489.6 + 6240.0 + 6739.2 + 14.5 + 2.5 + + + + + {{ parent_link }} + uwb_range_link + true + true + + + + {{ mount }} + - - servo_camera_gimbal_link - servo_camera_link - {{ offset_pitch_link_x }} {{ offset_pitch_link_y }} {{ offset_pitch_link_z }} {{ offset_pitch_link_roll }} {{ offset_pitch_link_pitch }} {{ offset_pitch_link_yaw }} - - 0 1 0 - - -1.57 - 1.57 - 1 - 10 - - - 0.0 - 0.0 - - - + -{%- endmacro -%} - + {%- endif -%} - -{%- macro vio_macro(namespace, sensor_name, parent_link, frame_rate, noise, imu_rate, imu_noise_mean, x, y, z, roll, pitch, yaw) -%} -{{ fisheye_camera_macro( - namespace = namespace, - camera_name = sensor_name, - topic_ns_prefix = "camera/", - parent_link = parent_link, - frame_rate = frame_rate, - noise = noise, - x = x, - y = y, - z = z, - roll = roll, - pitch = pitch, - yaw = yaw) -}} - -{{ custom_imu_macro( - sensor_name = sensor_name + "_imu", - parent_link = sensor_name + "_link", - update_rate = imu_rate, - topic_name = "/" + namespace + "/" + sensor_name + "/imu", - frame_name = namespace + "/" + sensor_name, - noise_mean = imu_noise_mean, - x = 0, - y = 0, - z = 0, - roll = -rad90, - pitch = 0, - yaw = -rad90) -}} + {%- endif -%} {%- endmacro -%} - +{# #} - - - - - -{%- macro teraranger_evo_macro(parent_link, id, visualize, frame_name, parent_frame_name, gaussian_noise, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - - - model://mrs_robots_description/meshes/sensors/teraranger_evo.stl - 1 1 1 - - - - - - - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ visualize }} - 120 - - - - 10 - 1 - -0.01750 - 0.01750 - - - 10 - 1 - -0.01750 - 0.01750 - - - - 0.5 - 60.0 - 0.05 - - - - /{{ frame_name }}/range_{{ id }} - {{ frame_name }}/range_{{ id }} - {{ parent_frame_name }} - {{ gaussian_noise }} - 20 - ultrasound - 0.035 - {{ x }} - {{ y }} - {{ z }} - {{ roll }} - {{ pitch }} - {{ yaw }} - - +{# water_gun_macro {--> #} +{%- macro water_gun_macro(parent_link, nozzle_offset_x, nozzle_offset_y, nozzle_offset_z, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-water-gun' -%} + {%- set spawner_description = 'Add a water gun for firefighting. Requires a dummy object (spawning_reservoir) to be present in the simulation' -%} + {%- set spawner_default_args = {'muzzle_velocity': 15.0, 'num_particles': 30, 'spread': 1.0, 'spawning_reservoir': 'the_void'} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_colored_box_macro('water_bag', 0.02, 0.08, 0.03, 'Blue', 0, 0, 0, 0, 0, 0) }} + + {{ nozzle_offset_x - 0.08/2 }} {{ nozzle_offset_y }} {{ nozzle_offset_z - 0.006/2 }} 0 {{ math.radians(90) }} 0 + + + 0.08 + 0.006 + + + + + + + + + + {{ spawner_args[spawner_keyword]['muzzle_velocity'] }} + {{ nozzle_offset_x }} + {{ nozzle_offset_y }} + {{ nozzle_offset_z }} + {{ spawner_args[spawner_keyword]['spread'] }} + {{ spawner_args[spawner_keyword]['num_particles'] }} + {{ spawner_args[spawner_keyword]['spawning_reservoir'] }} + + + + {{ parent_link }} + water_gun_link + + + + {%- endif -%} {%- endmacro -%} - +{# #} - -{%- macro teraranger_evo_tower_macro(parent_link, visualize, frame_name, parent_frame_name, gaussian_noise, x, y, z, roll, pitch, yaw) -%} -{# -- parameters of sensors -- (id, x, y, z, roll, pitch, yaw) #} -{%- set sensor_parameters = [(0, 0.046, 0.0, 0.001, 0.0, 0.0, 0.0), - (1, 0.032, 0.032, 0.001, 0.0, 0.0, rad45), - (2, 0.000, 0.046, 0.001, 0.0, 0.0, rad90), - (3, -0.032, 0.032, 0.001, 0.0, 0.0, rad135), - (4, -0.046, 0.000, 0.001, 0.0, 0.0, rad180), - (5, -0.032, -0.032, 0.001, 0.0, 0.0, -rad135), - (6, 0.000, -0.046, 0.001, 0.0, 0.0, -rad90), - (7, 0.032, -0.032, 0.001, 0.0, 0.0, -rad45)] -%} - - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - 0 0 -0.014 0 0 0 - - - 0.001 - 0.06 - - - - - - - -{% for id_, x_, y_, z_, roll_, pitch_, yaw_ in sensor_parameters %} - {{ teraranger_evo_macro( - parent_link = parent_link, - id = id_, - visualize = visualize, - frame_name = frame_name, - parent_frame_name = parent_frame_name, - gaussian_noise = gaussian_noise, - x = x_, - y = y_, - z = z_, - roll = roll_, - pitch = pitch_, - yaw = yaw_) - }} -{% endfor %} - +{# whycon_box_macro {--> #} +{%- macro whycon_box_macro(parent_link, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-whycon-box' -%} + {%- set spawner_description = 'Add a Whycon box for relative localization using visual markers' -%} + {%- set spawner_default_args = none -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- set mesh_file = 'model://mrs_robots_description/meshes/sensors/whycon_box.dae' -%} + {%- set mesh_scale = '1 1 1' -%} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_mesh_textured_macro("whycon_box", mesh_file, mesh_scale, x, y, z, roll, pitch, yaw) }} + + + + {{ parent_link }} + whycon_box_link + + + + {%- endif -%} - - {{ parent_link }} - teraranger_evo_tower_link - {%- endmacro -%} - - +{# ==================== Unsupported components ==================== #} + +{# TODO: not supported in the current version #} +{# TODO: not used #} +{# gps_satelites_blocking_macro {--> #} {%- macro gps_satelites_blocking_macro(parent_link) -%} -{# not supported in the current version #} - 0 0 0 0 {{ -rad90 }} 0 - {{ zero_inertial_macro() }} + 0 0 0 0 {{ -math.radians(90) }} 0 + {{ generic.zero_inertial_macro() }} false 2 @@ -2915,528 +2962,48 @@ limitations under the License. {%- endmacro -%} - -{%- macro magnet_gripper_visualization_macro(parent_link, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - -0.09 0 0.03 0 0 0 - - - 0.005 0.05 0.06 - - - - - - - - 0.09 0 0.03 0 0 0 - - - 0.005 0.05 0.06 - - - - - - - - 0 0 0 0 0 0 - - - 0.18 0.05 0.005 - - - - - - - - 0 0 0.06 0 0 0 - - - 0.18 0.05 0.005 - - - - - - - - 0 0 0 0 0 0 - - - 0.18 0.05 0.005 - - - - - - - {{ parent_link }} - magnet_gripper_visualization_link - -{%- endmacro -%} - - - -{%- macro timepix_macro(parent_link, name, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - 0 0.05 0 0 0 0 - - - 0.02 0.09 0.02 - - - - - - - - - - 0.1 - si - 0.0003 0.01408 0.01408 - - - - {{ parent_link }} - {{ name }}_link - -{%- endmacro -%} - - - -{%- macro timepix3_macro(parent_link, name, material, thickness, max_message_window, sensor_suffix, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - 0 0.05 0 0 0 0 - - - 0.02 0.09 0.02 - - - - - - - - - - {{ material }} - {{ thickness }} 0.01408 0.01408 - {{ max_message_window }} - {{ sensor_suffix }} - - - - {{ parent_link }} - {{ name }}_link - -{%- endmacro -%} - - - -{%- macro water_gun_macro(parent_link, muzzle_velocity, particle_capacity, spread, spawning_reservoir, nozzle_offset_x, nozzle_offset_y, nozzle_offset_z, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - 0 0 0 0 0 0 - - - 0.02 0.08 0.03 - - - - - - - - {{ nozzle_offset_x - 0.08/2 }} {{ nozzle_offset_y }} {{ nozzle_offset_z - 0.006/2 }} 0 {{ rad90 }} 0 - - - 0.08 - 0.006 - - - - - - - - - - {{ muzzle_velocity }} - {{ nozzle_offset_x }} - {{ nozzle_offset_y }} - {{ nozzle_offset_z }} - {{ spread }} - {{ particle_capacity }} - {{ spawning_reservoir }} - - - - {{ parent_link }} - water_gun_link - -{%- endmacro -%} - - - -{%- macro light_macro(parent_link, update_rate, max_pitch_rate, initial_on, compensate_tilt, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - 0 0 0 0 0 0 - - - 0.01 0.01 0.01 - - - - - - - - - - {{ x }} - {{ y }} - {{ z }} - {{ roll }} - {{ pitch }} - {{ yaw }} - {{ parent_link }} - {{ update_rate }} - {{ max_pitch_rate }} - {{ initial_on }} - {{ compensate_tilt }} - - - - {{ parent_link }} - light_macro_link - - 0 1 0 - - -1.57 - 1.57 - 1 - 10 - - - 0.0 - 0.0 - - - -{%- endmacro -%} - - - -{%- macro safety_led_macro(parent_link, failure_duration_threshold, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - - - safety_led - {{ failure_duration_threshold }} - 5.0 - {{ x }} - {{ y }} - {{ z }}} - {{ roll }} - {{ pitch }} - {{ yaw }} - - - - {{ parent_link }} - safety_led_link - -{%- endmacro -%} - - - -{%- macro uvled_macro(parent_link, device_id, signal_id , x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - 0 0 -0.0025 0 0 0 - - - 0.005 - 0.007 - - - - - - - - 0 0 -0.0025 0 0 0 - - - 0.001 0.02 0.001 - - - - - - - - 0 0 0 0 0 0 - - - 0.005 - - - - - - - - 1 - - false - 1 - {{ signal_id }} - {{ device_id }} - - - - - - {{ parent_link }} - uvled_{{ device_id }}_link - true - true - -{%- endmacro -%} - - - - - - - +{# TODO: not supported in the current version #} +{# TODO: not used #} +{# parachute_macro {--> #} {%- macro parachute_macro(parent_link, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - 0 0 0 0 0 0 - - - 0.1 - 0.048 - - - - - - - - - - 1.225 - 500 - 0.25 - 0 - 0 - -1.56 - - - - {{ parent_link }} - parachute_link - -{%- endmacro -%} - - - -{%- macro whycon_box_macro(parent_link, mesh_scale, x, y, z, roll, pitch, yaw) -%} - - {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} - {{ zero_inertial_macro() }} - - 0 0 0 0 0 0 - - - model://mrs_robots_description/meshes/sensors/whycon_box.dae - {{ mesh_scale }} - - - - - - {{ parent_link }} - whycon_box_link - -{%- endmacro -%} - + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + 0 0 0 0 0 0 + + + 0.1 + 0.048 + + + + + + + + + + 1.225 + 500 + 0.25 + 0 + 0 + -1.56 + - - - -{%- macro single_chain_pendulum_macro(parent_link, id, chain_mass, chain_radius, chain_length, joint_offset_x, joint_offset_y, joint_offset_z) -%} - - 0 0 {{ -chain_length/2 }} 0 0 0 - - {{ chain_mass }} - {{ - cylinder_inertia( - m = chain_mass, - r = chain_radius, - h = chain_length) - }} - - 1 - - 0 0 0 0 0 0 - - - {{ chain_length }} - {{ chain_radius }} - - - - - - - - 0 0 {{ chain_length/2 }} 0 0 0 - - - 0.012 - - - - - - - + + {{ parent_link }} + parachute_link + + - - {{ parent_link }} - pendulum_chain_{{ id }}_link - {{ joint_offset_x }} {{ joint_offset_y }} {{ joint_offset_z }} 0 0 0 - - 1 0 0 - - -3.1415 - 3.1415 - -1 - -1 - - - 0.0 - 0.0 - - - - 0 1 0 - - -3.1415 - 3.1415 - -1 - -1 - - - 0.0 - 0.0 - - - {%- endmacro -%} - +{# #} - -{%- macro pendulum_macro(parent_link, number_of_chains, chain_length, chain_radius, chain_mass, x, y, z) -%} -{{ single_chain_pendulum_macro( - parent_link = parent_link, - id = 0, - chain_mass = chain_mass, - chain_radius = chain_radius, - chain_length = chain_length, - joint_offset_x = x, - joint_offset_y = y, - joint_offset_z = z, - ) -}} - -{% for id_ in range(1, number_of_chains) %} - {{ single_chain_pendulum_macro( - parent_link = "pendulum_chain_" + (id_ - 1)| string() + "_link", - id = id_, - chain_mass = chain_mass, - chain_radius = chain_radius, - chain_length = chain_length, - joint_offset_x = 0, - joint_offset_y = 0, - joint_offset_z = -chain_length/2, - ) - }} -{% endfor %} -{%- endmacro -%} - - + diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja.orig b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja.orig new file mode 100644 index 0000000..fb47f16 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja.orig @@ -0,0 +1,3046 @@ + + + +{%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} + +{# !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! #} +{# !! THIS DOCUMENT CONTAINS ONLY COMPLEX COMPONENT MACROS DESIGNED !! #} +{# !! FOR USE WITH THE MRS DRONE SPAWNER AND USE THE SPAWNER API !! #} +{# !! GENERIC SENSORS, VISUAL BLOCKS, PLUGINS ETC BELONG TO GENERIC !! #} +{# !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! #} + + + + + +{# ================================================================== #} +{# || Components using spawner API || #} +{# ================================================================== #} + +{# ground_truth_macro {--> #} +{%- macro ground_truth_macro(parent_link, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-ground-truth' -%} + {%- set spawner_description = 'Enable ROS topic with ground truth odometry published under model namespace' -%} + {%- set spawner_default_args = {'topic_name': 'ground_truth', 'frame_name': 'world', 'update_rate': 150} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + {{ generic.odometry_sensor_macro( + odometry_sensor_name = 'ground_truth', + parent_link = parent_link, + topic_name = spawner_args[spawner_keyword]['topic_name'], + noise = 0, + frame_name = spawner_args[spawner_keyword]['frame_name'], + update_rate = spawner_args[spawner_keyword]['update_rate'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + {%- endif -%} + +{%- endmacro -%} +{# #} + +{# propellers_macro {--> #} +{%- macro propellers_macro(prop_list, rotor_velocity_slowdown_sim, motor_constant, moment_constant, parent, mass, radius, time_constant_up, time_constant_down, max_rot_velocity, rotor_drag_coefficient, rolling_moment_coefficient, meshes_z_offset, prop_ixx, prop_ixy, prop_ixz, prop_iyy, prop_iyz, prop_izz, spawner_args) -%} + + {%- set spawner_keyword = 'disable-motor-crash' -%} + {%- set spawner_description = 'Disables motor failure after a crash with the environment.' -%} + {%- set spawner_default_args = none -%} + + {%- set enable_motor_crash = spawner_args[spawner_keyword] is undefined -%} + + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + {%- for prop_params in prop_list -%} + + {%- if prop_params['mesh_files'] | length == 1 -%} + + {{ generic.prop_macro( + direction = prop_params['direction'], + rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, + motor_constant = motor_constant, + moment_constant = moment_constant, + parent = parent, + mass = mass, + radius = radius, + time_constant_up = time_constant_up, + time_constant_down = time_constant_down, + max_rot_velocity = max_rot_velocity, + motor_number = prop_params['motor_number'], + rotor_drag_coefficient = rotor_drag_coefficient, + rolling_moment_coefficient = rolling_moment_coefficient, + enable_motor_crash = enable_motor_crash, + color = prop_params['color'], + mesh_file = prop_params['mesh_files'][0], + mesh_scale = prop_params['mesh_scale'], + x = prop_params['x'], + y = prop_params['y'], + z = prop_params['z'], + roll = 0, + pitch = 0, + yaw = 0, + ixx = prop_ixx, + ixy = prop_ixy, + ixz = prop_ixz, + iyy = prop_iyy, + iyz = prop_iyz, + izz = prop_izz) + }} + + {%- elif prop_params['mesh_files'] | length == 2 -%} + + {{ generic.prop_macro_2_meshes( + direction = prop_params['direction'], + rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, + motor_constant = motor_constant, + moment_constant = moment_constant, + parent = parent, + mass = mass, + radius = radius, + time_constant_up = time_constant_up, + time_constant_down = time_constant_down, + max_rot_velocity = max_rot_velocity, + motor_number = prop_params['motor_number'], + rotor_drag_coefficient = rotor_drag_coefficient, + rolling_moment_coefficient = rolling_moment_coefficient, + enable_motor_crash = enable_motor_crash, + color = prop_params['color'], + mesh_file_1 = prop_params['mesh_files'][0], + mesh_file_2 = prop_params['mesh_files'][1], + meshes_z_offset = meshes_z_offset, + mesh_scale = prop_params['mesh_scale'], + x = prop_params['x'], + y = prop_params['y'], + z = prop_params['z'], + roll = 0, + pitch = 0, + yaw = 0, + ixx = prop_ixx, + ixy = prop_ixy, + ixz = prop_ixz, + iyy = prop_iyy, + iyz = prop_iyz, + izz = prop_izz) + }} + + {%- endif -%} + + {%- endfor -%} + + +{%- endmacro -%} +{# #} + +{# ======================= rangefinder sensors ====================== #} + +{# garmin_down_macro (connected through pixhawk) {--> #} +{%- macro garmin_down_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-rangefinder' -%} + {%- set spawner_description = 'Add a laser rangefinder (Garmin LIDAR-Lite v3) pointing down. Creates a Gazebo publisher for gazebo_mavlink_interface. ROS topic must be created by mavros. Do not set range outside <0.06 - 40.0> (unrealistic)' -%} + {%- set spawner_default_args = {'update_rate': 100, 'min_range': 0.1, 'max_range': 36.0, 'noise': 0.01} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- set sensor_name = 'lidar0' -%} + + + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + {{ generic.zero_inertial_macro() }} + + {{ generic.visual_mesh_macro( + name = sensor_name, + mesh_file = 'model://mrs_robots_description/meshes/sensors/garmin_lidar_v3.stl', + mesh_scale = '0.001 0.001 0.001', + color = 'DarkGrey', + x = 0.015, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + {{ generic.gazebo_rangefinder_sensor_macro( + name = sensor_name, + parent_frame_name = spawner_args['name'] + '/fcu', + sensor_frame_name = spawner_args['name'] + '/garmin', + topic = sensor_name, + update_rate = spawner_args[spawner_keyword]['update_rate'], + samples = 1, + fov = 0.03, + min_distance = spawner_args[spawner_keyword]['min_range'], + max_distance = spawner_args[spawner_keyword]['max_range'], + resolution = 0.005, + noise = spawner_args[spawner_keyword]['noise'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + + + {{ sensor_name }}_link + {{ parent_link }} + + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# garmin_down_external_macro (external -> connected to computer) {--> #} +{%- macro garmin_down_external_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-rangefinder-external' -%} + {%- set spawner_description = 'Add a laser rangefinder (Garmin LIDAR-Lite v3) pointing down. Creates a ROS topic /robot_name/garmin/range. Do not set range outside <0.06 - 40.0> (unrealistic)' -%} + {%- set spawner_default_args = {'update_rate': 100, 'min_range': 0.1, 'max_range': 36.0, 'noise': 0.01} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- set sensor_name = 'garmin' -%} + + + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + {{ generic.zero_inertial_macro() }} + + {{ generic.visual_mesh_macro( + name = sensor_name, + mesh_file = 'model://mrs_robots_description/meshes/sensors/garmin_lidar_v3.stl', + mesh_scale = '0.001 0.001 0.001', + color = 'DarkGrey', + x = 0.015, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + {{ generic.rangefinder_sensor_macro( + name = sensor_name, + parent_frame_name = spawner_args['name'] + '/fcu', + sensor_frame_name = spawner_args['name'] + '/' + sensor_name, + topic = sensor_name + '/range', + update_rate = spawner_args[spawner_keyword]['update_rate'], + samples = 1, + fov = 0.03, + min_distance = spawner_args[spawner_keyword]['min_range'], + max_distance = spawner_args[spawner_keyword]['max_range'], + resolution = 0.005, + noise = spawner_args[spawner_keyword]['noise'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + + + {{ sensor_name }}_link + {{ parent_link }} + + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# garmin_up_external_macro (external -> connected to computer) {--> #} +{%- macro garmin_up_external_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-rangefinder-up' -%} + {%- set spawner_description = 'Add a laser rangefinder (Garmin LIDAR-Lite v3) pointing up. Creates a ROS topic /robot_name/garmin/range. Do not set range outside <0.06 - 40.0> (unrealistic)' -%} + {%- set spawner_default_args = {'update_rate': 100, 'min_range': 0.1, 'max_range': 36.0, 'noise': 0.01} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- set sensor_name = 'garmin_up' -%} + + + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + {{ generic.zero_inertial_macro() }} + + {{ generic.visual_mesh_macro( + name = sensor_name, + mesh_file = 'model://mrs_robots_description/meshes/sensors/garmin_lidar_v3.stl', + mesh_scale = '0.001 0.001 0.001', + color = 'DarkGrey', + x = 0.015, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + {{ generic.rangefinder_sensor_macro( + name = sensor_name, + parent_frame_name = spawner_args['name'] + '/fcu', + sensor_frame_name = spawner_args['name'] + '/' + sensor_name, + topic = sensor_name + '/range', + update_rate = spawner_args[spawner_keyword]['update_rate'], + samples = 1, + fov = 0.03, + min_distance = spawner_args[spawner_keyword]['min_range'], + max_distance = spawner_args[spawner_keyword]['max_range'], + resolution = 0.005, + noise = spawner_args[spawner_keyword]['noise'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + + + {{ sensor_name }}_link + {{ parent_link }} + + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# teraranger_macro {--> #} +{%- macro teraranger_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-teraranger' -%} + {%- set spawner_description = 'Add a laser rangefinder (Teraranger One) pointing down' -%} + {%- set spawner_default_args = {'update_rate': 100, 'min_range': 0.1, 'max_range': 14.0, 'noise': 0.04} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- set sensor_name = 'teraranger' -%} + + + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_colored_box_macro(sensor_name, 0.015, 0.027, 0.033, 'Yellow', 0, 0, 0, 0, 0, 0) }} + {{ generic.rangefinder_sensor_macro( + name = sensor_name, + parent_frame_name = spawner_args['name'] + '/fcu', + sensor_frame_name = spawner_args['name'] + '/' + sensor_name, + topic = sensor_name + '/range', + update_rate = spawner_args[spawner_keyword]['update_rate'], + samples = 1, + fov = 0.03, + min_distance = spawner_args[spawner_keyword]['min_range'], + max_distance = spawner_args[spawner_keyword]['max_range'], + resolution = 0.005, + noise = spawner_args[spawner_keyword]['noise'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ parent_link }} + {{ sensor_name}}_link + + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# teraranger_evo_tower_macro {--> #} +{%- macro teraranger_evo_tower_macro(parent_link, visualize, frame_name, parent_frame_name, gaussian_noise, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-teraranger-evo-tower' -%} + {%- set spawner_description = 'Add the Teraranger Evo Tower laser scanner to the vehicle' -%} + {%- set spawner_default_args = none -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {# -- parameters of sensors -- (id, x, y, z, roll, pitch, yaw) #} + {%- set sensor_parameters = [(0, 0.046, 0.0, 0.001, 0.0, 0.0, 0.0), + (1, 0.032, 0.032, 0.001, 0.0, 0.0, math.radians(45)), + (2, 0.000, 0.046, 0.001, 0.0, 0.0, math.radians(90)), + (3, -0.032, 0.032, 0.001, 0.0, 0.0, math.radians(135)), + (4, -0.046, 0.000, 0.001, 0.0, 0.0, math.radians(180)), + (5, -0.032, -0.032, 0.001, 0.0, 0.0, -math.radians(135)), + (6, 0.000, -0.046, 0.001, 0.0, 0.0, -math.radians(90)), + (7, 0.032, -0.032, 0.001, 0.0, 0.0, -math.radians(45))] -%} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + 0 0 -0.014 0 0 0 + + + 0.001 + 0.06 + + + + + + + + {%- for id_, x_, y_, z_, roll_, pitch_, yaw_ in sensor_parameters -%} + + {{ generic.teraranger_evo_macro( + parent_link = parent_link, + id = id_, + visualize = visualize, + frame_name = frame_name, + parent_frame_name = parent_frame_name, + gaussian_noise = gaussian_noise, + x = x_, + y = y_, + z = z_, + roll = roll_, + pitch = pitch_, + yaw = yaw_) + }} + + {%- endfor -%} + + + + {{ parent_link }} + teraranger_evo_tower_link + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# ========================== LIDAR sensors ========================= #} + +{# rplidar_macro {--> #} +{%- macro rplidar_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-rplidar' -%} + {%- set spawner_description = 'Add the RPlidar A3 laser scanner. Do not set range outside <0.10 - 15.0> (unrealistic)' -%} + {%- set spawner_default_args = {'horizontal_samples': 1600, 'update_rate': 20.0, 'min_range': 0.15, 'max_range': 14.0, 'noise': 0.01} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- set sensor_name = 'rplidar' -%} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + + {{ generic.visual_mesh_macro( + name = 'base_visual', + mesh_file = 'model://mrs_robots_description/meshes/sensors/rplidar.stl', + mesh_scale = '0.001 0.001 0.001', + color = 'FlatBlack', + x = 0, + y = 0, + z = -0.029, + roll = 0, + pitch = 0, + yaw = math.radians(180)) + }} + + 0 0 -0.011 0 0 0 + + + 0.001 + 0.038 + + + + + + + + + + + false + {{ spawner_args[spawner_keyword]['update_rate'] }} + + + + {{ spawner_args[spawner_keyword]['horizontal_samples'] | int }} + 1 + -3.1241390751 + 3.1241390751 + + + + {{ spawner_args[spawner_keyword]['min_range'] }} + {{ spawner_args[spawner_keyword]['max_range'] }} + 0.01 + + + gaussian + 0.0 + {{ spawner_args[spawner_keyword]['noise'] }} + + + + {{ sensor_name }}/scan + {{ spawner_args['name'] }}/{{ sensor_name }} + {{ spawner_args['name'] }}/fcu + {{ x }} + {{ y }} + {{ z }} + {{ roll }} + {{ pitch }} + {{ yaw }} + + + + + + + + {{ parent_link }} + {{ sensor_name }}_link + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# scanse_sweep_macro {--> #} +{%- macro scanse_sweep_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-scanse-sweep' -%} + {%- set spawner_description = 'Add the Scanse Sweep laser scanner' -%} + {%- set spawner_default_args = {'horizontal_samples': 500, 'update_rate': 10.0, 'min_range': 0.45, 'max_range': 10.0, 'noise': 0.01} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + + + 0 0 -0.031 0 0 0 + + + 0.0255 + 0.0234 + + + + + + + + + + 0.0385 + 0.0325 + + + + + + + + + + + false + {{ spawner_args[spawner_keyword]['update_rate'] }} + + + + {{ spawner_args[spawner_keyword]['horizontal_samples'] | int }} + 1 + 0 + 6.283185 + + + + {{ spawner_args[spawner_keyword]['min_range'] }} + {{ spawner_args[spawner_keyword]['max_range'] }} + 0.01 + + + gaussian + 0.0 + {{ spawner_args[spawner_keyword]['noise'] }} + + + + scanse_sweep/range + {{ spawner_args['name'] }}/scanse_sweep + {{ spawner_args['name'] }}/fcu + {{ x }} + {{ y }} + {{ z }} + {{ roll }} + {{ pitch }} + {{ yaw }} + + + + + + + + {{ parent_link }} + sweeper_link + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# ouster_macro {--> #} +{%- macro ouster_macro(parent_link, sensor_name, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-ouster' -%} + {%- set spawner_description = 'Add Ouster laser scanner to the vehicle. Select a model to automatically set number of lines, vertical FOV and range. Available models: OS0-32, OS0-64, OS0-128, OS1-16, OS1-32G1, OS1-32, OS1-64, OS1-128, OS2-32, OS2-64, OS2-128' -%} + {%- set spawner_default_args = {'model': 'OS1-16', 'horizontal_samples': 2048, 'update_rate': 10, 'noise': 0.03, 'use_gpu': false} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {# model selection {--> #} + {%- set ouster_model = spawner_args[spawner_keyword]['model'] -%} + + {# OS0 {--> #} + + {# OS0-32 #} + {%- if ouster_model == 'OS0-32' -%} + {%- set lasers = 32 -%} + {%- set vfov_angle = 90 -%} + {%- set range = 55 -%} + {%- endif -%} + + {# OS0-64 #} + {%- if ouster_model == 'OS0-64' -%} + {%- set lasers = 64 -%} + {%- set vfov_angle = 90 -%} + {%- set range = 55 -%} + {%- endif -%} + + {# OS0-128 #} + {%- if ouster_model == 'OS0-128' -%} + {%- set lasers = 128 -%} + {%- set vfov_angle = 90 -%} + {%- set range = 55 -%} + {%- endif -%} + + {# #} + + {# OS1 Generation 1 {--> #} + {# #} + {%- if ouster_model == 'OS1-16' -%} + {%- set lasers = 16 -%} + {%- set vfov_angle = 33.2 -%} + {%- set range = 120 -%} + {%- endif -%} + {# #} + + {# OS1 Generation 2 {--> #} + + {# OS1-32 Generation 2 #} + {%- if ouster_model == 'OS1-32' -%} + {%- set lasers = 32 -%} + {%- set vfov_angle = 45 -%} + {%- set range = 120 -%} + {%- endif -%} + + {# OS1-64 Generation 2 #} + {%- if ouster_model == 'OS1-64' -%} + {%- set lasers = 64 -%} + {%- set vfov_angle = 45 -%} + {%- set range = 120 -%} + {%- endif -%} + + {# OS1-128 Generation 2 #} + {%- if ouster_model == 'OS1-128' -%} + {%- set lasers = 128 -%} + {%- set vfov_angle = 45 -%} + {%- set range = 120 -%} + {%- endif -%} + + {# #} + + {# OS2 {--> #} + + {# OS2-32 #} + {%- if ouster_model == 'OS2-32' -%} + {%- set lasers = 32 -%} + {%- set vfov_angle = 22.5 -%} + {%- set range = 240 -%} + {%- endif -%} + + {# OS2-64 #} + {%- if ouster_model == 'OS2-64' -%} + {%- set lasers = 64 -%} + {%- set vfov_angle = 22.5 -%} + {%- set range = 240 -%} + {%- endif -%} + + {# OS2-128 #} + {%- if ouster_model == 'OS2-128' -%} + {%- set lasers = 128 -%} + {%- set vfov_angle = 22.5 -%} + {%- set range = 240 -%} + {%- endif -%} + + {# #} + + {# #} + + {# 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. #} + + {# setup local variables {--> #} + + {# -- gazebo links -- #} + {%- set sensor_link = sensor_name + '_sensor_link' -%} + + {# -- frame names -- #} + {%- set frame_fcu = spawner_args['name'] + '/fcu' -%} + {%- set frame_sensor = spawner_args['name'] + '/' + sensor_name + '_sensor' -%} + {%- set frame_lidar = spawner_args['name'] + '/' + sensor_name + '_lidar' -%} + {%- set frame_imu = spawner_args['name'] + '/' + sensor_name + '_imu' -%} + + {# -- topics -- #} + {%- set topic_lidar = '/' + spawner_args['name'] + '/' + sensor_name + '_cloud_nodelet/points' -%} + {%- set topic_imu = '/' + spawner_args['name'] + '/' + sensor_name + '_cloud_nodelet/imu' -%} + {%- set topic_diag = '/' + spawner_args['name'] + '/' + sensor_name + '_cloud_nodelet/is_alive' -%} + + {# -- tf from sensor to lidar -- #} + {%- set lidar_x = 0 -%} + {%- set lidar_y = 0 -%} + {%- set lidar_z = 0.0344 -%} + {%- set lidar_roll = 0 -%} + {%- set lidar_pitch = 0 -%} + {%- set lidar_yaw = 0 -%} + + {# -- tf from sensor to imu -- #} + {%- set imu_x = 0.006253 -%} + {%- set imu_y = -0.011775 -%} + {%- set imu_z = 0.007645 -%} + {%- set imu_roll = 0 -%} + {%- set imu_pitch = 0 -%} + {%- set imu_yaw = 0 -%} + + {%- if spawner_args[spawner_keyword]['use_gpu'] -%} + {%- set ouster_plugin_filename ='libMrsGazeboCommonResources_3DLidarGpuPlugin.so' -%} + {%- set sensor_type = 'gpu_ray' -%} + {%- else -%} + {%- set ouster_plugin_filename ='libMrsGazeboCommonResources_3DLidarPlugin.so' -%} + {%- set sensor_type = 'ray' -%} + {%- endif -%} + + {# #} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + + {{ generic.visual_mesh_macro( + name = 'base_visual', + mesh_file = 'model://mrs_robots_description/meshes/sensors/os1_64.dae', + mesh_scale = '1 1 1', + color = 'White', + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = math.radians(90)) + }} + + 0 0 {{ lidar_z }} 0 0 + + + 0.035 + 0.038 + + + + + + + + + + + {{ imu_x }} {{ imu_y }} {{ imu_z }} {{ imu_roll }} {{ imu_pitch }} {{ imu_yaw }} + true + true + 100 + false + __default_topic__ + + + {{ topic_imu }} + {{ sensor_link }} + 100 + 0.005 + 0 0 0 + 0 0 0 + {{ frame_imu }} + + + + + + + {{ lidar_x }} {{ lidar_y }} {{ lidar_z }} {{ lidar_roll }} {{ lidar_pitch }} {{ lidar_yaw }} + false + {{ spawner_args[spawner_keyword]['update_rate'] }} + + + + {{ spawner_args[spawner_keyword]['horizontal_samples'] }} + 1 + 0 + {{ 2*math.pi }} + + + {{ lasers }} + 1 + {{ -vfov_angle/2*math.radians(180)/180.0 }} + {{ vfov_angle/2*math.radians(180)/180.0 }} + + + + 0.1 + {{ range }} + 0.03 + + + + {{ frame_fcu }} + {{ frame_sensor }} + {{ x + lidar_x }} + {{ y + lidar_y }} + {{ z + lidar_z }} + {{ roll + lidar_roll }} + {{ pitch + lidar_pitch }} + {{ yaw + lidar_yaw }} + + + {{ frame_lidar }} + 0 + 0 + 0 + 0 + 0 + {{ math.radians(180) }} + {{ topic_lidar }} + {{ topic_diag }} + 0.1 + {{ range }} + true + {{ spawner_args[spawner_keyword]['noise'] }} + + + true + {{ frame_imu }} + {{ imu_x - lidar_x }} + {{ imu_y - lidar_y }} + {{ imu_z - lidar_z }} + {{ imu_roll - lidar_roll }} + {{ imu_pitch - lidar_pitch }} + {{ imu_yaw - lidar_yaw }} + + + + + + + + {{ parent_link }} + {{ sensor_link }} + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# velodyne_macro {--> #} +{%- macro velodyne_macro(parent_link, sensor_name, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-velodyne' -%} + {%- set spawner_description = 'Add Velodyne PUCK laser scanner to the vehicle' -%} + {%- set spawner_default_args = {'horizontal_samples': 3600, 'lasers': 16, 'noise': 0.01, 'range': 100, 'vfov': 30, 'update_rate': 20, 'use_gpu': false} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {# setup local variables {--> #} + + {# 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. #} + {# Velodyne macro is using the same plugin as ouster macro. Therefore we need to render data in the same way. #} + + {# -- frame names -- #} + {%- set frame_fcu = spawner_args['name'] + '/fcu' -%} + {%- set frame_sensor = spawner_args['name'] + '/' + sensor_name + '_sensor' -%} + {%- set frame_lidar = spawner_args['name'] + '/' + sensor_name -%} + + {# -- topics -- #} + {%- set topic_lidar = '/' + spawner_args['name'] + '/' + sensor_name + '/scan' -%} + {%- set topic_diag = '/' + spawner_args['name'] + '/' + sensor_name + '/is_alive' -%} + + {# -- tf from sensor to lidar -- #} + {# The laser rays should be coming approximately from the half of sensor height #} + {%- set lidar_x = 0 -%} + {%- set lidar_y = 0 -%} + {%- set lidar_z = 0.037725 -%} + {%- set lidar_roll = 0 -%} + {%- set lidar_pitch = 0 -%} + {%- set lidar_yaw = 0 -%} + + {%- if spawner_args[spawner_keyword]['use_gpu'] -%} + {%- set velodyne_plugin_filename ='libMrsGazeboCommonResources_3DLidarGpuPlugin.so' -%} + {%- set sensor_type = 'gpu_ray' -%} + {%- else -%} + {%- set velodyne_plugin_filename ='libMrsGazeboCommonResources_3DLidarPlugin.so' -%} + {%- set sensor_type = 'ray' -%} + {%- endif -%} + {# #} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + + + 0 0 0.0094 0 0 0 + + + 0.0188 + 0.062 + + + + + + + + 0 0 0.0643 0 0 0 + + + 0.0148 + 0.062 + + + + + + + + 0 0 0.03785 0 0 0 + + + 0.0381 + 0.058 + + + + + + + + + + + {{ lidar_x }} {{ lidar_y }} {{ lidar_z }} {{ lidar_roll }} {{ lidar_pitch }} {{ lidar_yaw }} + {# 0 0 0 0 0 0 #} + false + {{ spawner_args[spawner_keyword]['update_rate'] }} + + + + {{ spawner_args[spawner_keyword]['horizontal_samples'] }} + 1 + {{ -math.radians(180) }} + {{ math.radians(180) }} + + + {{ spawner_args[spawner_keyword]['lasers'] }} + 1 + {{ -spawner_args[spawner_keyword]['vfov']/2*math.radians(180)/180.0 }} + {{ spawner_args[spawner_keyword]['vfov']/2*math.radians(180)/180.0 }} + + + + 0.1 + {{ spawner_args[spawner_keyword]['range'] }} + 0.03 + + + + {{ frame_fcu }} + {{ frame_sensor }} + {{ x + lidar_x }} + {{ y + lidar_y }} + {{ z + lidar_z }} + {{ roll }} + {{ pitch }} + {{ yaw }} + 0 + 0 + 0 + {{ lidar_roll }} + {{ lidar_pitch }} + {{ lidar_yaw }} + {{ topic_lidar }} + {{ topic_diag }} + {{ frame_lidar }} + 0.1 + {{ spawner_args[spawner_keyword]['range'] }} + {{ spawner_args[spawner_keyword]['noise'] }} + false + + + + + + + + {{ parent_link }} + velodyne_link + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# ============================= cameras ============================ #} + +{# realsense_top_macro {--> #} +{%- macro realsense_top_macro(camera_name, camera_suffix, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-realsense-top' -%} + {%- set spawner_description = 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 @ 30hz], pointed forward, placed on an aluminum frame above the body' -%} + {%- set spawner_default_args = {'realistic': False, 'update_rate': 30} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.realsense_d435_macro( + namespace = spawner_args['name'], + camera_name = camera_name, + camera_suffix = camera_suffix, + parent_link = parent_link, + realistic = spawner_args[spawner_keyword]['realistic'], + update_rate = spawner_args[spawner_keyword]['update_rate'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ mount }} + + + + + {%- endif -%} + +{%- endmacro -%} +{# #} + +{# realsense_front_macro {--> #} +{%- macro realsense_front_macro(camera_name, camera_suffix, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-realsense-front' -%} + {%- set spawner_description = 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 @ 30hz], pointed forward placed on the front holder between the legs' -%} + {%- set spawner_default_args = {'realistic': False, 'update_rate': 30} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.realsense_d435_macro( + namespace = spawner_args['name'], + camera_name = camera_name, + camera_suffix = camera_suffix, + parent_link = parent_link, + realistic = spawner_args[spawner_keyword]['realistic'], + update_rate = spawner_args[spawner_keyword]['update_rate'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ mount }} + + + + + {%- endif -%} + +{%- endmacro -%} +{# #} + +{# realsense_front_pitched_macro {--> #} +{%- macro realsense_front_pitched_macro(camera_name, camera_suffix, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-realsense-front-pitched' -%} + {%- set spawner_description = 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 @ 30hz], pointed forward pitched down by 10-45 deg (angle depends on vehicle model)' -%} + {%- set spawner_default_args = {'realistic': False, 'update_rate': 30} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.realsense_d435_macro( + namespace = spawner_args['name'], + camera_name = camera_name, + camera_suffix = camera_suffix, + parent_link = parent_link, + realistic = spawner_args[spawner_keyword]['realistic'], + update_rate = spawner_args[spawner_keyword]['update_rate'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ mount }} + + + + + {%- endif -%} + +{%- endmacro -%} +{# #} + +{# realsense_up_macro {--> #} +{%- macro realsense_up_macro(camera_name, camera_suffix, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-realsense-up' -%} + {%- set spawner_description = 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 @ 30hz], pointed upwards' -%} + {%- set spawner_default_args = {'realistic': False, 'update_rate': 30} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.realsense_d435_macro( + namespace = spawner_args['name'], + camera_name = camera_name, + camera_suffix = camera_suffix, + parent_link = parent_link, + realistic = spawner_args[spawner_keyword]['realistic'], + update_rate = spawner_args[spawner_keyword]['update_rate'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ mount }} + + + + + {%- endif -%} + +{%- endmacro -%} +{# #} + +{# realsense_down_macro {--> #} +{%- macro realsense_down_macro(camera_name, camera_suffix, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-realsense-down' -%} + {%- set spawner_description = 'Add Intel Realsense D435 depth camera to the vehicle [1280x720 @ 30hz], pointed down' -%} + {%- set spawner_default_args = {'realistic': False, 'update_rate': 30} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.realsense_d435_macro( + namespace = spawner_args['name'], + camera_name = camera_name, + camera_suffix = camera_suffix, + parent_link = parent_link, + realistic = spawner_args[spawner_keyword]['realistic'], + update_rate = spawner_args[spawner_keyword]['update_rate'], + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ mount }} + + + + + {%- endif -%} + +{%- endmacro -%} +{# #} + +{# bluefox_camera_macro {--> #} +{%- macro bluefox_camera_macro(camera_name, parent_link, x, y, z, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-bluefox-camera' -%} + {%- set spawner_description = 'Add bluefox camera to the vehicle [752x480 @ 50hz], pointing down' -%} + {%- set spawner_default_args = {'update_rate': 100, 'noise': 0.0} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + frame_rate = spawner_args[spawner_keyword]['update_rate'], + horizontal_fov = 2.1, + image_width = 752, + image_height = 480, + min_distance = 0.02, + max_distance = 80, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = 0, + visual_y = 0, + visual_z = 0, + visual_roll = math.radians(90), + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = 0, + pitch = math.radians(90), + yaw = 0) + }} + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# bluefox_camera_reverse_macro {--> #} +{%- macro bluefox_camera_reverse_macro(camera_name, parent_link, x, y, z, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-bluefox-camera-reverse' -%} + {%- set spawner_description = 'Add bluefox camera to the vehicle [752x480 @ 50hz], pointing down but image is rotated by 180 deg' -%} + {%- set spawner_default_args = {'update_rate': 100, 'noise': 0.0} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + frame_rate = spawner_args[spawner_keyword]['update_rate'], + horizontal_fov = 2.1, + image_width = 752, + image_height = 480, + min_distance = 0.02, + max_distance = 80, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = 0, + visual_y = 0, + visual_z = 0, + visual_roll = math.radians(90), + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = 0, + pitch = math.radians(90), + yaw = math.radians(180)) + }} + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# bluefox_camera_front_macro {--> #} +{%- macro bluefox_camera_front_macro(camera_name, parent_link, x, y, z, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-bluefox-camera-front' -%} + {%- set spawner_description = 'Add bluefox camera to the vehicle [752x480 @ 50hz], pointing forward' -%} + {%- set spawner_default_args = {'update_rate': 100, 'noise': 0.0} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + frame_rate = spawner_args[spawner_keyword]['update_rate'], + horizontal_fov = 2.1, + image_width = 752, + image_height = 480, + min_distance = 0.02, + max_distance = 80, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = 0, + visual_y = 0, + visual_z = 0, + visual_roll = math.radians(90), + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = 0, + pitch = 0, + yaw = 0) + }} + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# mobius_down_macro {-->#} +{%- macro mobius_down_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-mobius-camera-down' -%} + {%- set spawner_description = 'Add mobius camera to the vehicle [1280x720 @ 30hz], pointing down' -%} + {%- set spawner_default_args = {'update_rate': 30, 'noise': 0.007} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + frame_rate = spawner_args[spawner_keyword]['update_rate'], + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + horizontal_fov = 2.28638, + image_width = 1280, + image_height = 720, + min_distance = 0.02, + max_distance = 40, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/mobius.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = -0.004, + visual_y = 0.0045, + visual_z = 0, + visual_roll = 0, + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# mobius_front_macro {-->#} +{%- macro mobius_front_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-mobius-camera-front' -%} + {%- set spawner_description = 'Add mobius camera to the vehicle [1280x720 @ 30hz], pointing forward' -%} + {%- set spawner_default_args = {'update_rate': 30, 'noise': 0.007} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + frame_rate = spawner_args[spawner_keyword]['update_rate'], + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + horizontal_fov = 2.28638, + image_width = 1280, + image_height = 720, + min_distance = 0.02, + max_distance = 40, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/mobius.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = -0.004, + visual_y = 0.0045, + visual_z = 0, + visual_roll = 0, + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# mobius_back_left_macro {-->#} +{%- macro mobius_back_left_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-mobius-camera-back-left' -%} + {%- set spawner_description = 'Add mobius camera to the vehicle [1280x720 @ 30hz], pointing to the back left' -%} + {%- set spawner_default_args = {'update_rate': 30, 'noise': 0.007} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + frame_rate = spawner_args[spawner_keyword]['update_rate'], + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + horizontal_fov = 2.28638, + image_width = 1280, + image_height = 720, + min_distance = 0.02, + max_distance = 40, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/mobius.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = -0.004, + visual_y = 0.0045, + visual_z = 0, + visual_roll = 0, + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# mobius_back_right_macro {-->#} +{%- macro mobius_back_right_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-mobius-camera-back-right' -%} + {%- set spawner_description = 'Add mobius camera to the vehicle [1280x720 @ 30hz], pointing to the back right' -%} + {%- set spawner_default_args = {'update_rate': 30, 'noise': 0.007} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + frame_rate = spawner_args[spawner_keyword]['update_rate'], + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + horizontal_fov = 2.28638, + image_width = 1280, + image_height = 720, + min_distance = 0.02, + max_distance = 40, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/mobius.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = -0.004, + visual_y = 0.0045, + visual_z = 0, + visual_roll = 0, + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# fisheye_bluefox_macro {--> #} +{%- macro fisheye_bluefox_macro(camera_name, topic_ns_prefix, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-fisheye-camera' -%} + {%- set spawner_description = 'Add a fisheye bluefox camera [752x480 @ 45hz, 180 deg] to the vehicle' -%} + {%- set spawner_default_args = {'update_rate': 45, 'noise': 0.0} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.fisheye_camera_macro( + camera_name = camera_name, + topic_ns_prefix = topic_ns_prefix, + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + parent_link = parent_link, + frame_rate = spawner_args[spawner_keyword]['update_rate'], + horizontal_fov = math.radians(180), + image_width = 752, + image_height = 480, + lens_type = 'custom', + lens_c1 = 1.05, + lens_c2 = 4, + lens_f = 1.0, + lens_fun = 'tan', + lens_scale = True, + lens_cutoff_angle = math.radians(90), + lens_texture_size = 512, + min_distance = 0.02, + max_distance = 40, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = 0, + visual_y = 0, + visual_z = 0, + visual_roll = math.radians(90), + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# servo_camera_macro {--> #} +{%- macro servo_camera_macro(parent, offset_pitch_link_x, offset_pitch_link_y, offset_pitch_link_z, offset_pitch_link_yaw, offset_pitch_link_roll, offset_pitch_link_pitch, offset_roll_link_x, offset_roll_link_y, offset_roll_link_z, offset_roll_link_yaw, offset_roll_link_roll, offset_roll_link_pitch, tilt_update_rate, max_pitch, min_pitch, max_pitch_rate, max_roll, min_roll, max_roll_rate, compensate_tilt_roll, compensate_tilt_pitch, parent_frame_name, sensor_base_frame_name, camera_frame_name, camera_update_rate, horizontal_fov, img_width, img_height, roll_link_mesh_file, pitch_link_mesh_file, mesh_scale, spawner_args) -%} + + {%- set spawner_keyword = 'enable-servo-camera' -%} + {%- set spawner_description = 'Add a camera on virtual servo to the vehicle' -%} + {%- set spawner_default_args = none -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + + 0 0 0 0 0 0 + {{ generic.zero_inertial_macro() }} + + 0 0 0 0 0 0 + + {%- if roll_link_mesh_file == '' -%} + + 0.01 0.01 0.01 + + {%- else -%} + + {{ roll_link_mesh_file }} + {{ mesh_scale }} + + {%- endif -%} + + + + + + + + + + + 0 0 0 0 0 0 + {{ generic.zero_inertial_macro() }} + + 0 0 0 0 0 0 + + {%- if pitch_link_mesh_file == '' -%} + + 0.01 0.01 0.01 + + {%- else -%} + + {{ pitch_link_mesh_file }} + {{ mesh_scale }} + + {%- endif -%} + + + + + + + {{ camera_update_rate }} + + {{ horizontal_fov }} + + {{ img_width }} + {{ img_height }} + + + 0.25 + 100 + + + gaussian + + 0.0 + 0.01 + + + + true + {{ camera_update_rate }} + servo_camera + image_raw + camera_info + {{ camera_frame_name }} + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + {{ parent_frame_name }} + {{ sensor_base_frame_name }} + + + + + + {{ tilt_update_rate }} + {{ max_pitch_rate }} + {{ max_pitch }} + {{ min_pitch }} + {{ max_roll_rate }} + {{ max_roll }} + {{ min_roll }} + servo_camera_pitch_joint + servo_camera_roll_joint + servo_camera_gimbal_link + {{ parent }} + {{ compensate_tilt_roll }} + {{ compensate_tilt_pitch }} + + + + + + {{ parent }} + servo_camera_gimbal_link + {{ offset_roll_link_x }} {{ offset_roll_link_y }} {{ offset_roll_link_z }} {{ offset_roll_link_roll }} {{ offset_roll_link_pitch }} {{ offset_roll_link_yaw }} + + 1 0 0 + + -0.5 + 0.5 + 1 + 10 + + + 0.0 + 0.0 + + + + + + servo_camera_gimbal_link + servo_camera_link + {{ offset_pitch_link_x }} {{ offset_pitch_link_y }} {{ offset_pitch_link_z }} {{ offset_pitch_link_roll }} {{ offset_pitch_link_pitch }} {{ offset_pitch_link_yaw }} + + 0 1 0 + + -1.57 + 1.57 + 1 + 10 + + + 0.0 + 0.0 + + + + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# TODO: Thermal camera plugin is not core #} +{# thermal_cameras_macro {--> #} +{%- macro thermal_cameras_macro(cameras_list, parent_link, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-thermal-cameras' -%} + {%- set spawner_description = 'Add several thermal cameras to the vehicle' -%} + {%- set spawner_default_args = {'image_width': 32, 'image_height': 32, 'update_rate': 14} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- if cameras_list | length > 0 -%} + + {%- for camera in cameras_list -%} + + {{ generic.thermal_camera_macro( + camera_name = camera['name'], + camera_topic_name = '/' + spawner_args['name'] + '/thermal/' + camera['name'], + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = 'thermal/' + camera['name'] + '_optical', + sensor_base_frame_name = spawner_args['name'] + '/thermal/' + camera['name'], + parent_link = parent_link, + frame_rate = spawner_args[spawner_keyword]['update_rate'], + hfov = 0.575959, + image_width = spawner_args[spawner_keyword]['image_width'] | int, + image_height = spawner_args[spawner_keyword]['image_height'] | int, + x = camera['x'], + y = camera['y'], + z = camera['z'], + roll = camera['roll'], + pitch = camera['pitch'], + yaw = camera['yaw']) + }} + + {%- endfor -%} + + + {{ mount }} + + + + {%- endif -%} + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# basler_camera_down_macro {--> #} +{%- macro basler_camera_down_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-basler-camera-down' -%} + {%- set spawner_description = 'Add a Basler Dart camera pointing down' -%} + {%- set spawner_default_args = {'image_width': 1920, 'image_height': 1080, 'update_rate': 30, 'hfov': 1.990252, 'noise': 0.007} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + frame_rate = spawner_args[spawner_keyword]['update_rate'], + sensor_base_frame_name = spawner_args['name'] + '/' + camera_name, + parent_frame_name = spawner_args['name'] + '/fcu', + horizontal_fov = spawner_args[spawner_keyword]['hfov'], + image_width = spawner_args[spawner_keyword]['image_width'], + image_height = spawner_args[spawner_keyword]['image_height'], + min_distance = 0.02, + max_distance = 80, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['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) + }} + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# ========================= UVDAR components ======================= #} + +{# dual_uv_cameras_macro {--> #} +{%- macro dual_uv_cameras_macro(parent_link, x1, y1, z1, roll1, pitch1, yaw1, x2, y2, z2, roll2, pitch2, yaw2, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-dual-uv-cameras' -%} + {%- set spawner_description = 'Add right and left UV cameras to the vehicle. Requires param "calib_file" to be set. Setting param "occlusions" to True requires a BEEFY computer.' -%} + {%- set spawner_default_args = {'occlusions': False, 'calib_file': none, 'update_rate': 60} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- if spawner_args[spawner_keyword]['calib_file'] is not none -%} + + {{ generic.uvcam_macro( + parent_link = parent_link, + camera_publish_topic = '/' + spawner_args['name'] + '/uvdar_bluefox_left/image_raw', + calibration_file = spawner_args[spawner_keyword]['calib_file'], + occlusions = spawner_args[spawner_keyword]['occlusions'], + frame_rate = spawner_args[spawner_keyword]['update_rate'], + device_id = spawner_args['name'] + '_1', + x = x1, + y = y1, + z = z1, + roll = roll1, + pitch = pitch1, + yaw = yaw1) + }} + + {{ generic.uvcam_macro( + parent_link = parent_link, + camera_publish_topic = '/' + spawner_args['name'] + '/uvdar_bluefox_right/image_raw', + calibration_file = spawner_args[spawner_keyword]['calib_file'], + occlusions = spawner_args[spawner_keyword]['occlusions'], + frame_rate = spawner_args[spawner_keyword]['update_rate'], + device_id = spawner_args['name'] + '_2', + x = x2, + y = y2, + z = z2, + roll = roll2, + pitch = pitch2, + yaw = yaw2) + }} + + + {{ mount }} + + + + {%- endif -%} + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# back_uv_camera_macro {--> #} +{%- macro back_uv_camera_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-back-uv-camera' -%} + {%- set spawner_description = 'Add a back UV camera to the vehicle. Requires param "calib_file" to be set' -%} + {%- set spawner_default_args = {'occlusions': False, 'calib_file': none, 'update_rate': 60} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + {{ generic.uvcam_macro( + parent_link = parent_link, + camera_publish_topic = '/' + spawner_args['name'] + '/uvdar_bluefox_back/image_raw', + calibration_file = spawner_args[spawner_keyword]['calib_file'], + occlusions = spawner_args[spawner_keyword]['occlusions'], + frame_rate = spawner_args[spawner_keyword]['update_rate'], + device_id = spawner_args['name'] + '_3', + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# downward_uv_camera_macro {--> #} +{%- macro downward_uv_camera_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-downward-uv-camera' -%} + {%- set spawner_description = 'Add a UV camera to the vehicle pointed down. Requires param "calib_file" to be set' -%} + {%- set spawner_default_args = {'occlusions': False, 'calib_file': none, 'update_rate': 60} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + {{ generic.uvcam_macro( + parent_link = parent_link, + camera_publish_topic = '/' + spawner_args['name'] + '/uvdar_bluefox_bottom/image_raw', + calibration_file = spawner_args[spawner_keyword]['calib_file'], + occlusions = spawner_args[spawner_keyword]['occlusions'], + frame_rate = spawner_args[spawner_keyword]['update_rate'], + device_id = spawner_args['name'] + '_3', + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# uv_leds_macro {--> #} +{%- macro uv_leds_macro(parent_link, x1, x2, y1, y2, z, spawner_args) -%} + + {%- set spawner_keyword = 'enable-uv-leds' -%} + {%- set spawner_description = 'Add UV LEDs to the vehicle. Requires param "signal_id" to be set. Legacy params "left_id" and "right_id" may replace "signal_id" (for backwards compatibility)' -%} + {%- set spawner_default_args = {'signal_id': none, 'left_id': none, 'right_id': none} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {# setup led signals {--> #} + {%- if spawner_args[spawner_keyword]['signal_id'] is not none and spawner_args[spawner_keyword]['signal_id'] | length > 3 -%} + + {%- set led1 = spawner_args[spawner_keyword]['signal_id'][0] -%} + {%- set led2 = spawner_args[spawner_keyword]['signal_id'][1] -%} + {%- set led4 = spawner_args[spawner_keyword]['signal_id'][3] -%} + {%- set led3 = spawner_args[spawner_keyword]['signal_id'][2] -%} + + {%- elif spawner_args[spawner_keyword]['left_id'] is not none and spawner_args[spawner_keyword]['right_id'] is not none -%} + + {%- set led1 = spawner_args[spawner_keyword]['left_id'] -%} + {%- set led2 = spawner_args[spawner_keyword]['right_id'] -%} + {%- set led3 = spawner_args[spawner_keyword]['left_id'] -%} + {%- set led4 = spawner_args[spawner_keyword]['right_id'] -%} + + {%- endif -%} + {# #} + + {%- if led1 is not none and led2 is not none and led3 is not none and led4 is not none -%} + + {# -- leds configuration -- (id, signal_id, x, y, z, roll, pitch, yaw) #} + {%- set uv_leds_macro_parameters = [(1, led1, x1, y1, z, 0.0, math.radians(90), 0.0), + (2, led1, x2, y2, z, 0.0, math.radians(90), math.radians(90)), + (3, led2, x2, -y2, z, 0.0, math.radians(90), -math.radians(90)), + (4, led2, x1, -y1, z, 0.0, math.radians(90), 0.0), + (5, led3, -x2, y2, z, 0.0, math.radians(90), math.radians(90)), + (6, led3, -x1, y1, z, 0.0, math.radians(90), math.radians(180)), + (7, led4, -x1, -y1, z, 0.0, math.radians(90), -math.radians(180)), + (8, led4, -x2, -y2, z, 0.0, math.radians(90), -math.radians(90))] -%} + + + {%- for id_, signal_id_, x_, y_, z_, roll_, pitch_, yaw_ in uv_leds_macro_parameters -%} + + {{ generic.uvled_macro( + parent_link = parent_link, + device_id = spawner_args['name'] + '_' + id_ | string(), + signal_id = signal_id_, + x = x_, + y = y_, + z = z_, + roll = roll_, + pitch = pitch_, + yaw = yaw_) + }} + + {%- endfor -%} + + + {%- endif -%} + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# uv_leds_beacon_macro {--> #} +{%- macro uv_leds_beacon_macro(parent_link, x1, x2, y1, y2, z, spawner_args) -%} + + {%- set spawner_keyword = 'enable-uv-leds-beacon' -%} + {%- set spawner_description = 'Add UV LED beacon to the top of the vehicle. Requires param "signal_id" to be set.' -%} + {%- set spawner_default_args = {'signal_id': none} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- if spawner_args[spawner_keyword]['signal_id'] is not none -%} + + {# setup led signals {--> #} + {# -- leds configuration -- (id, signal_id, x, y, z, roll, pitch, yaw) #} + {%- set uv_leds_macro_parameters = [('b1', spawner_args[spawner_keyword]['signal_id'], x1, y1, z, 0.0, math.radians(90), 0.0), + ('b2', spawner_args[spawner_keyword]['signal_id'], x2, y2, z, 0.0, math.radians(90), math.radians(90)), + ('b3', spawner_args[spawner_keyword]['signal_id'], -x1, -y1, z, 0.0, math.radians(90), math.radians(180)), + ('b4', spawner_args[spawner_keyword]['signal_id'], -x2, -y2, z, 0.0, math.radians(90), math.radians(270))] -%} + + {# #} + + {%- for id_, signal_id_, x_, y_, z_, roll_, pitch_, yaw_ in uv_leds_macro_parameters -%} + + {{ generic.uvled_macro( + parent_link = parent_link, + device_id = spawner_args['name'] + '_' + id_ | string(), + signal_id = signal_id_, + x = x_, + y = y_, + z = z_, + roll = roll_, + pitch = pitch_, + yaw = yaw_) + }} + + {%- endfor -%} + + {%- endif -%} + + {%- endif -%} +{%- endmacro -%} +{# #} + + +{# ========================== VIO components ======================== #} + +{# vio_macro {--> #} +{%- macro vio_macro(sensor_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-vio' -%} + {%- set spawner_description = 'Add a forward-looking fisheye camera and a high-frequency IMU' -%} + {%- set spawner_default_args = {'camera_rate': 45, 'camera_noise': 0.0, 'imu_rate': 100, 'imu_noise': 0.0} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.fisheye_camera_macro( + camera_name = sensor_name, + topic_ns_prefix = 'camera/', + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/' + sensor_name + '_optical', + sensor_base_frame_name = spawner_args['name'] + '/' + sensor_name, + parent_link = parent_link, + frame_rate = spawner_args[spawner_keyword]['camera_rate'], + horizontal_fov = math.radians(180), + image_width = 752, + image_height = 480, + lens_type = 'custom', + lens_c1 = 1.05, + lens_c2 = 4, + lens_f = 1.0, + lens_fun = 'tan', + lens_scale = True, + lens_cutoff_angle = math.radians(90), + lens_texture_size = 512, + min_distance = 0.02, + max_distance = 40, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['camera_noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = 0, + visual_y = 0, + visual_z = 0, + visual_roll = math.radians(90), + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ generic.imu_sensor_macro( + sensor_name = sensor_name + '_imu', + parent_link = parent_link, + update_rate = spawner_args[spawner_keyword]['imu_rate'], + topic_name = '/' + spawner_args['name'] + '/' + sensor_name + '/imu', + frame_name = spawner_args['name'] + '/' + sensor_name, + noise_mean = spawner_args[spawner_keyword]['imu_noise'], + x = 0, + y = 0, + z = 0, + roll = -math.radians(90), + pitch = 0, + yaw = -math.radians(90)) + }} + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# vio_down_macro {--> #} +{%- macro vio_down_macro(sensor_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-vio-down' -%} + {%- set spawner_description = 'Add a downward-looking fisheye camera and a high-frequency IMU' -%} + {%- set spawner_default_args = {'camera_rate': 45, 'camera_noise': 0.0, 'imu_rate': 100, 'imu_noise': 0.0} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + + {{ generic.fisheye_camera_macro( + camera_name = sensor_name, + topic_ns_prefix = 'camera/', + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/' + sensor_name + '_optical', + sensor_base_frame_name = spawner_args['name'] + '/' + sensor_name, + parent_link = parent_link, + frame_rate = spawner_args[spawner_keyword]['camera_rate'], + horizontal_fov = math.radians(180), + image_width = 752, + image_height = 480, + lens_type = 'custom', + lens_c1 = 1.05, + lens_c2 = 4, + lens_f = 1.0, + lens_fun = 'tan', + lens_scale = True, + lens_cutoff_angle = math.radians(90), + lens_texture_size = 512, + min_distance = 0.02, + max_distance = 40, + noise_mean = 0.0, + noise_stddev = spawner_args[spawner_keyword]['camera_noise'], + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + visual_x = 0, + visual_y = 0, + visual_z = 0, + visual_roll = math.radians(90), + visual_pitch = 0, + visual_yaw = 0, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + {{ generic.imu_sensor_macro( + sensor_name = sensor_name + '_imu', + parent_link = parent_link, + update_rate = spawner_args[spawner_keyword]['imu_rate'], + topic_name = '/' + spawner_args['name'] + '/' + sensor_name + '/imu', + frame_name = spawner_args['name'] + '/' + sensor_name, + noise_mean = spawner_args[spawner_keyword]['imu_noise'], + x = 0, + y = 0, + z = 0, + roll = -math.radians(90), + pitch = 0, + yaw = -math.radians(90)) + }} + + + + {{ mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# ==================== miscellaneous components ==================== #} + +{# light_macro {--> #} +{%- macro light_macro(parent_link, max_pitch_rate, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-light' -%} + {%- set spawner_description = 'Add a light on a gimbal to the vehicle. Requires a Gazebo world with enabled shadows' -%} + {%- set spawner_default_args = {'update_rate': 30, 'compensate_tilt': True, 'initial_on': True} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_colored_box_macro('light', 0.01, 0.01, 0.01, 'Yellow', 0, 0, 0, 0, 0, 0) }} + + + + {{ x }} + {{ y }} + {{ z }} + {{ roll }} + {{ pitch }} + {{ yaw }} + {{ parent_link }} + {{ spawner_args[spawner_keyword]['update_rate'] }} + {{ max_pitch_rate }} + {{ spawner_args[spawner_keyword]['initial_on'] }} + {{ spawner_args[spawner_keyword]['compensate_tilt'] }} + + + + {{ parent_link }} + light_macro_link + + 0 1 0 + + -1.57 + 1.57 + 1 + 10 + + + 0.0 + 0.0 + + + + + + {%- endif -%} +{%- endmacro -%} + + +{# magnet_gripper_visualization_macro {--> #} +{%- macro magnet_gripper_visualization_macro(parent_link, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-magnetic-gripper' -%} + {%- set spawner_description = 'Add a magnetic gripper visualization' -%} + {%- set spawner_default_args = none %} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_colored_box_macro('front', 0.005, 0.05, 0.06, 'Black', -0.09, 0, 0.03, 0, 0, 0) }} + {{ generic.visual_colored_box_macro('back', 0.005, 0.05, 0.06, 'Black', 0.09, 0, 0.03, 0, 0, 0) }} + {{ generic.visual_colored_box_macro('down', 0.18, 0.05, 0.005, 'Black', 0, 0, 0, 0, 0, 0) }} + {{ generic.visual_colored_box_macro('up', 0.18, 0.05, 0.005, 'Black', 0, 0, 0.06, 0, 0, 0) }} + + 0 0 0 0 0 0 + + + 0.18 0.05 0.005 + + + + + + + {{ parent_link }} + magnet_gripper_visualization_link + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# omni_ultrasounds_macro {--> #} +{%- macro omni_ultrasounds_macro(sensors_list, parent_link, spawner_args) -%} + + {%- set spawner_keyword = 'enable-omni-ultrasounds' -%} + {%- set spawner_description = 'Add omnidirectional ultrasonic sensors (up, down, 4 in horizontal plane)' -%} + {%- set spawner_default_args = none -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {%- for sensor in sensors_list -%} + + + {{ generic.ultrasonic_sensor_macro( + namespace = spawner_args['name'], + parent_link = parent_link, + suffix = sensor['suffix'], + x = sensor['x'], + y = sensor['y'], + z = sensor['z'], + roll = sensor['roll'], + pitch = sensor['pitch'], + yaw = sensor['yaw']) + }} + + + {%- endfor -%} + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# pendulum_macro {--> #} +{%- macro pendulum_macro(parent_link, x, y, z, spawner_args) -%} + + {%- set spawner_keyword = 'enable-pendulum' -%} + {%- set spawner_description = 'Add a pendulum to the vehicle. Specify the length and mass of the whole chain. Length and mass is uniformly distributed between the links' -%} + {%- set spawner_default_args = {'chain_length': 3.0, 'chain_mass': 0.5, 'num_links': 30, 'radius': 0.01} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ generic.single_chain_pendulum_macro( + parent_link = parent_link, + id = 0, + link_mass = spawner_args[spawner_keyword]['chain_mass'] / spawner_args[spawner_keyword]['num_links'], + link_radius = spawner_args[spawner_keyword]['radius'], + link_length = spawner_args[spawner_keyword]['chain_length'] / spawner_args[spawner_keyword]['num_links'], + joint_offset_x = x, + joint_offset_y = y, + joint_offset_z = z) + }} + + + {%- for id_ in range(1, spawner_args[spawner_keyword]['num_links'] | int) -%} + + + {{ generic.single_chain_pendulum_macro( + parent_link = 'pendulum_chain_' + (id_ - 1)| string() + '_link', + id = id_, + link_mass = spawner_args[spawner_keyword]['chain_mass'] / spawner_args[spawner_keyword]['num_links'], + link_radius = spawner_args[spawner_keyword]['radius'], + link_length = spawner_args[spawner_keyword]['chain_length'] / spawner_args[spawner_keyword]['num_links'], + joint_offset_x = 0, + joint_offset_y = 0, + joint_offset_z = -0.5 * (spawner_args[spawner_keyword]['chain_length'] / spawner_args[spawner_keyword]['num_links'])) + }} + + + {%- endfor -%} + + + {%- endif -%} +{%- endmacro -%} + + +{# safety_led_macro {--> #} +{%- macro safety_led_macro(parent_link, failure_duration_threshold, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-safety-led' -%} + {%- set spawner_description = 'Add a safety LED' -%} + {%- set spawner_default_args = none -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + + + safety_led + {{ failure_duration_threshold }} + 5.0 + {{ x }} + {{ y }} + {{ z }}} + {{ roll }} + {{ pitch }} + {{ yaw }} + + + + {{ parent_link }} + safety_led_link + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# TODO: Timepix is not core #} +{# Timepix detector {--> #} +{%- macro timepix_macro(parent_link, sensor_name, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-timepix' -%} + {%- set spawner_description = 'Add a Timepix radiation detector to the vehicle' -%} + {%- set spawner_default_args = {'material': 'si', 'exposition_time': 0.1, 'thickness': 0.0003} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_colored_box_macro(sensor_name, 0.02, 0.09, 0.02, 'Green', 0, 0.05, 0, 0, 0, 0) }} + + + + {{ spawner_args[spawner_keyword]['exposition_time'] }} + {{ spawner_args[spawner_keyword]['material'] }} + {{ spawner_args[spawner_keyword]['thickness'] }} 0.01408 0.01408 + + + + {{ parent_link }} + {{ sensor_name }}_link + + + + {%- endif -%} + +{%- endmacro -%} + + +{# TODO: Timepix3 is not core #} +{# Timepix3 detector {--> #} +{%- macro timepix3_macro(parent_link, sensor_name, sensor_suffix, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-timepix3' -%} + {%- set spawner_description = 'Add a Timepix3 gamma event camera to the vehicle' -%} + {%- set spawner_default_args = {'material': 'cdte', 'max_message_window': 1.0, 'thickness': 0.002} %} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_colored_box_macro(sensor_name, 0.02, 0.09, 0.02, 'Green', 0.05, 0, 0, 0, 0, 0) }} + + + + {{ material }} + {{ spawner_args[spawner_keyword]['thickness'] }} 0.01408 0.01408 + {{ spawner_args[spawner_keyword]['max_message_window'] }} + {{ sensor_suffix }} + + +<<<<<<< HEAD + + {{ parent_link }} + {{ sensor_name }}_link + + + + {%- endif -%} + +{%- endmacro -%} + + +{# TODO: UWB plugin is not core #} +{# uwb_range_macro {--> #} +{%- macro uwb_range_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + + {%- set spawner_keyword = 'enable-uwb-range' -%} + {%- set spawner_description = 'Add Qorvo DW1000 UWB transceiver. Requires param "signal_id" to be set.' -%} + {%- set spawner_default_args = {'signal_id': none, 'update_rate': 10} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- if spawner_args[spawner_keyword]['signal_id'] is not none -%} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + {{ spawner_args[spawner_keyword]['update_rate'] }} + 1 + + {{ spawner_args[spawner_keyword]['signal_id'] }} + /{{ spawner_args['name'] }}/uwb_range/range + {{ spawner_args['name'] }}/uwb + -150 + 0.1 + + + 6489.6 + 6240.0 + 6739.2 + 14.5 + 2.5 + + + + + {{ parent_link }} + uwb_range_link + true + true + + + + {{ mount }} + +======= + + {{ tilt_update_rate }} + {{ max_pitch_rate }} + {{ max_pitch }} + {{ min_pitch }} + {{ max_roll_rate }} + {{ max_roll }} + {{ min_roll }} + servo_camera_pitch_joint + servo_camera_roll_joint + servo_camera_gimbal_link + {{ parent }} + {{ compensate_tilt_roll }} + {{ compensate_tilt_pitch }} + + + + {{ parent }} + servo_camera_gimbal_link + {{ offset_roll_link_x }} {{ offset_roll_link_y }} {{ offset_roll_link_z }} {{ offset_roll_link_roll }} {{ offset_roll_link_pitch }} {{ offset_roll_link_yaw }} + + 1 0 0 + + -0.5 + 0.5 + 1 + 10 + + + 0.0 + 0.0 + + + +>>>>>>> master + + + + {%- endif -%} + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# water_gun_macro {--> #} +{%- macro water_gun_macro(parent_link, nozzle_offset_x, nozzle_offset_y, nozzle_offset_z, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-water-gun' -%} + {%- set spawner_description = 'Add a water gun for firefighting. Requires a dummy object (spawning_reservoir) to be present in the simulation' -%} + {%- set spawner_default_args = {'muzzle_velocity': 15.0, 'num_particles': 30, 'spread': 1.0, 'spawning_reservoir': 'the_void'} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_colored_box_macro('water_bag', 0.02, 0.08, 0.03, 'Blue', 0, 0, 0, 0, 0, 0) }} + + {{ nozzle_offset_x - 0.08/2 }} {{ nozzle_offset_y }} {{ nozzle_offset_z - 0.006/2 }} 0 {{ math.radians(90) }} 0 + + + 0.08 + 0.006 + + + + + + + + + + {{ spawner_args[spawner_keyword]['muzzle_velocity'] }} + {{ nozzle_offset_x }} + {{ nozzle_offset_y }} + {{ nozzle_offset_z }} + {{ spawner_args[spawner_keyword]['spread'] }} + {{ spawner_args[spawner_keyword]['num_particles'] }} + {{ spawner_args[spawner_keyword]['spawning_reservoir'] }} + + + + {{ parent_link }} + water_gun_link + + + + {%- endif -%} +{%- endmacro -%} +{# #} + +{# whycon_box_macro {--> #} +{%- macro whycon_box_macro(parent_link, x, y, z, roll, pitch, yaw, spawner_args) -%} + + {%- set spawner_keyword = 'enable-whycon-box' -%} + {%- set spawner_description = 'Add a Whycon box for relative localization using visual markers' -%} + {%- set spawner_default_args = none -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {%- set mesh_file = 'model://mrs_robots_description/meshes/sensors/whycon_box.dae' -%} + {%- set mesh_scale = '1 1 1' -%} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_mesh_textured_macro("whycon_box", mesh_file, mesh_scale, x, y, z, roll, pitch, yaw) }} + + + + {{ parent_link }} + whycon_box_link + + + + {%- endif -%} + +{%- endmacro -%} + + +{# ==================== Unsupported components ==================== #} + +{# TODO: not supported in the current version #} +{# TODO: not used #} +{# gps_satelites_blocking_macro {--> #} +{%- macro gps_satelites_blocking_macro(parent_link) -%} + + 0 0 0 0 {{ -math.radians(90) }} 0 + {{ generic.zero_inertial_macro() }} + + false + 2 + + + + 4 + 1 + -1.1 + 1.1 + + + 4 + 1 + -0.9 + 0.9 + + + + 0.5 + 50 + 0.5 + + + + gps_sat_blocking + gps_blocking + + + + + + {{ parent_link }} + gps_blocking_link + +{%- endmacro -%} + + +{# TODO: not supported in the current version #} +{# TODO: not used #} +{# parachute_macro {--> #} +{%- macro parachute_macro(parent_link, x, y, z, roll, pitch, yaw) -%} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + + 0 0 0 0 0 0 + + + 0.1 + 0.048 + + + + + + + + + + 1.225 + 500 + 0.25 + 0 + 0 + -1.56 + + + + {{ parent_link }} + parachute_link + + + +{%- endmacro -%} +{# #} + + diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f330.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f330.sdf.jinja index eb0ebb9..cc32c6c 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f330.sdf.jinja +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f330.sdf.jinja @@ -1,7 +1,8 @@ - {%- import "mrs_robots_description/sdf/component_snippets.sdf.jinja" as components -%} + {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} @@ -26,12 +27,7 @@ {%- set leg_height = 0.11 -%} {# [m] #} {%- set pixhawk_offset = 0.0073 -%} {# [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 %} + {%- set root = 'base_link' -%} {# #} {# Motor constants {--> #} @@ -42,7 +38,7 @@ {%- set time_constant_down = 1.0 / 40.0 -%} {# [s] #} {%- set max_rot_velocity = 1 -%} {# [rad/s] #} {%- set rotor_drag_coefficient = 0.001 -%} {# orig 8.06428e-04 #} - {%- set rolling_moment_coefficient = "1.0e-6" -%} + {%- set rolling_moment_coefficient = '1.0e-6' -%} {# #} {# Inertia constants {--> #} @@ -53,27 +49,27 @@ {# Meshes {--> #} {# Drone parts {--> #} - {%- set central_board_mesh_top = "model://mrs_robots_description/meshes/dji/f330/f330_top_board.dae" -%} - {%- set central_board_mesh_bottom = "model://mrs_robots_description/meshes/dji/f330/f330_bottom_board.dae" -%} - {%- set arm_mesh_file = "model://mrs_robots_description/meshes/dji/f330/f330_arm.dae" -%} - {%- set leg_mesh_file = "model://mrs_robots_description/meshes/dji/f330/f330_leg_110mm.dae"-%} - {%- set motor_mesh_file = "model://mrs_robots_description/meshes/dji/f330/f330_rotor.dae" -%} - {%- set prop_mesh_file = "model://mrs_robots_description/meshes/dji/f330/f330_prop.dae"-%} - {%- set pixhawk_mesh_file = "model://mrs_robots_description/meshes/sensors/pixhawk.dae" -%} - {%- set nuc_mesh_file = "model://mrs_robots_description/meshes/dji/f330/f330_nuc.dae" -%} + {%- set central_board_mesh_top = 'model://mrs_robots_description/meshes/dji/f330/f330_top_board.dae' -%} + {%- set central_board_mesh_bottom = 'model://mrs_robots_description/meshes/dji/f330/f330_bottom_board.dae' -%} + {%- set arm_mesh_file = 'model://mrs_robots_description/meshes/dji/f330/f330_arm.dae' -%} + {%- set leg_mesh_file = 'model://mrs_robots_description/meshes/dji/f330/f330_leg_110mm.dae'-%} + {%- set motor_mesh_file = 'model://mrs_robots_description/meshes/dji/f330/f330_rotor.dae' -%} + {%- set prop_mesh_file = 'model://mrs_robots_description/meshes/dji/f330/f330_prop.dae'-%} + {%- set pixhawk_mesh_file = 'model://mrs_robots_description/meshes/sensors/pixhawk.dae' -%} + {%- set nuc_mesh_file = 'model://mrs_robots_description/meshes/dji/f330/f330_nuc.dae' -%} {# #} - {# Holders {--> #} - {%- set battery_mount_mesh = "model://mrs_robots_description/meshes/dji/f330/f330_battery_holder.dae" -%} - {%- set bluefox_mount_mesh = "model://mrs_robots_description/meshes/dji/f330/f330_bluefox_holder.dae" -%} - {%- set compton_mount_mesh = "model://mrs_robots_description/meshes/dji/f330/f330_compton_holder.dae" -%} + {# Mounts {--> #} + {%- set battery_mount_mesh = 'model://mrs_robots_description/meshes/dji/f330/f330_battery_holder.dae' -%} + {%- set bluefox_mount_mesh = 'model://mrs_robots_description/meshes/dji/f330/f330_bluefox_holder.dae' -%} + {%- set compton_mount_mesh = 'model://mrs_robots_description/meshes/dji/f330/f330_compton_holder.dae' -%} {# #} {# Scales {--> #} - {%- set 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" -%} + {%- set 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' -%} {# #} {# #} @@ -96,14 +92,14 @@ - - - + {# ================================================================== #} + {# || bare body definitions || #} + {# ================================================================== #} - {{ components.multirotor_physics_macro( + {{ generic.multirotor_physics_macro( mass = mass, body_radius = body_radius, body_height = body_height, @@ -120,108 +116,108 @@ - {{ components.visual_mesh_macro( - name = "lower_central_board", + {{ generic.visual_mesh_macro( + name = 'lower_central_board', mesh_file = central_board_mesh_bottom, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = 0.001, roll = 0, pitch = 0, - yaw = -components.rad90) + yaw = -math.radians(90)) }} - {{ components.visual_mesh_macro( - name = "upper_central_board", + {{ generic.visual_mesh_macro( + name = 'upper_central_board', mesh_file = central_board_mesh_top, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = 0, roll = 0, pitch = 0, - yaw = -components.rad90) + yaw = -math.radians(90)) }} - {{ components.visual_mesh_macro( - name = "front_right_arm", + {{ generic.visual_mesh_macro( + name = 'front_right_arm', mesh_file = arm_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = 0, y = 0, z = 0, roll = 0, pitch = 0, - yaw = components.rad(-135)) + yaw = math.radians(-135)) }} - {{ components.visual_mesh_macro( - name = "front_left_arm", + {{ generic.visual_mesh_macro( + name = 'front_left_arm', mesh_file = arm_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = 0, y = 0, z = 0, roll = 0, pitch = 0, - yaw = components.rad(135)) + yaw = math.radians(135)) }} - {{ components.visual_mesh_macro( - name = "back_right_arm", + {{ generic.visual_mesh_macro( + name = 'back_right_arm', mesh_file = arm_mesh_file, mesh_scale = mesh_scale, - color = "Red", + color = 'Red', x = 0, y = 0, z = 0, roll = 0, pitch = 0, - yaw = components.rad(45)) + yaw = math.radians(45)) }} - {{ components.visual_mesh_macro( - name = "back_left_arm", + {{ generic.visual_mesh_macro( + name = 'back_left_arm', mesh_file = arm_mesh_file, mesh_scale = mesh_scale, - color = "Red", + color = 'Red', x = 0, y = 0, z = 0, roll = 0, pitch = 0, - yaw = components.rad(-45)) + yaw = math.radians(-45)) }} - - {{ components.visual_mesh_macro( - name = "NUC", + + {{ generic.visual_mesh_macro( + name = 'NUC', mesh_file = nuc_mesh_file, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = 0, roll = 0, pitch = 0, - yaw = components.rad(270)) + yaw = math.radians(270)) }} - - {{ components.visual_mesh_macro( - name = "pixhawk", + + {{ generic.visual_mesh_macro( + name = 'pixhawk', mesh_file = pixhawk_mesh_file, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = pixhawk_offset, @@ -231,123 +227,127 @@ }} - - {{ components.leg_macro_collision_offset( - name = "front_right_leg", + + {{ generic.leg_collision_offset_macro( + name = 'front_right_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale, - color = "Black", - parent = root, - x = arm_length * components.sin45, - y = -arm_length * components.sin45, + color = 'Black', + x = arm_length * math.sin(math.radians(45)), + y = -arm_length * math.sin(math.radians(45)), z = 0.008, roll = 0, pitch = 0, - yaw = components.rad(135), + yaw = math.radians(135), collision_height = leg_height, collision_radius = leg_radius, - collision_offset = -0.055) + offset_x = 0, + offset_y = 0, + offset_z = -0.055) }} - {{ components.leg_macro_collision_offset( - name = "front_left_leg", + {{ generic.leg_collision_offset_macro( + name = 'front_left_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale, - color = "Black", - parent = root, - x = arm_length * components.sin45, - y = arm_length * components.sin45, + color = 'Black', + x = arm_length * math.sin(math.radians(45)), + y = arm_length * math.sin(math.radians(45)), z = 0.008, roll = 0, pitch = 0, - yaw = components.rad(-135), + yaw = math.radians(-135), collision_height = leg_height, collision_radius = leg_radius, - collision_offset = -0.055) + offset_x = 0, + offset_y = 0, + offset_z = -0.055) }} - {{ components.leg_macro_collision_offset( - name = "back_right_leg", + {{ generic.leg_collision_offset_macro( + name = 'back_right_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale, - color = "Red", - parent = root, - x = -arm_length * components.sin45, - y = arm_length * components.sin45, + color = 'Red', + x = -arm_length * math.sin(math.radians(45)), + y = arm_length * math.sin(math.radians(45)), z = 0.008, roll = 0, pitch = 0, - yaw = components.rad(45), + yaw = math.radians(45), collision_height = leg_height, collision_radius = leg_radius, - collision_offset = -0.055) + offset_x = 0, + offset_y = 0, + offset_z = -0.055) }} - {{ components.leg_macro_collision_offset( - name = "back_left_leg", + {{ generic.leg_collision_offset_macro( + name = 'back_left_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale, - color = "Red", - parent = root, - x = -arm_length * components.sin45, - y = -arm_length * components.sin45, + color = 'Red', + x = -arm_length * math.sin(math.radians(45)), + y = -arm_length * math.sin(math.radians(45)), z = 0.008, roll = 0, pitch = 0, - yaw = components.rad(-45), + yaw = math.radians(-45), collision_height = leg_height, collision_radius = leg_radius, - collision_offset = -0.055) + offset_x = 0, + offset_y = 0, + offset_z = -0.055) }} - - {{ components.visual_mesh_macro( - name = "front_right_motor", + + {{ generic.visual_mesh_macro( + name = 'front_right_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Grey", - x = arm_length * components.sin45, - y = -arm_length * components.sin45, + color = 'Grey', + x = arm_length * math.sin(math.radians(45)), + y = -arm_length * math.sin(math.radians(45)), z = rotor_offset_top, roll = 0, pitch = 0, yaw = 0) }} - {{ components.visual_mesh_macro( - name = "front_left_motor", + {{ generic.visual_mesh_macro( + name = 'front_left_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Grey", - x = arm_length * components.sin45, - y = arm_length * components.sin45, + color = 'Grey', + x = arm_length * math.sin(math.radians(45)), + y = arm_length * math.sin(math.radians(45)), z = rotor_offset_top, roll = 0, pitch = 0, yaw = 0) }} - {{ components.visual_mesh_macro( - name = "back_right_motor", + {{ generic.visual_mesh_macro( + name = 'back_right_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Grey", - x = -arm_length * components.sin45, - y = -arm_length * components.sin45, + color = 'Grey', + x = -arm_length * math.sin(math.radians(45)), + y = -arm_length * math.sin(math.radians(45)), z = rotor_offset_top, roll = 0, pitch = 0, yaw = 0) }} - {{ components.visual_mesh_macro( - name = "back_left_motor", + {{ generic.visual_mesh_macro( + name = 'back_left_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Grey", - x = -arm_length * components.sin45, - y = arm_length * components.sin45, + color = 'Grey', + x = -arm_length * math.sin(math.radians(45)), + y = arm_length * math.sin(math.radians(45)), z = rotor_offset_top, roll = 0, pitch = 0, @@ -355,27 +355,26 @@ }} - {% if use_battery_mount %} - - {{ components.visual_mesh_macro( - name = "battery_mount", + + {{ generic.visual_mesh_macro( + name = 'battery_mount', mesh_file = battery_mount_mesh, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = 0, roll = 0, pitch = 0, - yaw = -components.rad90) + yaw = -math.radians(90)) }} - {{ components.visual_colored_box_macro( - name = "battery", + {{ generic.visual_colored_box_macro( + name = 'battery', size_x = 0.134, size_y = 0.03, size_z = 0.04, - color = "Grey", + color = 'Grey', x = -0.008, y = 0, z = 0.084, @@ -383,16 +382,81 @@ pitch = 0, yaw = 0) }} + - {% endif %} + + {# component mounts and holders (these must be passed as args into components, or must be called) {--> #} + + {# Bluefox mount front {--> #} + {%- set bluefox_mount_front -%} + {{ generic.visual_link_macro( + name = 'bluefox_mount_front', + mesh_file = bluefox_mount_mesh, + mesh_scale = mesh_scale, + color = 'DarkGrey', + x = 0, + y = 0, + z = 0, + roll = math.radians(180), + pitch = math.radians(180), + yaw = math.radians(90), + parent_link = root) + }} + {%- endset -%} + {# #} + + {# #} - - {{ components.prop_macro( - direction = "ccw", + {# Propellers {--> #} + {%- set prop_list = [ + { + 'motor_number': 0, + 'direction': 'ccw', + 'x': arm_length * math.sin(math.radians(45)), + 'y': -arm_length * math.sin(math.radians(45)), + 'z': rotor_offset_top + prop_offset_top, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'DarkGrey' + }, + { + 'motor_number': 1, + 'direction': 'ccw', + 'x': -arm_length * math.sin(math.radians(45)), + 'y': arm_length * math.sin(math.radians(45)), + 'z': rotor_offset_top + prop_offset_top, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'DarkGrey' + }, + { + 'motor_number': 2, + 'direction': 'cw', + 'x': arm_length * math.sin(math.radians(45)), + 'y': arm_length * math.sin(math.radians(45)), + 'z': rotor_offset_top + prop_offset_top, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'DarkGrey' + }, + { + 'motor_number': 3, + 'direction': 'cw', + 'x': -arm_length * math.sin(math.radians(45)), + 'y': -arm_length * math.sin(math.radians(45)), + 'z': rotor_offset_top + prop_offset_top, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'DarkGrey' + } + ] + -%} + {{ components.propellers_macro( + prop_list = prop_list, rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, motor_constant = motor_constant, moment_constant = moment_constant, @@ -402,159 +466,47 @@ 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 = "DarkGrey", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_ccw, - x = arm_length * components.sin45, - y = -arm_length * components.sin45, - z = rotor_offset_top + prop_offset_top, - roll = 0, - pitch = 0, - yaw = 0, - ixx = prop_ixx, - ixy = prop_ixy, - ixz = prop_ixz, - iyy = prop_iyy, - iyz = prop_iyz, - izz = prop_izz) + meshes_z_offset = 0, + prop_ixx = prop_ixx, + prop_ixy = prop_ixy, + prop_ixz = prop_ixz, + prop_iyy = prop_iyy, + prop_iyz = prop_iyz, + prop_izz = prop_izz, + spawner_args = spawner_args) }} + {# #} - {{ components.prop_macro( - direction = "ccw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - 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 = "DarkGrey", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_ccw, - x = -arm_length * components.sin45, - y = arm_length * components.sin45, - z = rotor_offset_top + prop_offset_top, - roll = 0, - pitch = 0, - yaw = 0, - ixx = prop_ixx, - ixy = prop_ixy, - ixz = prop_ixz, - iyy = prop_iyy, - iyz = prop_iyz, - izz = prop_izz) - }} + {# ================================================================== #} + {# || compulsory sensor definitions || #} + {# ================================================================== #} - {{ components.prop_macro( - direction = "cw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - 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 = "DarkGrey", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_cw, - x = arm_length * components.sin45, - y = arm_length * components.sin45, - z = rotor_offset_top + prop_offset_top, - roll = 0, - pitch = 0, - yaw = 0, - ixx = prop_ixx, - ixy = prop_ixy, - ixz = prop_ixz, - iyy = prop_iyy, - iyz = prop_iyz, - izz = prop_izz) - }} + + {%- set imu_topic = '/imu' -%} + {%- set mag_topic = '/mag' -%} + {%- set baro_topic = '/baro' -%} + {%- set lidar_topic = '/lidar' -%} - {{ components.prop_macro( - direction = "cw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - 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 = "DarkGrey", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_cw, - x = -arm_length * components.sin45, - y = -arm_length * components.sin45, - z = rotor_offset_top + prop_offset_top, - 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) + + {{ generic.gazebo_groundtruth_macro( + home_latitude = 0, + home_longitude = 0, + home_altitude = 0) }} - {{ components.gps_macro( - gps_name = "gps0", + {{ generic.gazebo_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_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, @@ -567,36 +519,39 @@ - {{ components.magnetometer_plugin_macro( + {{ generic.gazebo_magnetometer_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) + mag_topic = mag_topic) }} - {{ components.barometer_plugin_macro( - baro_topic = "/baro", + {{ generic.gazebo_barometer_macro( + baro_topic = baro_topic, pub_rate = 50, baro_drift_pa_per_sec = 0) }} + + {{ generic.gazebo_mavlink_interface_macro( + imu_topic = imu_topic, + mag_topic = mag_topic, + baro_topic = baro_topic, + lidar_topic = lidar_topic, + mavlink_config = spawner_args['mavlink_config']) + }} + + - {{ components.imu_plugin_macro( - imu_name = "imu", + + {{ generic.gazebo_imu_macro( + imu_name = 'imu', parent_link = root, - imu_topic = "/imu", + imu_topic = '/imu', gyroscope_noise_density = 0.00018665, gyroscope_random_walk = 0.000038785, gyroscope_bias_correlation_time = 1000.0, @@ -614,118 +569,96 @@ }} - - - + + + {# ================================================================== #} + {# || optional sensor definitions || #} + {# ================================================================== #} {# Ground truth {--> #} - {% if enable_ground_truth %} - - {{ components.odometry_plugin_macro( - odometry_sensor_name = "ground_truth", + {{ components.ground_truth_macro( 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) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# ======================= rangefinder sensors ====================== #} - {# 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, + {# Garmin down {--> #} + {{ components.garmin_down_macro( parent_link = root, - orientation = "", x = -0.067, y = 0, z = 0.007, roll = 0, - pitch = components.rad(90), - yaw = 0) + pitch = math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# ========================== camera sensors ======================== #} - + {# Realsense placements {--> #} - {% if enable_realsense_front %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="", + {# Realsense front {--> #} + {{ components.realsense_front_macro( + camera_name = 'rgbd', + camera_suffix='', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.07, y = 0, z = -0.025, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - + {# #} - + {# VIO placements {--> #} - {% if enable_vio %} - + {# VIO (classic) {--> #} {{ components.vio_macro( - namespace = namespace, - sensor_name = "vio", + sensor_name = 'vio', parent_link = root, - frame_rate = 45, - noise = 0.0, - imu_rate = 125, - imu_noise_mean = 0.0, x = 0.107, y = 0, z = -0.002, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + mount = bluefox_mount_front, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - {% if enable_vio_down %} - - {{ components.vio_macro( - namespace = namespace, - sensor_name = "vio", + {# VIO down {--> #} + {{ components.vio_down_macro( + sensor_name = 'vio', parent_link = root, - frame_rate = 45, - noise = 0.0, - imu_rate = 125, - imu_noise_mean = 0.0, x = 0.107, y = 0, z = -0.002, roll = 0, - pitch = components.rad90, - yaw = 0) + pitch = math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} - + {# #} + + {# #} 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 5cadfe1..e723599 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 @@ -1,7 +1,8 @@ - + - {%- import "mrs_robots_description/sdf/component_snippets.sdf.jinja" as components -%} + {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} @@ -24,13 +25,7 @@ {%- set leg_radius = 0.012 -%} {# [m] #} {%- set leg_height = 0.17 -%} {# [m] #} {%- set pixhawk_offset = -0.0075 -%} {# [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 %} + {%- set root = 'base_link' -%} {# #} {# Motor constants {--> #} @@ -41,7 +36,7 @@ {%- set time_constant_down = 1.0 / 40.0 -%} {# [s] #} {%- set max_rot_velocity = 1 -%} {# [rad/s] #} {%- set rotor_drag_coefficient = 0.001 -%} {# orig 8.06428e-04 #} - {%- set rolling_moment_coefficient = "1.0e-6" -%} + {%- set rolling_moment_coefficient = '1.0e-6' -%} {# #} {# Inertia constants {--> #} @@ -52,33 +47,33 @@ {# Meshes {--> #} {# Drone parts {--> #} - {%- set central_board_mesh_top = "model://mrs_robots_description/meshes/dji/f450/dji_f450_central_top.dae" -%} - {%- set central_board_mesh_bottom = "model://mrs_robots_description/meshes/dji/f450/dji_f450_central_bottom.dae" -%} - {%- set arm_mesh_file = "model://mrs_robots_description/meshes/dji/f450/dji_f450_arm.dae" -%} - {%- set leg_mesh_file = "model://mrs_robots_description/meshes/dji/f450/dji_f450_leg_170mm.dae"-%} - {%- set motor_mesh_file = "model://mrs_robots_description/meshes/dji/f450/dji_f450_rotor.dae" -%} - {%- set prop_mesh_file = "model://mrs_robots_description/meshes/dji/f450/dji_f450_prop.dae"-%} - {%- set pixhawk_mesh_file = "model://mrs_robots_description/meshes/sensors/pixhawk.dae" -%} - {%- set nuc_mesh_file = "model://mrs_robots_description/meshes/dji/f450/dji_f450_nuc.dae" -%} + {%- set central_board_mesh_top = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_central_top.dae' -%} + {%- set central_board_mesh_bottom = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_central_bottom.dae' -%} + {%- set arm_mesh_file = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_arm.dae' -%} + {%- set leg_mesh_file = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_leg_170mm.dae'-%} + {%- set motor_mesh_file = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_rotor.dae' -%} + {%- set prop_mesh_file = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_prop.dae'-%} + {%- set pixhawk_mesh_file = 'model://mrs_robots_description/meshes/sensors/pixhawk.dae' -%} + {%- set nuc_mesh_file = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_nuc.dae' -%} {# #} - {# Holders {--> #} - {%- set battery_mount_mesh = "model://mrs_robots_description/meshes/dji/f450/dji_f450_battery_mount.dae" -%} - {%- set bluefox_mount_mesh = "model://mrs_robots_description/meshes/dji/f450/dji_f450_bluefox_mount.dae" -%} - {%- set ouster_mount_mesh = "model://mrs_robots_description/meshes/dji/f450/dji_f450_ouster_mount.dae" -%} - {%- set garmin_mount_mesh = "model://mrs_robots_description/meshes/dji/f450/dji_f450_garmin_mount.dae" -%} - {%- set rplidar_mount_mesh = "model://mrs_robots_description/meshes/dji/f450/dji_f450_rplidar_mount.dae" -%} - {%- set realsense_front_mount_mesh = "model://mrs_robots_description/meshes/dji/f450/dji_f450_realsense_front_pitched_0_mount.dae" -%} - {%- set realsense_front_pitched_mount_mesh = "model://mrs_robots_description/meshes/dji/f450/dji_f450_realsense_front_pitched_45_mount.dae" -%} - {%- set realsense_front_down_mount_mesh = "model://mrs_robots_description/meshes/dji/f450/dji_f450_realsense_front_pitched_90_mount.dae" -%} - {%- set uvdar_mount_mesh = "model://mrs_robots_description/meshes/dji/f450/dji_f450_uvdar_mount.dae" -%} + {# Mounts {--> #} + {%- set battery_mount_mesh = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_battery_mount.dae' -%} + {%- set bluefox_mount_mesh = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_bluefox_mount.dae' -%} + {%- set ouster_mount_mesh = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_ouster_mount.dae' -%} + {%- set garmin_mount_mesh = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_garmin_mount.dae' -%} + {%- set rplidar_mount_mesh = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_rplidar_mount.dae' -%} + {%- set realsense_front_mount_mesh = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_realsense_front_pitched_0_mount.dae' -%} + {%- set realsense_front_pitched_mount_mesh = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_realsense_front_pitched_45_mount.dae' -%} + {%- set realsense_front_down_mount_mesh = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_realsense_front_pitched_90_mount.dae' -%} + {%- set uvdar_mount_mesh = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_uvdar_mount.dae' -%} {# #} {# Scales {--> #} - {%- set 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" -%} + {%- set 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' -%} {# #} {# #} @@ -101,14 +96,14 @@ - - - + {# ================================================================== #} + {# || bare body definitions || #} + {# ================================================================== #} - {{ components.multirotor_physics_macro( + {{ generic.multirotor_physics_macro( mass = mass, body_radius = body_radius, body_height = body_height, @@ -125,11 +120,11 @@ - {{ components.visual_mesh_macro( - name = "lower_central_board", + {{ generic.visual_mesh_macro( + name = 'lower_central_board', mesh_file = central_board_mesh_bottom, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = - body_height / 2, @@ -138,11 +133,11 @@ yaw = 0) }} - {{ components.visual_mesh_macro( - name = "upper_central_board", + {{ generic.visual_mesh_macro( + name = 'upper_central_board', mesh_file = central_board_mesh_top, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = arm_height - body_height / 2, @@ -153,65 +148,65 @@ - {{ components.visual_mesh_macro( - name = "front_right_arm", + {{ generic.visual_mesh_macro( + name = 'front_right_arm', mesh_file = arm_mesh_file, mesh_scale = mesh_scale, - color = "White", - x = board_radius * components.sin45, - y = -board_radius * components.sin45, + color = 'White', + x = board_radius * math.sin(math.radians(45)), + y = -board_radius * math.sin(math.radians(45)), z = arm_height - body_height / 2, roll = 0, pitch = 0, - yaw = components.rad(-45)) + yaw = math.radians(-45)) }} - {{ components.visual_mesh_macro( - name = "front_left_arm", + {{ generic.visual_mesh_macro( + name = 'front_left_arm', mesh_file = arm_mesh_file, mesh_scale = mesh_scale, - color = "White", - x = board_radius * components.sin45, - y = board_radius * components.sin45, + color = 'White', + x = board_radius * math.sin(math.radians(45)), + y = board_radius * math.sin(math.radians(45)), z = arm_height - body_height / 2, roll = 0, pitch = 0, - yaw = components.rad(45)) + yaw = math.radians(45)) }} - {{ components.visual_mesh_macro( - name = "back_right_arm", + {{ generic.visual_mesh_macro( + name = 'back_right_arm', mesh_file = arm_mesh_file, mesh_scale = mesh_scale, - color = "Red", - x = -board_radius * components.sin45, - y = -board_radius * components.sin45, + color = 'Red', + x = -board_radius * math.sin(math.radians(45)), + y = -board_radius * math.sin(math.radians(45)), z = arm_height - body_height / 2, roll = 0, pitch = 0, - yaw = components.rad(-135)) + yaw = math.radians(-135)) }} - {{ components.visual_mesh_macro( - name = "back_left_arm", + {{ generic.visual_mesh_macro( + name = 'back_left_arm', mesh_file = arm_mesh_file, mesh_scale = mesh_scale, - color = "Red", - x = -board_radius * components.sin45, - y = board_radius * components.sin45, + color = 'Red', + x = -board_radius * math.sin(math.radians(45)), + y = board_radius * math.sin(math.radians(45)), z = arm_height - body_height / 2, roll = 0, pitch = 0, - yaw = components.rad(135)) + yaw = math.radians(135)) }} - - {{ components.visual_mesh_macro( - name = "NUC", + + {{ generic.visual_mesh_macro( + name = 'NUC', mesh_file = nuc_mesh_file, mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = 0.023, @@ -221,12 +216,12 @@ }} - - {{ components.visual_mesh_macro( - name = "pixhawk", + + {{ generic.visual_mesh_macro( + name = 'pixhawk', mesh_file = pixhawk_mesh_file, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = -body_height / 2 - pixhawk_offset, @@ -236,119 +231,119 @@ }} - - {{ components.leg_macro( - name = "front_right_leg", + + {{ generic.leg_macro( + name = 'front_right_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale_milimeters, - color = "White", + color = 'White', parent = root, - x = (arm_length - leg_offset_r) * components.sin45, - y = -(arm_length - leg_offset_r) * components.sin45, + x = (arm_length - leg_offset_r) * math.sin(math.radians(45)), + y = -(arm_length - leg_offset_r) * math.sin(math.radians(45)), z = arm_height - body_height/2 - leg_offset_z, roll = 0, pitch = 0, - yaw = components.rad(135), + yaw = math.radians(135), collision_height = leg_height, collision_radius = leg_radius) }} - {{ components.leg_macro( - name = "front_left_leg", + {{ generic.leg_macro( + name = 'front_left_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale_milimeters, - color = "White", + color = 'White', parent = root, - x = (arm_length - leg_offset_r) * components.sin45, - y = (arm_length - leg_offset_r) * components.sin45, + x = (arm_length - leg_offset_r) * math.sin(math.radians(45)), + y = (arm_length - leg_offset_r) * math.sin(math.radians(45)), z = arm_height - body_height/2 - leg_offset_z, roll = 0, pitch = 0, - yaw = components.rad(-135), + yaw = math.radians(-135), collision_height = leg_height, collision_radius = leg_radius) }} - {{ components.leg_macro( - name = "back_right_leg", + {{ generic.leg_macro( + name = 'back_right_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale_milimeters, - color = "Red", + color = 'Red', parent = root, - x = -(arm_length - leg_offset_r) * components.sin45, - y = -(arm_length - leg_offset_r) * components.sin45, + x = -(arm_length - leg_offset_r) * math.sin(math.radians(45)), + y = -(arm_length - leg_offset_r) * math.sin(math.radians(45)), z = arm_height - body_height/2 - leg_offset_z, roll = 0, pitch = 0, - yaw = components.rad(45), + yaw = math.radians(45), collision_height = leg_height, collision_radius = leg_radius) }} - {{ components.leg_macro( - name = "back_left_leg", + {{ generic.leg_macro( + name = 'back_left_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale_milimeters, - color = "Red", + color = 'Red', parent = root, - x = -(arm_length - leg_offset_r) * components.sin45, - y = (arm_length - leg_offset_r) * components.sin45, + x = -(arm_length - leg_offset_r) * math.sin(math.radians(45)), + y = (arm_length - leg_offset_r) * math.sin(math.radians(45)), z = arm_height - body_height/2 - leg_offset_z, roll = 0, pitch = 0, - yaw = components.rad(-45), + yaw = math.radians(-45), collision_height = leg_height, collision_radius = leg_radius) }} - - {{ components.visual_mesh_macro( - name = "front_right_motor", + + {{ generic.visual_mesh_macro( + name = 'front_right_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", - x = arm_length * components.sin45, - y = -arm_length * components.sin45, + color = 'Black', + x = arm_length * math.sin(math.radians(45)), + y = -arm_length * math.sin(math.radians(45)), z = arm_height - body_height / 2, roll = 0, pitch = 0, yaw = 0) }} - {{ components.visual_mesh_macro( - name = "front_left_motor", + {{ generic.visual_mesh_macro( + name = 'front_left_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", - x = arm_length * components.sin45, - y = arm_length * components.sin45, + color = 'Black', + x = arm_length * math.sin(math.radians(45)), + y = arm_length * math.sin(math.radians(45)), z = arm_height - body_height / 2, roll = 0, pitch = 0, yaw = 0) }} - {{ components.visual_mesh_macro( - name = "back_right_motor", + {{ generic.visual_mesh_macro( + name = 'back_right_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", - x = -arm_length * components.sin45, - y = -arm_length * components.sin45, + color = 'Black', + x = -arm_length * math.sin(math.radians(45)), + y = -arm_length * math.sin(math.radians(45)), z = arm_height - body_height / 2, roll = 0, pitch = 0, yaw = 0) }} - {{ components.visual_mesh_macro( - name = "back_left_motor", + {{ generic.visual_mesh_macro( + name = 'back_left_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", - x = -arm_length * components.sin45, - y = arm_length * components.sin45, + color = 'Black', + x = -arm_length * math.sin(math.radians(45)), + y = arm_length * math.sin(math.radians(45)), z = arm_height - body_height / 2, roll = 0, pitch = 0, @@ -356,180 +351,233 @@ }} - {% if use_battery_mount %} - - {{ components.visual_mesh_macro( - name = "battery_mount", + + {{ generic.visual_mesh_macro( + name = 'battery_mount', mesh_file = battery_mount_mesh, mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = -body_height / 2 - 0.028, roll = 0, pitch = 0, - yaw = components.rad90) + yaw = math.radians(90)) }} - {{ components.visual_colored_box_macro( - name = "battery", + {{ generic.visual_colored_box_macro( + name = 'battery', size_x = 0.13, size_y = 0.042, size_z = 0.044, - color = "Grey", + color = 'Grey', x = 0, y = 0, z = -body_height / 2 - 0.027, roll = 0, pitch = 0, - yaw = components.rad90) + yaw = math.radians(90)) }} - {% endif %} - {% if enable_ouster or enable_velodyne %} - - {{ components.visual_mesh_macro( - name = "ouster_mount", - mesh_file = ouster_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = 0, - y = 0, - z = 0.061, - roll = 0, - pitch = 0, - yaw = 0) - }} - - {% endif %} - - {% if enable_rplidar or enable_scanse_sweep %} - - {{ components.visual_mesh_macro( - name = "rplidar_mount", - mesh_file = rplidar_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = 0, - y = 0, - z = 0.068, - roll = 0, - pitch = 0, - yaw = components.rad90) - }} - - {% endif %} - - {% if enable_rangefinder %} - - {{ components.visual_mesh_macro( - name = "garmin_mount", - mesh_file = garmin_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = -0.055, - y = 0, - z = -(arm_height + 0.016), - roll = components.rad90, - pitch = 0, - yaw = components.rad90) - }} - - {% endif %} - - {% if enable_bluefox_camera or enable_bluefox_camera_reverse %} - - {{ components.visual_mesh_macro( - name = "Bluefox_mount", + {# component mounts and holders (these must be passed as args into components, or must be called) {--> #} + + {# Garmin down mount {--> #} + {%- set garmin_down_mount -%} + {{ generic.visual_link_macro( + name = 'garmin_mount', + mesh_file = garmin_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = -0.055, + y = 0, + z = -(arm_height + 0.016), + roll = math.radians(90), + pitch = 0, + yaw = math.radians(90), + parent_link = root) + }} + {%- endset -%} + {# #} + + {# Lidar mount 2D {--> #} + {%- set lidar_mount_2d -%} + {{ generic.visual_link_macro( + name = 'lidar_mount_2d', + mesh_file = rplidar_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = 0, + y = 0, + z = 0.068, + roll = 0, + pitch = 0, + yaw = math.radians(90), + parent_link = root) + }} + {%- endset -%} + {# #} + + {# Lidar mount 3D {--> #} + {%- set lidar_mount_3d -%} + {{ generic.visual_link_macro( + name = 'lidar_mount_3d', + mesh_file = ouster_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = 0, + y = 0, + z = 0.061, + roll = 0, + pitch = 0, + yaw = 0, + parent_link = root) + }} + {%- endset -%} + {# #} + + {# Bluefox mount {--> #} + {%- set bluefox_mount -%} + {{ generic.visual_link_macro( + name = 'bluefox_mount', mesh_file = bluefox_mount_mesh, mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", + color = 'DarkGrey', x = 0.051, y = 0, z = -(arm_height + 0.02), - roll = components.rad90, - pitch = 0, - yaw = -components.rad90) - }} - - {% endif %} - - {% if enable_realsense_front %} - - {{ components.visual_mesh_macro( - name = "realsense_front_mount", - mesh_file = realsense_front_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = 0.153, - y = 0, - z = -0.089, - roll = 0, - pitch = 0, - yaw = 0) - }} - - {% endif %} - - {% if enable_realsense_front_pitched %} - - {{ components.visual_mesh_macro( - name = "realsense_front_pitched_mount", - mesh_file = realsense_front_pitched_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = 0.153, - y = 0, - z = -0.089, - roll = 0, + roll = math.radians(90), pitch = 0, - yaw = 0) + yaw = -math.radians(90), + parent_link = root) }} - - {% endif %} - - {% if enable_realsense_down %} - - {{ components.visual_mesh_macro( - name = "realsense_down_mount", - mesh_file = realsense_front_down_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = 0.153, - y = 0, - z = -0.089, - roll = 0, - pitch = 0, - yaw = 0) - }} - - {% endif %} - - {% if enable_dual_uv_cameras %} - - {{ components.visual_mesh_macro( - name = "uvdar_mount", + {%- endset -%} + {# #} + + {# Realsense front mount {--> #} + {%- set realsense_front_mount -%} + {{ generic.visual_link_macro( + name = 'realsense_front_mount', + mesh_file = realsense_front_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = 0.153, + y = 0, + z = -0.089, + roll = 0, + pitch = 0, + yaw = 0, + parent_link = root) + }} + {%- endset -%} + {# #} + + {# Realsense front pitched mount {--> #} + {%- set realsense_front_pitched_mount -%} + {{ generic.visual_link_macro( + name = 'realsense_front_pitched_mount', + mesh_file = realsense_front_pitched_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = 0.153, + y = 0, + z = -0.089, + roll = 0, + pitch = 0, + yaw = 0, + parent_link = root) + }} + {%- endset -%} + {# #} + + {# Realsense down mount {--> #} + {%- set realsense_down_mount -%} + {{ generic.visual_link_macro( + name = 'realsense_down_mount', + mesh_file = realsense_front_down_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = 0.153, + y = 0, + z = -0.089, + roll = 0, + pitch = 0, + yaw = 0, + parent_link = root) + }} + {%- endset -%} + {# #} + + {# UVDAR mount {--> #} + {%- set uvdar_mount -%} + {{ generic.visual_link_macro( + name = 'uvdar_mount', mesh_file = uvdar_mount_mesh, mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", + color = 'DarkGrey', x = 0.025, y = 0, z = 0.036, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + parent_link = root) }} - - {% endif %} + {%- endset -%} + {# #} - + {# #} + + - - {{ components.prop_macro( - direction = "ccw", + {# Propellers {--> #} + {%- set prop_list = [ + { + 'motor_number': 0, + 'direction': 'ccw', + 'x': arm_length * math.sin(math.radians(45)), + 'y': -arm_length * math.sin(math.radians(45)), + 'z': arm_height + prop_offset_top - body_height/2, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'White' + }, + { + 'motor_number': 1, + 'direction': 'ccw', + 'x': -arm_length * math.sin(math.radians(45)), + 'y': arm_length * math.sin(math.radians(45)), + 'z': arm_height + prop_offset_top - body_height/2, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'White' + }, + { + 'motor_number': 2, + 'direction': 'cw', + 'x': arm_length * math.sin(math.radians(45)), + 'y': arm_length * math.sin(math.radians(45)), + 'z': arm_height + prop_offset_top - body_height/2, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'White' + }, + { + 'motor_number': 3, + 'direction': 'cw', + 'x': -arm_length * math.sin(math.radians(45)), + 'y': -arm_length * math.sin(math.radians(45)), + 'z': arm_height + prop_offset_top - body_height/2, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'White' + } + ] + -%} + {{ components.propellers_macro( + prop_list = prop_list, rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, motor_constant = motor_constant, moment_constant = moment_constant, @@ -539,159 +587,47 @@ 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 = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_ccw, - x = arm_length * components.sin45, - y = -arm_length * components.sin45, - z = arm_height + prop_offset_top - body_height/2, - 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( - direction = "ccw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - 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 = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_ccw, - x = -arm_length * components.sin45, - y = arm_length * components.sin45, - z = arm_height + prop_offset_top - body_height/2, - roll = 0, - pitch = 0, - yaw = 0, - ixx = prop_ixx, - ixy = prop_ixy, - ixz = prop_ixz, - iyy = prop_iyy, - iyz = prop_iyz, - izz = prop_izz) + meshes_z_offset = 0, + prop_ixx = prop_ixx, + prop_ixy = prop_ixy, + prop_ixz = prop_ixz, + prop_iyy = prop_iyy, + prop_iyz = prop_iyz, + prop_izz = prop_izz, + spawner_args = spawner_args) }} + {# #} - {{ components.prop_macro( - direction = "cw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - 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 = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_cw, - x = arm_length * components.sin45, - y = arm_length * components.sin45, - z = arm_height + prop_offset_top - body_height/2, - roll = 0, - pitch = 0, - yaw = 0, - ixx = prop_ixx, - ixy = prop_ixy, - ixz = prop_ixz, - iyy = prop_iyy, - iyz = prop_iyz, - izz = prop_izz) - }} + {# ================================================================== #} + {# || compulsory sensor definitions || #} + {# ================================================================== #} - {{ components.prop_macro( - direction = "cw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - 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 = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_cw, - x = -arm_length * components.sin45, - y = -arm_length * components.sin45, - z = arm_height + prop_offset_top - body_height/2, - roll = 0, - pitch = 0, - yaw = 0, - ixx = prop_ixx, - ixy = prop_ixy, - ixz = prop_ixz, - iyy = prop_iyy, - iyz = prop_iyz, - izz = prop_izz) - }} - + + {%- set imu_topic = '/imu' -%} + {%- set mag_topic = '/mag' -%} + {%- set baro_topic = '/baro' -%} + {%- set lidar_topic = '/lidar' -%} - - - - - - {{ 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) + + {{ generic.gazebo_groundtruth_macro( + home_latitude = 0, + home_longitude = 0, + home_altitude = 0) }} - {{ components.gps_macro( - gps_name = "gps0", + {{ generic.gazebo_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_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, @@ -704,36 +640,39 @@ - {{ components.magnetometer_plugin_macro( + {{ generic.gazebo_magnetometer_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) + mag_topic = mag_topic) }} - {{ components.barometer_plugin_macro( - baro_topic = "/baro", + {{ generic.gazebo_barometer_macro( + baro_topic = baro_topic, pub_rate = 50, baro_drift_pa_per_sec = 0) }} + + {{ generic.gazebo_mavlink_interface_macro( + imu_topic = imu_topic, + mag_topic = mag_topic, + baro_topic = baro_topic, + lidar_topic = lidar_topic, + mavlink_config = spawner_args['mavlink_config']) + }} + + - {{ components.imu_plugin_macro( - imu_name = "imu", + + {{ generic.gazebo_imu_macro( + imu_name = 'imu', parent_link = root, - imu_topic = "/imu", + imu_topic = '/imu', gyroscope_noise_density = 0.00018665, gyroscope_random_walk = 0.000038785, gyroscope_bias_correlation_time = 1000.0, @@ -751,452 +690,353 @@ }} - - - + + + {# ================================================================== #} + {# || optional sensor definitions || #} + {# ================================================================== #} {# Ground truth {--> #} - {% if enable_ground_truth %} - - {{ components.odometry_plugin_macro( - odometry_sensor_name = "ground_truth", + {{ components.ground_truth_macro( 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) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# ======================= rangefinder sensors ====================== #} - {# 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, + {# Garmin down {--> #} + {{ components.garmin_down_macro( parent_link = root, - orientation = "", x = -0.057, y = 0, z = -0.069, roll = 0, - pitch = components.rad(90), - yaw = 0) + pitch = math.radians(90), + yaw = 0, + mount = garmin_down_mount, + spawner_args = spawner_args) }} - - {% endif %} {# #} - {# Garmin Up {--> #} - {% if enable_rangefinder_up %} - - {{ components.external_garmin_macro( - namespace = namespace, + {# Garmin up {--> #} + {{ components.garmin_up_external_macro( parent_link = root, - orientation = "_up", x = -0.077, y = 0, z = -0.033, roll = 0, - pitch = components.rad(-90), - yaw = 0) + pitch = math.radians(-90), + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Teraranger One {--> #} - {% if enable_teraranger %} - {{ components.teraranger_macro( - namespace = namespace, parent_link = root, x = 0.08, y = 0, z = -0.05, roll = 0, - pitch = components.rad(90), - yaw = 0) + pitch = math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# ========================== LIDAR sensors ========================= #} {# Scanse Sweep {--> #} - {% if enable_scanse_sweep %} - - {{ components.scanse_sweep_macro( - namespace = namespace, + {%- set scanse_sweep = components.scanse_sweep_macro( parent_link = root, x = 0.0, y = 0.0, z = 0.113, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ scanse_sweep }} {# #} {# Rplidar {--> #} - {% if enable_rplidar %} - - {{ components.rplidar_macro( - namespace = namespace, + {%- set rplidar = components.rplidar_macro( parent_link = root, - horizontal_samples = horizontal_samples, - rotation_freq = rotation_freq, x = 0.0, y = 0.0, z = 0.1, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ rplidar }} {# #} {# Velodyne {--> #} - {% if enable_velodyne %} - - - {{ components.velodyne_macro( - namespace = namespace, + {%- set velodyne = components.velodyne_macro( parent_link = root, - sensor_name = "velodyne", - 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, + sensor_name = 'velodyne', x = 0.0, y = 0.0, z = 0.066, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ velodyne }} {# #} {# Ouster {--> #} - {% if enable_ouster %} - - {{ components.ouster_macro( - namespace = namespace, + {%- set ouster = components.ouster_macro( 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, + sensor_name = 'os', x = 0.0, y = 0.0, z = 0.066, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ ouster }} {# #} - + {# ========================== camera sensors ======================== #} - - - {% if enable_fisheye_camera %} - - {{ components.fisheye_camera_macro( - namespace = namespace, - camera_name = "fisheye_camera", - topic_ns_prefix = "", + {# Fisheye camera placements {--> #} + {{ components.fisheye_bluefox_macro( + camera_name = 'fisheye_camera', + topic_ns_prefix = '', parent_link = root, - frame_rate = 45, - noise = 0.0, x = 0.05, y = 0, z = - (arm_height + 0.055), roll = 0, - pitch = components.rad90, - yaw = 0) + pitch = math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} - + {# #} - + {# Bluefox camera placements {--> #} - {% if enable_bluefox_camera_reverse %} - - {{ components.bluefox_camera_macro( - namespace = namespace, - camera_name = "bluefox_optflow", + {# Bluefox (classic) {--> #} + {%- set bluefox_camera = components.bluefox_camera_macro( + camera_name = 'bluefox_optflow', parent_link = root, - frame_rate = 100.0, - hfov = 2.1, - noise = 0.0, x = 0.05, y = 0, z = - (arm_height + 0.055), - roll = 0, - pitch = components.rad90, - yaw = components.rad180) - }} - - {% endif %} + mount = none, + spawner_args = spawner_args) + -%} + {{ bluefox_camera }} + {# #} - {% if enable_bluefox_camera %} - - {{ components.bluefox_camera_macro( - namespace = namespace, - camera_name = "bluefox_optflow", + {# Bluefox reverse {--> #} + {%- set bluefox_camera_reverse = components.bluefox_camera_reverse_macro( + camera_name = 'bluefox_optflow', parent_link = root, - frame_rate = 100.0, - hfov = 2.1, - noise = 0.0, x = 0.05, y = 0, z = - (arm_height + 0.055), - roll = 0, - pitch = components.rad90, - yaw = 0) - }} - - {% endif %} + mount = none, + spawner_args = spawner_args) + -%} + {{ bluefox_camera_reverse }} + {# #} - + {# #} - + {# Realsense placements {--> #} - {% if enable_realsense_down %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="_down", + {# Realsense down {--> #} + {{ components.realsense_down_macro( + camera_name = 'rgbd', + camera_suffix='_down', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.153, y = 0, z = -0.103, roll = 0, - pitch = components.rad90, - yaw = 0) + pitch = math.radians(90), + yaw = 0, + mount = realsense_down_mount, + spawner_args = spawner_args) }} - - {% endif %} - - {% if enable_realsense_front_pitched %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="_front_pitched", + {# #} + + {# Realsense front pitched {--> #} + {{ components.realsense_front_pitched_macro( + camera_name = 'rgbd', + camera_suffix='_front_pitched', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.16, y = 0, z = -0.089, roll = 0, - pitch = components.rad45, - yaw = 0) + pitch = math.radians(45), + yaw = 0, + mount = realsense_front_pitched_mount, + spawner_args = spawner_args) }} - - {% endif %} - - {% if enable_realsense_front %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="", + {# #} + + {# Realsense front {--> #} + {{ components.realsense_front_macro( + camera_name = 'rgbd', + camera_suffix='', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.155, y = 0, z = -0.089, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + mount = realsense_front_mount, + spawner_args = spawner_args) }} - - {% endif %} - - {% if enable_realsense_up %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="_up", + {# #} + + {# Realsense up {--> #} + {{ components.realsense_up_macro( + camera_name = 'rgbd', + camera_suffix='_up', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.07, y = 0, z = 0.025, roll = 0, - pitch = - components.rad90, - yaw = 0) + pitch = -math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - + {# #} - + {# Mobius camera placements {--> #} - {% if enable_mobius_camera_down %} - - {{ components.mobius_camera_macro( - namespace = namespace, - camera_name = "mobius_down", + {# Mobius down {--> #} + {{ components.mobius_down_macro( + camera_name = 'mobius_down', parent_link = root, - frame_rate = 30.0, x = 0.144, y = 0.0, z = -0.092, roll = 0, - pitch = components.rad90, - yaw = 0) + pitch = math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - {% if enable_mobius_camera_front %} - - {{ components.mobius_camera_macro( - namespace = namespace, - camera_name = "mobius_front", + {# Mobius front {--> #} + {{ components.mobius_front_macro( + camera_name = 'mobius_front', parent_link = root, - frame_rate = 30.0, x = 0.2, y = 0.0, z = -0.04, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - {% if enable_mobius_camera_back_left %} - - {{ components.mobius_camera_macro( - namespace = namespace, - camera_name = "mobius_back_left", + {# Mobius back left {--> #} + {{ components.mobius_back_left_macro( + camera_name = 'mobius_back_left', parent_link = root, - frame_rate = 30.0, x = -0.1, y = 0.1732, z = -0.04, roll = 0, pitch = 0, - yaw = components.rad120) + yaw = math.radians(120), + mount = none, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - {% if enable_mobius_camera_back_right %} - - {{ components.mobius_camera_macro( - namespace = namespace, - camera_name = "mobius_back_right", + {# Mobius back right {--> #} + {{ components.mobius_back_right_macro( + camera_name = 'mobius_back_right', parent_link = root, - frame_rate = 30.0, x = -0.1, y = -0.1732, z = -0.04, roll = 0, pitch = 0, - yaw = -components.rad120) + yaw = -math.radians(120), + mount = none, + spawner_args = spawner_args) }} - - {% endif %} - + {# #} - {# Dual UV cameras {--> #} - {% if enable_dual_uv_cameras %} - - {{ components.uvcam_macro( - parent_link = root, - camera_publish_topic = "/" + namespace + "/uvdar_bluefox_left/image_raw", - calibration_file = uvcam_calib_file, - occlusions = uvcam_occlusions, - frame_rate = 60, - device_id = namespace + "_1", - x = 0.037, - y = 0.1175, - z = 0.05, - roll = 0, - pitch = 0, - yaw = components.rad70) - }} + {# #} - {{ components.uvcam_macro( + {# Dual UV cameras {--> #} + {{ components.dual_uv_cameras_macro( parent_link = root, - camera_publish_topic = "/" + namespace + "/uvdar_bluefox_right/image_raw", - calibration_file = uvcam_calib_file, - occlusions = uvcam_occlusions, - frame_rate = 60, - device_id = namespace + "_2", - x = 0.037, - y = -0.1175, - z = 0.05, - roll = 0, - pitch = 0, - yaw = -components.rad70) + x1 = 0.037, + y1 = 0.1175, + z1 = 0.05, + roll1 = 0, + pitch1 = 0, + yaw1 = math.radians(70), + x2 = 0.037, + y2 = -0.1175, + z2 = 0.05, + roll2 = 0, + pitch2 = 0, + yaw2 = -math.radians(70), + mount = uvdar_mount, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Back UV cameras {--> #} - {% if enable_back_uv_camera %} - - {{ components.uvcam_macro( + {{ components.back_uv_camera_macro( parent_link = root, - camera_publish_topic = "/" + namespace + "/uvdar_bluefox_back/image_raw", - calibration_file = uvcam_calib_file, - occlusions = uvcam_occlusions, - frame_rate = 60, - device_id = namespace + "_3", x = -0.1, y = 0.0, z = -0.02, - roll = components.rad90, + roll = math.radians(90), pitch = 0, - yaw = components.rad180) + yaw = math.radians(180), + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# 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", + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/servo_camera_optical', + sensor_base_frame_name = spawner_args['name'] + '/servo_camera', offset_pitch_link_x = 0.0, offset_pitch_link_y = 0.0, offset_pitch_link_z = 0.0, @@ -1222,82 +1062,67 @@ img_height = 1080, compensate_tilt_roll = true, compensate_tilt_pitch = true, - pitch_link_mesh_file = "", - roll_link_mesh_file = "", - mesh_scale = "") + pitch_link_mesh_file = '', + roll_link_mesh_file = '', + mesh_scale = '', + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# VIO placements {--> #} - {% if enable_vio %} - + {# VIO (classic) {--> #} {{ components.vio_macro( - namespace = namespace, - sensor_name = "vio", + sensor_name = 'vio', parent_link = root, - frame_rate = 45, - noise = 0.0, - imu_rate = 100, - imu_noise_mean = 0.0, x = 0.107, y = 0, z = -0.002, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - {% if enable_vio_down %} - - {{ components.vio_macro( - namespace = namespace, - sensor_name = "vio", + {# VIO down {--> #} + {{ components.vio_down_macro( + sensor_name = 'vio', parent_link = root, - frame_rate = 45, - noise = 0.0, - imu_rate = 100, - imu_noise_mean = 0.0, x = 0.1, y = 0, z = -0.15, roll = 0, - pitch = components.rad90, - yaw = 0) + pitch = math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} - + {# #} + + {# #} - + {# ========================== other sensors ========================= #} {# Teraranger Tower Evo {--> #} - {% if enable_teraranger_evo_tower %} - {{ components.teraranger_evo_tower_macro( parent_link = root, visualize = False, - frame_name = namespace + "/teraranger_tower", - parent_frame_name = namespace + "/fcu", + frame_name = spawner_args['name'] + '/teraranger_tower', + parent_frame_name = spawner_args['name'] + '/fcu', gaussian_noise = 0.0, x = 0.0, y = 0.0, z = 0.1, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Magnetic gripper {--> #} - {% if enable_magnetic_gripper %} - {{ components.magnet_gripper_visualization_macro( parent_link = root, x = 0.0, @@ -1305,175 +1130,125 @@ z = - (0.20/2 + 0.01), roll = 0, pitch = 0, - yaw = components.rad90) + yaw = math.radians(90), + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Timepix {--> #} - {% if enable_timepix %} - {{ components.timepix_macro( parent_link = root, - name = "timepix", + sensor_name = 'timepix', x = 0.115, y = -0.05, z = -0.05, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Timepix3 {--> #} - {% if enable_timepix3 %} - {{ components.timepix3_macro( parent_link = root, - name = "timepix3", - material = "cdte", - thickness = 0.002, - max_message_window = 1.0, - sensor_suffix = "", + sensor_name = 'timepix3', + sensor_suffix = '', x = 0.115, y = -0.05, z = -0.05, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Light {--> #} - {% if enable_light %} - {{ components.light_macro( parent_link = root, - update_rate = 30, max_pitch_rate = 0.1, - initial_on = True, - compensate_tilt = True, x = 0.2, y = 0.0, z = -0.1, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# UV leds {--> #} - {% if enable_uv_leds %} - - - {% if uvled_s_l != "None" %} - {% set led1 = uvled_s_l %} - {% set led3 = uvled_s_l %} - {% else %} - {% set led1 = uvled_s[0] %} - {% set led3 = uvled_s[2] %} - {% endif %} - - {% if uvled_s_r != "None" %} - {% set led2 = uvled_s_r %} - {% set led4 = uvled_s_r %} - {% else %} - {% set led2 = uvled_s[1] %} - {% set led4 = uvled_s[3] %} - {% endif %} - - {# -- leds configuration -- (id, signal_id, x, y, z, roll, pitch, yaw) #} - {%- set uv_leds_macro_parameters = [(1, led1, 0.1767755, 0.1697045, -0.05, 0.0, components.rad90, 0.0), - (2, led1, 0.1697045, 0.1767755, -0.05, 0.0, components.rad90, components.rad90), - (3, led2, 0.1697045, -0.1767755, -0.05, 0.0, components.rad90, -components.rad90), - (4, led2, 0.1767755, -0.1697045, -0.05, 0.0, components.rad90, 0.0), - (5, led3, -0.1697045, 0.1767755, -0.05, 0.0, components.rad90, components.rad90), - (6, led3, -0.1767755, 0.1697045, -0.05, 0.0, components.rad90, components.rad180), - (7, led4, -0.1767755, -0.1697045, -0.05, 0.0, components.rad90, -components.rad180), - (8, led4, -0.1697045, -0.1767755, -0.05, 0.0, components.rad90, -components.rad90)] -%} - - {% for id_, signal_id_, x_, y_, z_, roll_, pitch_, yaw_ in uv_leds_macro_parameters %} - {{ components.uvled_macro( - parent_link = root, - device_id = namespace + "_" + id_ | string(), - signal_id = signal_id_, - x = x_, - y = y_, - z = z_, - roll = roll_, - pitch = pitch_, - yaw = yaw_) - }} - {% endfor %} - - {% endif %} + {{ components.uv_leds_macro( + parent_link = root, + x1 = 0.1767755, + x2 = 0.1697045, + y1 = 0.1697045, + y2 = 0.1767755, + z = -0.05, + spawner_args = spawner_args) + }} {# #} {# UV leds beacon {--> #} - {% if enable_uv_leds_beacon %} - - {# -- leds configuration -- (id, signal_id, x, y, z, roll, pitch, yaw) #} - {%- set uv_leds_macro_parameters = [("b1", uvled_beacon_s, 0.01, 0.0, 0.15, 0.0, components.rad90, 0.0), - ("b2", uvled_beacon_s, 0.0, 0.01, 0.15, 0.0, components.rad90, components.rad90), - ("b3", uvled_beacon_s, -0.01, 0.0, 0.15, 0.0, components.rad90, components.rad180), - ("b4", uvled_beacon_s, 0.0, -0.01, 0.15, 0.0, components.rad90, components.rad270)] -%} - - {% for id_, signal_id_, x_, y_, z_, roll_, pitch_, yaw_ in uv_leds_macro_parameters %} - {{ components.uvled_macro( - parent_link = root, - device_id = namespace + "_" + id_ | string(), - signal_id = signal_id_, - x = x_, - y = y_, - z = z_, - roll = roll_, - pitch = pitch_, - yaw = yaw_) - }} - {% endfor %} - - {% endif %} + {{ components.uv_leds_beacon_macro( + parent_link = root, + x1 = 0.01, + x2 = 0.0, + y1 = 0.0, + y2 = 0.01, + z = 0.15, + spawner_args = spawner_args) + }} {# #} {# Whycon box {--> #} - {% if enable_whycon_box %} - {{ components.whycon_box_macro( parent_link = root, - mesh_scale = "1 1 1", x = 0, y = 0, - z = 0.25, + z = 0.08, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} - {# Pendelum {--> #} - {% if enable_pendulum %} - + {# Pendulum {--> #} {{ components.pendulum_macro( parent_link = root, - number_of_chains = 30, - chain_length = 0.1, - chain_radius = 0.01, - chain_mass = 0.5 / 30, x = 0, y = 0, - z = -0.10) + z = -0.10, + spawner_args = spawner_args) }} - - {% endif %} + {# #} + + {# ====================== conditional components ==================== #} + + {# Bluefox mount {--> #} + {%- if bluefox_camera | replace('\s', '') | length != 0 or bluefox_camera_reverse | replace('\s', '') | length != 0 -%} + + {{ bluefox_mount }} + + {%- endif -%} + {# #} + + {# 2D lidar mount {--> #} + {%- if rplidar | replace('\s', '') | length != 0 or scanse_sweep | replace('\s', '') | length != 0 -%} + + {{ lidar_mount_2d }} + + {%- endif -%} + {# #} + + {# 3D lidar mount {--> #} + {%- if velodyne | replace('\s', '') | length != 0 or ouster | replace('\s', '') | length != 0 -%} + + {{ lidar_mount_3d }} + + {%- endif -%} {# #} 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 42e19a0..a71510d 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 @@ -1,7 +1,8 @@ - {%- import "mrs_robots_description/sdf/component_snippets.sdf.jinja" as components -%} + {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} @@ -24,13 +25,7 @@ {%- set leg_radius = 0.012 -%} {# [m] #} {%- set leg_height = 0.17 -%} {# [m] #} {%- set pixhawk_offset = -0.0075 -%} {# [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 %} + {%- set root = 'base_link' -%} {# #} {# Motor constants {--> #} @@ -41,7 +36,7 @@ {%- set time_constant_down = 1.0 / 40.0 -%} {# [s] #} {%- set max_rot_velocity = 1 -%} {# [rad/s] #} {%- set rotor_drag_coefficient = 0.001 -%} {# orig 8.06428e-04 #} - {%- set rolling_moment_coefficient = "1.0e-6" -%} + {%- set rolling_moment_coefficient = '1.0e-6' -%} {# #} {# Inertia constants {--> #} @@ -52,32 +47,32 @@ {# Meshes {--> #} {# Drone parts {--> #} - {%- set central_board_mesh_top = "model://mrs_robots_description/meshes/dji/f550/dji_f550_central_board.dae" -%} - {%- set central_board_mesh_bottom = "model://mrs_robots_description/meshes/dji/f550/dji_f550_central_board.dae" -%} - {%- set arm_mesh_file = "model://mrs_robots_description/meshes/dji/f550/dji_f550_arm.dae" -%} - {%- set leg_mesh_file = "model://mrs_robots_description/meshes/dji/f550/dji_f550_leg_170mm.dae"-%} - {%- set motor_mesh_file = "model://mrs_robots_description/meshes/dji/f550/dji_f550_rotor.dae" -%} - {%- set prop_mesh_file = "model://mrs_robots_description/meshes/dji/f550/dji_f550_prop.dae"-%} - {%- set pixhawk_mesh_file = "model://mrs_robots_description/meshes/sensors/pixhawk.dae" -%} - {%- set nuc_mesh_file = "model://mrs_robots_description/meshes/dji/f550/dji_f550_nuc.dae" -%} + {%- set central_board_mesh_top = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_central_board.dae' -%} + {%- set central_board_mesh_bottom = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_central_board.dae' -%} + {%- set arm_mesh_file = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_arm.dae' -%} + {%- set leg_mesh_file = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_leg_170mm.dae'-%} + {%- set motor_mesh_file = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_rotor.dae' -%} + {%- set prop_mesh_file = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_prop.dae'-%} + {%- set pixhawk_mesh_file = 'model://mrs_robots_description/meshes/sensors/pixhawk.dae' -%} + {%- set nuc_mesh_file = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_nuc.dae' -%} {# #} - {# Holders {--> #} - {%- set battery_and_garmin_mount_mesh = "model://mrs_robots_description/meshes/dji/f550/dji_f550_battery_and_garmin_mount.dae" -%} - {%- set ouster_mount_mesh = "model://mrs_robots_description/meshes/dji/f550/dji_f550_ouster_mount.dae" -%} - {%- set rplidar_mount_mesh = "model://mrs_robots_description/meshes/dji/f550/dji_f550_rplidar_mount.dae" -%} - {%- set realsense_top_mount_mesh = "model://mrs_robots_description/meshes/dji/f550/dji_f550_realsense_top_mount.dae" -%} - {%- set realsense_front_mount_mesh = "model://mrs_robots_description/meshes/dji/f550/dji_f550_realsense_front_pitched_0_mount.dae" -%} - {%- set realsense_front_pitched_mount_mesh = "model://mrs_robots_description/meshes/dji/f550/dji_f550_realsense_front_pitched_10_mount.dae" -%} - {%- set realsense_front_down_mount_mesh = "model://mrs_robots_description/meshes/dji/f550/dji_f550_realsense_front_pitched_90_mount.dae" -%} - {%- set uvdar_mount_mesh = "model://mrs_robots_description/meshes/dji/f550/dji_f550_uvdar_mount.dae" -%} + {# Mounts {--> #} + {%- set battery_and_garmin_mount_mesh = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_battery_and_garmin_mount.dae' -%} + {%- set ouster_mount_mesh = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_ouster_mount.dae' -%} + {%- set rplidar_mount_mesh = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_rplidar_mount.dae' -%} + {%- set realsense_top_mount_mesh = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_realsense_top_mount.dae' -%} + {%- set realsense_front_mount_mesh = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_realsense_front_pitched_0_mount.dae' -%} + {%- set realsense_front_pitched_mount_mesh = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_realsense_front_pitched_10_mount.dae' -%} + {%- set realsense_front_down_mount_mesh = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_realsense_front_pitched_90_mount.dae' -%} + {%- set uvdar_mount_mesh = 'model://mrs_robots_description/meshes/dji/f550/dji_f550_uvdar_mount.dae' -%} {# #} {# Scales {--> #} - {%- set 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" -%} + {%- set 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' -%} {# #} {# #} @@ -100,14 +95,14 @@ - - - + {# ================================================================== #} + {# || bare body definitions || #} + {# ================================================================== #} - {{ components.multirotor_physics_macro( + {{ generic.multirotor_physics_macro( mass = mass, body_radius = body_radius, body_height = body_height, @@ -124,119 +119,119 @@ - {{ components.visual_mesh_macro( - name = "lower_central_board", + {{ generic.visual_mesh_macro( + name = 'lower_central_board', mesh_file = central_board_mesh_bottom, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = - body_height / 2, roll = 0, pitch = 0, - yaw = components.rad30) + yaw = math.radians(30)) }} - {{ components.visual_mesh_macro( - name = "upper_central_board", + {{ generic.visual_mesh_macro( + name = 'upper_central_board', mesh_file = central_board_mesh_top, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = arm_height - body_height / 2, roll = 0, pitch = 0, - yaw = components.rad30) + yaw = math.radians(30)) }} - {{ components.visual_mesh_macro( - name = "front_right_arm", + {{ generic.visual_mesh_macro( + name = 'front_right_arm', mesh_file = arm_mesh_file, mesh_scale = mesh_scale, - color = "White", - x = board_radius * components.cos30, - y = -board_radius * components.sin30, + color = 'White', + x = board_radius * math.cos(math.radians(30)), + y = -board_radius * math.sin(math.radians(30)), z = arm_height - body_height / 2, roll = 0, pitch = 0, - yaw = components.rad(-30)) + yaw = math.radians(-30)) }} - {{ components.visual_mesh_macro( - name = "middle_right_arm", + {{ generic.visual_mesh_macro( + name = 'middle_right_arm', mesh_file = arm_mesh_file, mesh_scale = mesh_scale, - color = "White", + color = 'White', x = 0, y = -board_radius, z = arm_height - body_height / 2, roll = 0, pitch = 0, - yaw = components.rad(-90)) + yaw = math.radians(-90)) }} - {{ components.visual_mesh_macro( - name = "back_right_arm", + {{ generic.visual_mesh_macro( + name = 'back_right_arm', mesh_file = arm_mesh_file, mesh_scale = mesh_scale, - color = "Red", - x = -board_radius * components.cos30, - y = -board_radius * components.sin30, + color = 'Red', + x = -board_radius * math.cos(math.radians(30)), + y = -board_radius * math.sin(math.radians(30)), z = arm_height - body_height / 2, roll = 0, pitch = 0, - yaw = components.rad(-150)) + yaw = math.radians(-150)) }} - {{ components.visual_mesh_macro( - name = "front_left_arm", + {{ generic.visual_mesh_macro( + name = 'front_left_arm', mesh_file = arm_mesh_file, mesh_scale = mesh_scale, - color = "White", - x = board_radius * components.cos30, - y = board_radius * components.sin30, + color = 'White', + x = board_radius * math.cos(math.radians(30)), + y = board_radius * math.sin(math.radians(30)), z = arm_height - body_height / 2, roll = 0, pitch = 0, - yaw = components.rad(30)) + yaw = math.radians(30)) }} - {{ components.visual_mesh_macro( - name = "middle_left_arm", + {{ generic.visual_mesh_macro( + name = 'middle_left_arm', mesh_file = arm_mesh_file, mesh_scale = mesh_scale, - color = "White", + color = 'White', x = 0, y = board_radius, z = arm_height - body_height / 2, roll = 0, pitch = 0, - yaw = components.rad(90)) + yaw = math.radians(90)) }} - {{ components.visual_mesh_macro( - name = "back_left_arm", + {{ generic.visual_mesh_macro( + name = 'back_left_arm', mesh_file = arm_mesh_file, mesh_scale = mesh_scale, - color = "Red", - x = -board_radius * components.cos30, - y = board_radius * components.sin30, + color = 'Red', + x = -board_radius * math.cos(math.radians(30)), + y = board_radius * math.sin(math.radians(30)), z = arm_height - body_height / 2, roll = 0, pitch = 0, - yaw = components.rad(150)) + yaw = math.radians(150)) }} - {{ components.visual_mesh_macro( - name = "NUC", + {{ generic.visual_mesh_macro( + name = 'NUC', mesh_file = nuc_mesh_file, mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = 0.023, @@ -247,11 +242,11 @@ - {{ components.visual_mesh_macro( - name = "pixhawk", + {{ generic.visual_mesh_macro( + name = 'pixhawk', mesh_file = pixhawk_mesh_file, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = -body_height / 2 - pixhawk_offset, @@ -262,50 +257,50 @@ - {{ components.leg_macro( - name = "front_right_leg", + {{ generic.leg_macro( + name = 'front_right_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale_milimeters, - color = "White", + color = 'White', parent = root, - x = (arm_length - leg_offset_r) * components.cos30, - y = -(arm_length - leg_offset_r) * components.sin30, + x = (arm_length - leg_offset_r) * math.cos(math.radians(30)), + y = -(arm_length - leg_offset_r) * math.sin(math.radians(30)), z = arm_height - body_height/2 - leg_offset_z, roll = 0, pitch = 0, - yaw = components.rad(150), + yaw = math.radians(150), collision_height = leg_height, collision_radius = leg_radius) }} - {{ components.leg_macro( - name = "middle_left_leg", + {{ generic.leg_macro( + name = 'middle_left_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale_milimeters, - color = "White", + color = 'White', parent = root, x = 0, y = arm_length - leg_offset_r, z = arm_height - body_height/2 - leg_offset_z, roll = 0, pitch = 0, - yaw = components.rad(-90), + yaw = math.radians(-90), collision_height = leg_height, collision_radius = leg_radius) }} - {{ components.leg_macro( - name = "back_right_leg", + {{ generic.leg_macro( + name = 'back_right_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale_milimeters, - color = "White", + color = 'White', parent = root, - x = -(arm_length - leg_offset_r) * components.cos30, - y = -(arm_length - leg_offset_r) * components.sin30, + x = -(arm_length - leg_offset_r) * math.cos(math.radians(30)), + y = -(arm_length - leg_offset_r) * math.sin(math.radians(30)), z = arm_height - body_height/2 - leg_offset_z, roll = 0, pitch = 0, - yaw = components.rad(30), + yaw = math.radians(30), collision_height = leg_height, collision_radius = leg_radius) }} @@ -313,24 +308,24 @@ - {{ components.visual_mesh_macro( - name = "front_right_motor", + {{ generic.visual_mesh_macro( + name = 'front_right_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", - x = arm_length * components.cos30, - y = -arm_length * components.sin30, + color = 'Black', + x = arm_length * math.cos(math.radians(30)), + y = -arm_length * math.sin(math.radians(30)), z = arm_height - body_height / 2, roll = 0, pitch = 0, yaw = 0) }} - {{ components.visual_mesh_macro( - name = "middle_right_motor", + {{ generic.visual_mesh_macro( + name = 'middle_right_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = 0, y = -arm_length, z = arm_height - body_height / 2, @@ -339,37 +334,37 @@ yaw = 0) }} - {{ components.visual_mesh_macro( - name = "back_right_motor", + {{ generic.visual_mesh_macro( + name = 'back_right_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", - x = -arm_length * components.cos30, - y = -arm_length * components.sin30, + color = 'Black', + x = -arm_length * math.cos(math.radians(30)), + y = -arm_length * math.sin(math.radians(30)), z = arm_height - body_height / 2, roll = 0, pitch = 0, yaw = 0) }} - {{ components.visual_mesh_macro( - name = "front_left_motor", + {{ generic.visual_mesh_macro( + name = 'front_left_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", - x = arm_length * components.cos30, - y = arm_length * components.sin30, + color = 'Black', + x = arm_length * math.cos(math.radians(30)), + y = arm_length * math.sin(math.radians(30)), z = arm_height - body_height / 2, roll = 0, pitch = 0, yaw = 0) }} - {{ components.visual_mesh_macro( - name = "middle_left_motor", + {{ generic.visual_mesh_macro( + name = 'middle_left_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = 0, y = arm_length, z = arm_height - body_height / 2, @@ -378,13 +373,13 @@ yaw = 0) }} - {{ components.visual_mesh_macro( - name = "back_left_motor", + {{ generic.visual_mesh_macro( + name = 'back_left_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", - x = -arm_length * components.cos30, - y = arm_length * components.sin30, + color = 'Black', + x = -arm_length * math.cos(math.radians(30)), + y = arm_length * math.sin(math.radians(30)), z = arm_height - body_height / 2, roll = 0, pitch = 0, @@ -393,323 +388,315 @@ - {% if use_battery_mount %} - {{ components.visual_mesh_macro( - name = "battery_mount", + {{ generic.visual_mesh_macro( + name = 'battery_mount', mesh_file = battery_and_garmin_mount_mesh, mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", + color = 'DarkGrey', x = 0.03, y = 0, z = -body_height / 2 - 0.01, roll = 0, pitch = 0, - yaw = components.rad180) + yaw = math.radians(180)) }} - {{ components.visual_colored_box_macro( - name = "battery", + {{ generic.visual_colored_box_macro( + name = 'battery', size_x = 0.135, size_y = 0.042, size_z = 0.044, - color = "Grey", + color = 'Grey', x = 0, y = 0, z = -body_height / 2 - 0.024, roll = 0, pitch = 0, - yaw = components.rad180) - }} - - {% endif %} - - {% if enable_ouster or enable_velodyne %} - - {{ components.visual_mesh_macro( - name = "ouster_mount", - mesh_file = ouster_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = 0, - y = 0, - z = 0.061, - roll = 0, - pitch = 0, - yaw = 0) - }} - - {% endif %} - - {% if enable_rplidar or enable_scanse_sweep %} - - {{ components.visual_mesh_macro( - name = "rplidar_mount", - mesh_file = rplidar_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = 0, - y = 0, - z = 0.068, - roll = 0, - pitch = 0, - yaw = components.rad90) - }} - - {% endif %} - - {% if enable_realsense_top %} - - {{ components.visual_mesh_macro( - name = "realsense_top_mount", - mesh_file = realsense_top_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = 0.06, - y = -0.040, - z = 0.163, - roll = 0, - pitch = 0, - yaw = components.rad180) - }} - - - {{ components.visual_colored_box_with_collision_macro( - name = "top_holder_1", - size_x = 0.006, - size_y = 0.035, - size_z = 0.175, - color = "Grey", - x = -0.095 * components.cos30, - y = 0.095 * components.sin30, - z = 0.075, - roll = 0, - pitch = 0, - yaw = -components.rad30) - }} - - {{ components.visual_colored_box_with_collision_macro( - name = "top_holder_2", - size_x = 0.006, - size_y = 0.035, - size_z = 0.175, - color = "Grey", - x = 0.095 * components.cos30, - y = -0.095 * components.sin30, - z = 0.075, - roll = 0, - pitch = 0, - yaw = -components.rad30) - }} - - {{ components.visual_colored_box_with_collision_macro( - name = "top_holder_3", - size_x = 0.190, - size_y = 0.035, - size_z = 0.006, - color = "Grey", - x = 0, - y = 0, - z = 0.160, - roll = 0, - pitch = 0, - yaw = -components.rad30) - }} - - - {% endif %} - - {% if enable_realsense_front %} - - {{ components.visual_mesh_macro( - name = "realsense_front_mount", - mesh_file = realsense_front_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = 0.223, - y = 0, - z = -0.089, - roll = components.rad90, - pitch = 0, - yaw = -components.rad90) + yaw = math.radians(180)) }} - {% endif %} - {% if enable_realsense_front_pitched %} - - {{ components.visual_mesh_macro( - name = "realsense_front_pitched_mount", - mesh_file = realsense_front_pitched_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = 0.223, - y = 0, - z = -0.089, - roll = components.rad90, - pitch = 0, - yaw = -components.rad90) - }} - - {% endif %} - - {% if enable_realsense_down %} - - {{ components.visual_mesh_macro( - name = "realsense_down_mount", - mesh_file = realsense_front_pitched_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = 0.223, - y = 0, - z = -0.089, - roll = components.rad90, - pitch = 0, - yaw = -components.rad90) - }} - - {% endif %} - - {% if enable_realsense_front or enable_realsense_front_pitched or enable_realsense_down %} - - {{ components.leg_macro( - name = "front_left_leg", - mesh_file = leg_mesh_file, - mesh_scale = mesh_scale_milimeters, - color = "White", - parent = root, - x = (arm_length - leg_offset_r) * components.cos30, - y = (arm_length - leg_offset_r) * components.sin30, - z = arm_height - body_height/2 - leg_offset_z, - roll = 0, - pitch = 0, - yaw = components.rad(210), - collision_height = leg_height, - collision_radius = leg_radius) - }} - - {% endif %} - - {% if enable_uv_camera %} - - {{ components.visual_mesh_macro( - name = "uvdar_mount", - mesh_file = uvdar_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = 0.025, - y = 0, - z = 0.036, - roll = 0, - pitch = 0, - yaw = 0) - }} - - {% endif %} + {# component mounts and holders (these must be passed as args into components, or must be called) {--> #} + + {# lidar_mount_2d {--> #} + {%- set lidar_mount_2d -%} + {{ generic.visual_link_macro( + name = 'rplidar_mount', + mesh_file = rplidar_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = 0, + y = 0, + z = 0.068, + roll = 0, + pitch = 0, + yaw = math.radians(90), + parent_link = root) + }} + {%- endset -%} + {# #} + + {# lidar_mount_3d {--> #} + {%- set lidar_mount_3d -%} + {{ generic.visual_link_macro( + name = 'lidar_mount_3d', + mesh_file = ouster_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = 0, + y = 0, + z = 0.061, + roll = 0, + pitch = 0, + yaw = 0, + parent_link = root) + }} + {%- endset -%} + {# #} + + {# Realsense top mount {--> #} + {%- set realsense_top_mount -%} + {{ generic.visual_link_macro( + name = 'realsense_top_mount', + mesh_file = realsense_top_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = 0.06, + y = -0.040, + z = 0.163, + roll = 0, + pitch = 0, + yaw = math.radians(180), + parent_link = root) + }} + + + + {{ generic.zero_inertial_macro() }} + {{ generic.visual_colored_box_with_collision_macro( + name = 'top_holder_1', + size_x = 0.006, + size_y = 0.035, + size_z = 0.175, + color = 'Grey', + x = -0.095 * math.cos(math.radians(30)), + y = 0.095 * math.sin(math.radians(30)), + z = 0.075, + roll = 0, + pitch = 0, + yaw = -math.radians(30)) + }} + + {{ generic.visual_colored_box_with_collision_macro( + name = 'top_holder_2', + size_x = 0.006, + size_y = 0.035, + size_z = 0.175, + color = 'Grey', + x = 0.095 * math.cos(math.radians(30)), + y = -0.095 * math.sin(math.radians(30)), + z = 0.075, + roll = 0, + pitch = 0, + yaw = -math.radians(30)) + }} + + {{ generic.visual_colored_box_with_collision_macro( + name = 'top_holder_3', + size_x = 0.190, + size_y = 0.035, + size_z = 0.006, + color = 'Grey', + x = 0, + y = 0, + z = 0.160, + roll = 0, + pitch = 0, + yaw = -math.radians(30)) + }} + + + {{ root }} + aluminum_holder_link + + + + {%- endset -%} + {# #} + + {# Realsense front mount {--> #} + {%- set realsense_front_mount -%} + {{ generic.visual_link_macro( + name = 'realsense_front_mount', + mesh_file = realsense_front_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = 0.223, + y = 0, + z = -0.089, + roll = math.radians(90), + pitch = 0, + yaw = -math.radians(90), + parent_link = root) + }} + {%- endset -%} + {# #} + + {# Realsense front pitched mount {--> #} + {%- set realsense_front_pitched_mount -%} + {{ generic.visual_link_macro( + name = 'realsense_front_pitched_mount', + mesh_file = realsense_front_pitched_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = 0.223, + y = 0, + z = -0.089, + roll = math.radians(90), + pitch = 0, + yaw = -math.radians(90), + parent_link = root) + }} + {%- endset -%} + {# #} + + {# Realsense down mount {--> #} + {%- set realsense_down_mount -%} + {{ generic.visual_link_macro( + name = 'realsense_down_mount', + mesh_file = realsense_front_pitched_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = 0.223, + y = 0, + z = -0.089, + roll = math.radians(90), + pitch = 0, + yaw = -math.radians(90), + parent_link = root) + }} + {%- endset -%} + {# #} + + {# Front left leg {--> #} + {%- set front_left_leg -%} + + + {{ generic.zero_inertial_macro() }} + {{ generic.leg_macro( + name = 'front_left_leg', + mesh_file = leg_mesh_file, + mesh_scale = mesh_scale_milimeters, + color = 'White', + parent = root, + x = (arm_length - leg_offset_r) * math.cos(math.radians(30)), + y = (arm_length - leg_offset_r) * math.sin(math.radians(30)), + z = arm_height - body_height/2 - leg_offset_z, + roll = 0, + pitch = 0, + yaw = math.radians(210), + collision_height = leg_height, + collision_radius = leg_radius) + }} + + + {{ root }} + extra_leg_link + + + {%- endset -%} + {# #} + + {# UVDAR mount {--> #} + {%- set uvdar_mount -%} + {{ generic.visual_mesh_macro( + name = 'uvdar_mount', + mesh_file = uvdar_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = 0.025, + y = 0, + z = 0.036, + roll = 0, + pitch = 0, + yaw = 0) + }} + {%- endset -%} + {# #} + + {# #} - - {{ components.prop_macro( - direction = "cw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - 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 = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_cw, - x = 0, - y = -arm_length, - z = arm_height + prop_offset_top - body_height/2, - 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( - direction = "ccw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - 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 = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_ccw, - x = 0, - y = arm_length, - z = arm_height + prop_offset_top - body_height/2, - 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( - direction = "cw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - 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 = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_cw, - x = arm_length * components.cos30, - y = arm_length * components.sin30, - z = arm_height + prop_offset_top - body_height/2, - 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( - direction = "ccw", + {# Propellers {--> #} + {%- set prop_list = [ + { + 'motor_number': 0, + 'direction': 'cw', + 'x': 0, + 'y': -arm_length, + 'z': arm_height + prop_offset_top - body_height/2, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'White' + }, + { + 'motor_number': 1, + 'direction': 'ccw', + 'x': 0, + 'y': arm_length, + 'z': arm_height + prop_offset_top - body_height/2, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'White' + }, + { + 'motor_number': 2, + 'direction': 'cw', + 'x': arm_length * math.cos(math.radians(30)), + 'y': arm_length * math.sin(math.radians(30)), + 'z': arm_height + prop_offset_top - body_height/2, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'White' + }, + { + 'motor_number': 3, + 'direction': 'ccw', + 'x': -arm_length * math.cos(math.radians(30)), + 'y': -arm_length * math.sin(math.radians(30)), + 'z': arm_height + prop_offset_top - body_height/2, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'White' + }, + { + 'motor_number': 4, + 'direction': 'ccw', + 'x': arm_length * math.cos(math.radians(30)), + 'y': -arm_length * math.sin(math.radians(30)), + 'z': arm_height + prop_offset_top - body_height/2, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'White' + }, + { + 'motor_number': 5, + 'direction': 'cw', + 'x': -arm_length * math.cos(math.radians(30)), + 'y': arm_length * math.sin(math.radians(30)), + 'z': arm_height + prop_offset_top - body_height/2, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'White' + }, + ] + -%} + {{ components.propellers_macro( + prop_list = prop_list, rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, motor_constant = motor_constant, moment_constant = moment_constant, @@ -719,128 +706,47 @@ 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 = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_ccw, - x = -arm_length * components.cos30, - y = -arm_length * components.sin30, - z = arm_height + prop_offset_top - body_height/2, - 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( - direction = "ccw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - time_constant_up = time_constant_up, - time_constant_down = time_constant_down, - max_rot_velocity = max_rot_velocity, - motor_number = 4, - rotor_drag_coefficient = rotor_drag_coefficient, - rolling_moment_coefficient = rolling_moment_coefficient, - enable_motor_crash = enable_motor_crash, - color = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_ccw, - x = arm_length * components.cos30, - y = -arm_length * components.sin30, - z = arm_height + prop_offset_top - body_height/2, - 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( - direction = "cw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - time_constant_up = time_constant_up, - time_constant_down = time_constant_down, - max_rot_velocity = max_rot_velocity, - motor_number = 5, - rotor_drag_coefficient = rotor_drag_coefficient, - rolling_moment_coefficient = rolling_moment_coefficient, - enable_motor_crash = enable_motor_crash, - color = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_cw, - x = -arm_length * components.cos30, - y = arm_length * components.sin30, - z = arm_height + prop_offset_top - body_height/2, - roll = 0, - pitch = 0, - yaw = 0, - ixx = prop_ixx, - ixy = prop_ixy, - ixz = prop_ixz, - iyy = prop_iyy, - iyz = prop_iyz, - izz = prop_izz) + meshes_z_offset = 0, + prop_ixx = prop_ixx, + prop_ixy = prop_ixy, + prop_ixz = prop_ixz, + prop_iyy = prop_iyy, + prop_iyz = prop_iyz, + prop_izz = prop_izz, + spawner_args = spawner_args) }} + {# #} - + {# ================================================================== #} + {# || compulsory sensor definitions || #} + {# ================================================================== #} - - - + + {%- set imu_topic = '/imu' -%} + {%- set mag_topic = '/mag' -%} + {%- set baro_topic = '/baro' -%} + {%- set lidar_topic = '/lidar' -%} - - {{ 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) + + {{ generic.gazebo_groundtruth_macro( + home_latitude = 0, + home_longitude = 0, + home_altitude = 0) }} - {{ components.gps_macro( - gps_name = "gps0", + {{ generic.gazebo_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_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, @@ -853,36 +759,39 @@ - {{ components.magnetometer_plugin_macro( + {{ generic.gazebo_magnetometer_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) + mag_topic = mag_topic) }} - {{ components.barometer_plugin_macro( - baro_topic = "/baro", + {{ generic.gazebo_barometer_macro( + baro_topic = baro_topic, pub_rate = 50, baro_drift_pa_per_sec = 0) }} + + {{ generic.gazebo_mavlink_interface_macro( + imu_topic = imu_topic, + mag_topic = mag_topic, + baro_topic = baro_topic, + lidar_topic = lidar_topic, + mavlink_config = spawner_args['mavlink_config']) + }} + + - {{ components.imu_plugin_macro( - imu_name = "imu", + + {{ generic.gazebo_imu_macro( + imu_name = 'imu', parent_link = root, - imu_topic = "/imu", + imu_topic = '/imu', gyroscope_noise_density = 0.00018665, gyroscope_random_walk = 0.000038785, gyroscope_bias_correlation_time = 1000.0, @@ -900,427 +809,354 @@ }} - - - + + + {# ================================================================== #} + {# || optional sensor definitions || #} + {# ================================================================== #} {# Ground truth {--> #} - {% if enable_ground_truth %} - - {{ components.odometry_plugin_macro( - odometry_sensor_name = "ground_truth", + {{ components.ground_truth_macro( 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) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# ======================= rangefinder sensors ====================== #} - {# 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, + {# Garmin down {--> #} + {{ components.garmin_down_macro( parent_link = root, - orientation = "", - x = 0.7 * board_radius * components.cos90, - y = 0.7 * board_radius * -components.sin90, - z = - (arm_height + 0.033), + x = 0.7 * board_radius * math.cos(math.radians(90)), + y = 0.7 * board_radius * -math.sin(math.radians(90)), + z = -0.0605, roll = 0, - pitch = components.rad(90), - yaw = components.rad(90)) + pitch = math.radians(90), + yaw = math.radians(90), + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} - {# Garmin Up {--> #} - {% if enable_rangefinder_up %} - - {{ components.external_garmin_macro( - namespace = namespace, + {# Garmin up {--> #} + {{ components.garmin_up_external_macro( parent_link = root, - orientation = "_up", - x = 0.85 * board_radius * components.cos30, - y = 0.85 * board_radius * -components.sin30, - z = 0.006, + x = 0.95 * board_radius * math.cos(math.radians(-30)), + y = 0.95 * board_radius * -math.sin(math.radians(-30)), + z = -0.007, roll = 0, - pitch = components.rad(-90), - yaw = components.rad(-30)) + pitch = math.radians(-90), + yaw = math.radians(-60), + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Teraranger One {--> #} - {% if enable_teraranger %} - {{ components.teraranger_macro( - namespace = namespace, parent_link = root, x = 0.19918, y = 0.115, z = -0.025, roll = 0, - pitch = components.rad(90), - yaw = components.rad(30)) + pitch = math.radians(90), + yaw = math.radians(30), + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# ========================== LIDAR sensors ========================= #} {# Scanse Sweep {--> #} - {% if enable_scanse_sweep %} - - {{ components.scanse_sweep_macro( - namespace = namespace, + {%- set scanse_sweep = components.scanse_sweep_macro( parent_link = root, x = 0.0, y = 0.0, z = 0.113, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ scanse_sweep }} {# #} {# Rplidar {--> #} - {% if enable_rplidar %} - - {{ components.rplidar_macro( - namespace = namespace, + {%- set rplidar = components.rplidar_macro( parent_link = root, - horizontal_samples = horizontal_samples, - rotation_freq = rotation_freq, x = 0.0, y = 0.0, z = 0.1, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ rplidar }} {# #} {# Velodyne {--> #} - {% if enable_velodyne %} - - - {{ components.velodyne_macro( - namespace = namespace, + {%- set velodyne = components.velodyne_macro( parent_link = root, - sensor_name = "velodyne", - 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, + sensor_name = 'velodyne', x = 0.0, y = 0.0, z = 0.066, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ velodyne }} {# #} {# Ouster {--> #} - {% if enable_ouster %} - - {{ components.ouster_macro( - namespace = namespace, + {%- set ouster = components.ouster_macro( 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, + sensor_name = 'os', x = 0.0, y = 0.0, z = 0.066, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ ouster }} {# #} - + {# ========================== camera sensors ======================== #} - + {# Bluefox camera placements {--> #} - {% if enable_bluefox_camera_reverse %} - + {# Bluefox (classic) {--> #} {{ components.bluefox_camera_macro( - namespace = namespace, - camera_name = "bluefox_optflow", + camera_name = 'bluefox_optflow', parent_link = root, - frame_rate = 100.0, - hfov = 2.1, - noise = 0.0, x = 0.87 * board_radius, y = 0, z = -0.077, - roll = 0, - pitch = components.rad90, - yaw = components.rad180) + mount = none, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - {% if enable_bluefox_camera %} - - {{ components.bluefox_camera_macro( - namespace = namespace, - camera_name = "bluefox_optflow", + {# Bluefox reverse {--> #} + {{ components.bluefox_camera_reverse_macro( + camera_name = 'bluefox_optflow', parent_link = root, - frame_rate = 100.0, - hfov = 2.1, - noise = 0.0, x = 0.87 * board_radius, y = 0, z = -0.077, - roll = 0, - pitch = components.rad90, - yaw = 0) + mount = none, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - + {# #} - + {# Realsense placements {--> #} - {% if enable_realsense_top %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="_top", + {# Realsense top {--> #} + {{ components.realsense_top_macro( + camera_name = 'rgbd', + camera_suffix='_top', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.085, y = -0.041, z = 0.184, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + mount = realsense_top_mount, + spawner_args = spawner_args) }} - - {% endif %} - - {% if enable_realsense_down %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="_down", + {# #} + + {# Realsense down {--> #} + {%- set realsense_down = components.realsense_down_macro( + camera_name = 'rgbd', + camera_suffix='_down', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.22, y = 0, z = -0.1017, roll = 0, - pitch = components.rad90, - yaw = 0) - }} - - {% endif %} - - {% if enable_realsense_front_pitched %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="_front_pitched", + pitch = math.radians(90), + yaw = 0, + mount = realsense_down_mount, + spawner_args = spawner_args) + -%} + {{ realsense_down }} + {# #} + + {# Realsense front pitched {--> #} + {%- set realsense_front_pitched = components.realsense_front_pitched_macro( + camera_name = 'rgbd', + camera_suffix='_front_pitched', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.226, y = 0, z = -0.089, roll = 0, - pitch = components.rad10, - yaw = 0) - }} - - {% endif %} - - {% if enable_realsense_front %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="", + pitch = math.radians(10), + yaw = 0, + mount = realsense_front_pitched_mount, + spawner_args = spawner_args) + -%} + {{ realsense_front_pitched }} + {# #} + + {# Realsense front {--> #} + {%- set realsense_front = components.realsense_front_macro( + camera_name = 'rgbd', + camera_suffix='', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.223, y = 0, z = -0.089, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} - - {% if enable_realsense_up %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="_up", + yaw = 0, + mount = realsense_front_mount, + spawner_args = spawner_args) + -%} + {{ realsense_front }} + {# #} + + {# Realsense up {--> #} + {{ components.realsense_up_macro( + camera_name = 'rgbd', + camera_suffix='_up', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.07, y = 0, z = 0.025, roll = 0, - pitch = -components.rad90, - yaw = 0) + pitch = -math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - + {# #} - + {# Mobius camera placements {--> #} - {% if enable_mobius_camera_down %} - - {{ components.mobius_camera_macro( - namespace = namespace, - camera_name = "mobius_down", + {# Mobius down {--> #} + {{ components.mobius_down_macro( + camera_name = 'mobius_down', parent_link = root, - frame_rate = 30.0, x = 0.144, y = 0.0, z = -0.092, roll = 0, - pitch = components.rad90, - yaw = 0) + pitch = math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - {% if enable_mobius_camera_front %} - - {{ components.mobius_camera_macro( - namespace = namespace, - camera_name = "mobius_front", + {# Mobius front {--> #} + {{ components.mobius_front_macro( + camera_name = 'mobius_front', parent_link = root, - frame_rate = 30.0, x = 0.2, y = 0.0, z = -0.04, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - {% if enable_mobius_camera_back_left %} - - {{ components.mobius_camera_macro( - namespace = namespace, - camera_name = "mobius_back_left", + {# Mobius back left {--> #} + {{ components.mobius_back_left_macro( + camera_name = 'mobius_back_left', parent_link = root, - frame_rate = 30.0, x = -0.1, y = 0.1732, z = -0.04, roll = 0, pitch = 0, - yaw = components.rad120) + yaw = math.radians(120), + mount = none, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - {% if enable_mobius_camera_back_right %} - - {{ components.mobius_camera_macro( - namespace = namespace, - camera_name = "mobius_back_right", + {# Mobius back right {--> #} + {{ components.mobius_back_right_macro( + camera_name = 'mobius_back_right', parent_link = root, - frame_rate = 30.0, x = -0.1, y = -0.1732, z = -0.04, roll = 0, pitch = 0, - yaw = -components.rad120) + yaw = -math.radians(120), + mount = none, + spawner_args = spawner_args) }} - - {% endif %} - + {# #} - {# UV camera {--> #} - {% if enable_dual_uv_cameras %} - - {{ components.uvcam_macro( + {# #} + + {# Dual UV cameras {--> #} + {{ components.dual_uv_cameras_macro( parent_link = root, - camera_publish_topic = "/" + namespace + "/uvdar_bluefox_left/image_raw", - calibration_file = uvcam_calib_file, - occlusions = uvcam_occlusions, - frame_rate = 60, - device_id = namespace + "_1", - x = 0.037, - y = 0.1175, - z = 0.05, - roll = 0, - pitch = 0, - yaw = components.rad70) + x1 = 0.037, + y1 = 0.1175, + z1 = 0.05, + roll1 = 0, + pitch1 = 0, + yaw1 = math.radians(70), + x2 = 0.037, + y2 = -0.1175, + z2 = 0.05, + roll2 = 0, + pitch2 = 0, + yaw2 = -math.radians(70), + mount = uvdar_mount, + spawner_args = spawner_args) }} + {# #} - {{ components.uvcam_macro( + {# Back UV cameras {--> #} + {{ components.back_uv_camera_macro( parent_link = root, - camera_publish_topic = "/" + namespace + "/uvdar_bluefox_right/image_raw", - calibration_file = uvcam_calib_file, - occlusions = uvcam_occlusions, - frame_rate = 60, - device_id = namespace + "_2", - x = 0.037, - y = -0.1175, - z = 0.05, - roll = 0, + x = -0.1, + y = 0.0, + z = -0.02, + roll = math.radians(90), pitch = 0, - yaw = -components.rad70) + yaw = math.radians(180), + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# 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", + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/servo_camera_optical', + sensor_base_frame_name = spawner_args['name'] + '/servo_camera', offset_pitch_link_x = 0.0, offset_pitch_link_y = 0.0, offset_pitch_link_z = 0.0, @@ -1346,39 +1182,33 @@ img_height = 1080, compensate_tilt_roll = true, compensate_tilt_pitch = true, - pitch_link_mesh_file = "", - roll_link_mesh_file = "", - mesh_scale = "") + pitch_link_mesh_file = '', + roll_link_mesh_file = '', + mesh_scale = '', + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# ========================== other sensors ========================= #} {# Teraranger Tower Evo {--> #} - {% if enable_teraranger_evo_tower %} - {{ components.teraranger_evo_tower_macro( parent_link = root, visualize = False, - frame_name = namespace + "/teraranger_tower", - parent_frame_name = namespace + "/fcu", + frame_name = spawner_args['name'] + '/teraranger_tower', + parent_frame_name = spawner_args['name'] + '/fcu', gaussian_noise = 0.0, x = 0.0, y = 0.0, z = 0.178, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Magnetic gripper {--> #} - {% if enable_magnetic_gripper %} - {{ components.magnet_gripper_visualization_macro( parent_link = root, x = 0.0, @@ -1386,130 +1216,84 @@ z = - (0.20/2 + 0.01), roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} - {# Timepix {--> #} - {% if enable_timepix %} - - {{ components.timepix_macro( + {# UV leds {--> #} + {{ components.uv_leds_macro( parent_link = root, - name = "timepix", - x = 0.115, - y = -0.05, - z = -0.05, - roll = 0, - pitch = 0, - yaw = 0) + x1 = 0.2382, + x2 = 0.0, + y1 = 0.1375, + y2 = 0.2750, + z = -0.07, + spawner_args = spawner_args) }} - - {% endif %} - {# #} - - {# UV leds {--> #} - {% if enable_uv_leds %} - - - {% if uvled_s_l != "None" %} - {% set led1 = uvled_s_l %} - {% set led3 = uvled_s_l %} - {% set led5 = uvled_s_l %} - {% else %} - {% set led1 = uvled_s[0] %} - {% set led3 = uvled_s[2] %} - {% set led5 = uvled_s[4] %} - {% endif %} - - {% if uvled_s_r != "None" %} - {% set led2 = uvled_s_r %} - {% set led4 = uvled_s_r %} - {% set led6 = uvled_s_r %} - {% else %} - {% set led2 = uvled_s[1] %} - {% set led4 = uvled_s[3] %} - {% set led6 = uvled_s[5] %} - {% endif %} - - {# -- leds configuration -- (id, signal_id, x, y, z, roll, pitch, yaw) #} - {%- set uv_leds_macro_parameters = [(1, led1, 0.2382, 0.1375, -0.07, 0.0, components.rad90, components.rad30), - (2, led2, 0.2382, -0.1375, -0.07, 0.0, components.rad90, components.rad330), - (3, led3, 0, 0.2750, -0.07, 0.0, components.rad90, components.rad90), - (4, led4, 0, -0.2750, -0.07, 0.0, components.rad90, components.rad270), - (5, led5, -0.2382, 0.1375, -0.07, 0.0, components.rad90, components.rad150), - (6, led6, -0.2382, -0.1375, -0.07, 0.0, components.rad90, components.rad210)] -%} - - {% for id_, signal_id_, x_, y_, z_, roll_, pitch_, yaw_ in uv_leds_macro_parameters %} - {{ components.uvled_macro( - parent_link = root, - device_id = namespace + "_" + id_ | string(), - signal_id = signal_id_, - x = x_, - y = y_, - z = z_, - roll = roll_, - pitch = pitch_, - yaw = yaw_) - }} - {% endfor %} - - {% endif %} {# #} {# Light {--> #} - {% if enable_light %} - {{ components.light_macro( parent_link = root, - update_rate = 30, max_pitch_rate = 0.1, - initial_on = True, - compensate_tilt = True, x = 0.2, y = 0.0, z = -0.1, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Whycon box {--> #} - {% if enable_whycon_box %} - {{ components.whycon_box_macro( parent_link = root, - mesh_scale = "1 1 1", x = 0, y = 0, - z = 0.25, + z = 0.08, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} - {# Pendelum {--> #} - {% if enable_pendulum %} - + {# Pendulum {--> #} {{ components.pendulum_macro( parent_link = root, - number_of_chains = 30, - chain_length = 0.1, - chain_radius = 0.01, - chain_mass = 0.5 / 30, x = 0, y = 0, - z = -0.10) + z = -0.10, + spawner_args = spawner_args) }} - - {% endif %} + {# #} + + {# ====================== conditional components ==================== #} + + {# 2D lidar mount {--> #} + {%- if rplidar | replace('\s', '') | length != 0 or scanse_sweep | replace('\s', '') | length != 0 -%} + + {{ lidar_mount_2d }} + + {%- endif -%} + {# #} + + {# 3D lidar mount {--> #} + {%- if velodyne | replace('\s', '') | length != 0 or ouster | replace('\s', '') | length != 0 -%} + + {{ lidar_mount_3d }} + + {%- endif -%} + {# #} + + {# Extra leg to hold a Realsense mount {--> #} + {%- if realsense_front | replace('\s', '') | length != 0 or realsense_front_pitched | replace('\s', '') | length != 0 or realsense_down | replace('\s', '') | length != 0 -%} + + {{ front_left_leg }} + + {%- endif -%} {# #} diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/generic_components.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/generic_components.sdf.jinja new file mode 100644 index 0000000..d6a977e --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/generic_components.sdf.jinja @@ -0,0 +1,1693 @@ + + + +{# !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! #} +{# !! THIS DOCUMENT CONTAINS ONLY BASIC BUILDING BLOCKS, PLUGINS, !! #} +{# !! AND GENERIC SENSORS DEFINITIONS. DO NOT DEFINE ARGUMENTS !! #} +{# !! OR COMPLEX COMPONENTS THAT ARE USED BY THE MRS DRONE SPAWNER !! #} +{# !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! #} + + + + + +{# ========================== general blocks ========================= #} + +{# handle_spawner_args {--> #} +{%- macro handle_spawner_args(keyword, default_args, args) -%} + + {%- if default_args is defined -%} + + {%- if args[keyword] is not defined or args[keyword] is none -%} + {%- set _ = args.update({keyword: default_args}) -%} + + {%- elif args[keyword] is iterable -%} + + {%- if args[keyword] is mapping -%} + {%- set tmp_args = dict() -%} + {%- set _ = tmp_args.update(default_args) -%} + {%- for key, value in args[keyword].items() -%} + {%- set _ = tmp_args.update({key: value}) -%} + {%- endfor -%} + {%- set _ = args.update({keyword: tmp_args}) -%} + + {%- elif args[keyword] is not string and args[keyword] | length != default_args | length -%} + {%- set _ = args.update({keyword: default_args}) -%} + {%- endif -%} + + {%- endif -%} + {%- endif -%} +{%- endmacro -%} +{# #} + +{# zero_inertial_macro {--> #} +{# macro to add the lowest mass and intertia that gazebo allows #} +{%- macro zero_inertial_macro() -%} + + 0.00001 + + 1e-7 + 0 + 0 + 1e-7 + 0 + 1e-7 + + +{%- endmacro -%} +{# #} + +{# multirotor_physics_macro {--> #} +{%- macro multirotor_physics_macro(mass, body_radius, body_height, rotor_velocity_slowdown_sim, ixx, ixy, ixz, iyy, iyz, izz) -%} + + {{ mass }} + + {{ ixx }} + {{ ixy }} + {{ ixz }} + {{ iyy }} + {{ iyz }} + {{ izz }} + + + + + 0 + 0 + {{ - body_height / 2 }} + 0 + 0 + 0 + + + + {{ body_height }} + {{ body_radius }} + + + + + + 0.001 + 0.0 + + + + + + + + + + + base_link + {{ rotor_velocity_slowdown_sim }} + +{%- endmacro -%} +{# #} + +{# collision_cylinder_macro {--> #} +{%- 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 }} + + + + + 0.001 + 0.0 + + + + + + +{%- endmacro -%} +{# #} + +{# cylinder_inertia {--> #} +{%- macro cylinder_inertia(m, r, h) -%} + + {{ m * ( 3 * r * r + h * h ) / 12 }} + 0 + 0 + {{ m * ( 3 * r * r + h * h ) / 12 }} + 0 + {{ m * r * r / 2 }} + +{%- endmacro -%} +{# #} + +{# single_chain_pendulum_macro {--> #} +{%- macro single_chain_pendulum_macro(parent_link, id, link_mass, link_radius, link_length, joint_offset_x, joint_offset_y, joint_offset_z) -%} + + 0 0 {{ -link_length/2.0 }} 0 0 0 + + {{ link_mass }} + {{ cylinder_inertia( + m = link_mass, + r = link_radius, + h = link_length) + }} + + 1 + + 0 0 0 0 0 0 + + + {{ link_length }} + {{ link_radius }} + + + + + + + + 0 0 {{ link_length/2.0 }} 0 0 0 + + + 0.012 + + + + + + + + + + {{ parent_link }} + pendulum_chain_{{ id }}_link + {{ joint_offset_x }} {{ joint_offset_y }} {{ joint_offset_z }} 0 0 0 + + 1 0 0 + + -3.1415 + 3.1415 + -1 + -1 + + + 0.0 + 0.0 + + + + 0 1 0 + + -3.1415 + 3.1415 + -1 + -1 + + + 0.0 + 0.0 + + + +{%- endmacro -%} +{# #} + +{# ========================== visual blocks ========================= #} + +{# visual_mesh_macro {--> #} +{%- macro visual_mesh_macro(name, mesh_file, mesh_scale, color, x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + + {{ mesh_file }} + {{ mesh_scale }} + + + + + + +{%- endmacro -%} +{# #} + +{# visual_mesh_textured_macro {--> #} +{%- macro visual_mesh_textured_macro(name, mesh_file, mesh_scale, x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + + {{ mesh_file }} + {{ mesh_scale }} + + + +{%- endmacro -%} +{# #} + +{# visual_mesh_mrs_material_macro {--> #} +{%- macro visual_mesh_mrs_material_macro(name, mesh_file, mesh_scale, color, x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + + {{ mesh_file }} + {{ mesh_scale }} + + + + + + +{%- endmacro -%} +{# #} + +{# visual_link_macro {--> #} +{%- macro visual_link_macro(name, mesh_file, mesh_scale, color, x, y, z, roll, pitch, yaw, parent_link) -%} + + {{ zero_inertial_macro() }} + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ visual_mesh_macro(name, mesh_file, mesh_scale, color, 0, 0, 0, 0, 0, 0) }} + + + {{ name }}_link + {{ parent_link }} + +{%- endmacro -%} +{# #} + +{# visual_mesh_with_collision_macro {--> #} +{%- macro visual_mesh_with_collision_macro(name, mesh_file, mesh_scale, color, x, y, z, roll, pitch, yaw) -%} + {{ visual_mesh_macro(name, mesh_file, mesh_scale, color, x, y, z, roll, pitch, yaw) }} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + + {{ mesh_file }} + {{ mesh_scale }} + + + +{%- endmacro -%} +{# #} + +{# visual_mesh_textured_with_collision_macro {--> #} +{%- macro visual_mesh_textured_with_collision_macro(name, mesh_file, mesh_scale, x, y, z, roll, pitch, yaw) -%} + {{ visual_mesh_textured_macro(name, mesh_file, mesh_scale, x, y, z, roll, pitch, yaw) }} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + + {{ mesh_file }} + {{ mesh_scale }} + + + +{%- endmacro -%} +{# #} + +{# visual_colored_box_macro {--> #} +{%- macro visual_colored_box_macro(name, size_x, size_y, size_z, color, x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + + {{ size_x }} {{ size_y }} {{ size_z }} + + + + + + +{%- endmacro -%} +{# #} + +{# visual_colored_box_with_collision_macro {--> #} +{%- macro visual_colored_box_with_collision_macro(name, size_x, size_y, size_z, color, x, y, z, roll, pitch, yaw) -%} + {{ visual_colored_box_macro(name, size_x, size_y, size_z, color, x, y, z, roll, pitch, yaw) }} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + + {{ size_x }} {{ size_y }} {{ size_z }} + + + +{%- endmacro -%} +{# #} + +{# ============================ drone legs =========================== #} + +{# leg_macro {--> #} +{%- macro leg_macro(name, mesh_file, mesh_scale, color, parent, x, y, z, roll, pitch, yaw, collision_height, collision_radius) -%} + {{ visual_mesh_macro( + name = name, + mesh_file = mesh_file, + mesh_scale = mesh_scale, + color = color, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + {{ collision_cylinder_macro( + name = name, + collision_length = collision_height, + collision_radius = collision_radius, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} +{%- endmacro -%} +{# #} + +{# leg_collision_offset_macro {--> #} +{%- macro leg_collision_offset_macro(name, mesh_file, mesh_scale, color, x, y, z, roll, pitch, yaw, collision_height, collision_radius, offset_x, offset_y, offset_z) -%} + {{ visual_mesh_macro( + name = name, + mesh_file = mesh_file, + mesh_scale = mesh_scale, + color = color, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + {{ collision_cylinder_macro( + name = name, + collision_length = collision_height, + collision_radius = collision_radius, + x = x + offset_x, + y = y + offset_y, + z = z + offset_z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} +{%- endmacro -%} +{# #} + +{# ========================= drone propellers ======================== #} + +{# motor_plugins_macro {--> #} +{%- macro motor_plugins_macro(motor_number, direction, time_constant_up, time_constant_down, max_rot_velocity, motor_constant, moment_constant, rotor_drag_coefficient, rolling_moment_coefficient, rotor_velocity_slowdown_sim, enable_motor_crash) -%} + + + 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 }} + +{%- endmacro -%} +{# #} + +{# prop_macro {--> #} +{%- macro prop_macro(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, 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 }} + + + {{ visual_mesh_macro("prop_{{ motor_number }}", mesh_file, mesh_scale, color, 0, 0, 0, 0, 0, 0) }} + {{ collision_cylinder_macro( + name = "prop_{{ motor_number }}_link", + collision_length = 2 * radius, + collision_radius = 0.01, + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = math.radians(90), + yaw = 0) + }} + + {{ motor_plugins_macro(motor_number, direction, time_constant_up, time_constant_down, max_rot_velocity, motor_constant, moment_constant, rotor_drag_coefficient, rolling_moment_coefficient, rotor_velocity_slowdown_sim, enable_motor_crash) }} + + {{ parent }} + prop_{{ motor_number }}_link + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + 1 + + +{%- endmacro -%} +{# #} + +{# prop_macro_2_meshes {--> #} +{%- macro prop_macro_2_meshes(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 }} + + + {{ visual_mesh_textured_macro("prop_{{ motor_number }}_first", mesh_file_1, mesh_scale, 0, 0, 0, 0, 0, 0) }} + {{ visual_mesh_macro("prop_{{ motor_number }}_second", mesh_file_2, mesh_scale, color, 0, 0, meshes_z_offset, 0, 0, 0) }} + {{ collision_cylinder_macro( + name = "prop_{{ motor_number }}_link", + collision_length = 2 * radius, + collision_radius = 0.01, + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = math.radians(90), + yaw = 0) + }} + + {{ motor_plugins_macro(motor_number, direction, time_constant_up, time_constant_down, max_rot_velocity, motor_constant, moment_constant, rotor_drag_coefficient, rolling_moment_coefficient, rotor_velocity_slowdown_sim, enable_motor_crash) }} + + {{ parent }} + prop_{{ motor_number }}_link + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + 1 + + +{%- endmacro -%} + + +{# TODO: UNUSED #} +{# mrs_motor_plugins_macro {--> #} +{%- macro motor_plugins_macro(motor_number, direction, time_constant_up, time_constant_down, max_rot_velocity, motor_constant, moment_constant, rotor_drag_coefficient, rolling_moment_coefficient, rotor_velocity_slowdown_sim, enable_motor_crash) -%} + + + 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 }} + +{%- endmacro -%} +{# #} + +{# TODO: UNUSED #} +{# prop_macro_2_meshes_mrs_motor_model {--> #} +{%- 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 {{ math.radians(90) }} 0 + + + {{ 2*radius }} + 0.01 + + + + + + + + + + + + + +{{ mrs_motor_plugins_macro(motor_number, direction, time_constant_up, time_constant_down, max_rot_velocity, motor_constant, moment_constant, rotor_drag_coefficient, rolling_moment_coefficient, rotor_velocity_slowdown_sim, enable_motor_crash) }} + + + {{ parent }} + prop_{{ motor_number }}_link + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + 1 + + +{%- endmacro -%} +{# #} + +{# =========================== base sensors ========================== #} + +{# odometry_sensor_macro {--> #} +{%- macro odometry_sensor_macro(odometry_sensor_name, parent_link, topic_name, noise, frame_name, update_rate, x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ zero_inertial_macro() }} + + + {{ parent_link }} + {{ odometry_sensor_name }}_link + + 0 0 1 + + 0 + 0 + 0 + 0 + + 1 + + + + true + {{ odometry_sensor_name }}_link + {{ update_rate }} + {{ topic_name }} + {{ noise }} + {{ frame_name }} + {{ x }} {{ y }} {{ z }} + {{ roll }} {{ pitch }} {{ yaw }} + +{%- endmacro -%} +{# #} + +{# gazebo_rangefinder_sensor_macro {--> #} +{%- macro gazebo_rangefinder_sensor_macro(name, parent_frame_name, sensor_frame_name, topic, update_rate, samples, fov, min_distance, max_distance, resolution, noise, x, y, z, roll, pitch, yaw) -%} + + true + false + {{ update_rate }} + + + + {{ samples }} + 1 + -{{ fov/2 }} + {{ fov/2 }} + + + {{ samples }} + 1 + -{{ fov/2 }} + {{ fov/2 }} + + + + {{ min_distance }} + {{ max_distance }} + {{ resolution }} + + + gaussian + 0.0 + {{ noise }} + + + + + {{ max_distance }} + {{ min_distance }} + {{ topic }} + {{ fov }} + {{ x }} + {{ y }} + {{ z }} + {{ roll }} + {{ pitch }} + {{ yaw }} + {{ sensor_frame_name }} + {{ parent_frame_name }} + + +{%- endmacro -%} +{# #} + +{# rangefinder_sensor_macro {--> #} +{%- macro rangefinder_sensor_macro(name, parent_frame_name, sensor_frame_name, topic, update_rate, samples, fov, min_distance, max_distance, resolution, noise, x, y, z, roll, pitch, yaw) -%} + + {{ update_rate }} + + + + {{ samples }} + 1 + -{{ fov/2 }} + {{ fov/2 }} + + + {{ samples }} + 1 + -{{ fov/2 }} + {{ fov/2 }} + + + + {{ min_distance }} + {{ max_distance }} + {{ resolution }} + + + + {{ noise }} + true + {{ update_rate }} + {{ topic }} + {{ sensor_frame_name }} + {{ fov }} + radiation + {{ parent_frame_name }} + {{ x }} + {{ y }} + {{ z }} + {{ roll }} + {{ pitch }} + {{ yaw }} + + +{%- endmacro -%} +{# #} + +{# imu_sensor_macro {--> #} +{%- macro imu_sensor_macro(sensor_name, parent_link, update_rate, topic_name, frame_name, noise_mean, x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ zero_inertial_macro() }} + + true + true + {{ update_rate }} + false + __default_topic__ + + + {{ topic_name }} + {{ update_rate }} + {{ noise_mean }} + 0 0 0 + 0 0 0 + {{ frame_name }} + + + + + + {{ parent_link }} + {{ sensor_name }}_link + +{%- endmacro -%} +{# #} + +{# teraranger_evo_macro {--> #} +{%- macro teraranger_evo_macro(parent_link, id, visualize, frame_name, parent_frame_name, gaussian_noise, x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + + + model://mrs_robots_description/meshes/sensors/teraranger_evo.stl + 1 1 1 + + + + + + + {{ rangefinder_sensor_macro( + name = 'teraranger_evo_' ~ id, + parent_frame_name = parent_frame_name, + sensor_frame_name = frame_name + '/range_' ~ id, + topic = '/' + frame_name + '/range_' ~ id, + update_rate = 120, + samples = 10, + fov = 0.035, + min_distance = 0.5, + max_distance = 60.0, + resolution = 1, + noise = noise, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} +{%- endmacro -%} +{# #} + +{# ultrasionic_sensor_macro (URM37) {--> #} +{%- macro ultrasonic_sensor_macro(namespace, parent_link, suffix, x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ zero_inertial_macro() }} + {{ visual_mesh_macro( + name = 'ultrasound' + suffix, + mesh_file = 'model://mrs_robots_description/meshes/sensors/ultrasonic_URM37.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + {{ rangefinder_sensor_macro( + name = 'ultrasound' ~ suffix, + parent_frame_name = namespace ~ '/fcu', + sensor_frame_name = namespace ~ '/ultrasound' ~ suffix, + topic = '/' ~ namespace ~ '/ultrasound' ~ suffix ~ '/range', + update_rate = 40, + samples = 5, + fov = 0.698131701, + min_distance = 0.04, + max_distance = 5.0, + resolution = 0.01, + noise = 0.04, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + {{ parent_link }} + ultrasound{{ suffix }}_link + +{%- endmacro -%} +{# #} +{%- macro gazebo_gps_macro(gps_name, parent_link, update_rate, gps_noise, gps_xy_random_walk, gps_z_random_walk, gps_xy_noise_density, gps_z_noise_density, gps_vxy_noise_density, gps_vz_noise_density, x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ zero_inertial_macro() }} + + + + {{ update_rate }} + {{ gps_noise }} + {{ gps_xy_random_walk }} + {{ gps_z_random_walk }} + {{ gps_xy_noise_density }} + {{ gps_z_noise_density }} + {{ gps_vxy_noise_density }} + {{ gps_vz_noise_density }} + {{ gps_name }} + + + + + + {{ gps_name }}_link + {{ parent_link }} + +{%- endmacro -%} +{# #} + +{# gazebo_magnetometer_macro {--> #} +{%- macro gazebo_magnetometer_macro(pub_rate, noise_density, random_walk, bias_correlation_time, mag_topic) -%} + + + {{ pub_rate }} + {{ noise_density }} + {{ random_walk }} + {{ bias_correlation_time }} + {{ mag_topic }} + +{%- endmacro -%} +{# #} + +{# gazebo_barometer_macro {--> #} +{%- macro gazebo_barometer_macro(baro_topic, pub_rate, baro_drift_pa_per_sec) -%} + + + {{ pub_rate }} + {{ baro_topic }} + {{ baro_drift_pa_per_sec }} + +{%- endmacro -%} +{# #} + +{# gazebo_imu_macro {--> #} +{%- macro gazebo_imu_macro(imu_name, parent_link, imu_topic, gyroscope_noise_density, gyroscope_random_walk, gyroscope_bias_correlation_time, gyroscope_turn_on_bias_sigma, accelerometer_noise_density, accelerometer_random_walk, accelerometer_bias_correlation_time, accelerometer_turn_on_bias_sigma, x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ zero_inertial_macro() }} + + + {{ parent_link }} + {{ imu_name }}_link + + 0 0 1 + + 0 + 0 + 0 + 0 + + 1 + + + + + + {{ imu_name }}_link + {{ imu_topic }} + {{ gyroscope_noise_density }} + {{ gyroscope_random_walk }} + {{ gyroscope_bias_correlation_time }} + {{ gyroscope_turn_on_bias_sigma }} + {{ accelerometer_noise_density }} + {{ accelerometer_random_walk }} + {{ accelerometer_bias_correlation_time }} + {{ accelerometer_turn_on_bias_sigma }} + +{%- endmacro -%} +{# #} + +{# gazebo_groundtruth_macro {--> #} +{%- macro gazebo_groundtruth_macro(home_latitude, home_longitude, home_altitude) -%} + + + + + {{ home_latitude }} + {{ home_longitude }} + {{ home_altitude }} + +{%- endmacro -%} +{# #} + +{# gazebo_mavlink_interface_macro {--> #} +{%- macro gazebo_mavlink_interface_macro(imu_topic, mag_topic, baro_topic, lidar_topic, mavlink_config) -%} + + + {{ imu_topic }} + {{ mag_topic }} + {{ baro_topic }} + {{ lidar_topic }} + INADDR_ANY + {{ mavlink_config['mavlink_udp_port'] }} + {{ mavlink_config['mavlink_tcp_port'] }} + 0 + /dev/ttyACM0 + 921600 + INADDR_ANY + {{ mavlink_config['qgc_udp_port'] }} + INADDR_ANY + {{ mavlink_config['sdk_udp_port'] }} + 0 + 0 + {{ mavlink_config['send_vision_estimation'] }} + {{ mavlink_config['send_odometry'] }} + {{ mavlink_config['enable_lockstep'] }} + {{ mavlink_config['use_tcp'] }} + /gazebo/command/motor_speed + + + 0 + 0.0 + 0.85 + 0 + 0.15 + velocity + + + 1 + 0.0 + 0.85 + 0 + 0.15 + velocity + + + 2 + 0.0 + 0.85 + 0 + 0.15 + velocity + + + 3 + 0.0 + 0.85 + 0 + 0.15 + velocity + + + 4 + 0.0 + 0.85 + 0 + 0.15 + velocity + + + 5 + 0.0 + 0.85 + 0 + 0.15 + velocity + + + 6 + 0.0 + 0.85 + 0 + 0.15 + velocity + + + 7 + 0.0 + 0.85 + 0 + 0.15 + velocity + + + +{%- endmacro -%} +{# #} + +{# TODO: UNUSED #} +{# gazebo_fluid_resistance_plugin_macro {--> #} +{%- macro gazebo_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 -%} +{# #} + +{# TODO: UNUSED #} +{# gazebo_wind_plugin_macro {--> #} +{%- macro gazebo_wind_plugin_macro(xyz_offset, wind_direction, wind_force_mean, wind_gust_direction, wind_gust_duration, wind_gust_start, wind_gust_force_mean) -%} + + base_link + base_link + + {{ xyz_offset }} + {{ wind_direction }} + {{ wind_force_mean }} + {{ wind_gust_direction }} + {{ wind_gust_duration }} + {{ wind_gust_start }} + {{ wind_gust_force_mean }} + +{%- endmacro -%} +{# #} + +{# ========================== camera sensors ========================= #} + +{# camera_macro {--> #} +{%- macro camera_macro(parent_link, camera_name, camera_frame_name, sensor_base_frame_name, frame_rate, parent_frame_name, horizontal_fov, image_width, image_height, min_distance, max_distance, noise_mean, noise_stddev, mesh_file, mesh_scale, color, visual_x, visual_y, visual_z, visual_roll, visual_pitch, visual_yaw, x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ zero_inertial_macro() }} + {{ visual_mesh_macro( + name = camera_name + "_visual", + mesh_file = mesh_file, + mesh_scale = mesh_scale, + color = color, + x = visual_x, + y = visual_y, + z = visual_z, + roll = visual_roll, + pitch = visual_pitch, + yaw = visual_yaw) + }} + + {{ frame_rate }} + + {{ horizontal_fov }} + + {{ image_width }} + {{ image_height }} + + + {{ min_distance }} + {{ max_distance }} + + + gaussian + + {{ noise_mean }} + {{ noise_stddev }} + + + + true + {{ frame_rate }} + {{ camera_name }} + image_raw + camera_info + /{{ camera_frame_name }} + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + {{ parent_frame_name }} + {{ sensor_base_frame_name }} + {{ x }} + {{ y }} + {{ z }} + {{ roll }} + {{ pitch }} + {{ yaw }} + + + + + + {{ parent_link }} + {{ camera_name }}_link + + +{%- endmacro -%} +{# #} + +{# realsense_d435_macro {--> #} +{%- macro realsense_d435_macro(namespace, camera_name, camera_suffix, parent_link, realistic, update_rate, x, y, z, roll, pitch, yaw) -%} + {# -- frame names -- #} + {%- set frame_fcu = namespace + "/fcu" -%} + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ zero_inertial_macro() }} + + + + 0 0 0 {{ -math.radians(90) }} 0 {{ -math.radians(90) }} + + + model://mrs_robots_description/meshes/sensors/realsense_body.stl + 0.001 0.001 0.001 + + + + + + + + 0 0 0 {{ -math.radians(90) }} 0 {{ -math.radians(90) }} + + + model://mrs_robots_description/meshes/sensors/realsense_glass.stl + 0.001 0.001 0.001 + + + + + + + + + + + + + {# 0 -0.046 0.004 0 0 0 #} + 0 0 -0.0115 0 0 0 + + {# 1.211259 #} + 1.211259 + + 1280 + 720 + RGB_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.007 + + + 1 + {{ update_rate }} + 0 + + + + + + 1 + {{ update_rate }} + 0 + + {# 1.7523106 #} + 1.211259 + + 640 + 360 + L_INT8 + + + 0.1 + 50 + + + gaussian + 0.0 + 0.005 + + + + 0 0.05 0 0 0 0 + {# 1.7523106 #} + 1.211259 + + 640 + 360 + L_INT8 + + + 0.1 + 50 + + + gaussian + 0.0 + 0.005 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 -0.0115 0.0 0 0 0 + + 1.211259 + + {% if realistic %} + 320 + 180 + {% else %} + 1280 + 720 + {% endif %} + + + 0.3 + 12 + + + gaussian + 0.0 + 5000.0 + + + 1 + {{ update_rate }} + 0 + + + + + + + + + {{ camera_name }} + {{ camera_suffix }} + {{ realistic }} + 0.2 + 4.0 + 0.8 + 0.2 + 4 + 15 + 5 + {{ frame_fcu }} + {{ x }} + {{ y }} + {{ z }} + {{ roll }} + {{ pitch }} + {{ yaw }} + + + + {{ parent_link }} + {{ camera_name }}{{ camera_suffix }}_link + +{%- endmacro -%} + + +{# fisheye_camera_macro {--> #} +{%- macro fisheye_camera_macro(parent_link, camera_name, topic_ns_prefix, camera_frame_name, sensor_base_frame_name, frame_rate, parent_frame_name, horizontal_fov, image_width, image_height, min_distance, max_distance, lens_type, lens_c1, lens_c2, lens_f, lens_fun, lens_scale, lens_cutoff_angle, lens_texture_size, noise_mean, noise_stddev, mesh_file, mesh_scale, color, visual_x, visual_y, visual_z, visual_roll, visual_pitch, visual_yaw, x, y, z, roll, pitch, yaw) -%} + {# -- topics -- #} + {%- set topic_image = topic_ns_prefix + "image_raw" -%} + {%- set topic_camera_ifo = topic_ns_prefix + "camera_info" -%} + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ zero_inertial_macro() }} + {{ visual_mesh_macro( + name = "", + mesh_file = mesh_file, + mesh_scale = mesh_scale, + color = color, + x = visual_x, + y = visual_y, + z = visual_z, + roll = visual_roll, + pitch = visual_pitch, + yaw = visual_yaw) + }} + + {{ frame_rate }} + + {{ horizontal_fov }} + + {{ image_width }} + {{ image_height }} + + + {{ min_distance }} + {{ max_distance }} + + + gaussian + + {{ noise_mean }} + {{ noise_stddev }} + + + {{ lens_type }} + + {{ lens_c1 }} + {{ lens_c2 }} + {{ lens_f }} + {{ lens_fun }} + + {{ lens_scale }} + {{ lens_cutoff_angle }} + {{ lens_texture_size }} + + + + true + {{ frame_rate }} + {{ camera_name }} + {{ topic_image }} + {{ topic_camera_ifo }} + /{{ camera_frame_name }} + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + {{ parent_frame_name }} + {{ sensor_base_frame_name }} + {{ x }} + {{ y }} + {{ z }} + {{ roll }} + {{ pitch }} + {{ yaw }} + + + + + + {{ parent_link }} + {{ camera_name }}_link + + {%- endmacro -%} + + +{# thermal_camera_macro {--> #} +{%- macro thermal_camera_macro(camera_name, camera_topic_name, parent_frame_name, camera_frame_name, sensor_base_frame_name, parent_link, frame_rate, hfov, image_width, image_height, x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ zero_inertial_macro() }} + {{ visual_mesh_macro( + name = 'base', + mesh_file = 'model://mrs_robots_description/meshes/sensors/teraranger_evo_thermal_33.dae', + mesh_scale = '0.001 0.001 0.001', + color = 'Yellow', + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + {{ frame_rate }} + + {{ hfov }} + + R8G8B8 + {{ 3 * image_width }} + {{ 3 * image_height }} + + + 0.1 + 300 + + + gaussian + 0.0 + 0.0 + + + + true + {{ camera_name }} + {{ frame_rate }} + {{ camera_topic_name }}/rgb_image + {{ camera_topic_name }}/camera_info + {{ camera_topic_name }}/raw_temp_array + 20 + 150 + 0.2 + 4.0 + 20.0 + {{ camera_frame_name }} + {{ parent_frame_name }} + {{ sensor_base_frame_name }} + {{ x }} + {{ y }} + {{ z }} + {{ roll }} + {{ pitch }} + {{ yaw }} + + + + + + {{ parent_link }} + {{ camera_name }}_link + +{%- endmacro -%} +{# #} + +{# ======================== UV leds and cameras ======================== #} + + {# uvcam_macro {--> #} + {%- macro uvcam_macro(parent_link, calibration_file, occlusions, frame_rate, device_id, camera_publish_topic, x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ zero_inertial_macro() }} + + 0 0 0 {{ math.radians(90) }} 0 0 + + + model://mrs_robots_description/meshes/sensors/bluefox.dae + 1 1 1 + + + + + + + + 1 + + false + {{ frame_rate }} + {{ occlusions }} + {{ frame_rate }} + {{ calibration_file }} + {{ device_id }} + {{ camera_publish_topic }} + + + + + + {{ parent_link }} + uvcam_{{ device_id }}_link + +{%- endmacro -%} +{# #} + +{# uvled_macro {--> #} +{%- macro uvled_macro(parent_link, device_id, signal_id , x, y, z, roll, pitch, yaw) -%} + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ zero_inertial_macro() }} + + 0 0 -0.0025 0 0 0 + + + 0.005 + 0.007 + + + + + + + + 0 0 -0.0025 0 0 0 + + + 0.001 0.02 0.001 + + + + + + + + 0 0 0 0 0 0 + + + 0.005 + + + + + + + + 1 + + false + 1 + {{ signal_id }} + {{ device_id }} + + + + + + {{ parent_link }} + uvled_{{ device_id }}_link + true + true + +{%- endmacro -%} +{# #} + + 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 index bfe87b1..069dfa9 100644 --- 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 @@ -1,7 +1,8 @@ - {%- import "mrs_robots_description/sdf/component_snippets.sdf.jinja" as components -%} + {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} @@ -19,14 +20,7 @@ {%- 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 %} - + {%- set root = 'base_link' -%} {# #} {# Motor constants {--> #} @@ -37,7 +31,7 @@ {%- 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" -%} + {%- set rolling_moment_coefficient = '1.0e-6' -%} {# #} {# Inertia constants {--> #} @@ -48,29 +42,26 @@ {# 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" -%} + {%- 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 {--> #} + {%- 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' -%} {# #} {# Mounts {--> #} - {%- set battery_mesh_file = "model://mrs_robots_description/meshes/t_drones/m690b/t_drone_m690b_battery.stl" -%} + {%- 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" -%} + {%- 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' -%} {# #} {# #} @@ -93,150 +84,150 @@ - - - + {# ================================================================== #} + {# || bare body definitions || #} + {# ================================================================== #} - {{ 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) + {{ generic.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) + {{ generic.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) + {{ generic.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) + {{ generic.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) + {{ generic.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 = math.radians(180)) }} - {{ 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) + {{ generic.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 = math.radians(180)) }} - - {{ 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) + + {{ generic.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) + {{ generic.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 = -math.radians(90), + 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) + {{ generic.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 = -math.radians(90), + 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) + + {{ generic.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) }} @@ -244,43 +235,52 @@ - - {{ 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", + {# Propellers {--> #} + {%- set prop_list = [ + { + 'motor_number': 0, + 'direction': 'ccw', + 'x': rotor_x_offset, + 'y': -rotor_y_offset, + 'z': rotor_z_offset, + 'mesh_files': [prop_mesh_file, motor_top_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'Grey' + }, + { + 'motor_number': 1, + 'direction': 'ccw', + 'x': -rotor_x_offset, + 'y': rotor_y_offset, + 'z': rotor_z_offset, + 'mesh_files': [prop_mesh_file, motor_top_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'Grey' + }, + { + 'motor_number': 2, + 'direction': 'cw', + 'x': rotor_x_offset, + 'y': rotor_y_offset, + 'z': rotor_z_offset, + 'mesh_files': [prop_mesh_file, motor_top_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'Grey' + }, + { + 'motor_number': 3, + 'direction': 'cw', + 'x': -rotor_x_offset, + 'y': -rotor_y_offset, + 'z': rotor_z_offset, + 'mesh_files': [prop_mesh_file, motor_top_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'Grey' + } + ] + -%} + {{ components.propellers_macro( + prop_list = prop_list, rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, motor_constant = motor_constant, moment_constant = moment_constant, @@ -290,133 +290,47 @@ 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) - }} + prop_ixx = prop_ixx, + prop_ixy = prop_ixy, + prop_ixz = prop_ixz, + prop_iyy = prop_iyy, + prop_iyz = prop_iyz, + prop_izz = prop_izz, + spawner_args = spawner_args) + }} + {# #} + + {# ================================================================== #} + {# || compulsory sensor definitions || #} + {# ================================================================== #} + + + {%- set imu_topic = '/imu' -%} + {%- set mag_topic = '/mag' -%} + {%- set baro_topic = '/baro' -%} + {%- set lidar_topic = '/lidar' -%} + + + {{ generic.gazebo_groundtruth_macro( + home_latitude = 0, + home_longitude = 0, + home_altitude = 0) + }} - {{ components.gps_macro( - gps_name = "gps0", + {{ generic.gazebo_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_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, @@ -425,40 +339,43 @@ roll = 0, pitch = 0, yaw = 0) - }} + }} - {{ components.magnetometer_plugin_macro( + {{ generic.gazebo_magnetometer_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) - }} + mag_topic = mag_topic) + }} - {{ components.barometer_plugin_macro( - baro_topic = "/baro", + {{ generic.gazebo_barometer_macro( + baro_topic = baro_topic, pub_rate = 50, baro_drift_pa_per_sec = 0) - }} + }} + + + + {{ generic.gazebo_mavlink_interface_macro( + imu_topic = imu_topic, + mag_topic = mag_topic, + baro_topic = baro_topic, + lidar_topic = lidar_topic, + mavlink_config = spawner_args['mavlink_config']) + }} - {{ components.imu_plugin_macro( - imu_name = "imu", + + {{ generic.gazebo_imu_macro( + imu_name = 'imu', parent_link = root, - imu_topic = "/imu", + imu_topic = '/imu', gyroscope_noise_density = 0.00018665, gyroscope_random_walk = 0.000038785, gyroscope_bias_correlation_time = 1000.0, @@ -473,128 +390,101 @@ roll = 0, pitch = 0, yaw = 0) - }} + }} + + - - - + {# ================================================================== #} + {# || optional sensor definitions || #} + {# ================================================================== #} - {# Ground truth {--> #} - {% if enable_ground_truth %} - - {{ components.odometry_plugin_macro( - odometry_sensor_name = "ground_truth", + {# Ground truth {--> #} + {{ components.ground_truth_macro( 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 %} - {# #} + yaw = 0, + spawner_args = spawner_args) + }} + {# #} - + {# ======================= rangefinder sensors ====================== #} - {# 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, + {# Garmin down {--> #} + {{ components.garmin_down_macro( parent_link = root, - orientation = "", x = -0.05, y = 0.0, z = -0.055, roll = 0, - pitch = components.rad(90), - yaw = 0) - }} - - {% endif %} - {# #} + pitch = math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) + }} + {# #} - + {# ========================== LIDAR sensors ========================= #} - {# Ouster {--> #} - {% if enable_ouster %} - - {{ components.ouster_macro( - namespace = namespace, + {# Ouster {--> #} + {{ components.ouster_macro( 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, + sensor_name = 'os', x = 0.0, y = 0.0, z = 0.107, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} - {# #} + yaw = 0, + mount = none, + spawner_args = spawner_args) + }} + {# #} - + {# ========================== camera sensors ======================== #} - {% if enable_realsense_down %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="_down", + {# Realsense down {--> #} + {{ components.realsense_down_macro( + camera_name = 'down_rgbd', + camera_suffix='', 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", + pitch = math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) + }} + {# #} + + {# TODO: this does not actually point down #} + {# Basler down {--> #} + {{ components.basler_camera_down_macro( + 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 + roll = math.radians(90), + pitch = math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) + }} + {# #} - {# Servo camera {--> #} - {% if enable_servo_camera %} - - {{ components.servo_camera_macro( + {# 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", + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/servo_camera_optical', + sensor_base_frame_name = spawner_args['name'] + '/servo_camera', offset_pitch_link_x = 0.0, offset_pitch_link_y = 0.0, offset_pitch_link_z = 0.0, @@ -620,76 +510,51 @@ 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", + pitch_link_mesh_file = '', + roll_link_mesh_file = '', + mesh_scale = '', + spawner_args = spawner_args) + }} + {# #} + + {# Three thermal cameras {--> #} + {%- set three_thermal_cameras = [ + { + 'name': 'top', + 'x': 0.15, + 'y': 0.06, + 'z': -0.025, + 'roll': 0.0, + 'pitch': math.radians(-30), + 'yaw': 0.0 + }, + { + 'name': 'middle', + 'x': 0.16, + 'y': 0.06, + 'z': -0.055, + 'roll': 0.0, + 'pitch': 0.0, + 'yaw': 0.0 + }, + { + 'name': 'bottom', + 'x': 0.15, + 'y': 0.06, + 'z': -0.085, + 'roll': 0.0, + 'pitch': math.radians(30), + 'yaw': 0.0 + } + ] + -%} + {{ components.thermal_cameras_macro( + cameras_list = three_thermal_cameras, 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 %} - {# #} + mount = none, + spawner_args = spawner_args) + }} + {# #} 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 6a28336..f91b6d9 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 @@ -1,7 +1,8 @@ - {%- import "mrs_robots_description/sdf/component_snippets.sdf.jinja" as components -%} + {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} @@ -23,18 +24,12 @@ {%- set motor_z_offset = -0.01875 -%} {# [m] #} {%- set motor_height = 0.0235 -%} {# [m] #} {%- set pixhawk_offset = 0.008 -%} {# [m] #} - {%- set use_battery_mount = true -%} {# [bool] #} {%- set ultrasonic_x = 0.3173 -%} {# [bool] #} {%- set ultrasonic_y = 0.3390 -%} {# [bool] #} {%- set ultrasonic_yaw = 0.822463619 -%} {# [bool] #} - {%- set root = "base_link" -%} - - {%- set enable_motor_crash = true -%} - {% if disable_motor_crash %} - {%- set enable_motor_crash = false -%} - {% endif %} + {%- set root = 'base_link' -%} {# #} {# Motor constants {--> #} @@ -45,7 +40,7 @@ {%- set time_constant_down = 1.0 / 40.0 -%} {# [s] #} {%- set max_rot_velocity = 1 -%} {# [rad/s] #} {%- set rotor_drag_coefficient = 0.001 -%} {# orig 8.06428e-04 #} - {%- set rolling_moment_coefficient = "1.0e-6" -%} + {%- set rolling_moment_coefficient = '1.0e-6' -%} {# #} {# Inertia constants {--> #} @@ -56,20 +51,20 @@ {# Meshes {--> #} {# Drone parts {--> #} - {%- set frame_legless_sensorless_mesh_file = "model://mrs_robots_description/meshes/custom/naki/NAKI_legless_frame.dae" -%} - {%- set legs_mesh_file = "model://mrs_robots_description/meshes/custom/naki/NAKI_legs.dae"-%} - {%- set motor_mesh_file = "model://mrs_robots_description/meshes/custom/naki/dji_f450_rotor.dae" -%} - {%- set prop_mesh_file = "model://mrs_robots_description/meshes/custom/naki/NAKI_propeller.dae"-%} - {%- set pixhawk_mesh_file = "model://mrs_robots_description/meshes/sensors/pixhawk.dae" -%} - {%- set sony_camera_mesh_file = "model://mrs_robots_description/meshes/custom/naki/NAKI_sony_alpha_6500_w_gimbal_mount.dae" -%} - {%- set gimbal_mesh_file = "model://mrs_robots_description/meshes/custom/naki/NAKI_gimbal_movable_part.dae" -%} + {%- set frame_legless_sensorless_mesh_file = 'model://mrs_robots_description/meshes/custom/naki/NAKI_legless_frame.dae' -%} + {%- set legs_mesh_file = 'model://mrs_robots_description/meshes/custom/naki/NAKI_legs.dae'-%} + {%- set motor_mesh_file = 'model://mrs_robots_description/meshes/custom/naki/dji_f450_rotor.dae' -%} + {%- set prop_mesh_file = 'model://mrs_robots_description/meshes/custom/naki/NAKI_propeller.dae'-%} + {%- set pixhawk_mesh_file = 'model://mrs_robots_description/meshes/sensors/pixhawk.dae' -%} + {%- set sony_camera_mesh_file = 'model://mrs_robots_description/meshes/custom/naki/NAKI_sony_alpha_6500_w_gimbal_mount.dae' -%} + {%- set gimbal_mesh_file = 'model://mrs_robots_description/meshes/custom/naki/NAKI_gimbal_movable_part.dae' -%} {# #} {# Scales {--> #} - {%- set 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" -%} + {%- set 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' -%} {# #} {# #} @@ -92,14 +87,14 @@ - - - + {# ================================================================== #} + {# || bare body definitions || #} + {# ================================================================== #} - {{ components.multirotor_physics_macro( + {{ generic.multirotor_physics_macro( mass = mass, body_radius = body_radius, body_height = body_height, @@ -116,8 +111,8 @@ - {{ components.visual_mesh_textured_macro( - name = "frame_legless_sensorless", + {{ generic.visual_mesh_textured_macro( + name = 'frame_legless_sensorless', mesh_file = frame_legless_sensorless_mesh_file, mesh_scale = mesh_scale, x = 0, @@ -129,12 +124,12 @@ }} - - {{ components.visual_mesh_macro( - name = "pixhawk", + + {{ generic.visual_mesh_macro( + name = 'pixhawk', mesh_file = pixhawk_mesh_file, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = pixhawk_offset, @@ -144,9 +139,9 @@ }} - - {{ components.visual_mesh_textured_with_collision_macro( - name = "legs", + + {{ generic.visual_mesh_textured_with_collision_macro( + name = 'legs', mesh_file = legs_mesh_file, mesh_scale = mesh_scale, x = 0, @@ -158,12 +153,12 @@ }} - - {{ components.visual_mesh_macro( - name = "top_front_right_motor", + + {{ generic.visual_mesh_macro( + name = 'top_front_right_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = arm_length_x, y = -arm_length_y, z = motor_z_offset + motor_to_mount, @@ -172,11 +167,11 @@ yaw = 0) }} - {{ components.visual_mesh_macro( - name = "top_back_right_motor", + {{ generic.visual_mesh_macro( + name = 'top_back_right_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = -arm_length_x, y = arm_length_y, z = motor_z_offset + motor_to_mount, @@ -185,11 +180,11 @@ yaw = 0) }} - {{ components.visual_mesh_macro( - name = "top_front_left_motor", + {{ generic.visual_mesh_macro( + name = 'top_front_left_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = arm_length_x, y = arm_length_y, z = motor_z_offset + motor_to_mount, @@ -198,11 +193,11 @@ yaw = 0) }} - {{ components.visual_mesh_macro( - name = "top_back_left_motor", + {{ generic.visual_mesh_macro( + name = 'top_back_left_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = -arm_length_x, y = -arm_length_y, z = motor_z_offset + motor_to_mount, @@ -211,55 +206,55 @@ yaw = 0) }} - {{ components.visual_mesh_macro( - name = "bottom_front_right_motor", + {{ generic.visual_mesh_macro( + name = 'bottom_front_right_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = arm_length_x, y = -arm_length_y, z = motor_z_offset - motor_to_mount, roll = 0, - pitch = components.rad180, + pitch = math.radians(180), yaw = 0) }} - {{ components.visual_mesh_macro( - name = "bottom_back_right_motor", + {{ generic.visual_mesh_macro( + name = 'bottom_back_right_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = -arm_length_x, y = arm_length_y, z = motor_z_offset - motor_to_mount, roll = 0, - pitch = components.rad180, + pitch = math.radians(180), yaw = 0) }} - {{ components.visual_mesh_macro( - name = "bottom_front_left_motor", + {{ generic.visual_mesh_macro( + name = 'bottom_front_left_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = arm_length_x, y = arm_length_y, z = motor_z_offset - motor_to_mount, roll = 0, - pitch = components.rad180, + pitch = math.radians(180), yaw = 0) }} - {{ components.visual_mesh_macro( - name = "bottom_back_left_motor", + {{ generic.visual_mesh_macro( + name = 'bottom_back_left_motor', mesh_file = motor_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = -arm_length_x, y = -arm_length_y, z = motor_z_offset - motor_to_mount, roll = 0, - pitch = components.rad180, + pitch = math.radians(180), yaw = 0) }} @@ -268,233 +263,92 @@ - - {{ components.prop_macro( - direction = "ccw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - 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 = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_ccw, - x = arm_length_x, - y = -arm_length_y, - z = motor_z_offset + motor_to_mount + motor_height, - 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( - direction = "ccw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - 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 = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_ccw, - x = -arm_length_x, - y = arm_length_y, - z = motor_z_offset + motor_to_mount + motor_height, - 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( - direction = "cw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - 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 = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_cw, - x = arm_length_x, - y = arm_length_y, - z = motor_z_offset + motor_to_mount + motor_height, - 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( - direction = "cw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - 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 = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_cw, - x = -arm_length_x, - y = -arm_length_y, - z = motor_z_offset + motor_to_mount + motor_height, - 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( - direction = "cw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - time_constant_up = time_constant_up, - time_constant_down = time_constant_down, - max_rot_velocity = max_rot_velocity, - motor_number = 5, - rotor_drag_coefficient = rotor_drag_coefficient, - rolling_moment_coefficient = rolling_moment_coefficient, - enable_motor_crash = enable_motor_crash, - color = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_cw, - x = arm_length_x, - y = -arm_length_y, - z = motor_z_offset - motor_to_mount - motor_height, - 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( - direction = "cw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - time_constant_up = time_constant_up, - time_constant_down = time_constant_down, - max_rot_velocity = max_rot_velocity, - motor_number = 7, - rotor_drag_coefficient = rotor_drag_coefficient, - rolling_moment_coefficient = rolling_moment_coefficient, - enable_motor_crash = enable_motor_crash, - color = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_cw, - x = -arm_length_x, - y = arm_length_y, - z = motor_z_offset - motor_to_mount - motor_height, - 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( - direction = "ccw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_prop, - time_constant_up = time_constant_up, - time_constant_down = time_constant_down, - max_rot_velocity = max_rot_velocity, - motor_number = 4, - rotor_drag_coefficient = rotor_drag_coefficient, - rolling_moment_coefficient = rolling_moment_coefficient, - enable_motor_crash = enable_motor_crash, - color = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_ccw, - x = arm_length_x, - y = arm_length_y, - z = motor_z_offset - motor_to_mount - motor_height, - 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( - direction = "ccw", + {# Propellers {--> #} + {%- set prop_list = [ + { + 'motor_number': 0, + 'direction': 'ccw', + 'x': arm_length_x, + 'y': -arm_length_y, + 'z': motor_z_offset + motor_to_mount + motor_height, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'White' + }, + { + 'motor_number': 2, + 'direction': 'ccw', + 'x': -arm_length_x, + 'y': arm_length_y, + 'z': motor_z_offset + motor_to_mount + motor_height, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'White' + }, + { + 'motor_number': 1, + 'direction': 'cw', + 'x': arm_length_x, + 'y': arm_length_y, + 'z': motor_z_offset + motor_to_mount + motor_height, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'White' + }, + { + 'motor_number': 3, + 'direction': 'cw', + 'x': -arm_length_x, + 'y': -arm_length_y, + 'z': motor_z_offset + motor_to_mount + motor_height, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'White' + }, + { + 'motor_number': 5, + 'direction': 'cw', + 'x': arm_length_x, + 'y': -arm_length_y, + 'z': motor_z_offset - motor_to_mount - motor_height, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'White' + }, + { + 'motor_number': 7, + 'direction': 'cw', + 'x': -arm_length_x, + 'y': arm_length_y, + 'z': motor_z_offset - motor_to_mount - motor_height, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'White' + }, + { + 'motor_number': 4, + 'direction': 'ccw', + 'x': arm_length_x, + 'y': arm_length_y, + 'z': motor_z_offset - motor_to_mount - motor_height, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'White' + }, + { + 'motor_number': 6, + 'direction': 'ccw', + 'x': -arm_length_x, + 'y': -arm_length_y, + 'z': motor_z_offset - motor_to_mount - motor_height, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'White' + } + ] + -%} + {{ components.propellers_macro( + prop_list = prop_list, rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, motor_constant = motor_constant, moment_constant = moment_constant, @@ -504,64 +358,47 @@ time_constant_up = time_constant_up, time_constant_down = time_constant_down, max_rot_velocity = max_rot_velocity, - motor_number = 6, rotor_drag_coefficient = rotor_drag_coefficient, rolling_moment_coefficient = rolling_moment_coefficient, - enable_motor_crash = enable_motor_crash, - color = "White", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_ccw, - x = -arm_length_x, - y = -arm_length_y, - z = motor_z_offset - motor_to_mount - motor_height, - roll = 0, - pitch = 0, - yaw = 0, - ixx = prop_ixx, - ixy = prop_ixy, - ixz = prop_ixz, - iyy = prop_iyy, - iyz = prop_iyz, - izz = prop_izz) + meshes_z_offset = 0, + prop_ixx = prop_ixx, + prop_ixy = prop_ixy, + prop_ixz = prop_ixz, + prop_iyy = prop_iyy, + prop_iyz = prop_iyz, + prop_izz = prop_izz, + spawner_args = spawner_args) }} + {# #} - + {# ================================================================== #} + {# || compulsory sensor definitions || #} + {# ================================================================== #} - - - + + {%- set imu_topic = '/imu' -%} + {%- set mag_topic = '/mag' -%} + {%- set baro_topic = '/baro' -%} + {%- set lidar_topic = '/lidar' -%} - - {{ 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) + + {{ generic.gazebo_groundtruth_macro( + home_latitude = 0, + home_longitude = 0, + home_altitude = 0) }} - {{ components.gps_macro( - gps_name = "gps0", + {{ generic.gazebo_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_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, @@ -574,36 +411,39 @@ - {{ components.magnetometer_plugin_macro( + {{ generic.gazebo_magnetometer_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) + mag_topic = mag_topic) }} - {{ components.barometer_plugin_macro( - baro_topic = "/baro", + {{ generic.gazebo_barometer_macro( + baro_topic = baro_topic, pub_rate = 50, baro_drift_pa_per_sec = 0) }} + + {{ generic.gazebo_mavlink_interface_macro( + imu_topic = imu_topic, + mag_topic = mag_topic, + baro_topic = baro_topic, + lidar_topic = lidar_topic, + mavlink_config = spawner_args['mavlink_config']) + }} + + - {{ components.imu_plugin_macro( - imu_name = "imu", + + {{ generic.gazebo_imu_macro( + imu_name = 'imu', parent_link = root, - imu_topic = "/imu", + imu_topic = '/imu', gyroscope_noise_density = 0.00018665, gyroscope_random_walk = 0.000038785, gyroscope_bias_correlation_time = 1000.0, @@ -621,237 +461,182 @@ }} - - - + + + {# ================================================================== #} + {# || optional sensor definitions || #} + {# ================================================================== #} {# Ground truth {--> #} - {% if enable_ground_truth %} - - {{ components.odometry_plugin_macro( - odometry_sensor_name = "ground_truth", + {{ components.ground_truth_macro( 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) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# ======================= rangefinder sensors ====================== #} - {# 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, + {# Garmin down {--> #} + {{ components.garmin_down_macro( parent_link = root, - orientation = "", x = 0.0775, y = 0, z = -0.039, roll = 0, - pitch = components.rad(90), - yaw = 0) + pitch = math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} - {# Garmin Up {--> #} - {% if enable_rangefinder_up %} - - {{ components.external_garmin_macro( - namespace = namespace, + {# Garmin up {--> #} + {{ components.garmin_up_external_macro( parent_link = root, - orientation = "_up", x = 0.0775, y = 0, z = 0, roll = 0, - pitch = components.rad(-90), - yaw = 0) + pitch = math.radians(-90), + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Ultrasonic sensors {--> #} - {% if enable_omni_ultrasounds %} - - - {{ components.ultrasonic_sensor_macro( - namespace = namespace, + {%- set ultrasonic_sensors_list = [ + { + 'suffix': '_down', + 'x': 0.02, + 'y': 0, + 'z': -0.1535, + 'roll': 0, + 'pitch': math.radians(90), + 'yaw': 0 + }, + { + 'suffix': '_up', + 'x': -0.1686, + 'y': 0, + 'z': 0.001, + 'roll': 0, + 'pitch': math.radians(-90), + 'yaw': 0 + }, + { + 'suffix': '_fl', + 'x': ultrasonic_x, + 'y': ultrasonic_y, + 'z': -0.019, + 'roll': 0, + 'pitch': 0, + 'yaw': ultrasonic_yaw + }, + { + 'suffix': '_fr', + 'x': ultrasonic_x, + 'y': -ultrasonic_y, + 'z': -0.019, + 'roll': 0, + 'pitch': 0, + 'yaw': -ultrasonic_yaw + }, + { + 'suffix': '_bl', + 'x': -ultrasonic_x, + 'y': ultrasonic_y, + 'z': -0.019, + 'roll': 0, + 'pitch': 0, + 'yaw': ultrasonic_yaw + math.radians(90) + }, + { + 'suffix': '_br', + 'x': -ultrasonic_x, + 'y': -ultrasonic_y, + 'z': -0.019, + 'roll': 0, + 'pitch': 0, + 'yaw': -(ultrasonic_yaw + math.radians(90)) + }, + ] + -%} + {{ components.omni_ultrasounds_macro( + sensors_list = ultrasonic_sensors_list, parent_link = root, - suffix = "_down", - x = 0.02, - y = 0, - z = -0.1535, - roll = 0, - pitch = components.rad(90), - yaw = 0) + spawner_args = spawner_args) }} - - - - {{ components.ultrasonic_sensor_macro( - namespace = namespace, - parent_link = root, - suffix = "_up", - x = -0.1686, - y = 0, - z = 0.001, - roll = 0, - pitch = components.rad(-90), - yaw = 0) - }} - - - - {{ components.ultrasonic_sensor_macro( - namespace = namespace, - parent_link = root, - suffix = "_fl", - x = ultrasonic_x, - y = ultrasonic_y, - z = -0.019, - roll = 0, - pitch = 0, - yaw = ultrasonic_yaw) - }} - - - - {{ components.ultrasonic_sensor_macro( - namespace = namespace, - parent_link = root, - suffix = "_fr", - x = ultrasonic_x, - y = -ultrasonic_y, - z = -0.019, - roll = 0, - pitch = 0, - yaw = -ultrasonic_yaw) - }} - - - - {{ components.ultrasonic_sensor_macro( - namespace = namespace, - parent_link = root, - suffix = "_bl", - x = -ultrasonic_x, - y = ultrasonic_y, - z = -0.019, - roll = 0, - pitch = 0, - yaw = ultrasonic_yaw + components.rad90) - }} - - - - {{ components.ultrasonic_sensor_macro( - namespace = namespace, - parent_link = root, - suffix = "_br", - x = -ultrasonic_x, - y = -ultrasonic_y, - z = -0.019, - roll = 0, - pitch = 0, - yaw = -(ultrasonic_yaw + components.rad90)) - }} - - {% endif %} {# #} - + {# ========================== LIDAR sensors ========================= #} {# Rplidar {--> #} - {% if enable_rplidar %} - {{ 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, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Ouster {--> #} - {% if enable_ouster %} - - {{ components.ouster_macro( - namespace = namespace, + {%- set ouster = components.ouster_macro( 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, + sensor_name = 'os', x = 0.0, y = 0.0, z = 0.0611, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ ouster }} {# #} - + {# ========================== camera sensors ======================== #} - + {# Realsense placements {--> #} - {% if enable_realsense_front %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="", + {# Realsense front {--> #} + {{ components.realsense_front_macro( + camera_name = 'rgbd', + camera_suffix='', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.2255, y = 0, z = -0.0155, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - + {# #} {# 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", + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/servo_camera_optical', + sensor_base_frame_name = spawner_args['name'] + '/servo_camera', offset_pitch_link_x = 0.12928, - offset_pitch_link_y = "7.605e-5", + offset_pitch_link_y = '7.605e-5', offset_pitch_link_z = -0.006051, offset_pitch_link_roll = 0.0, offset_pitch_link_yaw = 0.0, @@ -877,37 +662,28 @@ compensate_tilt_pitch = true, pitch_link_mesh_file = sony_camera_mesh_file, roll_link_mesh_file = gimbal_mesh_file, - mesh_scale = "") + mesh_scale = '', + spawner_args = spawner_args) }} - - {% endif %} {# #} - - + + {# ========================== other sensors ========================= #} {# Light {--> #} - {% if enable_light %} - {{ components.light_macro( parent_link = root, - update_rate = 30, max_pitch_rate = 0.1, - initial_on = True, - compensate_tilt = True, x = 0.2, y = 0.0, z = -0.1, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Safety LED {--> #} - {% if enable_safety_led %} - {{ components.safety_led_macro( parent_link = root, failure_duration_threshold = 0.2, @@ -916,10 +692,9 @@ z = -0.0195, roll = 0, pitch = 0, - yaw = components.rad180) + yaw = math.radians(180), + spawner_args = spawner_args) }} - - {% endif %} {# #} 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 f99f148..0bdb1fa 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 @@ -1,7 +1,8 @@ - {%- import "mrs_robots_description/sdf/component_snippets.sdf.jinja" as components -%} + {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} @@ -12,20 +13,14 @@ {%- set body_radius = 0.325 -%} {# [m] #} {%- set body_height = 0.15 -%} {# [m] #} {%- set mass_prop = 0.005 -%} {# [kg] #} - {%- set radius_rotor = 0.195 -%} {# [m] #} + {%- set radius_prop = 0.195 -%} {# [m] #} {%- set rotor_offset = 0.021 -%} {# [m] #} {%- set propeller_offset = 0.01 -%} {# [m] #} {%- set arm_length = 0.3 -%} {# [m] #} {%- set leg_height = 0.24 -%} {# [m] #} {%- set leg_offset_r = 0.05 -%} {# [m] #} {%- set leg_radius = 0.012 -%} {# [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 %} + {%- set root = 'base_link' -%} {# #} {# Motor constants {--> #} @@ -36,7 +31,7 @@ {%- 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" -%} + {%- set rolling_moment_coefficient = '1.0e-6' -%} {# #} {# Inertia constants {--> #} @@ -47,31 +42,31 @@ {# Meshes {--> #} {# Drone parts {--> #} - {%- set top_board_mesh_file = "model://mrs_robots_description/meshes/tarot/t650/tarot_t650_top_board.dae" -%} - {%- set bottom_board_mesh_file = "model://mrs_robots_description/meshes/tarot/t650/tarot_t650_bottom_board.dae" -%} - {%- set arm_mesh_file_front = "model://mrs_robots_description/meshes/tarot/t650/tarot_t650_arm_black.dae" -%} - {%- set arm_mesh_file_back = "model://mrs_robots_description/meshes/tarot/t650/tarot_t650_arm_red.dae" -%} - {%- set leg_mesh_file = "model://mrs_robots_description/meshes/tarot/t650/tarot_t650_leg.dae" -%} - {%- set rotor_mesh_file_front = "model://mrs_robots_description/meshes/tarot/t650/tarot_t650_motor_black.dae" -%} - {%- set rotor_mesh_file_back = "model://mrs_robots_description/meshes/tarot/t650/tarot_t650_motor_orange.dae" -%} - {%- set prop_mesh_file = "model://mrs_robots_description/meshes/tarot/t650/tarot_t650_prop_low_poly.dae" -%} - {%- set pixhawk_mesh_file = "model://mrs_robots_description/meshes/sensors/pixhawk.dae" -%} - {%- set nuc_mesh_file = "model://mrs_robots_description/meshes/tarot/t650/tarot_t650_nuc.dae" -%} + {%- set top_board_mesh_file = 'model://mrs_robots_description/meshes/tarot/t650/tarot_t650_top_board.dae' -%} + {%- set bottom_board_mesh_file = 'model://mrs_robots_description/meshes/tarot/t650/tarot_t650_bottom_board.dae' -%} + {%- set arm_mesh_file_front = 'model://mrs_robots_description/meshes/tarot/t650/tarot_t650_arm_black.dae' -%} + {%- set arm_mesh_file_back = 'model://mrs_robots_description/meshes/tarot/t650/tarot_t650_arm_red.dae' -%} + {%- set leg_mesh_file = 'model://mrs_robots_description/meshes/tarot/t650/tarot_t650_leg.dae' -%} + {%- set rotor_mesh_file_front = 'model://mrs_robots_description/meshes/tarot/t650/tarot_t650_motor_black.dae' -%} + {%- set rotor_mesh_file_back = 'model://mrs_robots_description/meshes/tarot/t650/tarot_t650_motor_orange.dae' -%} + {%- set prop_mesh_file = 'model://mrs_robots_description/meshes/tarot/t650/tarot_t650_prop_low_poly.dae' -%} + {%- set pixhawk_mesh_file = 'model://mrs_robots_description/meshes/sensors/pixhawk.dae' -%} + {%- set nuc_mesh_file = 'model://mrs_robots_description/meshes/tarot/t650/tarot_t650_nuc.dae' -%} {# #} - {# Holders {--> #} - {%- set battery_mount_mesh = "model://mrs_robots_description/meshes/tarot/t650/tarot_t650_battery_mount.dae" -%} - {%- set bluefox_garmin_mount_mesh = "model://mrs_robots_description/meshes/tarot/t650/tarot_t650_bluefox_garmin_mount.dae" -%} - {%- set ouster_mount_mesh = "model://mrs_robots_description/meshes/tarot/t650/tarot_t650_ouster_mount.dae" -%} - {%- set rplidar_mount_mesh = "model://mrs_robots_description/meshes/tarot/t650/tarot_t650_rplidar_mount.dae" -%} - {%- set realsense_front_mount_mesh = "model://mrs_robots_description/meshes/tarot/t650/tarot_t650_realsense_front_mount.dae" -%} + {# Mounts {--> #} + {%- set battery_mount_mesh = 'model://mrs_robots_description/meshes/tarot/t650/tarot_t650_battery_mount.dae' -%} + {%- set bluefox_garmin_mount_mesh = 'model://mrs_robots_description/meshes/tarot/t650/tarot_t650_bluefox_garmin_mount.dae' -%} + {%- set ouster_mount_mesh = 'model://mrs_robots_description/meshes/tarot/t650/tarot_t650_ouster_mount.dae' -%} + {%- set rplidar_mount_mesh = 'model://mrs_robots_description/meshes/tarot/t650/tarot_t650_rplidar_mount.dae' -%} + {%- set realsense_front_mount_mesh = 'model://mrs_robots_description/meshes/tarot/t650/tarot_t650_realsense_front_mount.dae' -%} {# #} {# Scales {--> #} - {%- set 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" -%} + {%- set 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' -%} {# #} {# #} @@ -94,14 +89,14 @@ - - - + {# ================================================================== #} + {# || bare body definitions || #} + {# ================================================================== #} - {{ components.multirotor_physics_macro( + {{ generic.multirotor_physics_macro( mass = mass, body_radius = body_radius, body_height = body_height, @@ -118,11 +113,11 @@ - {{ components.visual_mesh_macro( - name = "bottom_board", + {{ generic.visual_mesh_macro( + name = 'bottom_board', mesh_file = bottom_board_mesh_file, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = -1.7*body_height, @@ -131,11 +126,11 @@ yaw = 0) }} - {{ components.visual_mesh_macro( - name = "top_board", + {{ generic.visual_mesh_macro( + name = 'top_board', mesh_file = top_board_mesh_file, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = -1.7*body_height, @@ -146,65 +141,65 @@ - {{ components.visual_mesh_macro( - name = "front_right_arm", + {{ generic.visual_mesh_macro( + name = 'front_right_arm', mesh_file = arm_mesh_file_front, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = -1.7*body_height, roll = 0, pitch = 0, - yaw = components.rad(-45)) + yaw = math.radians(-45)) }} - {{ components.visual_mesh_macro( - name = "back_right_arm", + {{ generic.visual_mesh_macro( + name = 'back_right_arm', mesh_file = arm_mesh_file_back, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = -1.7*body_height, roll = 0, pitch = 0, - yaw = components.rad(-135)) + yaw = math.radians(-135)) }} - {{ components.visual_mesh_macro( - name = "front_left_arm", + {{ generic.visual_mesh_macro( + name = 'front_left_arm', mesh_file = arm_mesh_file_front, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = -1.7*body_height, roll = 0, pitch = 0, - yaw = components.rad(45)) + yaw = math.radians(45)) }} - {{ components.visual_mesh_macro( - name = "back_left_arm", + {{ generic.visual_mesh_macro( + name = 'back_left_arm', mesh_file = arm_mesh_file_back, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = -1.7*body_height, roll = 0, pitch = 0, - yaw = components.rad(135)) + yaw = math.radians(135)) }} - {{ components.visual_mesh_macro( - name = "NUC", + {{ generic.visual_mesh_macro( + name = 'NUC', mesh_file = nuc_mesh_file, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0.002, y = 0, z = -1.7*body_height, @@ -215,11 +210,11 @@ - {{ components.visual_mesh_macro( - name = "pixhawk", + {{ generic.visual_mesh_macro( + name = 'pixhawk', mesh_file = pixhawk_mesh_file, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = 0.021, @@ -230,114 +225,114 @@ - {{ components.leg_macro( - name = "front_right_leg", + {{ generic.leg_macro( + name = 'front_right_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', parent = root, - x = (arm_length - leg_offset_r) * components.sin45, - y = -(arm_length - leg_offset_r) * components.sin45, + x = (arm_length - leg_offset_r) * math.sin(math.radians(45)), + y = -(arm_length - leg_offset_r) * math.sin(math.radians(45)), z = -0.9*body_height, roll = 0, pitch = 0, - yaw = components.rad(-45), + yaw = math.radians(-45), collision_height = leg_height, collision_radius = leg_radius) }} - {{ components.leg_macro( - name = "front_left_leg", + {{ generic.leg_macro( + name = 'front_left_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', parent = root, - x = (arm_length - leg_offset_r) * components.sin45, - y = (arm_length - leg_offset_r) * components.sin45, + x = (arm_length - leg_offset_r) * math.sin(math.radians(45)), + y = (arm_length - leg_offset_r) * math.sin(math.radians(45)), z = -0.9*body_height, roll = 0, pitch = 0, - yaw = components.rad(45), + yaw = math.radians(45), collision_height = leg_height, collision_radius = leg_radius) }} - {{ components.leg_macro( - name = "rear_left_leg", + {{ generic.leg_macro( + name = 'rear_left_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale, - color = "Orange", + color = 'Orange', parent = root, - x = -(arm_length - leg_offset_r) * components.sin45, - y = (arm_length - leg_offset_r) * components.sin45, + x = -(arm_length - leg_offset_r) * math.sin(math.radians(45)), + y = (arm_length - leg_offset_r) * math.sin(math.radians(45)), z = -0.9*body_height, roll = 0, pitch = 0, - yaw = components.rad(135), + yaw = math.radians(135), collision_height = leg_height, collision_radius = leg_radius) }} - {{ components.leg_macro( - name = "rear_right_leg", + {{ generic.leg_macro( + name = 'rear_right_leg', mesh_file = leg_mesh_file, mesh_scale = mesh_scale, - color = "Orange", + color = 'Orange', parent = root, - x = -(arm_length - leg_offset_r) * components.sin45, - y = -(arm_length - leg_offset_r) * components.sin45, + x = -(arm_length - leg_offset_r) * math.sin(math.radians(45)), + y = -(arm_length - leg_offset_r) * math.sin(math.radians(45)), z = -0.9*body_height, roll = 0, pitch = 0, - yaw = components.rad(-135), + yaw = math.radians(-135), collision_height = leg_height, collision_radius = leg_radius) }} - {{ components.visual_mesh_textured_macro( - name = "front_right_motor", + {{ generic.visual_mesh_textured_macro( + name = 'front_right_motor', mesh_file = rotor_mesh_file_front, mesh_scale = mesh_scale, - x = arm_length * components.sin45, - y = -arm_length * components.sin45, + x = arm_length * math.sin(math.radians(45)), + y = -arm_length * math.sin(math.radians(45)), z = -rotor_offset, roll = 0, pitch = 0, yaw = 0) }} - {{ components.visual_mesh_textured_macro( - name = "back_right_rotor", + {{ generic.visual_mesh_textured_macro( + name = 'back_right_rotor', mesh_file = rotor_mesh_file_back, mesh_scale = mesh_scale, - x = -arm_length * components.sin45, - y = -arm_length * components.sin45, + x = -arm_length * math.sin(math.radians(45)), + y = -arm_length * math.sin(math.radians(45)), z = -rotor_offset, roll = 0, pitch = 0, yaw = 0) }} - {{ components.visual_mesh_textured_macro( - name = "front_left_rotor", + {{ generic.visual_mesh_textured_macro( + name = 'front_left_rotor', mesh_file = rotor_mesh_file_front, mesh_scale = mesh_scale, - x = arm_length * components.sin45, - y = arm_length * components.sin45, + x = arm_length * math.sin(math.radians(45)), + y = arm_length * math.sin(math.radians(45)), z = -rotor_offset, roll = 0, pitch = 0, yaw = 0) }} - {{ components.visual_mesh_textured_macro( - name = "back_left_rotor", + {{ generic.visual_mesh_textured_macro( + name = 'back_left_rotor', mesh_file = rotor_mesh_file_back, mesh_scale = mesh_scale, - x = -arm_length * components.sin45, - y = arm_length * components.sin45, + x = -arm_length * math.sin(math.radians(45)), + y = arm_length * math.sin(math.radians(45)), z = -rotor_offset, roll = 0, pitch = 0, @@ -345,13 +340,12 @@ }} - {% if use_battery_mount %} - {{ components.visual_mesh_macro( - name = "battery_mount", + {{ generic.visual_mesh_macro( + name = 'battery_mount', mesh_file = battery_mount_mesh, mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = -0.24*body_height, @@ -360,12 +354,12 @@ yaw = 0) }} - {{ components.visual_colored_box_macro( - name = "battery", + {{ generic.visual_colored_box_macro( + name = 'battery', size_x = 0.16, size_y = 0.065, size_z = 0.044, - color = "Grey", + color = 'Grey', x = 0, y = 0, z = -0.45*body_height, @@ -374,244 +368,183 @@ yaw = 0) }} - {% endif %} - - {% if enable_bluefox_camera or enable_bluefox_camera_reverse or enable_rangefinder or wall_challenge %} - - {{ components.visual_mesh_macro( - name = "bluefox_garmin_mount", - mesh_file = bluefox_garmin_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "White", - x = 0, - y = -0.145, - z = -0.025, - roll = 0, - pitch = 0, - yaw = -components.rad90) - }} - - {% endif %} - {% if enable_rplidar or fire_challenge_blanket %} - - {{ components.visual_mesh_macro( - name = "rplidar_mount", - mesh_file = rplidar_mount_mesh, + {# component mounts and holders (these must be passed as args into components, or must be called) {--> #} + + {# bluefox + garmin mount {--> #} + {%- set bluefox_garmin_mount -%} + {{ generic.visual_link_macro( + name = 'bluefox_garmin_mount', + mesh_file = bluefox_garmin_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'White', + x = 0, + y = -0.145, + z = -0.025, + roll = 0, + pitch = 0, + yaw = -math.radians(90), + parent_link = root) + }} + {%- endset -%} + {# #} + + {# rplidar mount {--> #} + {%- set rplidar_mount -%} + {{ generic.visual_link_macro( + name = 'rplidar_mount', + mesh_file = rplidar_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'White', + x = 0, + y = 0.0, + z = 0.075, + roll = 0, + pitch = 0, + yaw = 0, + parent_link = root) + }} + {%- endset -%} + {# #} + + {# 3D lidar mount {--> #} + {%- set lidar_mount_3d -%} + {{ generic.visual_link_macro( + name = 'ouster_mount', + mesh_file = ouster_mount_mesh, mesh_scale = mesh_scale_milimeters, - color = "White", - x = 0, + color = 'White', + x = 0.0, y = 0.0, - z = 0.075, + z = 0.068, roll = 0, pitch = 0, - yaw = 0) + yaw = math.radians(90), + parent_link = root) }} - - {% endif %} + {%- endset -%} + {# #} - {% if enable_realsense_front %} - - {{ components.visual_mesh_macro( - name = "realsense_front_mount", + {# Realsense front mount {--> #} + {%- set realsense_front_mount -%} + {{ generic.visual_link_macro( + name = 'realsense_front_mount', mesh_file = realsense_front_mount_mesh, mesh_scale = mesh_scale_milimeters, - color = "White", + color = 'White', x = 0.125, y = 0.0, z = -0.046, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + parent_link = root) }} - - {% endif %} + {%- endset -%} + {# #} - {% if enable_ouster or enable_velodyne %} - - {{ components.visual_mesh_macro( - name = "ouster_mount", - mesh_file = ouster_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "White", - x = 0.0, - y = 0.0, - z = 0.068, - roll = 0, - pitch = 0, - yaw = components.rad90) - }} - - {% endif %} + {# #} + - - {{ components.prop_macro( - direction = "ccw", + {# Propellers {--> #} + {%- set prop_list = [ + { + 'motor_number': 0, + 'direction': 'ccw', + 'x': arm_length * math.sin(math.radians(45)), + 'y': -arm_length * math.sin(math.radians(45)), + 'z': propeller_offset, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'Black' + }, + { + 'motor_number': 1, + 'direction': 'ccw', + 'x': -arm_length * math.sin(math.radians(45)), + 'y': arm_length * math.sin(math.radians(45)), + 'z': propeller_offset, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'Black' + }, + { + 'motor_number': 2, + 'direction': 'cw', + 'x': arm_length * math.sin(math.radians(45)), + 'y': arm_length * math.sin(math.radians(45)), + 'z': propeller_offset, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'Black' + }, + { + 'motor_number': 3, + 'direction': 'cw', + 'x': -arm_length * math.sin(math.radians(45)), + 'y': -arm_length * math.sin(math.radians(45)), + 'z': propeller_offset, + 'mesh_files': [prop_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'Black' + } + ] + -%} + {{ components.propellers_macro( + prop_list = prop_list, rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, motor_constant = motor_constant, moment_constant = moment_constant, parent = root, mass = mass_prop, - radius = radius_rotor, + radius = radius_prop, 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 = "Black", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_ccw, - x = arm_length * components.sin45, - y = -arm_length * components.sin45, - z = propeller_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) + meshes_z_offset = 0, + prop_ixx = prop_ixx, + prop_ixy = prop_ixy, + prop_ixz = prop_ixz, + prop_iyy = prop_iyy, + prop_iyz = prop_iyz, + prop_izz = prop_izz, + spawner_args = spawner_args) }} + {# #} - {{ components.prop_macro( - direction = "ccw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_rotor, - 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 = "Black", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_ccw, - x = -arm_length * components.sin45, - y = arm_length * components.sin45, - z = propeller_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) - }} + {# ================================================================== #} + {# || compulsory sensor definitions || #} + {# ================================================================== #} - {{ components.prop_macro( - direction = "cw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_rotor, - 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 = "Black", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_cw, - x = arm_length * components.sin45, - y = arm_length * components.sin45, - z = propeller_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) - }} + + {%- set imu_topic = '/imu' -%} + {%- set mag_topic = '/mag' -%} + {%- set baro_topic = '/baro' -%} + {%- set lidar_topic = '/lidar' -%} - {{ components.prop_macro( - direction = "cw", - rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, - motor_constant = motor_constant, - moment_constant = moment_constant, - parent = root, - mass = mass_prop, - radius = radius_rotor, - 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 = "Black", - mesh_file = prop_mesh_file, - mesh_scale = mesh_scale_prop_cw, - x = -arm_length * components.sin45, - y = -arm_length * components.sin45, - z = propeller_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) + + {{ generic.gazebo_groundtruth_macro( + home_latitude = 0, + home_longitude = 0, + home_altitude = 0) }} - {{ components.gps_macro( - gps_name = "gps0", + {{ generic.gazebo_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_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, @@ -624,36 +557,39 @@ - {{ components.magnetometer_plugin_macro( + {{ generic.gazebo_magnetometer_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) + mag_topic = mag_topic) }} - {{ components.barometer_plugin_macro( - baro_topic = "/baro", + {{ generic.gazebo_barometer_macro( + baro_topic = baro_topic, pub_rate = 50, baro_drift_pa_per_sec = 0) }} + + {{ generic.gazebo_mavlink_interface_macro( + imu_topic = imu_topic, + mag_topic = mag_topic, + baro_topic = baro_topic, + lidar_topic = lidar_topic, + mavlink_config = spawner_args['mavlink_config']) + }} + + - {{ components.imu_plugin_macro( - imu_name = "imu", + + {{ generic.gazebo_imu_macro( + imu_name = 'imu', parent_link = root, - imu_topic = "/imu", + imu_topic = '/imu', gyroscope_noise_density = 0.00018665, gyroscope_random_walk = 0.000038785, gyroscope_bias_correlation_time = 1000.0, @@ -671,371 +607,280 @@ }} - - - + + + {# ================================================================== #} + {# || optional sensor definitions || #} + {# ================================================================== #} {# Ground truth {--> #} - {% if enable_ground_truth %} - - {{ components.odometry_plugin_macro( - odometry_sensor_name = "ground_truth", + {{ components.ground_truth_macro( 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) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# ======================= rangefinder sensors ====================== #} - {# 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, + {# Garmin down {--> #} + {%- set garmin_down = components.garmin_down_macro( parent_link = root, - orientation = "", x = 0.0, y = -0.221, z = -0.035, roll = 0, - pitch = components.rad(90), - yaw = components.rad(90)) - }} - - {% endif %} + pitch = math.radians(90), + yaw = math.radians(90), + mount = garmin_down_mount, + spawner_args = spawner_args) + -%} + {{ garmin_down }} {# #} - {# Garmin Up {--> #} - {% if enable_rangefinder_up %} - - {{ components.external_garmin_macro( - namespace = namespace, + {# Garmin up {--> #} + {{ components.garmin_up_external_macro( parent_link = root, - orientation = "_up", x = 0.0, y = 0.08, z = -0.0075, roll = 0, - pitch = components.rad(-90), - yaw = components.rad(-90)) + pitch = math.radians(-90), + yaw = math.radians(-90), + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# ========================== LIDAR sensors ========================= #} {# Scanse Sweep {--> #} - {% if enable_scanse_sweep %} - {{ components.scanse_sweep_macro( - namespace = namespace, parent_link = root, x = 0.0, y = 0.0, z = 0.12, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Rplidar {--> #} - {% if enable_rplidar %} - {{ 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, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + mount = rplidar_mount, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Velodyne {--> #} - {% if enable_velodyne %} - - - {{ components.velodyne_macro( - namespace = namespace, + {%- set velodyne = components.velodyne_macro( parent_link = root, - sensor_name = "velodyne", - 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, + sensor_name = 'velodyne', x = 0.0, y = 0.0, z = 0.073, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ velodyne }} {# #} {# Ouster {--> #} - {% if enable_ouster %} - - {{ components.ouster_macro( - namespace = namespace, + {%- set ouster = components.ouster_macro( 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, + sensor_name = 'os', x = 0.0, y = 0.0, z = 0.073, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ ouster }} {# #} - + {# ========================= camera sensors ========================= #} - + {# Bluefox camera placements {--> #} - {% if enable_bluefox_camera_reverse %} - - {{ components.bluefox_camera_macro( - namespace = namespace, - camera_name = "bluefox_optflow", + {# bluefox (classic) {--> #} + {%- set bluefox_camera = components.bluefox_camera_macro( + camera_name = 'bluefox_optflow', parent_link = root, - frame_rate = 100.0, - hfov = 2.1, - noise = 0.007, x = 0.0, y = -0.15, z = -0.057, - roll = 0, - pitch = components.rad90, - yaw = components.rad180) - }} - - {% endif %} + mount = none, + spawner_args = spawner_args) + -%} + {{ bluefox_camera }} + {# #} - {% if enable_bluefox_camera %} - - {{ components.bluefox_camera_macro( - namespace = namespace, - camera_name = "bluefox_optflow", + {# bluefox reverse {--> #} + {%- set bluefox_camera_reverse = components.bluefox_camera_reverse_macro( + camera_name = 'bluefox_optflow', parent_link = root, - frame_rate = 100.0, - hfov = 2.1, - noise = 0.007, x = 0.0, y = -0.15, z = -0.057, - roll = 0, - pitch = components.rad90, - yaw = 0) - }} - - {% endif %} + mount = none, + spawner_args = spawner_args) + -%} + {{ bluefox_camera_reverse }} + {# #} - + {# #} - + {# Realsense placements {--> #} - {% if enable_realsense_front %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="", + {# Realsense front {--> #} + {{ components.realsense_front_macro( + camera_name = 'rgbd', + camera_suffix='', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.17, y = -0.011, z = -0.063, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + mount = realsense_front_mount, + spawner_args = spawner_args) }} - - {% endif %} - - {% if enable_realsense_up %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="_up", + {# #} + + {# Realsense up {--> #} + {{ components.realsense_up_macro( + camera_name = 'rgbd', + camera_suffix='_up', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.08, y = 0, z = 0.03, roll = 0, - pitch = - components.rad90, - yaw = 0) + pitch = -math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} - - {% if enable_realsense_down %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="_down", + {# #} + + {# Realsense down {--> #} + {{ components.realsense_down_macro( + camera_name = 'rgbd', + camera_suffix='_down', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.1, y = 0, z = -0.05, roll = 0, - pitch = components.rad90, - yaw = 0) + pitch = math.radians(90), + yaw = 0, + mount = realsense_down_mount, + spawner_args = spawner_args) }} - - {% endif %} + {# #} - + {# #} - + {# Mobius placements {--> #} - {% if enable_mobius_camera_front %} - - {{ components.mobius_camera_macro( - namespace = namespace, - camera_name = "mobius_front", + {# mobius front {--> #} + {{ components.mobius_front_macro( + camera_name = 'mobius_front', parent_link = root, - frame_rate = 30.0, x = 0.2, y = 0.0, z = -0.04, roll = 0, pitch = 0, - yaw = 0) - }} - - {% 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) + yaw = 0, + mount = none, + spawner_args = spawner_args) }} + {# #} - {{ 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", + {# Three thermal cameras {--> #} + {%- set three_thermal_cameras = [ + { + 'name': 'top', + 'x': 0.15, + 'y': 0.06, + 'z': -0.025, + 'roll': 0.0, + 'pitch': math.radians(-30), + 'yaw': 0.0 + }, + { + 'name': 'middle', + 'x': 0.16, + 'y': 0.06, + 'z': -0.055, + 'roll': 0.0, + 'pitch': 0.0, + 'yaw': 0.0 + }, + { + 'name': 'bottom', + 'x': 0.15, + 'y': 0.06, + 'z': -0.085, + 'roll': 0.0, + 'pitch': math.radians(30), + 'yaw': 0.0 + } + ] + -%} + {{ components.thermal_cameras_macro( + cameras_list = three_thermal_cameras, 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) + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} - {% if enable_downward_uv_camera %} - - {{ components.uvcam_macro( + {# Downward UV camera {--> #} + {{ components.downward_uv_camera_macro( parent_link = root, - camera_publish_topic = "/" + namespace + "/uvdar_bluefox_bottom/image_raw", - calibration_file = uvcam_calib_file, - occlusions = uvcam_occlusions, - frame_rate = 60, - device_id = namespace + "_3", x = 0.0, y = 0.0, z = -0.1, roll = 0, - pitch = components.rad(90), - yaw = 0) + pitch = math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# 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", + parent_frame_name = spawner_args['name'] + '/fcu', + camera_frame_name = spawner_args['name'] + '/servo_camera_optical', + sensor_base_frame_name = spawner_args['name'] + '/servo_camera', offset_pitch_link_x = 0.0, offset_pitch_link_y = 0.0, offset_pitch_link_z = 0.0, @@ -1061,39 +906,33 @@ img_height = 1080, compensate_tilt_roll = true, compensate_tilt_pitch = true, - pitch_link_mesh_file = "", - roll_link_mesh_file = "", - mesh_scale = "") + pitch_link_mesh_file = '', + roll_link_mesh_file = '', + mesh_scale = '', + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# ========================== other sensors ========================= #} {# Teraranger Tower Evo {--> #} - {% if enable_teraranger_evo_tower %} - {{ components.teraranger_evo_tower_macro( parent_link = root, visualize = False, - frame_name = namespace + "/teraranger_tower", - parent_frame_name = namespace + "/fcu", + frame_name = spawner_args['name'] + '/teraranger_tower', + parent_frame_name = spawner_args['name'] + '/fcu', gaussian_noise = 0.0, x = 0.0, y = 0.0, z = 0.14, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Magnetic gripper {--> #} - {% if enable_magnetic_gripper %} - {{ components.magnet_gripper_visualization_macro( parent_link = root, x = 0.0, @@ -1101,96 +940,67 @@ z = - (0.20/2 + 0.01), roll = 0, pitch = 0, - yaw = components.rad90) + yaw = math.radians(90), + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Timepix {--> #} - {% if enable_timepix %} - {{ components.timepix_macro( parent_link = root, - name = "timepix", + sensor_name = 'timepix', x = 0.115, y = -0.05, z = -0.05, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Timepix3 {--> #} - {% if enable_timepix3 %} - {{ components.timepix3_macro( parent_link = root, - name = "timepix3", - material = "cdte", - thickness = 0.002, - max_message_window = 1.0, - sensor_suffix = "", + sensor_name = 'timepix3', + sensor_suffix = '', x = 0.115, y = -0.05, z = -0.05, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Light {--> #} - {% if enable_light %} - {{ components.light_macro( parent_link = root, - update_rate = 30, max_pitch_rate = 0.1, - initial_on = True, - compensate_tilt = True, x = 0.2, y = 0.0, z = -0.1, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} - {# Pendelum {--> #} - {% if enable_pendulum %} - + {# Pendulum {--> #} {{ components.pendulum_macro( parent_link = root, - number_of_chains = 30, - chain_length = 0.1, - chain_radius = 0.01, - chain_mass = 0.5 / 30, x = 0, y = 0, - z = -0.1) + z = -0.1, + spawner_args = spawner_args) }} - - {% endif %} {# #} {# Water gun {--> #} - {% if enable_water_gun %} - {{ components.water_gun_macro( parent_link = root, - muzzle_velocity = 15.0, - particle_capacity = 30, - spread = 1.0, - spawning_reservoir = "the_void", nozzle_offset_x = 0.21, nozzle_offset_y = 0.0, nozzle_offset_z = -0.085, @@ -1199,158 +1009,27 @@ z = -0.11, roll = 0, pitch = 0, - yaw = 0) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} - {# MBZIRC PROFILES {--> #} - - {% if wall_challenge %} - - - - {{ components.external_garmin_macro( - namespace = namespace, - parent_link = root, - orientation = "", - x = 0.0, - y = -0.221, - z = -0.035, - roll = 0, - pitch = components.rad(90), - yaw = components.rad(90)) - }} - - - - {{ components.bluefox_camera_macro( - namespace = namespace, - camera_name = "bluefox_brick", - parent_link = root, - frame_rate = 100.0, - hfov = 2.05, - noise = 0.007, - x = 0.0, - y = -0.187, - z = -0.055, - roll = 0, - pitch = components.rad(90), - yaw = components.rad(-90)) - }} - - - - {{ components.bluefox_camera_macro( - namespace = namespace, - camera_name = "bluefox_optflow", - parent_link = root, - frame_rate = 100.0, - hfov = 1.9, - noise = 0.007, - x = 0.0, - y = -0.152, - z = -0.055, - roll = 0, - pitch = components.rad(90), - yaw = components.rad(-90)) - }} - - - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "rgbd", - camera_suffix="_down", - parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, - x = 0.15, - y = 0.15, - z = -0.05, - roll=-0.9727464, - pitch=1.2572625, - yaw=-2.5665787) - }} - + {# ====================== conditional components ==================== #} - - {% endif %} - - {% if fire_challenge_blanket %} - - - - {{ components.external_garmin_macro( - namespace = namespace, - parent_link = root, - orientation = "", - x = 0.177, - y = 0.0735, - z = -0.07, - roll = 0, - pitch = components.rad(90), - yaw = 0) - }} - - - - {{ components.rplidar_macro( - namespace = namespace, - parent_link = root, - x = 0.0, - y = 0.0, - z = 0.107, - roll = 0, - pitch = 0, - yaw = 0) - }} - - - - {{ components.thermal_camera_macro( - camera_name = "thermal_front", - camera_topic_name = "/" + namespace + "/thermal/front", - parent_frame_name = namespace + "/fcu", - camera_frame_name = "thermal/front_optical", - sensor_base_frame_name = namespace + "/thermal/front", - parent_link = root, - frame_rate = 14.0, - hfov = 0.575959, - image_width = 32, - image_height = 32, - x = 0.13, - y = 0.0, - z = -0.085, - roll = 0, - pitch = 1.0472, - 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.13, - y = 0.0, - z = -0.085, - roll = 0, - pitch = 1.5708, - yaw = 0) - }} - + {# Bluefox + Garmin mount {--> #} + {%- if garmin_down | replace('\s', '') | length != 0 or bluefox_camera | replace('\s', '') | length != 0 or bluefox_camera_reverse | replace('\s', '') | length != 0 -%} + + {{ bluefox_garmin_mount }} + + {%- endif -%} + {# #} - - {% endif %} + {# 3D lidar mount {--> #} + {%- if velodyne | replace('\s', '') | length != 0 or ouster | replace('\s', '') | length != 0 -%} + + {{ lidar_mount_3d }} + + {%- 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 d7d6943..d48ff62 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 @@ -1,7 +1,8 @@ - + - {%- import "mrs_robots_description/sdf/component_snippets.sdf.jinja" as components -%} + {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} @@ -18,14 +19,7 @@ {%- set rotor_xy_offset = 0.1812 -%} {# [m] #} {%- set rotor_z_offset = 0.057 -%} {# [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 %} - + {%- set root = 'base_link' -%} {# #} {# Motor constants {--> #} @@ -36,7 +30,7 @@ {%- 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" -%} + {%- set rolling_moment_coefficient = '1.0e-6' -%} {# #} {# Inertia constants {--> #} @@ -44,50 +38,51 @@ {%- set inertia_body_height = 0.05 -%} {# [m] #} {# #} + {# Scales {--> #} + {%- set 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' -%} + {# #} + {# Meshes {--> #} {# Frame parts {--> #} - {%- set top_board_mesh_file = "model://mrs_robots_description/meshes/x500v2/top_board_mesh_file.dae" -%} - {%- set bottom_board_mesh_file = "model://mrs_robots_description/meshes/x500v2/bottom_board_mesh_file.dae" -%} - {%- set arm_carbon_mesh_file = "model://mrs_robots_description/meshes/x500v2/arm_carbon_mesh_file.dae" -%} - {%- set arm_plastic1_mesh_file = "model://mrs_robots_description/meshes/x500v2/arm_plastic1_mesh_file.stl" -%} - {%- set arm_plastic2_mesh_file = "model://mrs_robots_description/meshes/x500v2/arm_plastic2_mesh_file.stl" -%} - {%- set arm_3d_print_mesh_file = "model://mrs_robots_description/meshes/x500v2/arm_3d_print_mesh_file.stl" -%} - {%- set leg_3d_print_mesh_file = "model://mrs_robots_description/meshes/x500v2/leg_3d_print_mesh_file.stl" -%} + {%- set top_board_mesh_file = 'model://mrs_robots_description/meshes/x500v2/top_board_mesh_file.dae' -%} + {%- set bottom_board_mesh_file = 'model://mrs_robots_description/meshes/x500v2/bottom_board_mesh_file.dae' -%} + {%- set arm_carbon_mesh_file = 'model://mrs_robots_description/meshes/x500v2/arm_carbon_mesh_file.dae' -%} + {%- set arm_plastic1_mesh_file = 'model://mrs_robots_description/meshes/x500v2/arm_plastic1_mesh_file.stl' -%} + {%- set arm_plastic2_mesh_file = 'model://mrs_robots_description/meshes/x500v2/arm_plastic2_mesh_file.stl' -%} + {%- set arm_3d_print_mesh_file = 'model://mrs_robots_description/meshes/x500v2/arm_3d_print_mesh_file.stl' -%} + {%- set leg_3d_print_mesh_file = 'model://mrs_robots_description/meshes/x500v2/leg_3d_print_mesh_file.stl' -%} {# #} {# Motors and props {--> #} - {%- set motor_bottom_mesh_file = "model://mrs_robots_description/meshes/x500v2/motor_bottom_mesh_file.stl" -%} - {%- set motor_middle_mesh_file = "model://mrs_robots_description/meshes/x500v2/motor_middle_mesh_file.stl" -%} - {%- set motor_top_mesh_file = "model://mrs_robots_description/meshes/x500v2/motor_top_mesh_file.stl" -%} - {%- set prop_mesh_file = "model://mrs_robots_description/meshes/x500v2/prop_mesh_file.dae" -%} + {%- set motor_bottom_mesh_file = 'model://mrs_robots_description/meshes/x500v2/motor_bottom_mesh_file.stl' -%} + {%- set motor_middle_mesh_file = 'model://mrs_robots_description/meshes/x500v2/motor_middle_mesh_file.stl' -%} + {%- set motor_top_mesh_file = 'model://mrs_robots_description/meshes/x500v2/motor_top_mesh_file.stl' -%} + {%- set prop_mesh_file = 'model://mrs_robots_description/meshes/x500v2/prop_mesh_file.dae' -%} {# #} {# Sensors and computers {--> #} - {%- set pixhawk6c_mesh_file = "model://mrs_robots_description/meshes/x500v2/pixhawk6c_mesh_file.stl" -%} - {%- set gps_mount_mesh_file = "model://mrs_robots_description/meshes/x500v2/gps_mount_mesh_file.stl" -%} - {%- set nuc_board_mesh_file = "model://mrs_robots_description/meshes/x500v2/nuc_board_mesh_file.stl" -%} - {%- set nuc_cooler_mesh_file = "model://mrs_robots_description/meshes/x500v2/nuc_cooler_mesh_file.stl" -%} + {%- set pixhawk6c_mesh_file = 'model://mrs_robots_description/meshes/x500v2/pixhawk6c_mesh_file.stl' -%} + {%- set gps_mount_mesh_file = 'model://mrs_robots_description/meshes/x500v2/gps_mount_mesh_file.stl' -%} + {%- set nuc_board_mesh_file = 'model://mrs_robots_description/meshes/x500v2/nuc_board_mesh_file.stl' -%} + {%- set nuc_cooler_mesh_file = 'model://mrs_robots_description/meshes/x500v2/nuc_cooler_mesh_file.stl' -%} {# #} {# Mounts {--> #} - {%- set battery_mesh_file = "model://mrs_robots_description/meshes/x500v2/battery_mesh_file.stl" -%} - {%- set battery_mount_mesh_file = "model://mrs_robots_description/meshes/x500v2/battery_mount_mesh_file.stl" -%} - {%- set nuc_hex_posts_mesh_file = "model://mrs_robots_description/meshes/x500v2/nuc_hex_posts_mesh_file.stl" -%} - {%- set gps_module_mesh_file = "model://mrs_robots_description/meshes/x500v2/gps_module_mesh_file.stl" -%} - {%- set garmin_mount_mesh_file = "model://mrs_robots_description/meshes/x500v2/garmin_mount_mesh_file.stl" -%} - {%- set lidar_mount_mesh_file = "model://mrs_robots_description/meshes/x500v2/lidar_mount_mesh_file.stl" -%} - {%- set realsense_mount_mesh_file = "model://mrs_robots_description/meshes/x500v2/realsense_mount_mesh_file.stl" -%} - {%- set bluefox_mount_mesh_file = "model://mrs_robots_description/meshes/x500v2/bluefox_mount_mesh_file.stl" -%} - {%- set uvdar_mount_mesh = "model://mrs_robots_description/meshes/dji/f450/dji_f450_uvdar_mount.dae" -%} - {%- set qorvo_dw1000_mount_file = "model://mrs_robots_description/meshes/x500v2/qorvo_dw1000_mount.stl" -%} - {# #} + {%- set battery_mesh_file = 'model://mrs_robots_description/meshes/x500v2/battery_mesh_file.stl' -%} + {%- set battery_mount_mesh_file = 'model://mrs_robots_description/meshes/x500v2/battery_mount_mesh_file.stl' -%} + {%- set nuc_hex_posts_mesh_file = 'model://mrs_robots_description/meshes/x500v2/nuc_hex_posts_mesh_file.stl' -%} + {%- set gps_module_mesh_file = 'model://mrs_robots_description/meshes/x500v2/gps_module_mesh_file.stl' -%} + {%- set garmin_mount_mesh_file = 'model://mrs_robots_description/meshes/x500v2/garmin_mount_mesh_file.stl' -%} + {%- set lidar_mount_mesh_file = 'model://mrs_robots_description/meshes/x500v2/lidar_mount_mesh_file.stl' -%} + {%- set realsense_mount_mesh_file = 'model://mrs_robots_description/meshes/x500v2/realsense_mount_mesh_file.stl' -%} + {%- set bluefox_mount_mesh_file = 'model://mrs_robots_description/meshes/x500v2/bluefox_mount_mesh_file.stl' -%} + {%- set uvdar_mount_mesh = 'model://mrs_robots_description/meshes/dji/f450/dji_f450_uvdar_mount.dae' -%} + {%- set qorvo_dw1000_mount_file = 'model://mrs_robots_description/meshes/x500v2/qorvo_dw1000_mount.stl' -%} - {# Scales {--> #} - {%- set 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" -%} {# #} {# #} @@ -110,14 +105,14 @@ - - - + {# ================================================================== #} + {# || bare body definitions || #} + {# ================================================================== #} - {{ components.multirotor_physics_macro( + {{ generic.multirotor_physics_macro( mass = mass, body_radius = body_radius, body_height = body_height, @@ -134,8 +129,8 @@ - {{ components.visual_mesh_textured_macro( - name = "top_board", + {{ generic.visual_mesh_textured_macro( + name = 'top_board', mesh_file = top_board_mesh_file, mesh_scale = mesh_scale, x = 0, @@ -143,11 +138,11 @@ z = 0, roll = 0, pitch = 0, - yaw = components.rad(90)) + yaw = math.radians(90)) }} - {{ components.visual_mesh_textured_macro( - name = "bottom_board", + {{ generic.visual_mesh_textured_macro( + name = 'bottom_board', mesh_file = bottom_board_mesh_file, mesh_scale = mesh_scale, x = 0, @@ -155,29 +150,29 @@ z = 0, roll = 0, pitch = 0, - yaw = components.rad(90)) + yaw = math.radians(90)) }} - {% for id in range(0, 4) %} - - {%- set affix = "front_right" -%} - {%- set print_color = "Black" -%} - {% if id == 1 %} - {%- set affix = "front_left" -%} - {% endif %} - {% if id == 2 %} - {%- set affix = "back_left" -%} - {%- set print_color = "Red" -%} - {% endif %} - {% if id == 3 %} - {%- set affix = "back_right" -%} - {%- set print_color = "Red" -%} - {% endif %} - - {{ components.visual_mesh_textured_macro( - name = "arm_carbon_" + affix, + {%- for id in range(0, 4) -%} + + {%- set affix = 'front_right' -%} + {%- set print_color = 'Black' -%} + {%- if id == 1 -%} + {%- set affix = 'front_left' -%} + {%- endif -%} + {%- if id == 2 -%} + {%- set affix = 'back_left' -%} + {%- set print_color = 'Red' -%} + {%- endif -%} + {%- if id == 3 -%} + {%- set affix = 'back_right' -%} + {%- set print_color = 'Red' -%} + {%- endif -%} + + {{ generic.visual_mesh_textured_macro( + name = 'arm_carbon_' + affix, mesh_file = arm_carbon_mesh_file, mesh_scale = mesh_scale, x = 0, @@ -188,11 +183,11 @@ yaw = id * 1.57079633) }} - {{ components.visual_mesh_mrs_material_macro( - name = "arm_plastic1_" + affix, + {{ generic.visual_mesh_mrs_material_macro( + name = 'arm_plastic1_' + affix, mesh_file = arm_plastic1_mesh_file, mesh_scale = mesh_scale, - color = "X500Blue", + color = 'X500Blue', x = 0, y = 0, z = 0, @@ -200,12 +195,12 @@ pitch = 0, yaw = id * 1.57079633) }} - - {{ components.visual_mesh_mrs_material_macro( - name = "arm_plastic2_" + affix, + + {{ generic.visual_mesh_mrs_material_macro( + name = 'arm_plastic2_' + affix, mesh_file = arm_plastic2_mesh_file, mesh_scale = mesh_scale, - color = "X500Blue", + color = 'X500Blue', x = 0, y = 0, z = 0, @@ -213,9 +208,9 @@ pitch = 0, yaw = id * 1.57079633) }} - - {{ components.visual_mesh_macro( - name = "arm_3d_print_" + affix, + + {{ generic.visual_mesh_macro( + name = 'arm_3d_print_' + affix, mesh_file = arm_3d_print_mesh_file, mesh_scale = mesh_scale, color = print_color, @@ -227,13 +222,13 @@ yaw = id * 1.57079633) }} - {% endfor %} + {%- endfor -%} - {{ components.leg_offset_macro( - name = "leg_3d_print_front_right", + {{ generic.leg_collision_offset_macro( + name = 'leg_3d_print_front_right', mesh_file = leg_3d_print_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = 0, y = 0, z = 0, @@ -247,11 +242,11 @@ offset_z = -0.07) }} - {{ components.leg_offset_macro( - name = "leg_3d_print_front_left", + {{ generic.leg_collision_offset_macro( + name = 'leg_3d_print_front_left', mesh_file = leg_3d_print_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = 0, y = 0, z = 0, @@ -265,11 +260,11 @@ offset_z = -0.07) }} - {{ components.leg_offset_macro( - name = "leg_3d_print_back_left", + {{ generic.leg_collision_offset_macro( + name = 'leg_3d_print_back_left', mesh_file = leg_3d_print_mesh_file, mesh_scale = mesh_scale, - color = "Red", + color = 'Red', x = 0, y = 0, z = 0, @@ -283,11 +278,11 @@ offset_z = -0.07) }} - {{ components.leg_offset_macro( - name = "leg_3d_print_back_right", + {{ generic.leg_collision_offset_macro( + name = 'leg_3d_print_back_right', mesh_file = leg_3d_print_mesh_file, mesh_scale = mesh_scale, - color = "Red", + color = 'Red', x = 0, y = 0, z = 0, @@ -303,81 +298,80 @@ - - {{ components.visual_mesh_macro( - name = "nuc_board", + + {{ generic.visual_mesh_macro( + name = 'nuc_board', mesh_file = nuc_board_mesh_file, mesh_scale = mesh_scale, - color = "Grass", + color = 'Grass', x = 0, y = 0, z = 0, roll = 0, pitch = 0, - yaw = components.rad(90)) + yaw = math.radians(90)) }} - {{ components.visual_mesh_macro( - name = "nuc_cooler", + {{ generic.visual_mesh_macro( + name = 'nuc_cooler', mesh_file = nuc_cooler_mesh_file, mesh_scale = mesh_scale, - color = "Grey", + color = 'Grey', x = 0, y = 0, z = 0, roll = 0, pitch = 0, - yaw = components.rad(90)) + yaw = math.radians(90)) }} - {{ components.visual_mesh_macro( - name = "nuc_hex_posts", + {{ generic.visual_mesh_macro( + name = 'nuc_hex_posts', mesh_file = nuc_hex_posts_mesh_file, mesh_scale = mesh_scale, - color = "Grey", + color = 'Grey', x = 0, y = 0, z = 0, roll = 0, pitch = 0, - yaw = components.rad(90)) + yaw = math.radians(90)) }} - - - {{ components.visual_mesh_macro( - name = "pixhawk6c", + + {{ generic.visual_mesh_macro( + name = 'pixhawk6c', mesh_file = pixhawk6c_mesh_file, mesh_scale = mesh_scale, - color = "DarkGrey", + color = 'DarkGrey', x = 0, y = 0, z = 0, roll = 0, pitch = 0, - yaw = components.rad(90)) + yaw = math.radians(90)) }} - - {%- set affix = "front_right" -%} + + {%- set affix = 'front_right' -%} - {% for id in range(0, 4) %} + {%- for id in range(0, 4) -%} - {% if id == 1 %} - {%- set affix = "front_left" -%} - {% endif %} - {% if id == 2 %} - {%- set affix = "back_left" -%} - {% endif %} - {% if id == 3 %} - {%- set affix = "back_right" -%} - {% endif %} + {%- if id == 1 -%} + {%- set affix = 'front_left' -%} + {%- endif -%} + {%- if id == 2 -%} + {%- set affix = 'back_left' -%} + {%- endif -%} + {%- if id == 3 -%} + {%- set affix = 'back_right' -%} + {%- endif -%} - {{ components.visual_mesh_macro( - name = "motor_bottom_" + affix, + {{ generic.visual_mesh_macro( + name = 'motor_bottom_' + affix, mesh_file = motor_bottom_mesh_file, mesh_scale = mesh_scale, - color = "Grey", + color = 'Grey', x = 0, y = 0, z = 0, @@ -385,12 +379,12 @@ pitch = 0, yaw = id * 1.57079633) }} - - {{ components.visual_mesh_macro( - name = "motor_middle_" + affix, + + {{ generic.visual_mesh_macro( + name = 'motor_middle_' + affix, mesh_file = motor_middle_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = 0, y = 0, z = 0, @@ -399,277 +393,234 @@ yaw = id * 1.57079633) }} - {% endfor %} + {%- endfor -%} - {% if use_battery_mount %} - - {{ components.visual_mesh_macro( - name = "battery_mount", + + {{ generic.visual_mesh_macro( + name = 'battery_mount', mesh_file = battery_mount_mesh_file, mesh_scale = mesh_scale, - color = "Red", + color = 'Red', x = 0, y = 0, z = 0.001, roll = 0, pitch = 0, - yaw = components.rad(90)) + yaw = math.radians(90)) }} - {{ components.visual_mesh_macro( - name = "battery", + {{ generic.visual_mesh_macro( + name = 'battery', mesh_file = battery_mesh_file, mesh_scale = mesh_scale, - color = "Grey", + color = 'Grey', x = 0, y = 0, z = 0.001, roll = 0, pitch = 0, - yaw = components.rad(90)) + yaw = math.radians(90)) }} - {% endif %} - {% if enable_rplidar or enable_ouster %} - - {{ components.visual_mesh_macro( - name = "rplidar_mount", - mesh_file = lidar_mount_mesh_file, - mesh_scale = mesh_scale, - color = "FlatBlack", - x = 0, - y = 0, - z = 0, - roll = 0, - pitch = 0, - yaw = components.rad(90)) - }} - - {% else %} - - {{ components.visual_mesh_macro( - name = "gps_mount", + {# GPS antenna and mount {--> #} + {%- set gps_visuals -%} + + {{ generic.visual_link_macro( + name = 'gps_mount', mesh_file = gps_mount_mesh_file, mesh_scale = mesh_scale, - color = "Grey", + color = 'Grey', x = 0, y = 0, z = 0, roll = 0, pitch = 0, - yaw = components.rad(90)) - }} - {{ components.visual_mesh_macro( - name = "gps_module", - mesh_file = gps_module_mesh_file, - mesh_scale = mesh_scale, - color = "Black", - x = 0, - y = 0, - z = 0, - roll = 0, - pitch = 0, - yaw = components.rad(90)) - }} - - {% endif %} - - {% if enable_rangefinder %} - - {{ components.visual_mesh_macro( - name = "garmin_mount", - mesh_file = garmin_mount_mesh_file, - mesh_scale = mesh_scale, - color = "Black", - x = 0, - y = 0, - z = 0.001, - roll = 0, - pitch = 0, - yaw = components.rad(90)) + yaw = math.radians(90), + parent_link = root) }} - {% endif %} - {% if enable_realsense_front or enable_realsense_down or enable_realsense_up%} - - {{ components.visual_mesh_macro( - name = "realsense_mount", - mesh_file = realsense_mount_mesh_file, + + {{ generic.visual_link_macro( + name = 'gps_module', + mesh_file = gps_module_mesh_file, mesh_scale = mesh_scale, - color = "Black", + color = 'Black', x = 0, y = 0, z = 0, roll = 0, pitch = 0, - yaw = components.rad(90)) - }} - - {% endif %} - - {% if enable_bluefox_camera_reverse or enable_bluefox_camera%} - - {{ components.visual_mesh_macro( - name = "bluefox_mount", - mesh_file = bluefox_mount_mesh_file, - mesh_scale = mesh_scale_milimeters, - color = "FlatBlack", - x = 0, - y = 0, - z = 0, - roll = 0, - pitch = 0, - yaw = components.rad(90)) - }} - - {% endif %} - - {% if enable_dual_uv_cameras %} - - {{ components.visual_mesh_macro( - name = "uvdar_mount", - mesh_file = uvdar_mount_mesh, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = 0.027, - y = 0, - z = 0.075, - roll = 0, - pitch = 0, - yaw = 0) - }} - - {% endif %} - - - {% if enable_qorvo_dw1000 %} - - {{components.visual_mesh_macro( - name = "qorvo_dw1000_mount", - mesh_file = qorvo_dw1000_mount_file, - mesh_scale = mesh_scale_milimeters, - color = "DarkGrey", - x = 0.0, - y = -0.01, - z = -0.078, - roll = 0, - pitch = 0, - yaw = 0) + yaw = math.radians(90), + parent_link = root) }} - {% endif %} + {%- endset -%} + {# #} - - - + {# component mounts and holders (these must be passed as args into components, or must be called) {--> #} - - {{ 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 = radius_rotor, - 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_xy_offset, - y = -rotor_xy_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) - }} + {# garmin down mount {--> #} + {%- set garmin_down_mount -%} + {{ generic.visual_link_macro( + name = 'garmin_mount', + mesh_file = garmin_mount_mesh_file, + mesh_scale = mesh_scale, + color = 'Black', + x = 0, + y = 0, + z = -0.001, + roll = 0, + pitch = 0, + yaw = math.radians(90), + parent_link = root) + }} + {%- endset -%} + {# #} + + {# lidar mount {--> #} + {%- set lidar_mount -%} + {{ generic.visual_link_macro( + name = 'rplidar_mount', + mesh_file = lidar_mount_mesh_file, + mesh_scale = mesh_scale, + color = 'FlatBlack', + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = math.radians(90), + parent_link = root) + }} + {%- endset -%} + {# #} + + {# realsense mount {--> #} + {%- set realsense_mount -%} + {{ generic.visual_link_macro( + name = 'realsense_mount', + mesh_file = realsense_mount_mesh_file, + mesh_scale = mesh_scale, + color = 'Black', + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = math.radians(90), + parent_link = root) + }} + {%- endset -%} + {# #} + + {# bluefox mount {--> #} + {%- set bluefox_mount -%} + {{ generic.visual_link_macro( + name = 'bluefox_mount', + mesh_file = bluefox_mount_mesh_file, + mesh_scale = mesh_scale_milimeters, + color = 'FlatBlack', + x = 0, + y = 0, + z = 0, + roll = 0, + pitch = 0, + yaw = math.radians(90), + parent_link = root) + }} + {%- endset -%} + {# #} + + {# uvdar mount {--> #} + {%- set uvdar_mount -%} + {{ generic.visual_link_macro( + name = 'uvdar_mount', + mesh_file = uvdar_mount_mesh, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = 0.027, + y = 0, + z = 0.075, + roll = 0, + pitch = 0, + yaw = 0, + parent_link = root) + }} + {%- endset -%} + {# #} + + {# Qorvo DW1000 mount {--> #} + {%- set qorvo_dw1000_mount -%} + {{ generic.visual_link_macro( + name = 'qorvo_dw1000_mount', + mesh_file = qorvo_dw1000_mount_file, + mesh_scale = mesh_scale_milimeters, + color = 'DarkGrey', + x = 0.0, + y = -0.01, + z = -0.078, + roll = 0, + pitch = 0, + yaw = 0, + parent_link = root) + }} + {%- endset -%} + {# #} - {{ 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 = radius_rotor, - 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_xy_offset, - y = rotor_xy_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 = radius_rotor, - 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_xy_offset, - y = rotor_xy_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", + {# Propellers {--> #} + {%- set prop_list = [ + { + 'motor_number': 0, + 'direction': 'ccw', + 'x': rotor_xy_offset, + 'y': -rotor_xy_offset, + 'z': rotor_z_offset, + 'mesh_files': [prop_mesh_file, motor_top_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'Grey' + }, + { + 'motor_number': 1, + 'direction': 'ccw', + 'x': -rotor_xy_offset, + 'y': rotor_xy_offset, + 'z': rotor_z_offset, + 'mesh_files': [prop_mesh_file, motor_top_mesh_file], + 'mesh_scale': mesh_scale_prop_ccw, + 'color': 'Grey' + }, + { + 'motor_number': 2, + 'direction': 'cw', + 'x': rotor_xy_offset, + 'y': rotor_xy_offset, + 'z': rotor_z_offset, + 'mesh_files': [prop_mesh_file, motor_top_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'Grey' + }, + { + 'motor_number': 3, + 'direction': 'cw', + 'x': -rotor_xy_offset, + 'y': -rotor_xy_offset, + 'z': rotor_z_offset, + 'mesh_files': [prop_mesh_file, motor_top_mesh_file], + 'mesh_scale': mesh_scale_prop_cw, + 'color': 'Grey' + } + ] + -%} + {{ components.propellers_macro( + prop_list = prop_list, rotor_velocity_slowdown_sim = rotor_velocity_slowdown_sim, motor_constant = motor_constant, moment_constant = moment_constant, @@ -679,65 +630,47 @@ 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_xy_offset, - y = -rotor_xy_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) + prop_ixx = prop_ixx, + prop_ixy = prop_ixy, + prop_ixz = prop_ixz, + prop_iyy = prop_iyy, + prop_iyz = prop_iyz, + prop_izz = prop_izz, + spawner_args = spawner_args) }} - + {# #} - - - + {# ================================================================== #} + {# || compulsory sensor definitions || #} + {# ================================================================== #} - - {{ 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) + + {%- set imu_topic = '/imu' -%} + {%- set mag_topic = '/mag' -%} + {%- set baro_topic = '/baro' -%} + {%- set lidar_topic = '/lidar' -%} + + + {{ generic.gazebo_groundtruth_macro( + home_latitude = 0, + home_longitude = 0, + home_altitude = 0) }} - {{ components.gps_macro( - gps_name = "gps0", + {{ generic.gazebo_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_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, @@ -750,36 +683,39 @@ - {{ components.magnetometer_plugin_macro( + {{ generic.gazebo_magnetometer_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) + mag_topic = mag_topic) }} - {{ components.barometer_plugin_macro( - baro_topic = "/baro", + {{ generic.gazebo_barometer_macro( + baro_topic = baro_topic, pub_rate = 50, baro_drift_pa_per_sec = 0) }} + + {{ generic.gazebo_mavlink_interface_macro( + imu_topic = imu_topic, + mag_topic = mag_topic, + baro_topic = baro_topic, + lidar_topic = lidar_topic, + mavlink_config = spawner_args['mavlink_config']) + }} + + - {{ components.imu_plugin_macro( - imu_name = "imu", + + {{ generic.gazebo_imu_macro( + imu_name = 'imu', parent_link = root, - imu_topic = "/imu", + imu_topic = '/imu', gyroscope_noise_density = 0.00018665, gyroscope_random_walk = 0.000038785, gyroscope_bias_correlation_time = 1000.0, @@ -797,348 +733,263 @@ }} - - - + + + {# ================================================================== #} + {# || optional sensor definitions || #} + {# ================================================================== #} {# Ground truth {--> #} - {% if enable_ground_truth %} - - {{ components.odometry_plugin_macro( - odometry_sensor_name = "ground_truth", + {{ components.ground_truth_macro( 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) + yaw = 0, + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# ======================= rangefinder sensors ====================== #} - {# 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, + {# Garmin down {--> #} + {{ components.garmin_down_macro( parent_link = root, - orientation = "", x = 0.0, y = 0.0625, - z = -0.022, + z = -0.009, roll = 0, - pitch = components.rad(90), - yaw = components.rad(-90)) + pitch = math.radians(90), + yaw = math.radians(-90), + mount = garmin_down_mount, + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# ========================== LIDAR sensors ========================= #} {# Rplidar {--> #} - {% if enable_rplidar %} - - {{ components.rplidar_macro( - namespace = namespace, + {%- set rplidar = components.rplidar_macro( parent_link = root, - horizontal_samples = horizontal_samples, - rotation_freq = rotation_freq, x = 0.0, y = 0.0, z = 0.136, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ rplidar }} {# #} {# Ouster {--> #} - {% if enable_ouster %} - - {{ components.ouster_macro( - namespace = namespace, + {%- set ouster = components.ouster_macro( 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, + sensor_name = 'os', x = 0.0, y = 0.0, z = 0.107, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ ouster }} {# #} - - - + {# ========================== camera sensors ======================== #} - {% if enable_realsense_front %} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "front_rgbd", - camera_suffix="", + {# Realsense placements {--> #} + + {# realsense front {--> #} + {%- set realsense_front = components.realsense_front_macro( + camera_name = 'front_rgbd', + camera_suffix='', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.092, y = 0.0, z = 0.016, roll = 0, pitch = 0, - yaw = 0) - }} - - {% endif %} - - {% if enable_realsense_up%} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "up_rgbd", - camera_suffix="", + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ realsense_front }} + {# #} + + {# realsense up {--> #} + {%- set realsense_up = components.realsense_up_macro( + camera_name = 'up_rgbd', + camera_suffix='', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.086, y = 0, z = 0.03, roll = 0, - pitch = - components.rad90, - yaw = 0) - }} - - {% endif %} - - {% if enable_realsense_down%} - - {{ components.realsense_macro( - namespace = namespace, - camera_name = "down_rgbd", - camera_suffix="", + pitch = -math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ realsense_up }} + {# #} + + {# realsense down {--> #} + {%- set realsense_down = components.realsense_down_macro( + camera_name = 'down_rgbd', + camera_suffix='', parent_link = root, - enable_realistic_realsense = enable_realistic_realsense, x = 0.086, y = 0, z = 0.002, roll = 0, - pitch = components.rad90, - yaw = 0) - }} - - {% endif %} + pitch = math.radians(90), + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ realsense_down }} + {# #} - + {%- if realsense_front | replace('\s', '') | length != 0 or realsense_up | replace('\s', '') | length != 0 or realsense_down | replace('\s', '') | length != 0 -%} + {{ realsense_mount }} + {%- endif -%} + + {# #} - + {# Bluefox camera placements {--> #} - {% if enable_bluefox_camera_reverse %} - - {{ components.bluefox_camera_macro( - namespace = namespace, - camera_name = "bluefox_optflow", + {# bluefox (classic) {--> #} + {%- set bluefox_camera = components.bluefox_camera_macro( + camera_name = 'bluefox_optflow', parent_link = root, - frame_rate = 100.0, - hfov = 2.1, - noise = 0.0, x = 0.0, y = -0.0515, z = -0.038, - roll = 0, - pitch = components.rad90, - yaw = components.rad180) - }} - - {% endif %} + mount = none, + spawner_args = spawner_args) + -%} + {{ bluefox_camera }} + {# #} - {% if enable_bluefox_camera %} - - {{ components.bluefox_camera_macro( - namespace = namespace, - camera_name = "bluefox_optflow", + {# bluefox reverse {--> #} + {%- set bluefox_camera_reverse = components.bluefox_camera_reverse_macro( + camera_name = 'bluefox_optflow', parent_link = root, - frame_rate = 100.0, - hfov = 2.1, - noise = 0.0, x = 0.0, y = -0.0515, z = -0.038, - roll = 0, - pitch = components.rad90, - yaw = 0) - }} - - {% endif %} + mount = none, + spawner_args = spawner_args) + -%} + {{ bluefox_camera_reverse }} + {# #} - + {# #} - {# Dual UV cameras {--> #} - {% if enable_dual_uv_cameras %} - - {{ components.uvcam_macro( - parent_link = root, - camera_publish_topic = "/" + namespace + "/uvdar_bluefox_left/image_raw", - calibration_file = uvcam_calib_file, - occlusions = uvcam_occlusions, - frame_rate = 60, - device_id = namespace + "_1", - x = 0.039, - y = 0.1175, - z = 0.089, - roll = 0, - pitch = 0, - yaw = components.rad70) - }} + {# UV camera placements {--> #} - {{ components.uvcam_macro( + {# Dual UV cameras {--> #} + {{ components.dual_uv_cameras_macro( parent_link = root, - camera_publish_topic = "/" + namespace + "/uvdar_bluefox_right/image_raw", - calibration_file = uvcam_calib_file, - occlusions = uvcam_occlusions, - frame_rate = 60, - device_id = namespace + "_2", - x = 0.039, - y = -0.1175, - z = 0.089, - roll = 0, - pitch = 0, - yaw = -components.rad70) + x1 = 0.039, + y1 = 0.1175, + z1 = 0.089, + roll1 = 0, + pitch1 = 0, + yaw1 = math.radians(70), + x2 = 0.039, + y2 = -0.1175, + z2 = 0.089, + roll2 = 0, + pitch2 = 0, + yaw2 = -math.radians(70), + mount = uvdar_mount, + spawner_args = spawner_args) }} - - {% endif %} {# #} - {# Back UV cameras {--> #} - {% if enable_back_uv_camera %} - - {{ components.uvcam_macro( + {# Back UV camera {--> #} + {{ components.back_uv_camera_macro( parent_link = root, - camera_publish_topic = "/" + namespace + "/uvdar_bluefox_back/image_raw", - calibration_file = uvcam_calib_file, - occlusions = uvcam_occlusions, - frame_rate = 60, - device_id = namespace + "_3", x = -0.1, y = 0.0, z = 0.085, - roll = components.rad90, + roll = math.radians(90), pitch = 0, - yaw = components.rad180) + yaw = math.radians(180), + mount = none, + spawner_args = spawner_args) }} - - {% endif %} {# #} - + {# #} + + {# ========================== other sensors ========================= #} {# UV leds {--> #} - {% if enable_uv_leds %} - - - {% if uvled_s_l != "None" %} - {% set led1 = uvled_s_l %} - {% set led3 = uvled_s_l %} - {% else %} - {% set led1 = uvled_s[0] %} - {% set led3 = uvled_s[2] %} - {% endif %} - - {% if uvled_s_r != "None" %} - {% set led2 = uvled_s_r %} - {% set led4 = uvled_s_r %} - {% else %} - {% set led2 = uvled_s[1] %} - {% set led4 = uvled_s[3] %} - {% endif %} - - {# -- leds configuration -- (id, signal_id, x, y, z, roll, pitch, yaw) #} - {%- set uv_leds_macro_parameters = [(1, led1, 0.2017755, 0.1947045, 0.025, 0.0, components.rad90, 0.0), - (2, led1, 0.1947045, 0.2017755, 0.025, 0.0, components.rad90, components.rad90), - (3, led2, 0.1947045, -0.2017755, 0.025, 0.0, components.rad90, -components.rad90), - (4, led2, 0.2017755, -0.1947045, 0.025, 0.0, components.rad90, 0.0), - (5, led3, -0.1947045, 0.2017755, 0.025, 0.0, components.rad90, components.rad90), - (6, led3, -0.2017755, 0.1947045, 0.025, 0.0, components.rad90, components.rad180), - (7, led4, -0.2017755, -0.1947045, 0.025, 0.0, components.rad90, -components.rad180), - (8, led4, -0.1947045, -0.2017755, 0.025, 0.0, components.rad90, -components.rad90)] -%} - - {% for id_, signal_id_, x_, y_, z_, roll_, pitch_, yaw_ in uv_leds_macro_parameters %} - {{ components.uvled_macro( - parent_link = root, - device_id = namespace + "_" + id_ | string(), - signal_id = signal_id_, - x = x_, - y = y_, - z = z_, - roll = roll_, - pitch = pitch_, - yaw = yaw_) - }} - {% endfor %} - - {% endif %} + {{ components.uv_leds_macro( + parent_link = root, + x1 = 0.2017755, + x2 = 0.1947045, + y1 = 0.1947045, + y2 = 0.2017755, + z = 0.025, + spawner_args = spawner_args) + }} {# #} {# UV leds beacon {--> #} - {% if enable_uv_leds_beacon %} - - {# -- leds configuration -- (id, signal_id, x, y, z, roll, pitch, yaw) #} - {%- set uv_leds_macro_parameters = [("b1", uvled_beacon_s, 0.01, 0.0, 0.125, 0.0, components.rad90, 0.0), - ("b2", uvled_beacon_s, 0.0, 0.01, 0.125, 0.0, components.rad90, components.rad90), - ("b3", uvled_beacon_s, -0.01, 0.0, 0.125, 0.0, components.rad90, components.rad180), - ("b4", uvled_beacon_s, 0.0, -0.01, 0.125, 0.0, components.rad90, components.rad270)] -%} - - {% for id_, signal_id_, x_, y_, z_, roll_, pitch_, yaw_ in uv_leds_macro_parameters %} - {{ components.uvled_macro( - parent_link = root, - device_id = namespace + "_" + id_ | string(), - signal_id = signal_id_, - x = x_, - y = y_, - z = z_, - roll = roll_, - pitch = pitch_, - yaw = yaw_) - }} - {% endfor %} - - {% endif %} + {{ components.uv_leds_beacon_macro( + parent_link = root, + x1 = 0.01, + x2 = 0.0, + y1 = 0.0, + y2 = 0.01, + z = 0.125, + spawner_args = spawner_args) + }} {# #} {# Qorvo UWB {--> #} - {% if enable_qorvo_dw1000 %} - - {{ components.uwb_range_macro( - parent_link = root, - uwb_id = uwb_id, - uav_name = namespace, - x = 0.0, - y = 0.08, - z = -0.10, - roll = 0.0, - pitch = 0.0, - yaw = 0.0) + {{ components.uwb_range_macro( + parent_link = root, + x = 0.0, + y = 0.08, + z = -0.10, + roll = 0.0, + pitch = 0.0, + yaw = 0.0, + mount = qorvo_dw1000_mount, + spawner_args = spawner_args) }} - - {% endif %} {# #} + {# ====================== conditional components ==================== #} + + {# lidar mount / gps antenna visuals {--> #} + {%- if rplidar | replace('\s', '') | length != 0 or ouster | replace('\s', '') | length != 0 -%} + + {{ lidar_mount }} + + {%- else -%} + {# otherwise put a GPS antenna to the empty space #} + {{ gps_visuals }} + {%- endif -%} + {# #} + + {# Bluefox mount {--> #} + {%- if bluefox_camera | replace('\s', '') | length != 0 or bluefox_camera_reverse | replace('\s', '') | length != 0 -%} + + {{ bluefox_mount }} + + {%- endif -%} + {# #} diff --git a/ros_packages/mrs_uav_gazebo_simulation/package.xml b/ros_packages/mrs_uav_gazebo_simulation/package.xml index 85a0a6f..9d1f34d 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/package.xml +++ b/ros_packages/mrs_uav_gazebo_simulation/package.xml @@ -25,9 +25,7 @@ px4 rospy std_msgs - ignition-math6 - mrs_uav_testing - + python3-jinja2 diff --git a/ros_packages/mrs_uav_gazebo_simulation/scripts/jinja_gen.py b/ros_packages/mrs_uav_gazebo_simulation/scripts/jinja_gen.py deleted file mode 100755 index b1b0233..0000000 --- a/ros_packages/mrs_uav_gazebo_simulation/scripts/jinja_gen.py +++ /dev/null @@ -1,127 +0,0 @@ -#!/usr/bin/env python3 - -import jinja2 -import argparse -import os -import shutil -import fnmatch -import rospkg -import numpy as np -import json - -def get_file_contents(filepath): - with open(filepath, 'rb') as f: - return f.read() - -def str2bool(v): - if v.lower() in ('yes', 'true', 't', 'y', '1'): - return True - elif v.lower() in ('no', 'false', 'f', 'n', '0'): - return False - else: - raise argparse.ArgumentTypeError('Boolean value expected.') - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument('filepath', help="full path to the jinja file that the sdf file should be generated from") - parser.add_argument('--mavlink_addr', default="INADDR_ANY", help="IP address for PX4 SITL") - parser.add_argument('--mavlink_udp_port', default=14560, help="Mavlink UDP port for mavlink access") - parser.add_argument('--mavlink_tcp_port', default=4560, help="TCP port for PX4 SITL") - parser.add_argument('--serial_enabled', default=0, help="Enable Serial device for HITL") - parser.add_argument('--serial_device', default="/dev/ttyACM0", help="Serial device for FMU") - parser.add_argument('--serial_baudrate', default=921600, help="Baudrate of Serial device for FMU") - parser.add_argument('--qgc_addr', default="INADDR_ANY", help="IP address for QGC") - parser.add_argument('--qgc_udp_port', default=14550, help="UDP port for QGC") - parser.add_argument('--sdk_addr', default="INADDR_ANY", help="IP address for MAVSDK") - parser.add_argument('--sdk_udp_port', default=14540, help="UDP port for MAVSDK") - parser.add_argument('--hil_mode', default=0, help="Enable HIL mode for HITL simulation") - parser.add_argument('--hil_state_level', default=0, help="HIL state level HITL simulation") - parser.add_argument('--send_vision_estimation', default=0) - parser.add_argument('--send_odometry', default=1) - parser.add_argument('--use_lockstep', default=1, help="Enable simulation lockstep for syncing physics&sensors") - parser.add_argument('--use_tcp', default=1, help="Use TCP instead of UDP for PX4 SITL") - parser.add_argument('--visual_material', default="DarkGrey", help="Default texture for 3D models") - parser.add_argument('--gps_indoor_jamming', default=0, help="Trigger bad GPS when the vehicle has obstacles above it") - parser.add_argument('--output-file', help="sdf output file") - parser.add_argument('--model_config_file', help="config file with list of sensors for particular drone") - parser.add_argument('--stdout', action='store_true', default=False, help="dump to stdout instead of file") - parser.add_argument('--namespace', default="uav", help="drone namespace") - args, _ = parser.parse_known_args() - - # print('Generating a templated model using jinja') - - filename = os.path.basename(args.filepath) - model_name = filename.split('.')[0] - model_dir = os.path.realpath(os.path.dirname(args.filepath)) - - # find the path to the mrs_robots_description/sdf directory which should always be included - rospack = rospkg.RosPack() - mrs_uav_gazebo_simulation_path = os.path.realpath(rospack.get_path('mrs_uav_gazebo_simulation')) - mrs_models_dir = os.path.join(mrs_uav_gazebo_simulation_path, 'models') - - env = jinja2.Environment(loader=jinja2.FileSystemLoader((model_dir, mrs_models_dir))) - # print("Jinja env. directories:\n\t{}\n\t{}".format(model_dir, mrs_models_dir)) - - template = env.get_template(filename) - - d = {'name' : model_name, \ - 'mavlink_addr': args.mavlink_addr, \ - 'mavlink_udp_port': args.mavlink_udp_port, \ - 'mavlink_tcp_port': args.mavlink_tcp_port, \ - 'serial_enabled': args.serial_enabled, \ - 'serial_device': args.serial_device, \ - 'serial_baudrate': args.serial_baudrate, \ - 'qgc_addr': args.qgc_addr, \ - 'qgc_udp_port': args.qgc_udp_port, \ - 'sdk_addr': args.sdk_addr, \ - 'sdk_udp_port': args.sdk_udp_port, \ - 'hil_mode': args.hil_mode, \ - 'hil_state_level': args.hil_state_level, \ - 'send_vision_estimation': args.send_vision_estimation, \ - 'send_odometry': args.send_odometry, \ - 'use_lockstep': args.use_lockstep, \ - 'use_tcp': args.use_tcp, \ - 'visual_material': args.visual_material, \ - 'gps_indoor_jamming': args.gps_indoor_jamming, \ - 'namespace': args.namespace} - - - # reading the data from the file - with open(args.model_config_file) as f: - data = f.read() - - # reconstructing the data as a dictionary - js = json.loads(data) - d.update(js) - - result = template.render(d) - - if args.output_file: - filename_out = args.output_file - else: - if not args.filename.endswith('.sdf.jinja'): - raise Exception("ERROR: Output file can only be determined automatically for " + \ - "input files with the .sdf.jinja extension") - filename_out = filename.replace('.sdf.jinja', '.sdf') - assert filename_out != filename, "Not allowed to overwrite template" - - # Overwrite protection mechanism: after generation, the file will be copied to a "last_generated" file. - # In the next run, we can check whether the target file is still unmodified. - filename_out_last_generated = filename_out + '.last_generated' - - if os.path.exists(filename_out) and os.path.exists(filename_out_last_generated): - # Check whether the target file is still unmodified. - if get_file_contents(filename_out).strip() != get_file_contents(filename_out_last_generated).strip(): - raise Exception('Generation would overwrite changes to "' + str(filename_out) +\ - '".\nChanges should only be made to the template file "' + str(filename) + \ - '"\nRemove "' + str(filename_out) + '"\n(after extracting your changes) to disable this overwrite protection.') - - with open(filename_out, 'w') as f_out: - # print('Output generated "%s"' % filename_out) - f_out.write(result) - - # Copy the contents to a "last_generated" file for overwrite protection check next time. - shutil.copy(filename_out, filename_out_last_generated) - - if args.stdout: - print(result) 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 deleted file mode 100755 index 180efaf..0000000 --- a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner.py +++ /dev/null @@ -1,684 +0,0 @@ -#!/usr/bin/python3 - -import copy -import csv -import os -import roslaunch -import rospkg -import rospy -import sys -import tempfile -import threading -import yaml -import math -import random -import json -import atexit - -from utils import print_error, print_info, print_ok, is_number, rinfo, rwarn, rerr - -from mrs_msgs.srv import String as StringSrv -from mrs_msgs.srv import StringResponse as StringSrvResponse -from gazebo_msgs.msg import ModelStates -from mavros_msgs.msg import State as MavrosState - -from mrs_msgs.msg import GazeboSpawnerDiagnostics - -VEHICLE_BASE_PORT = 14000 -MAVLINK_TCP_BASE_PORT = 4560 -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', 'm690'] -SPAWNING_DELAY_SECONDS = 6 - -glob_running_processes = [] - -class MrsDroneSpawner(): - - # #{ __init__ - def __init__(self, show_help=False, verbose=False): - - self.verbose = verbose - self.rospack = rospkg.RosPack() - - gazebo_simulation_path = self.rospack.get_path('mrs_uav_gazebo_simulation') - px4_api_path = self.rospack.get_path('mrs_uav_px4_api') - - path_to_spawner_params = gazebo_simulation_path + os.sep + 'config' + os.sep + 'spawner_params.yaml' - - with open(path_to_spawner_params, 'r') as params_file: - self.spawner_params = yaml.safe_load(params_file) - - if not self.params_integrity_ok(): - return - - if show_help: - self.print_help() - return - - # convert spawner_params to default_model_config (to get rid of help strings) - self.default_model_config = {} - for key, values in self.spawner_params.items(): - self.default_model_config[key] = values[0] - - self.path_to_launch_file_firmware = gazebo_simulation_path + os.sep + 'launch' + os.sep + 'run_simulation_firmware.launch' - self.path_to_launch_file_spawn_model = gazebo_simulation_path + os.sep + 'launch' + os.sep + 'spawn_simulation_model.launch' - self.path_to_launch_file_mavros = px4_api_path + os.sep + 'launch' + os.sep + 'mavros_gazebo_simulation.launch' - - rospy.init_node('mrs_drone_spawner', anonymous=True) - rinfo('Node initialization started. All parameters loaded correctly.') - - if self.verbose: - rinfo('Loaded the following params:') - for param, value in self.spawner_params.items(): - print('\t\t' + str(param) + ': ' + str(value)) - print('') - rinfo('remove arg \'verbose\' in mrs_drone_spawner.launch to stop listing params on startup') - - # #{ setup system variables - self.spawn_called = False - self.processing = False - self.process_queue = [] - self.process_queue_mutex = threading.Lock() - self.active_vehicles = [] - self.queued_vehicles = [] - self.got_mavlink = {} - self.mavlink_sub = {} - self.assigned_ids = {} # dictionary {id: process_handle} - # #} - - # #{ setup communication - - # diagnostics - self.diagnostics_pub = rospy.Publisher('~diagnostics', GazeboSpawnerDiagnostics,queue_size=1) - self.diagnostics_timer = rospy.Timer(rospy.Duration(0.1), self.callback_diagnostics_timer) - self.action_timer = rospy.Timer(rospy.Duration(0.1), self.callback_action_timer) - - # spawning - spawn_server = rospy.Service('~spawn', StringSrv, self.callback_spawn, buff_size=20) - - rospy.spin() - # #} - - # #} - - # #{ params_integrity_ok - def params_integrity_ok(self): - for pname, pvals in self.spawner_params.items(): - if not isinstance(pvals, list): - print('Error occured while parsing \'spawner_params.yaml\' at parameter \'' + str(pname) + '\'!') - print('Expected: \'parameter: [default_value, help_string, [vehicle_types]]') - print('Found: \'' + str(pname) + ': ' + str(pvals) + '\'') - return False - elif len(pvals) != 3: - print('Error occured while parsing \'spawner_params.yaml\' at parameter \'' + str(pname) + '\'!') - print('Expected: \'parameter: [default_value, help_string, [vehicle_types]]') - print('Found: \'' + str(pname) + ': ' + str(pvals) + '\'') - return False - else: - try: - str(pvals[1]) - except: - print('Error occured while parsing \'spawner_params.yaml\' at parameter \'' + str(pname) + '\'!') - print('Expected: \'parameter: [default_value, help_string, [vehicle_types]]') - print('Parameter does not have a valid help_string') - return False - if not isinstance(pvals[2], list): - print('Error occured while parsing \'spawner_params.yaml\' at parameter \'' + str(pname) + '\'!') - print('Expected: \'parameter: [default_value, help_string, [vehicle_types]]') - print('Found: \'' + str(pvals[2]) + '\' instead of a vehicle_types list') - return False - else: - for vehicle_type in pvals[2]: - if vehicle_type not in VEHICLE_TYPES: - print('Error occured while parsing \'spawner_params.yaml\' at parameter \'' + str(pname) + '\'!') - print('Expected: \'parameter: [default_value, help_string, [vehicle_types]]') - print('Unkown vehicle type \'' + str(vehicle_type) + '\'') - return False - return True - # #} - - # #{ print_help - def print_help(self): - BOLDGREEN = '\033[32;1m' - BOLD = '\033[1m' - ENDC = '\033[0m' - print('') - print(BOLD + '****************************' + ENDC) - print(BOLD + '** MRS DRONE SPAWNER HELP **' + ENDC) - print(BOLD + '****************************' + ENDC) - print('') - print('The mrs_drone_spawner is a ROS node, which allows you to dynamically add new vehicles into your Gazebo simulation\n') - print('To spawn a new drone, use:\nrosservice call /mrs_drone_spawner/spawn "...string of arguments..."\n') - print('Available arguments:') - print(BOLDGREEN + '1 2 3' + ENDC + ' ... : use numbers ' + BOLD + '[0 to 250]' + ENDC + ' to assign IDs to the vehicles. The vehicles will be named \'uav1, uav2, uav3 ... \'') - print(' ' + BOLDGREEN + ':' + ENDC + ' using a blank space instead of a number will automatically assign an unused ID to the vehicle') - - for param, data in self.spawner_params.items(): - default_value, help_string, vehicle_types = data - print(BOLDGREEN + '--' + str(param).replace('_', '-') + ENDC + BOLD + ' (default: ' + str(default_value) + ')' + ENDC + ': ' + str(help_string) ) - # #} - - # #{ callback_action_timer - def callback_action_timer(self, timer): - - self.process_queue_mutex.acquire() - - if len(self.process_queue) > 0: - - process, args = self.process_queue[0] - del self.process_queue[0] - self.process_queue_mutex.release() - - orig_signal_handler = roslaunch.pmon._init_signal_handlers - roslaunch.pmon._init_signal_handlers = self.dummy_function - process_handle = process(*args) - roslaunch.pmon._init_signal_handlers = orig_signal_handler - - if process_handle is not None: - glob_running_processes.append(process_handle) - else: - self.process_queue_mutex.acquire() - self.process_queue.append((process, args)) - self.process_queue_mutex.release() - - else: - self.processing = False - self.process_queue_mutex.release() - # rinfo('Nothing to do') - # #} - - # #{ callback_diagnostics_timer - def callback_diagnostics_timer(self, timer): - diagnostics = GazeboSpawnerDiagnostics() - diagnostics.spawn_called = self.spawn_called - diagnostics.processing = self.processing - diagnostics.active_vehicles = self.active_vehicles - diagnostics.queued_vehicles = self.queued_vehicles - self.process_queue_mutex.acquire() - diagnostics.queued_processes = len(self.process_queue) - self.process_queue_mutex.release() - self.diagnostics_pub.publish(diagnostics) - # #} - - # #{ callback_spawn - def callback_spawn(self, req): - - # #{ check gazebo running - try: - rospy.wait_for_message('/gazebo/model_states', ModelStates, 60) - except: - res = StringSrvResponse() - res.success = False - res.message = str('Gazebo model state topic not found. Is Gazebo running?') - return res - # #} - - # #{ input parsing - params_dict = None - try: - params_dict = self.parse_input_params(req.value) - except Exception as e: - res = StringSrvResponse() - res.success = False - res.message = str(e.args[0]) - return res - - if params_dict is None: - res = StringSrvResponse() - res.success = False - res.message = str('Cannot process input parameters') - return res - # #} - - # #{ roslaunch args generation - roslaunch_args = None - try: - roslaunch_args = self.generate_launch_args(params_dict) - except Exception as e: - res = StringSrvResponse() - res.success = False - res.message = str(e.args[0]) - return res - - if roslaunch_args is None: - res = StringSrvResponse() - res.success = False - res.message = str('Cannot generate roslaunch arguments') - return res - # #} - - # #{ prepare id <-> process map - self.spawn_called = True - for ID in params_dict['uav_ids']: - self.assigned_ids[ID] = None - # #} - - # #{ queue new launch processes - self.process_queue_mutex.acquire() - self.processing = True - for i, uav_roslaunch_args in enumerate(roslaunch_args): - ID = params_dict['uav_ids'][i] - self.queued_vehicles.append('uav' + str(ID)) - self.process_queue.append((self.launch_mavros, (ID, uav_roslaunch_args))) - self.process_queue.append((self.spawn_simulation_model, (ID, uav_roslaunch_args))) - self.process_queue.append((self.launch_firmware, (ID, uav_roslaunch_args))) - self.process_queue_mutex.release() - # #} - - res = StringSrvResponse() - res.success = True - res.message = str('Launch sequence queued') - return res - - # #} - - # #{ parse_input_params - def parse_input_params(self, data): - params_list = data.split() - - try: - uav_ids = self.get_ids(params_list) - vehicle_type = self.get_vehicle_type(params_list) - params_dict = self.get_params_dict(params_list, vehicle_type) - if params_dict['pos_file'] != 'None': - rinfo('Loading spawn poses from file \'' + str(params_dict['pos_file']) + '\'') - spawn_poses = self.get_spawn_poses_from_file(params_dict['pos_file'], uav_ids) - elif params_dict['pos'] != 'None': - rinfo('Using spawn poses provided from command line args \'' + str(params_dict['pos']) + '\'') - spawn_poses = self.get_spawn_poses_from_args(params_dict['pos'], uav_ids) - else: - rinfo('Assigning default spawn poses') - spawn_poses = self.get_spawn_poses_from_ids(uav_ids) - - except Exception as e: - rerr('Exception raised while parsing user input:') - rerr(str(e.args[0])) - raise Exception('Cannot spawn vehicle. Reason: ' + str(e.args[0])) - - params_dict['uav_ids'] = uav_ids - params_dict['vehicle_type'] = vehicle_type - params_dict['spawn_poses'] = spawn_poses - return params_dict - # #} - - # #{ assign_free_id - def assign_free_id(self): - for i in range(0,251): - if i not in self.assigned_ids.keys(): - return i - raise Exception('Cannot assign a free ID to the vehicle!') - # #} - - # #{ generate_launch_args - def generate_launch_args(self, params_dict): - - args_sequences = [] - num_uavs = len(params_dict['uav_ids']) - - for n in range(num_uavs): - - uav_args_sequence = [] - ID = params_dict['uav_ids'][n] - # get vehicle ID number - uav_args_sequence.append('ID:=' + str(ID)) - - # get vehicle type - uav_args_sequence.append('vehicle:=' + params_dict['vehicle_type']) - - # setup communication ports - comm_ports = self.get_comm_ports(ID) - for name,value in comm_ports.items(): - uav_args_sequence.append(str(name) + ':=' + str(value)) - - # setup vehicle spawn pose - uav_args_sequence.append('x:=' + str(params_dict['spawn_poses'][ID]['x'])) - uav_args_sequence.append('y:=' + str(params_dict['spawn_poses'][ID]['y'])) - uav_args_sequence.append('z:=' + str(params_dict['spawn_poses'][ID]['z'])) - uav_args_sequence.append('heading:=' + str(params_dict['spawn_poses'][ID]['heading'])) - - # generate a yaml file for the custom model config - fd, path = tempfile.mkstemp(prefix='simulation_', suffix='_uav' + str(ID) + '.yaml') - json_object = json.dumps(params_dict, indent = 2) - with os.fdopen(fd, 'w') as f: - f.write(json_object) - uav_args_sequence.append('model_config_file:=' + path) - - # setup the package where the model jinja should be found (mrs_uav_gazebo_simulation by default) - uav_args_sequence.append('resource_package_path:=' + self.rospack.get_path(str(params_dict['model_package']))) - - print('UAV' + str(ID) + ' ARGS_SEQUENCE:') - print(uav_args_sequence) - args_sequences.append(uav_args_sequence) - - return args_sequences - # #} - - # #{ get_comm_ports - def get_comm_ports(self, ID): - ''' - NOTE - ports have to match with values assigned in - mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix/rcS - ''' - ports = {} - ports['udp_offboard_port_remote'] = VEHICLE_BASE_PORT + (4 * ID) + 2 - ports['udp_offboard_port_local'] = VEHICLE_BASE_PORT + (4 * ID) + 1 - ports['mavlink_tcp_port'] = MAVLINK_TCP_BASE_PORT + ID - ports['mavlink_udp_port'] = MAVLINK_UDP_BASE_PORT + ID - ports['fcu_url'] = 'udp://127.0.0.1:' + str(ports['udp_offboard_port_remote']) + '@127.0.0.1:' + str(ports['udp_offboard_port_local']) - return ports - # #} - - # #{ get_ids - def get_ids(self, params_list): - requested_ids = [] - - # read params until non-numbers start comming - for p in params_list: - if is_number(p): - requested_ids.append(int(p)) - else: - break - - if len(requested_ids) < 1: - free_id = self.assign_free_id() - requested_ids.append(free_id) - rwarn('Vehicle ID not specified. Number ' + str(free_id) + ' assigned automatically.') - return requested_ids - - rinfo('Requested vehicle IDs: ' + str(requested_ids)) - - ids = [] - # remove all IDs that are already assigned or out of range - for ID in requested_ids: - if ID > 249: - rwarn('Cannot spawn uav' + str(ID) + ', ID out of range <0, 250>!') - continue - - if ID in self.assigned_ids.keys(): - rwarn('Cannot spawn uav' + str(ID) + ', ID already assigned!') - continue - ids.append(ID) - - if len(ids) < 1: - raise Exception('No valid ID provided') - - return ids - # #} - - # #{ get_params_dict - def get_params_dict(self, params_list, vehicle_type): - params_dict = copy.deepcopy(self.default_model_config) - custom_params = {} - for i, p in enumerate(params_list): - if '--' in p: - param_name = p[2:] - param_name = param_name.replace('-', '_') - if param_name not in self.default_model_config.keys() and param_name not in VEHICLE_TYPES: - raise Exception('Param \'' + str(param_name) + '\' not recognized!') - children = [] - for j in range(i+1, len(params_list)): - if '--' not in params_list[j]: - children.append(params_list[j]) - else: - break - if len(children) < 1: - custom_params[param_name] = True - elif len(children) == 1: - custom_params[param_name] = children[0] - else: - custom_params[param_name] = children - - if len(custom_params.keys()) > 0: - rinfo('Customized params:') - for pname, pval in custom_params.items(): - if pname not in VEHICLE_TYPES: - # check if the customized param is allowed for the desired vehicle type - allowed_vehicle_types = self.spawner_params[pname][2] - # print('For param ' + str(pname) + ' allowed: ' + str(allowed_vehicle_types)) - if vehicle_type not in allowed_vehicle_types: - raise Exception('Param \'' + str(pname) + '\' cannot be used with vehicle type \'' + str(vehicle_type) + '\'!') - - print('\t' + str(pname) + ': ' + str(pval)) - params_dict.update(custom_params) - return params_dict - # #} - - # #{ get_vehicle_type - def get_vehicle_type(self, params_list): - for p in params_list: - for v in VEHICLE_TYPES: - if v in p: - return v - return DEFAULT_VEHICLE_TYPE - # #} - - # #{ get_spawn_poses - - # #{ get_spawn_poses_from_ids - def get_spawn_poses_from_ids(self, ids): - spawn_poses = {} - uav_spacing = 5.0 - circle_diameter = 0.0 - total_positions_in_current_circle = 0; - angle_increment = 0; - remaining_positions_in_current_circle = 1; - circle_perimeter= math.pi*circle_diameter - random_angle_offset = 0 - random_x_offset = round(random.uniform(-5,5), 2) - random_y_offset = round(random.uniform(-5,5), 2) - - for ID in ids: - if remaining_positions_in_current_circle == 0: - circle_diameter = circle_diameter + uav_spacing - circle_perimeter= math.pi*circle_diameter - total_positions_in_current_circle = math.floor(circle_perimeter/uav_spacing) - remaining_positions_in_current_circle = total_positions_in_current_circle - angle_increment = (math.pi*2)/total_positions_in_current_circle - random_angle_offset = round(random.uniform(-math.pi,math.pi), 2) - - x = round(math.sin(angle_increment*remaining_positions_in_current_circle + random_angle_offset)*circle_diameter,2) + random_x_offset - y = round(math.cos(angle_increment*remaining_positions_in_current_circle + random_angle_offset)*circle_diameter,2) + random_y_offset - z = 0.3 - heading = round(random.uniform(-math.pi,math.pi), 2) - remaining_positions_in_current_circle = remaining_positions_in_current_circle - 1 - - # inteam_id = ID % 100 - # x = 0 - # y = 29.45 - # z = 0.3 - # heading = 0 - - # if ( inteam_id > 1 ): - # y -= 8*((inteam_id)//2) - - # if( inteam_id % 2 == 0 ): - # x += 4 - # if( inteam_id % 2 == 1 ): - # x -= 4 - - spawn_poses[ID] = {'x': x, 'y': y, 'z': z, 'heading': heading} - print("spawn poses len: " + str(len(spawn_poses))) - print("spawn poses: " + str(spawn_poses)) - return spawn_poses - # #} - - # #{ get_spawn_poses_from_args - def get_spawn_poses_from_args(self, pose_vec4, uav_ids): - spawn_poses = {} - x = float(pose_vec4[0]) - y = float(pose_vec4[1]) - z = float(pose_vec4[2]) - heading = float(pose_vec4[3]) - - spawn_poses[uav_ids[0]] = {'x': x, 'y': y, 'z': z, 'heading': heading} - - if len(uav_ids) > 1: - for i in range(len(uav_ids)): - x += 2 - spawn_poses[uav_ids[i]] = {'x': x, 'y': y, 'z': z, 'heading': heading} - - return spawn_poses - # #} - - # #{ get_spawn_poses_from_file - def get_spawn_poses_from_file(self, filename, uav_ids): - if not os.path.isfile(filename): - raise Exception('File \'' + str(filename) + '\' does not exist!') - - spawn_poses = {} - - # #{ csv - if filename.endswith('.csv'): - array_string = list(csv.reader(open(filename))) - for row in array_string: - if (len(row)!=5): - raise Exception('Incorrect data in file \'' + str(filename) + '\'! Data in \'.csv\' file type should be in format [id, x, y, z, heading] (example: int, float, float, float, float)') - if int(row[0]) in uav_ids: - spawn_poses[int(row[0])] = {'x' : float(row[1]), 'y' : float(row[2]), 'z' : float(row[3]), 'heading' : float(row[4])} - # #} - - # #{ yaml - elif filename.endswith('.yaml'): - dict_vehicle_info = yaml.safe_load(open(filename, 'r')) - for item, data in dict_vehicle_info.items(): - if (len(data.keys())!=5): - raise Exception('Incorrect data in file \'' + str(filename) + '\'! Data in \'.yaml\' file type should be in format \n uav_name: \n\t id: (int) \n\t x: (float) \n\t y: (float) \n\t z: (float) \n\t heading: (float)') - - if int(data['id']) in uav_ids: - spawn_poses[data['id']] = {'x' : float(data['x']), 'y' : float(data['y']), 'z' : float(data['z']), 'heading' : float(data['heading'])} - # #} - - else: - raise Exception('Incorrect file format, must be either \'.csv\' or \'.yaml\'') - - if len(spawn_poses.keys()) != len(uav_ids) or set(spawn_poses.keys()) != set(uav_ids): - raise Exception('File \'' + str(filename) + '\' does not cover all the UAV poses') - - rinfo('Spawn poses returned:') - rinfo(str(spawn_poses)) - return spawn_poses - # #} - - # #} - - # #{ launch_firmware - def launch_firmware(self, ID, uav_roslaunch_args): - rinfo('Running firmware for uav' + str(ID) + '...') - uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) - roslaunch.configure_logging(uuid) - roslaunch_sequence = [(self.path_to_launch_file_firmware, uav_roslaunch_args)] - launch = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_sequence) - try: - launch.start() - except: - rerr('Error occured while starting firmware for uav' + str(ID) + '!') - launch.shutdown() - return None - - try: - rospy.wait_for_message('/uav' + str(ID) + '/mavros/state', MavrosState, 60) - except: - rerr('Mavros did not respond while starting firmware for uav' + str(ID) + '!') - launch.shutdown() - return None - - process_name = 'uav' + str(ID) + '_firmware' - rinfo('Firmware for uav' + str(ID) + ' started!') - return launch, process_name - # #} - - # #{ spawn_gazebo_model - def spawn_simulation_model(self, ID, uav_roslaunch_args): - rinfo('Spawning Gazebo model for uav' + str(ID) + '...') - uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) - roslaunch.configure_logging(uuid) - roslaunch_sequence = [(self.path_to_launch_file_spawn_model, uav_roslaunch_args)] - launch = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_sequence) - try: - launch.start() - except: - rerr('Error occured while spawning Gazebo model for uav' + str(ID) + '!') - launch.shutdown() - return None - - rinfo('Gazebo model for uav' + str(ID) + ' spawned!') - self.queued_vehicles.remove('uav' + str(ID)) - self.active_vehicles.append('uav' + str(ID)) - process_name = 'uav' + str(ID) + '_simulation_model' - return launch, process_name - # #} - - # #{ launch_mavros - def launch_mavros(self, ID, uav_roslaunch_args): - rinfo('Running mavros for uav' + str(ID) + '...') - uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) - roslaunch.configure_logging(uuid) - roslaunch_sequence = [(self.path_to_launch_file_mavros, uav_roslaunch_args)] - launch = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_sequence) - try: - launch.start() - except: - rerr('Error occured while launching mavros for uav' + str(ID) + '!') - launch.shutdown() - return None - - rinfo('Waiting for uav' + str(ID) + '\'s mavros to start publishing...') - try: - rospy.wait_for_message('/uav' + str(ID) + '/mavros/state', MavrosState, 60) - except: - rerr('Mavros for uav' + str(ID) + ' did not start in 60 seconds!') - launch.shutdown() - return None - - process_name = 'uav' + str(ID) + '_mavros' - rinfo('Mavros for uav' + str(ID) + ' started!') - return launch, process_name - # #} - - # #{ dummy_function - def dummy_function(self): - pass - # #} - -# #{ exit_handler -def exit_handler(): - print('[INFO] [MrsDroneSpawner]: Exit requested') - - if len(glob_running_processes) > 0: - print(f'[INFO] [MrsDroneSpawner]: Shutting down {len(glob_running_processes)} subprocesses') - - num_zombies = 0 - for p, pid in glob_running_processes: - try: - p.shutdown() - print(f'[INFO] [MrsDroneSpawner]: Process {pid} shutdown') - except: - num_zombies += 1 - - if num_zombies > 0: - print(f'\033[91m[ERROR] [MrsDroneSpawner]: Could not stop {num_zombies} subprocesses\033[91m') - exit(1) - - print('[INFO] [MrsDroneSpawner]: Exited gracefully') - exit(0) -# #} - -if __name__ == '__main__': - - print('[INFO] [MrsDroneSpawner]: Starting') - - show_help = True - if 'no_help' in sys.argv: - show_help = False - - verbose = 'verbose' in sys.argv - - atexit.register(exit_handler) - - try: - spawner = MrsDroneSpawner(show_help, verbose) - except rospy.ROSInterruptException: - pass diff --git a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/component_wrapper.py b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/component_wrapper.py new file mode 100644 index 0000000..2846521 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/component_wrapper.py @@ -0,0 +1,16 @@ +class ComponentWrapper: + + def __init__(self, keyword, description, default_args): + ''' + A wrapper for params defined in the jinja template that are recognized and used by the mrs_drone_spawner + + :param keyword: string used to activate the component by using '--keyword' as spawner argument + + :param description: string used to display help for a component in a human-readable form + + :param default_args: a list of dict of configurable internal variables, user can override those by + adding them as args after the '--keyword' without the '--' + ''' + self.keyword = keyword + self.description = description + self.default_args = default_args diff --git a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py new file mode 100755 index 0000000..3238761 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/mrs_drone_spawner.py @@ -0,0 +1,1113 @@ +#!/usr/bin/python3 +import ast +import atexit +import copy +import csv +import datetime +import jinja2 +import jinja2.meta +import math +import os +import random +import re +import roslaunch +import rospkg +import rospy +import sys +import tempfile +import threading +import xml.dom.minidom +import yaml + +from inspect import getmembers, isfunction + +from component_wrapper import ComponentWrapper +from template_wrapper import TemplateWrapper + +from mrs_msgs.srv import String as StringSrv +from mrs_msgs.srv import StringResponse as StringSrvResponse +from mrs_msgs.srv import StringRequest as StringSrvRequest +from mrs_msgs.msg import GazeboSpawnerDiagnostics +from gazebo_msgs.msg import ModelStates +from gazebo_msgs.srv import SpawnModel as SpawnModelSrv +from geometry_msgs.msg import Pose + +glob_running_processes = [] + +# #{ Exceptions and Errors + +# #{ NoFreeIDAvailable(RuntimeError) +class NoFreeIDAvailable(RuntimeError): + '''Indicate that the vehicle limit imposed by px4 sitl has been reached''' + pass +# #} + +# #{ NoValidIDGiven(RuntimeError) +class NoValidIDGiven(RuntimeError): + '''Indicate that the user has provided only invalid IDs (non-integers, or all are already assigned)''' + pass +# #} + +# #{ FormattingError(ValueError) +class FormattingError(ValueError): + '''Indicate that the input is not properly formatted''' + pass +# #} + +# #{ WrongNumberOfArguments(ValueError) +class WrongNumberOfArguments(ValueError): + '''Indicate that the expected number of arguments is different''' + pass +# #} + +# #{ SuffixError(NameError) +class SuffixError(NameError): + '''Indicate that the file has an unexpected suffix''' + pass +# #} + +# #} + +# #{ dummy_function() +def dummy_function(): + '''Empty function to temporarily replace rospy signal handlers''' + pass +# #} + +# #{ filter_templates(template_name) +def filter_templates(template_name, suffix): + '''Comparator used to load files with given suffix''' + return template_name.endswith(suffix) +# #} + +# #{ exit_handler() +def exit_handler(): + ''' + Kill all subprocesses started by the spawner to prevent orphaned processes (mainly px4 and mavros) + ''' + print('[INFO] [MrsDroneSpawner]: Exit requested') + + if len(glob_running_processes) > 0: + print(f'[INFO] [MrsDroneSpawner]: Shutting down {len(glob_running_processes)} subprocesses') + + num_zombies = 0 + for p in glob_running_processes: + try: + p.shutdown() + print(f'[INFO] [MrsDroneSpawner]: Process {p.run_id} shutdown') + except: + num_zombies += 1 + + if num_zombies > 0: + print(f'\033[91m[ERROR] [MrsDroneSpawner]: Could not stop {num_zombies} subprocesses\033[91m') + exit(1) + + print('[INFO] [MrsDroneSpawner]: Exited gracefully') + exit(0) +# #} + +class MrsDroneSpawner: + + # #{ __init__(self, verbose=False) + def __init__(self, verbose=False): + + self.is_initialized = False + + self.verbose = verbose + + rospy.init_node('mrs_drone_spawner', anonymous=True) + + resource_paths = [] + rospack = rospkg.RosPack() + resource_paths.append(os.path.join(rospack.get_path('mrs_uav_gazebo_simulation'), 'models')) + + # # #{ load required params + try: + self.vehicle_base_port = rospy.get_param('~mavlink_config/vehicle_base_port') + self.mavlink_tcp_base_port = rospy.get_param('~mavlink_config/mavlink_tcp_base_port') + self.mavlink_udp_base_port = rospy.get_param('~mavlink_config/mavlink_udp_base_port') + self.qgc_udp_port = rospy.get_param('~mavlink_config/qgc_udp_port') + self.sdk_udp_port = rospy.get_param('~mavlink_config/sdk_udp_port') + self.send_vision_estimation = rospy.get_param('~mavlink_config/send_vision_estimation') + self.send_odometry = rospy.get_param('~mavlink_config/send_odometry') + self.enable_lockstep = rospy.get_param('~mavlink_config/enable_lockstep') + self.use_tcp = rospy.get_param('~mavlink_config/use_tcp') + self.template_suffix = rospy.get_param('~jinja_templates/suffix') + self.save_sdf_files = rospy.get_param('~jinja_templates/save_rendered_sdf') + self.default_robot_name = rospy.get_param('~gazebo_models/default_robot_name') + self.model_spacing = rospy.get_param('~gazebo_models/spacing') + except KeyError as err: + rospy.logerr(f'[MrsDroneSpawner]: Could not load required param {err}') + raise rospy.ROSInterruptException + # # #} + + # # #{ handle extra resource paths + extra_resource_paths = rospy.get_param('~extra_resource_paths', []) + for elem in extra_resource_paths: + if os.path.exists(elem): + rpath = elem + else: + rpath = rospack.get_path(elem) + rospy.loginfo(f'[MrsDroneSpawner]: Adding extra resources from {rpath}') + resource_paths.append(rpath) + + self.jinja_env = self.configure_jinja2_environment(resource_paths) + # # #} + + # # #{ find launchfiles for mavros and px4_firmware + gazebo_simulation_path = rospack.get_path('mrs_uav_gazebo_simulation') + px4_api_path = rospack.get_path('mrs_uav_px4_api') + self.mavros_launch_path = px4_api_path + os.sep + 'launch' + os.sep + 'mavros_gazebo_simulation.launch' + self.px4_fimrware_launch_path = gazebo_simulation_path + os.sep + 'launch' + os.sep + 'run_simulation_firmware.launch' + # # #} + + try: + self.jinja_templates = self.build_template_database() + except RecursionError as err: + rospy.logerr(f'[MrsDroneSpawner]: {err}') + raise rospy.ROSInterruptException + + rospy.loginfo(f'[MrsDroneSpawner]: ------------------------') + rospy.loginfo(f'[MrsDroneSpawner]: Jinja templates loaded:') + for name in self.jinja_templates.keys(): + rospy.loginfo(f'[MrsDroneSpawner]: "{name}" from "{self.jinja_templates[name].jinja_template.filename}"') + rospy.loginfo(f'[MrsDroneSpawner]: ------------------------') + + # # #{ setup communication + self.spawn_server = rospy.Service('~spawn', StringSrv, self.callback_spawn, buff_size=20) + self.diagnostics_pub = rospy.Publisher('~diagnostics', GazeboSpawnerDiagnostics, queue_size=1) + self.diagnostics_timer = rospy.Timer(rospy.Duration(0.1), self.callback_diagnostics_timer) + self.action_timer = rospy.Timer(rospy.Duration(0.1), self.callback_action_timer) + + self.gazebo_spawn_proxy = rospy.ServiceProxy('~gazebo_spawn_model', SpawnModelSrv) + # # #} + + # #{ setup system variables + self.spawn_called = False + self.processing = False + self.vehicle_queue = [] + self.queue_mutex = threading.Lock() + self.active_vehicles = [] + self.assigned_ids = set() + # #} + + self.is_initialized = True + + rospy.spin() + + # load all available models + # from current package + # from external packages (multiple) + + # compose all available params + + # display help (params) for individual models + + # #} end init + + # -------------------------------------------------------------- + # | jinja template utils | + # -------------------------------------------------------------- + + # #{ get_all_templates(self) + def get_all_templates(self): + ''' + Get all templates loaded by the given jinja environment + :returns a list of tuples, consisting of (str_name, jinja2.Template) + ''' + template_names = self.jinja_env.list_templates(filter_func=lambda template_name: filter_templates(template_name, self.template_suffix)) + templates = [] + for i, full_name in enumerate(template_names): + rospy.loginfo(f'[MrsDroneSpawner]: \t({i+1}/{len(template_names)}): {full_name}') + template_name = full_name.split(os.path.sep)[-1][:-(len(self.template_suffix))] + templates.append((template_name, self.jinja_env.get_template(full_name))) + return templates + # #} + + # #{ get_template_imports(self, jinja_template) + def get_template_imports(self, jinja_template): + '''Returns a list of sub-templates imported by a given jinja_template''' + with open(jinja_template.filename, 'r') as f: + template_source = f.read() + preprocessed_template = template_source.replace('\n', '') + parsed_template = self.jinja_env.parse(preprocessed_template) + import_names = [node.template.value for node in parsed_template.find_all(jinja2.nodes.Import)] + imported_templates = [] + for i in import_names: + template = self.jinja_env.get_template(i) + imported_templates.append(template) + return imported_templates + # #} + + # #{ get_spawner_components_from_template(self, template) + def get_spawner_components_from_template(self, template): + ''' + Builds a dict of spawner-compatible macros in a given template and their corresponding ComponentWrapper objects + Does NOT check for macros imported from other templates + :return a dict in format {macro name: component_wrapper.ComponentWrapper} + ''' + with open(template.filename, 'r') as f: + template_source = f.read() + preprocessed_template = template_source.replace('\n', '') + parsed_template = self.jinja_env.parse(preprocessed_template) + macro_nodes = [node for node in parsed_template.find_all(jinja2.nodes.Macro)] + spawner_components = {} + for node in macro_nodes: + spawner_keyword = None + spawner_description = None + spawner_default_args = None + for elem in node.body: + if isinstance(elem, jinja2.nodes.Assign) and elem.target.name == 'spawner_description': + spawner_description = elem.node.value + if isinstance(elem, jinja2.nodes.Assign) and elem.target.name == 'spawner_default_args': + if isinstance(elem.node, jinja2.nodes.Const): + spawner_default_args = elem.node.value + elif isinstance(elem.node, jinja2.nodes.List): + spawner_default_args = [] + for e in elem.node.items: + spawner_default_args.append(e.value) + elif isinstance(elem.node, jinja2.nodes.Dict): + spawner_default_args = {} + for pair in elem.node.items: + spawner_default_args[pair.key.value] = pair.value.value + else: + rospy.logwarn(f'[MrsDroneSpawner]: Unsupported param type "{type(elem.node)}" in template {template.filename}') + if isinstance(elem, jinja2.nodes.Assign) and elem.target.name == 'spawner_keyword': + spawner_keyword = elem.node.value + if spawner_keyword is not None: + spawner_components[node.name] = ComponentWrapper(spawner_keyword, spawner_description, spawner_default_args) + return spawner_components + # #} + + # #{ get_accessible_components(self, template_wrapper, all_components) + def get_accessible_components(self, template_wrapper, all_components): + ''' + Recursive function to get all spawner-compatible components accessible from template_wrapper + Includes components in imported sub-templates + :param template_wrapper: template_wrapper.TemplateWrapper for which we want to load components + :param all_components: a dict to which all found ComponentWrappers will be added + :returns a dict of objects {macro name: component_wrapper.ComponentWrapper} + ''' + all_components.update(template_wrapper.components) + for i in template_wrapper.imported_templates: + try: + all_components.update(self.get_accessible_components(i, all_components)) + except RecursionError as err: + raise RecursionError(f'Cyclic import detected in file {template_wrapper.jinja_template.filename}. Fix your templates') + return all_components + # #} + + # #{ get_callable_components(self, template) + def get_callable_components(self, template, accessible_components): + ''' + Get all components that are actually called from a template + :param template: a jinja template file + :param accessible_components: a dict of macros accessible from this template (including imported modules) + :returns a dictionary of callable components {macro_name: component_wrapper.ComponentWrapper} + sorted alphabetically by keywords + ''' + callable_components = {} + with open(template.filename, 'r') as f: + template_source = f.read() + preprocessed_template = template_source.replace('\n', '') + parsed_template = self.jinja_env.parse(preprocessed_template) + call_nodes = [node for node in parsed_template.find_all(jinja2.nodes.Call)] + callable_components = {} + for node in call_nodes: + if isinstance(node.node, jinja2.nodes.Getattr): + if node.node.attr in accessible_components.keys(): + callable_components[node.node.attr] = accessible_components[node.node.attr] + return dict(sorted(callable_components.items(), key=lambda item: item[1].keyword)) + # #} + + # #{ build_template_database(self) + def build_template_database(self): + ''' + Generate a database of jinja2 templates available to the spawner + Scans through all folders provided into the jinja2 environment for files with matching target suffix + Recursively checks templates imported by templates, prevents recursion loops + Returns a dictionary of template_wrapper.TemplateWrapper objects in format {template_name: template_wrapper.TemplateWrapper} + ''' + + template_wrappers = {} + + rospy.loginfo('[MrsDroneSpawner]: Loading all templates') + all_templates = self.get_all_templates() + for name, template in all_templates: + imports = self.get_template_imports(template) + components = self.get_spawner_components_from_template(template) + wrapper = TemplateWrapper(template, imports, components) + template_wrappers[name] = wrapper + + rospy.loginfo('[MrsDroneSpawner]: Reindexing imported templates') + for name, wrapper in template_wrappers.items(): + for i, it in enumerate(wrapper.imported_templates): + if not isinstance(it, TemplateWrapper): + for ww in template_wrappers.values(): + if ww.jinja_template == it: + wrapper.imported_templates[i] = ww + + rospy.loginfo('[MrsDroneSpawner]: Adding available components from dependencies') + for _, wrapper in template_wrappers.items(): + prev_limit = sys.getrecursionlimit() + sys.setrecursionlimit(len(template_wrappers) + 1) + wrapper.components = self.get_accessible_components(wrapper, {}) + sys.setrecursionlimit(prev_limit) + + rospy.loginfo('[MrsDroneSpawner]: Pruning components to only include callables') + callable_components = {} + for name, template in all_templates: + callable_components[name] = self.get_callable_components(template, template_wrappers[name].components) + + for name, wrapper in template_wrappers.items(): + wrapper.components = callable_components[name] + + rospy.loginfo('[MrsDroneSpawner]: Template database built') + + return template_wrappers + # #} + + # #{ configure_jinja2_environment(self, resource_paths) + def configure_jinja2_environment(self, resource_paths): + '''Create a jinja2 environment and setup its variables''' + env = jinja2.Environment( + loader=jinja2.FileSystemLoader(resource_paths), + autoescape=None + ) + # Allows use of math module directly in the templates + env.globals['math'] = math + + return env + # #} + + # #{ render(self, spawner_args) + def render(self, spawner_args): + ''' + Renders a jinja template into a sdf, creates a formatted xml + Input has to specify the template name in spawner_args["model"] + :param spawner_args: a dict to be passed into the template as variables, format {component_name (string): args (list or dict)} + :return: content of the xml file as a string or None + ''' + + params = { + "spawner_args": spawner_args + } + + try: + model_name = spawner_args['model'] + except KeyError: + rospy.logerr(f'[MrsDroneSpawner]: Cannot render template, model not specified') + return + + try: + template_wrapper = self.jinja_templates[model_name] + except KeyError: + rospy.logerr(f'[MrsDroneSpawner]: Cannot render model "{model_name}". Template not found!') + return + + rospy.loginfo(f'[MrsDroneSpawner]: Rendering model "{model_name}" using template {template_wrapper.jinja_template.filename}') + + context = template_wrapper.jinja_template.new_context(params) + rendered_template = template_wrapper.jinja_template.render(context) + try: + root = xml.dom.minidom.parseString(rendered_template) + except Exception as e: + rospy.logerr(f'[MrsDroneSpawner]: XML error: "{e}"') + fd, filepath = tempfile.mkstemp(prefix='mrs_drone_spawner_' + datetime.datetime.now().strftime("%Y_%m_%d__%H_%M_%S_"), suffix='_DUMP_' + str(model_name) + '.sdf') + with os.fdopen(fd, 'w') as output_file: + output_file.write(rendered_template) + rospy.loginfo(f'[MrsDroneSpawner]: Malformed XML for model {model_name} dumped to {filepath}') + return + + ugly_xml = root.toprettyxml(indent=' ') + + # Remove empty lines + pretty_xml = '\n'.join(line for line in ugly_xml.split('\n') if line.strip()) + + return pretty_xml + + # #} end render + + # -------------------------------------------------------------- + # | user input parsing | + # -------------------------------------------------------------- + + # #{ parse_user_input(self, input_str) + def parse_user_input(self, input_str): + ''' + Extract params from an input string, create spawner args + expected input: + device ids (integers separated by spaces) + keywords (specified in jinja components starting with '--') + component args following a keyword (values separated by spaces) + :param input_str: string containing all args in the format specified above + :return: a dict in format {keyword: component_args}, always contains keys "help", "model", "ids", "names", "spawn_poses" + NOTE: arguments of a component/keyword will always be parsed as a list/dict, even for a single value + + Raises: + AssertionError in case of unexpected data in mandatory values under keys "model", "ids", "names", "spawn_poses" + ''' + + input_dict = { + 'help': False, + 'model': None, + 'ids': [], + 'names': [], + 'spawn_poses': {}, + } + + # parse out the keywords starting with '--' + pattern = re.compile(r'(--\S*)') + substrings = [m.strip() for m in re.split(pattern, input_str) if len(m.strip()) > 0] + + if len(substrings) < 1: + input_dict['help'] = True + return input_dict + + # before the first keyword, there should only be device IDs + first_keyword_index = 0 + if '--' not in substrings[0]: + input_dict['ids'] = self.parse_string_to_objects(substrings[0]) + first_keyword_index = 1 + else: + input_dict['ids'].append(self.assign_free_id()) + + # pair up keywords with args + for i in range(first_keyword_index, len(substrings)): + + if substrings[i].startswith('--'): + input_dict[substrings[i][2:]] = None + continue + else: + input_keys = [*input_dict.keys()] + if len(input_keys) > 1: + input_dict[input_keys[-1]] = self.parse_string_to_objects(substrings[i]) + + # attempt to match model to available templates + for k in input_dict.keys(): + if k in self.jinja_templates.keys(): + input_dict['model'] = str(k) + del input_dict[k] + break + + + valid_ids = [] + + for ID in input_dict['ids']: + if not isinstance(ID, int): + if ID in self.jinja_templates.keys() and input_dict['model'] is None: + rospy.loginfo(f'[MrsDroneSpawner]: Using {ID} as model template') + input_dict['model'] = ID + else: + rospy.logwarn(f'[MrsDroneSpawner]: Ignored ID {ID}: Not an integer') + continue + if ID < 0 or ID > 255: + rospy.logwarn(f'[MrsDroneSpawner]: Ignored ID {ID}: Must be in range(0, 256)') + continue + if ID in self.assigned_ids: + rospy.logwarn(f'[MrsDroneSpawner]: Ignored ID {ID}: Already assigned') + continue + valid_ids.append(ID) + + input_dict['ids'].clear() + + if '--help' in substrings: + input_dict['help'] = True + return input_dict + + if len(valid_ids) > 0: + rospy.loginfo(f'[MrsDroneSpawner]: Valid robot IDs: {valid_ids}') + input_dict['ids'] = valid_ids + self.assigned_ids.update(input_dict['ids']) + else: + raise NoValidIDGiven('No valid ID given. Check your input') + + if 'pos' in input_dict.keys(): + try: + input_dict['spawn_poses'] = self.get_spawn_poses_from_args(input_dict['pos'], input_dict['ids']) + except (WrongNumberOfArguments, ValueError) as err: + rospy.logerr(f'[MrsDroneSpawner]: While parsing args for "--pos": {err}') + rospy.logwarn(f'[MrsDroneSpawner]: Assigning random spawn poses instead') + input_dict['spawn_poses'] = self.get_randomized_spawn_poses(input_dict['ids']) + finally: + del input_dict['pos'] + + elif 'pos-file' in input_dict.keys(): + try: + input_dict['spawn_poses'] = self.get_spawn_poses_from_file(input_dict['pos-file'][0], input_dict['ids']) + except (FileNotFoundError, SuffixError, FormattingError, WrongNumberOfArguments, ValueError) as err: + rospy.logerr(f'[MrsDroneSpawner]: While parsing args for "--pos-file": {err}') + rospy.logwarn(f'[MrsDroneSpawner]: Assigning random spawn poses instead') + input_dict['spawn_poses'] = self.get_randomized_spawn_poses(input_dict['ids']) + finally: + del input_dict['pos-file'] + + else: + input_dict['spawn_poses'] = self.get_randomized_spawn_poses(input_dict['ids']) + + if 'name' in input_dict.keys(): + for ID in input_dict['ids']: + input_dict['names'].append(str(input_dict['name'][0]) + str(ID)) + del input_dict['name'] + else: + for ID in input_dict['ids']: + input_dict['names'].append(str(self.default_robot_name) + str(ID)) + + assert isinstance(input_dict['ids'], list) and len(input_dict['ids']) > 0, 'No vehicle ID assigned' + assert input_dict['model'] is not None, 'Model not specified' + assert isinstance(input_dict['names'], list) and len(input_dict['names']) == len(input_dict['ids']), f'Invalid vehicle names {input_dict["names"]}' + assert isinstance(input_dict['spawn_poses'], dict) and len(input_dict['spawn_poses'].keys()) == len(input_dict['ids']), f'Invalid spawn poses {input_dict["spawn_poses"]}' + + return input_dict + # #} + + # #{ parse_string_to_objects(self, input_str) + def parse_string_to_objects(self, input_str): + ''' + Attempt to convert input_str into a dictionary or a list + Convert numerals into number datatypes whenever possible + Returns None if the input cannot be interpreted as dict or list + ''' + input_str = input_str.strip() + + params = [] + for s in input_str.split(): + if len(s) > 0: + try: + # try to convert input_str to numbers + params.append(ast.literal_eval(s)) + except (SyntaxError, ValueError): + # leave non-numbers as string + params.append(s) + + + params_dict = {} + if isinstance(params, list): + # try to convert named args into a dict + for p in params: + try: + if ':=' in p: + kw, arg = p.split(':=') + try: + # try to convert arg to number + params_dict[kw] = ast.literal_eval(arg) + except (SyntaxError, ValueError): + # leave non-numbers as string + params_dict[kw] = arg + except TypeError: + pass + + if len(params_dict.keys()) > 0 and len(params_dict.keys()) == len(params): + # whole input converted to a dict + return params_dict + else: + return params + + return None + # #} + + # #{ get_help_text(self, input_dict): + def get_help_text(self, input_dict): + ''' + Used to construct the help text (string) for a given dict of input args + Returns: + generic spawner help + or + help for a specific model + or + None (if the input does not contain "help") + ''' + if not input_dict['help']: + return None + + if input_dict['model'] is None: + display_text = self.get_spawner_help_text() + else: + display_text = self.get_model_help_text(input_dict['model']) + + return display_text + # #} + + # #{ get_model_help_text(self, model_name) + def get_model_help_text(self, model_name): + ''' + Create a help string by loading all callable components from a given template in the following format + Component name + Description: + Default args: + ''' + rospy.loginfo(f'[MrsDroneSpawner]: Getting help for model {model_name}') + try: + template_wrapper = self.jinja_templates[model_name] + response = f'[MrsDroneSpawner]: Components used in template "{template_wrapper.jinja_template.filename}":\n' + except ValueError: + return f'Template for model {model_name} not found' + + for name, component in template_wrapper.components.items(): + response += f'{component.keyword}\n\tDescription: {component.description}\n\tDefault args: {component.default_args}\n\n' + + return response + # #} + + # #{ get_spawner_help_text(self) + def get_spawner_help_text(self): + '''Create a generic help string for the spawner basic use''' + + rospy.loginfo(f'[MrsDroneSpawner]: Getting generic spawner help') + response = 'The spawn service expects the following input (as a string):\n' + response += '\tdevice ids (integers separated by spaces, auto-assigned if no ID is specified),\n' + response += '\tmodel (use \'--\' with a model name to select a specific model),\n' + response += '\tkeywords (specified inside jinja macros as "spawner_keyword". Add \'--\' before each keyword when calling spawn),\n' + response += '\tcomponent args following a keyword (values separated by spaces or a python dict, overrides "spawner_default_args" in jinja macros),\n' + response += '\n' + response += '\tModels available: ' + + for model_name in sorted(self.jinja_templates.keys()): + response += f'{model_name}, ' + + return response + # #} + + # -------------------------------------------------------------- + # | ROS callbacks | + # -------------------------------------------------------------- + + # #{ callback_spawn(self, req) + def callback_spawn(self, req): + + self.spawn_called = True + + rospy.loginfo(f'[MrsDroneSpawner]: Spawn called with args "{req.value}"') + res = StringSrvResponse() + res.success = False + + # #{ input parsing + params_dict = None + already_assigned_ids = copy.deepcopy(self.assigned_ids) # backup in case of mid-parse failure + try: + params_dict = self.parse_user_input(req.value) + except Exception as e: + rospy.logwarn(f'[MrsDroneSpawner]: While parsing user input: {e}') + res.message = str(e.args[0]) + self.assigned_ids = already_assigned_ids + return res + # #} + + # #{ display help if needed + help_text = self.get_help_text(params_dict) + if help_text is not None: + rospy.loginfo(help_text) + inline_help_text = help_text + inline_help_text = inline_help_text.replace('\n', ' ') + inline_help_text = inline_help_text.replace('\t', ' ') + res.message = inline_help_text + res.success = True + return res + # #} + + rospy.loginfo(f'[MrsDroneSpawner]: Spawner params assigned "{params_dict}"') + + # #{ check gazebo running + try: + rospy.loginfo('[MrsDroneSpawner]: Waiting for /gazebo/model_states') + rospy.wait_for_message('/gazebo/model_states', ModelStates, 60) + except rospy.exceptions.ROSException: + res.message = str('Gazebo model state topic not found. Is Gazebo running?') + return res + # #} + + rospy.loginfo('[MrsDroneSpawner]: Adding vehicles to a spawn queue') + self.processing = True + self.queue_mutex.acquire() + for i, ID in enumerate(params_dict['ids']): + robot_params = self.get_jinja_params_for_one_robot(params_dict, i, ID) + name = robot_params['name'] + self.vehicle_queue.append(robot_params) + self.queue_mutex.release() + + num_added = len(params_dict['ids']) + + res = StringSrvResponse() + res.success = True + res.message = f'Launch sequence queued for {num_added} robots' + return res + + # #} + + # #{ callback_diagnostics_timer + def callback_diagnostics_timer(self, timer): + diagnostics = GazeboSpawnerDiagnostics() + diagnostics.spawn_called = self.spawn_called + diagnostics.processing = self.processing + diagnostics.active_vehicles = self.active_vehicles + self.queue_mutex.acquire() + diagnostics.queued_vehicles = [params['name'] for params in self.vehicle_queue] + diagnostics.queued_processes = len(self.vehicle_queue) + self.queue_mutex.release() + self.diagnostics_pub.publish(diagnostics) + # #} + + # #{ callback_action_timer + def callback_action_timer(self, timer): + + self.queue_mutex.acquire() + + if len(self.vehicle_queue) > 0: + + robot_params = self.vehicle_queue[0] + del self.vehicle_queue[0] + self.queue_mutex.release() + + orig_signal_handler = roslaunch.pmon._init_signal_handlers + roslaunch.pmon._init_signal_handlers = dummy_function + + model_spawned = self.spawn_gazebo_model(robot_params) + if not model_spawned: + rospy.logerr(f'[MrsDroneSpawner]: Could not spawn Gazebo model for {robot_params["name"]}') + # self.queue_mutex.acquire() + # self.vehicle_queue.append(robot_params) + # self.queue_mutex.release() + return + + firmware_process = self.launch_px4_firmware(robot_params) + mavros_process = self.launch_mavros(robot_params) + roslaunch.pmon._init_signal_handlers = orig_signal_handler + + # One of the processes did not start -> shutdown the rest and try again later + if None in (firmware_process, mavros_process): + for p in (firmware_process, mavros_process): + try: + p.shutdown() + except: + pass + self.queue_mutex.acquire() + self.vehicle_queue.append(robot_params) + self.queue_mutex.release() + return + + glob_running_processes.append(firmware_process) + glob_running_processes.append(mavros_process) + + rospy.loginfo(f'[MrsDroneSpawner]: Vehicle {robot_params["name"]} successfully spawned') + self.active_vehicles.append(robot_params['name']) + + else: + self.processing = False + self.queue_mutex.release() + # rinfo('Nothing to do') + # #} + + # -------------------------------------------------------------- + # | Spawner utils | + # -------------------------------------------------------------- + + # #{ assign_free_id(self) + def assign_free_id(self): + ''' + Assign an unused ID in range <0, 255> + :return: unused ID for a robot (int) + :raise NoFreeIDAvailable: if max vehicle count has been reached + ''' + for i in range(0, 256): # 255 is a hard limit of px4 sitl + if i not in self.assigned_ids: + rospy.loginfo(f'[MrsDroneSpawner]: Assigned free ID "{i}" to a robot') + return i + raise NoFreeIDAvailable('Cannot assign a free ID') + # #} + + # #{ get_spawn_poses_from_file(self, filename, ids) + def get_spawn_poses_from_file(self, filename, ids): + ''' + Parses an input file and extracts spawn poses for vehicles. The file must be either ".csv" or ".yaml" + + CSV files have to include one line per robot, formatting: X, Y, Z, HEADING + YAML files have to include one block per robot, formatting: + block_header: # not used + id: int + x: float + y: float + z: float + heading: float + + + The file must contain spawn poses for all vehicles + :param fileame: full path to a file + :param ids: a list of ints containing unique vehicle IDs + :return: a dict in format {id: {'x': pos_x, 'y', pos_y, 'z': pos_z, 'heading': pos_heading}} + + Raises: + FileNotFoundError - if filename does not exist + FormattingError - if the csv or yaml file does not match the expected structure + SuffixError - filename has other suffix than ".csv" or ".yaml" + WrongNumberOfArguments - number of poses defined in the file does not match the number of ids + ValueError - spawn poses are not numbers + ''' + + rospy.loginfo(f'[MrsDroneSpawner]: Loading spawn poses from file "{filename}"') + if not os.path.isfile(filename): + raise FileNotFoundError(f'File "{filename}" does not exist!') + + spawn_poses = {} + + # #{ csv + if filename.endswith('.csv'): + array_string = list(csv.reader(open(filename))) + for row in array_string: + if (len(row)!=5): + raise FormattingError(f'Incorrect data in file "{filename}"! Data in ".csv" file type should be in format [id, x, y, z, heading] (types: int, float, float, float, float)') + if int(row[0]) in ids: + spawn_poses[int(row[0])] = {'x' : float(row[1]), 'y' : float(row[2]), 'z' : float(row[3]), 'heading' : float(row[4])} + # #} + + # #{ yaml + elif filename.endswith('.yaml'): + dict_vehicle_info = yaml.safe_load(open(filename, 'r')) + for item, data in dict_vehicle_info.items(): + if (len(data.keys())!=5): + raise FormattingError(f'Incorrect data in file "{filename}"! Data in ".yaml" file type should be in format \n uav_name: \n\t id: (int) \n\t x: (float) \n\t y: (float) \n\t z: (float) \n\t heading: (float)') + + if int(data['id']) in ids: + spawn_poses[data['id']] = {'x' : float(data['x']), 'y' : float(data['y']), 'z' : float(data['z']), 'heading' : float(data['heading'])} + # #} + + else: + raise SuffixError(f'Incorrect file type! Suffix must be either ".csv" or ".yaml"') + + if len(spawn_poses.keys()) != len(ids) or set(spawn_poses.keys()) != set(ids): + raise WrongNumberOfArguments(f'File "{filename}" does not specify poses for all robots!') + + rospy.loginfo(f'[MrsDroneSpawner]: Spawn poses returned: {spawn_poses}') + return spawn_poses + # #} + + # #{ get_spawn_poses_from_args(self, pos_args, ids) + def get_spawn_poses_from_args(self, pos_args, ids): + ''' + Parses the input args extracts spawn poses for vehicles. + If more vehicles are spawned at the same time, the given pose is used for the first vehicle. + Additional vehicles are spawned with an offset of {config param: gazebo_models/spacing} meters in X + + :param pos_args: a list of 4 numbers [x,y,z,heading] + :param ids: a list of ints containing unique vehicle IDs + :return: a dict in format {id: {'x': pos_x, 'y', pos_y, 'z': pos_z, 'heading': pos_heading}} + + Raises: + WrongNumberOfArguments - pos_args does not contain exactly 4 values + ValueError - input cannot be converted into numbers + ''' + spawn_poses = {} + if len(pos_args) != 4: + raise WrongNumberOfArguments(f'Expected exactly 4 args after keyword "--pos", got {len(pos_args)}') + + x = float(pos_args[0]) + y = float(pos_args[1]) + z = float(pos_args[2]) + heading = float(pos_args[3]) + + spawn_poses[ids[0]] = {'x': x, 'y': y, 'z': z, 'heading': heading} + + if len(ids) > 1: + rospy.logwarn(f'[MrsDroneSpawner]: Spawning more than one vehicle with "--pos". Each additional vehicle will be offset by {self.model_spacing} meters in X') + for i in range(len(ids)): + x += self.model_spacing + spawn_poses[ids[i]] = {'x': x, 'y': y, 'z': z, 'heading': heading} + + rospy.loginfo(f'[MrsDroneSpawner]: Spawn poses returned: {spawn_poses}') + return spawn_poses + # #} + + # #{ get_randomized_spawn_poses(self, ids) + def get_randomized_spawn_poses(self, ids): + ''' + Creates randomized spawn poses for all vehicles. + The poses are generated with spacing defined by config param: gazebo_models/spacing + Height is always set to 0.3 + + :param ids: a list of ints containing unique vehicle IDs + :return: a dict in format {id: {'x': pos_x, 'y', pos_y, 'z': pos_z, 'heading': pos_heading}} + ''' + spawn_poses = {} + + circle_diameter = 0.0 + total_positions_in_current_circle = 0; + angle_increment = 0; + remaining_positions_in_current_circle = 1; + circle_perimeter= math.pi * circle_diameter + random_angle_offset = 0 + random_x_offset = round(random.uniform(-self.model_spacing, self.model_spacing), 2) + random_y_offset = round(random.uniform(-self.model_spacing, self.model_spacing), 2) + + for ID in ids: + if remaining_positions_in_current_circle == 0: + circle_diameter = circle_diameter + self.model_spacing + circle_perimeter= math.pi*circle_diameter + total_positions_in_current_circle = math.floor(circle_perimeter / self.model_spacing) + remaining_positions_in_current_circle = total_positions_in_current_circle + angle_increment = (math.pi * 2) / total_positions_in_current_circle + random_angle_offset = round(random.uniform(-math.pi,math.pi), 2) + + x = round(math.sin(angle_increment * remaining_positions_in_current_circle + random_angle_offset) * circle_diameter, 2) + random_x_offset + y = round(math.cos(angle_increment * remaining_positions_in_current_circle + random_angle_offset) * circle_diameter, 2) + random_y_offset + z = 0.3 + heading = round(random.uniform(-math.pi,math.pi), 2) + remaining_positions_in_current_circle = remaining_positions_in_current_circle - 1 + spawn_poses[ID] = {'x': x, 'y': y, 'z': z, 'heading': heading} + + rospy.loginfo(f'[MrsDroneSpawner]: Spawn poses returned: {spawn_poses}') + return spawn_poses + # #} + + # #{ get_jinja_params_for_one_robot(self, params_dict, index, ID) + def get_jinja_params_for_one_robot(self, params_dict, index, ID): + '''Makes a deep copy of params dict, removes entries of other robots, assigns mavlink ports + :param index: index of the robot in the input sequence + :param ID: ID of the robot, should match the value in params_dict['ids'][index] + :return: a dict of params to be used in rendering the jinja template + ''' + + robot_params = copy.deepcopy(params_dict) + robot_params['ID'] = ID + robot_params['name'] = params_dict['names'][index] + robot_params['spawn_pose'] = params_dict['spawn_poses'][ID] + + del robot_params['names'] + del robot_params['help'] + del robot_params['ids'] + del robot_params['spawn_poses'] + + robot_params['mavlink_config'] = self.get_mavlink_config_for_robot(ID) + + return robot_params + # #} + + # #{ get_mavlink_config_for_robot(self, ID) + def get_mavlink_config_for_robot(self, ID): + '''Creates a mavlink port configuration based on default values offset by ID + + NOTE: The offsets have to match values assigned in px4-rc.* files located in mrs_uav_gazebo_simulation/ROMFS/px4fmu_common/init.d-posix!! + + ''' + mavlink_config = {} + udp_offboard_port_remote = self.vehicle_base_port + (4 * ID) + 2 + udp_offboard_port_local = self.vehicle_base_port + (4 * ID) + 1 + mavlink_config['udp_offboard_port_remote'] = udp_offboard_port_remote + mavlink_config['udp_offboard_port_local'] = udp_offboard_port_local + mavlink_config['mavlink_tcp_port'] = self.mavlink_tcp_base_port + ID + mavlink_config['mavlink_udp_port'] = self.mavlink_udp_base_port + ID + mavlink_config['qgc_udp_port'] = self.qgc_udp_port + mavlink_config['sdk_udp_port'] = self.sdk_udp_port + mavlink_config['send_vision_estimation'] = int(self.send_vision_estimation) + mavlink_config['send_odometry'] = int(self.send_odometry) + mavlink_config['enable_lockstep'] = int(self.enable_lockstep) + mavlink_config['use_tcp'] = int(self.use_tcp) + mavlink_config['fcu_url'] = f'udp://127.0.0.1:{udp_offboard_port_remote}@127.0.0.1:{udp_offboard_port_local}' + + return mavlink_config + # #} + + # -------------------------------------------------------------- + # | Launching subprocesses | + # -------------------------------------------------------------- + + # #{ launch_px4_firmware(self, robot_params) + def launch_px4_firmware(self, robot_params): + name = robot_params['name'] + rospy.loginfo(f'[MrsDroneSpawner]: Launching PX4 firmware for {name}') + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch.configure_logging(uuid) + + roslaunch_args = [ + 'ID:=' + str(robot_params['ID']), + 'PX4_SIM_MODEL:=' + str(robot_params['model']) + ] + roslaunch_sequence = [(self.px4_fimrware_launch_path, roslaunch_args)] + + firmware_process = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_sequence) + + try: + firmware_process.start() + except: + rospy.logerr(f'[MrsDroneSpawner]: Could not launch PX4 firmware for {name}') + return None + + rospy.loginfo(f'[MrsDroneSpawner]: PX4 firmware for {name} launched') + + return firmware_process + # #} + + # #{ launch_mavros(self, robot_params) + def launch_mavros(self, robot_params): + name = robot_params['name'] + rospy.loginfo(f'[MrsDroneSpawner]: Launching mavros for {name}') + + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch.configure_logging(uuid) + + roslaunch_args = [ + 'ID:=' + str(robot_params['ID']), + 'fcu_url:=' + str(robot_params['mavlink_config']['fcu_url']), + 'vehicle:=' + str(robot_params['model']) # TODO: rename this to PX4_SIM_MODEL in the mavros launch file? + ] + + roslaunch_sequence = [(self.mavros_launch_path, roslaunch_args)] + mavros_process = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_sequence) + + try: + mavros_process.start() + except: + rospy.logerr(f'[MrsDroneSpawner]: Could not launch mavros for {name}') + return None + + rospy.loginfo(f'[MrsDroneSpawner]: Mavros for {name} launched') + + return mavros_process + # #} + + # #{ spawn_gazebo_model(self, robot_params) + def spawn_gazebo_model(self, robot_params): + name = robot_params['name'] + sdf_content = self.render(robot_params) + + if sdf_content is None: + return False + + if self.save_sdf_files: + fd, filepath = tempfile.mkstemp(prefix='mrs_drone_spawner_' + datetime.datetime.now().strftime("%Y_%m_%d__%H_%M_%S_"), suffix='_' + str(robot_params['model']) + '_' + str(name) + '.sdf') + with os.fdopen(fd, 'w') as output_file: + output_file.write(sdf_content) + rospy.loginfo(f'[MrsDroneSpawner]: Model for {name} written to {filepath}') + + spawn_pose = Pose() + spawn_pose.position.x = robot_params['spawn_pose']['x'] + spawn_pose.position.y = robot_params['spawn_pose']['y'] + spawn_pose.position.z = robot_params['spawn_pose']['z'] + spawn_pose.orientation.w = math.cos(robot_params['spawn_pose']['heading'] / 2.0) + spawn_pose.orientation.x = 0 + spawn_pose.orientation.y = 0 + spawn_pose.orientation.z = math.sin(robot_params['spawn_pose']['heading'] / 2.0) + + rospy.loginfo(f'[MrsDroneSpawner]: Spawning gazebo model for {name}') + try: + response = self.gazebo_spawn_proxy(name, sdf_content, name, spawn_pose, "") + if not response.success: + rospy.logerr(f'MrsDroneSpawner]: While calling {self.gazebo_spawn_proxy.resolved_name}: {response.status_message}') + return False + except rospy.service.ServiceException as e: + rospy.logerr(f'[MrsDroneSpawner]: While calling {self.gazebo_spawn_proxy.resolved_name}: {e}') + return False + + return True + # #} + +if __name__ == '__main__': + + print('[INFO] [MrsDroneSpawner]: Starting') + + verbose = 'verbose' in sys.argv + + atexit.register(exit_handler) + + try: + spawner = MrsDroneSpawner(verbose) + except rospy.ROSInterruptException: + pass diff --git a/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/template_wrapper.py b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/template_wrapper.py new file mode 100644 index 0000000..bd5c879 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/template_wrapper.py @@ -0,0 +1,18 @@ +class TemplateWrapper: + + def __init__(self, jinja_template, imported_templates, components): + ''' + A wrapper for jinja templates for storing variables specific to the mrs_drone_spawner + :param jinja_template: a jinja2.Template with a parsed content of the template file + :param imported_templates: a list of references to other jinja2.Template files that are directly imported by this template + :param components: a dict of modular components in format "{name (string): component (component_wrapper.ComponentWrapper)}", + where name is the name of a jinja macro. Contains all components accessible from this template, including imported ones from other template snippets + ''' + self.jinja_template = jinja_template + self.imported_templates = imported_templates + self.components = components + + def __eq__(self, other): + if isinstance(other, TemplateWrapper): + return self.jinja_template.filename == other.jinja_template.filename + return False diff --git a/ros_packages/mrs_uav_gazebo_simulation/scripts/utils.py b/ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/utils.py similarity index 100% rename from ros_packages/mrs_uav_gazebo_simulation/scripts/utils.py rename to ros_packages/mrs_uav_gazebo_simulation/scripts/mrs_drone_spawner/utils.py diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/CMakeLists.txt b/ros_packages/mrs_uav_gazebo_simulation/test/CMakeLists.txt index 5084ddf..49d9d9c 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/test/CMakeLists.txt +++ b/ros_packages/mrs_uav_gazebo_simulation/test/CMakeLists.txt @@ -1,3 +1,25 @@ find_package(rostest REQUIRED) -add_subdirectory(takeoff) +### WARNING +### NEVER name your test build unit the same as the folder that it is placed in! +### e.g., this will cause problems: +### add_rostest_gtest(my_test +### my_test/basic.test +### my_test/test.cpp +### ) + +# basic test + +add_rostest_gtest(test_basic + basic/basic.test + basic/test.cpp + ) + +target_link_libraries(test_basic + ${catkin_LIBRARIES} + ) + +add_dependencies(test_basic + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/all_tests.sh b/ros_packages/mrs_uav_gazebo_simulation/test/all_tests.sh index 6614ec3..676421e 100755 --- a/ros_packages/mrs_uav_gazebo_simulation/test/all_tests.sh +++ b/ros_packages/mrs_uav_gazebo_simulation/test/all_tests.sh @@ -5,30 +5,31 @@ set -e trap 'last_command=$current_command; current_command=$BASH_COMMAND' DEBUG trap 'echo "$0: \"${last_command}\" command failed with exit code $?"' ERR -ORIGINAL_PATH=`pwd` - -while [ ! -e ".catkin_tools" ]; do - cd .. - if [[ `pwd` == "/" ]]; then - # we reached the root and didn't find the build/COLCON_IGNORE file - that's a fail! - echo "$0: could not find the root of the current workspace". - return 1 - fi -done +PACKAGE="mrs_uav_gazebo_simulation" +VERBOSE=1 -cd build +[ "$VERBOSE" = "0" ] && TEXT_OUTPUT="" +[ "$VERBOSE" = "1" ] && TEXT_OUTPUT="-t" -OLD_FILES=$(find . -name "*.gcda") +# build the package +catkin build $PACKAGE # it has to be fully built normally before building with --catkin-make-args tests +catkin build $PACKAGE --no-deps --catkin-make-args tests -for FILE in $OLD_FILES; do - echo "$0: removing old coverage file '$FILE'" - rm $FILE -done +TEST_FILES=$(find . -name "*.test" -type f -printf "%f\n") -cd $ORIGINAL_PATH +for TEST_FILE in `echo $TEST_FILES`; do -# build the package -catkin build --this # it has to be fully built normally before building with --catkin-make-args tests -catkin build --this --no-deps --catkin-make-args tests + echo "$0: running test '$TEST_FILE'" -catkin test --this -i -p 1 -s + # folder for test results + TEST_RESULT_PATH=$(realpath /tmp/$RANDOM) + mkdir -p $TEST_RESULT_PATH + + # run the test + rostest $PACKAGE $TEST_FILE $TEXT_OUTPUT --results-filename=$PACKAGE.test --results-base-dir="$TEST_RESULT_PATH" + + # evaluate the test results + echo test result path is $TEST_RESULT_PATH + catkin_test_results "$TEST_RESULT_PATH" + +done diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/basic/basic.test b/ros_packages/mrs_uav_gazebo_simulation/test/basic/basic.test new file mode 100644 index 0000000..b697bb9 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/test/basic/basic.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/config/custom_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/test/basic/config/custom_config.yaml similarity index 82% rename from ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/config/custom_config.yaml rename to ros_packages/mrs_uav_gazebo_simulation/test/basic/config/custom_config.yaml index 4b0275a..9f6bbb8 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/config/custom_config.yaml +++ b/ros_packages/mrs_uav_gazebo_simulation/test/basic/config/custom_config.yaml @@ -9,8 +9,8 @@ mrs_uav_managers: # loaded state estimator plugins state_estimators: [ - "gps_garmin", + "gps_baro", ] - initial_state_estimator: "gps_garmin" # will be used as the first state estimator + 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) diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/basic/config/network_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/test/basic/config/network_config.yaml new file mode 100644 index 0000000..08f370d --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/test/basic/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/test/basic/config/world_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/test/basic/config/world_config.yaml new file mode 100644 index 0000000..9b55067 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/test/basic/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/test/basic/launch/mrs_uav_system.launch b/ros_packages/mrs_uav_gazebo_simulation/test/basic/launch/mrs_uav_system.launch new file mode 100644 index 0000000..0efb4f1 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/test/basic/launch/mrs_uav_system.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/basic/test.cpp b/ros_packages/mrs_uav_gazebo_simulation/test/basic/test.cpp new file mode 100644 index 0000000..e605ac5 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/test/basic/test.cpp @@ -0,0 +1,242 @@ +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +/* class Tester //{ */ + +class Tester { + +public: + Tester(); + + bool test(); + +private: + ros::NodeHandle nh_; + std::shared_ptr spinner_; + + mrs_lib::SubscribeHandler sh_control_manager_diag_; + mrs_lib::SubscribeHandler sh_estim_manager_diag_; + mrs_lib::SubscribeHandler sh_spawner_diagnostics_; + mrs_lib::SubscribeHandler sh_can_takeoff; + + mrs_lib::ServiceClientHandler sch_arming_; + mrs_lib::ServiceClientHandler sch_offboard_; + + mrs_lib::ServiceClientHandler sch_spawn_; +}; + +//} + +/* Tester() //{ */ + +Tester::Tester() { + + // | ------------------ initialize test node ------------------ | + + nh_ = ros::NodeHandle("~"); + + ROS_INFO("[%s]: ROS node initialized", ros::this_node::getName().c_str()); + + ros::Time::waitForValid(); + + spinner_ = std::make_shared(4); + spinner_->start(); + + std::string uav_name = "uav1"; + + // | ----------------------- subscribers ---------------------- | + + mrs_lib::SubscribeHandlerOptions shopts; + shopts.nh = nh_; + shopts.node_name = "Test"; + shopts.no_message_timeout = mrs_lib::no_timeout; + shopts.threadsafe = true; + shopts.autostart = true; + shopts.queue_size = 10; + shopts.transport_hints = ros::TransportHints().tcpNoDelay(); + + sh_control_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/control_manager/diagnostics"); + sh_estim_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/estimation_manager/diagnostics"); + sh_can_takeoff = mrs_lib::SubscribeHandler(shopts, "/" + uav_name + "/automatic_start/can_takeoff"); + sh_spawner_diagnostics_ = mrs_lib::SubscribeHandler(shopts, "/mrs_drone_spawner/diagnostics"); + + ROS_INFO("[%s]: subscribers initialized", ros::this_node::getName().c_str()); + + // | --------------------- service clients -------------------- | + + sch_arming_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/hw_api/arming"); + sch_offboard_ = mrs_lib::ServiceClientHandler(nh_, "/" + uav_name + "/hw_api/offboard"); + sch_spawn_ = mrs_lib::ServiceClientHandler(nh_, "/mrs_drone_spawner/spawn"); + + ROS_INFO("[%s]: service client initialized", ros::this_node::getName().c_str()); +} + +//} + +/* test() //{ */ + +bool Tester::test() { + + // | ------------ wait for the spawner diagnostics ------------ | + + while (ros::ok()) { + + ROS_INFO_THROTTLE(1.0, "[%s]: waiting for spawner diagnostics", ros::this_node::getName().c_str()); + + if (sh_spawner_diagnostics_.hasMsg()) { + break; + } + + ros::Duration(0.01).sleep(); + } + + // | -------------------------- spawn ------------------------- | + + ROS_INFO("[%s]: spawning the drone", ros::this_node::getName().c_str()); + + { + mrs_msgs::String spawn; + spawn.request.value = "1 --x500 --enable-rangefinder"; + + { + bool service_call = sch_spawn_.call(spawn); + + if (!service_call || !spawn.response.success) { + ROS_ERROR("[%s]: spawning failed", ros::this_node::getName().c_str()); + return false; + } + } + } + + // | ------------------- wait for the spawn ------------------- | + + while (ros::ok()) { + + ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the spawn", ros::this_node::getName().c_str()); + + if (!sh_spawner_diagnostics_.getMsg()->processing) { + break; + } + + ros::Duration(0.01).sleep(); + } + + ROS_INFO("[%s]: The UAV is ready", ros::this_node::getName().c_str()); + + ros::Duration(1.0).sleep(); + + // | ---------------- wait for ready to takeoff --------------- | + + while (ros::ok()) { + + ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", ros::this_node::getName().c_str()); + + if (sh_control_manager_diag_.hasMsg() && sh_estim_manager_diag_.hasMsg()) { + break; + } + + ros::Duration(0.01).sleep(); + } + + ROS_INFO("[%s]: MRS UAV System is ready", ros::this_node::getName().c_str()); + + ros::Duration(1.0).sleep(); + + // | ---------------------- arm the drone --------------------- | + + ROS_INFO("[%s]: arming the edrone", ros::this_node::getName().c_str()); + + { + std_srvs::SetBool arming; + arming.request.data = true; + + { + bool service_call = sch_arming_.call(arming); + + if (!service_call || !arming.response.success) { + ROS_ERROR("[%s]: arming service call failed", ros::this_node::getName().c_str()); + return false; + } + } + } + + // | -------------------------- wait -------------------------- | + + ros::Duration(0.2).sleep(); + + // | -------------------- trigger offboard -------------------- | + + ROS_INFO("[%s]: triggering offboard", ros::this_node::getName().c_str()); + + { + std_srvs::Trigger trigger; + + { + bool service_call = sch_offboard_.call(trigger); + + if (!service_call || !trigger.response.success) { + ROS_ERROR("[%s]: offboard service call failed", ros::this_node::getName().c_str()); + return false; + } + } + } + + // | -------------- wait for the takeoff finished ------------- | + + while (ros::ok()) { + + ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the takeoff to finish", ros::this_node::getName().c_str()); + + if (sh_control_manager_diag_.getMsg()->flying_normally) { + + ROS_INFO("[%s]: finished", ros::this_node::getName().c_str()); + + return true; + } + + ros::Duration(0.01).sleep(); + } + + return false; +} + +//} + +std::shared_ptr tester_; + +TEST(TESTSuite, takeoff) { + + bool result = tester_->test(); + + if (result) { + GTEST_SUCCEED(); + } else { + GTEST_FAIL(); + } +} + +int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { + + ros::init(argc, argv, "takeoff_test"); + + tester_ = std::make_shared(); + + testing::InitGoogleTest(&argc, argv); + + Tester tester; + + return RUN_ALL_TESTS(); +} diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/compile_tests.sh b/ros_packages/mrs_uav_gazebo_simulation/test/compile_tests.sh deleted file mode 100755 index 999699e..0000000 --- a/ros_packages/mrs_uav_gazebo_simulation/test/compile_tests.sh +++ /dev/null @@ -1,10 +0,0 @@ -#!/bin/bash - -set -e - -trap 'last_command=$current_command; current_command=$BASH_COMMAND' DEBUG -trap 'echo "$0: \"${last_command}\" command failed with exit code $?"' ERR - -# build the package -catkin build --this # it has to be fully built normally before building with --catkin-make-args tests -catkin build --this --no-deps --catkin-make-args tests diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/coverage.sh b/ros_packages/mrs_uav_gazebo_simulation/test/coverage.sh new file mode 100755 index 0000000..db8ab5d --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/test/coverage.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +PACKAGE_NAME=mrs_uav_gazebo_simulation + +while [ ! -e ".catkin_tools" ]; do + cd .. + if [[ `pwd` == "/" ]]; then + # we reached the root and didn't find the build/COLCON_IGNORE file - that's a fail! + echo "$0: could not find the root of the current workspace". + return 1 + fi +done + +cd build + +lcov --capture --directory . --output-file coverage.info +lcov --remove coverage.info "*/test/*" --output-file coverage.info.removed +lcov --extract coverage.info.removed "*/src/*${PACKAGE_NAME}/*" --output-file coverage.info.cleaned +genhtml --title "${PACKAGE_NAME} - Test coverage report" --demangle-cpp --legend --frames --show-details -o coverage_html coverage.info.cleaned | tee /tmp/genhtml.log + +COVERAGE_PCT=`cat /tmp/genhtml.log | tail -n 1 | awk '{print $2}'` + +echo "Coverage: $COVERAGE_PCT" + +xdg-open coverage_html/index.html diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/single_test.sh b/ros_packages/mrs_uav_gazebo_simulation/test/single_test.sh new file mode 100755 index 0000000..e8abc69 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/test/single_test.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +set -e + +trap 'last_command=$current_command; current_command=$BASH_COMMAND' DEBUG +trap 'echo "$0: \"${last_command}\" command failed with exit code $?"' ERR + +PACKAGE="mrs_uav_gazebo_simulation" +VERBOSE=1 + +[ "$VERBOSE" = "0" ] && TEXT_OUTPUT="" +[ "$VERBOSE" = "1" ] && TEXT_OUTPUT="-t" + +# build the package +catkin build $PACKAGE # it has to be fully built normally before building with --catkin-make-args tests +catkin build $PACKAGE --no-deps --catkin-make-args tests + +# folder for test results +TEST_RESULT_PATH=$(realpath /tmp/$RANDOM) +mkdir -p $TEST_RESULT_PATH + +# run the test +rostest ./basic/basic.test $TEXT_OUTPUT --results-filename=$PACKAGE.test --results-base-dir="$TEST_RESULT_PATH" + +# evaluate the test results +echo test result path is $TEST_RESULT_PATH +catkin_test_results "$TEST_RESULT_PATH" diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/CMakeLists.txt b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/CMakeLists.txt deleted file mode 100644 index 328847d..0000000 --- a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -add_subdirectory(takeoff_gps_garmin) - -add_subdirectory(takeoff_gps_baro) diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/CMakeLists.txt b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/CMakeLists.txt deleted file mode 100644 index bf8f985..0000000 --- a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) - -catkin_add_executable_with_gtest(test_${TEST_NAME} - test.cpp - ) - -target_link_libraries(test_${TEST_NAME} - ${catkin_LIBRARIES} - ) - -add_dependencies(test_${TEST_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) - -add_rostest(${TEST_NAME}.test) diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/config/custom_config.yaml b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/config/custom_config.yaml deleted file mode 100644 index cd90283..0000000 --- a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/config/custom_config.yaml +++ /dev/null @@ -1,16 +0,0 @@ -# GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: -## -------------------------------------------------------------- -## | rosrun mrs_uav_core get_public_params.py # -## -------------------------------------------------------------- - -mrs_uav_managers: - - estimation_manager: - - # loaded state estimator plugins - state_estimators: [ - "gps_baro", - ] - - initial_state_estimator: "gps_baro" # will be used as the first state estimator - agl_height_estimator: "" # only slightly filtered height for checking min height (not used in control feedback) diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/takeoff_gps_baro.test b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/takeoff_gps_baro.test deleted file mode 100644 index 536da84..0000000 --- a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/takeoff_gps_baro.test +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/test.cpp b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/test.cpp deleted file mode 100644 index 6eb2ab8..0000000 --- a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_baro/test.cpp +++ /dev/null @@ -1,62 +0,0 @@ -#include - -#include - -class Tester : public mrs_uav_testing::TestGeneric { - -public: - bool test(); -}; - -bool Tester::test() { - - { - auto [success, message] = spawnGazeboUav(); - - if (!success) { - ROS_ERROR("[%s]: gazebo UAV spawning failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); - return false; - } - } - - { - auto [success, message] = takeoff(); - - if (!success) { - ROS_ERROR("[%s]: takeoff failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); - return false; - } - } - - this->sleep(5.0); - - if (this->isFlyingNormally()) { - return true; - } else { - ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); - return false; - } -} - - -TEST(TESTSuite, test) { - - Tester tester; - - bool result = tester.test(); - - if (result) { - GTEST_SUCCEED(); - } else { - GTEST_FAIL(); - } -} - -int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { - - ros::init(argc, argv, "test"); - - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/CMakeLists.txt b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/CMakeLists.txt deleted file mode 100644 index bf8f985..0000000 --- a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) - -catkin_add_executable_with_gtest(test_${TEST_NAME} - test.cpp - ) - -target_link_libraries(test_${TEST_NAME} - ${catkin_LIBRARIES} - ) - -add_dependencies(test_${TEST_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) - -add_rostest(${TEST_NAME}.test) diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/takeoff_gps_garmin.test b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/takeoff_gps_garmin.test deleted file mode 100644 index ba16e3a..0000000 --- a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/takeoff_gps_garmin.test +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/test.cpp b/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/test.cpp deleted file mode 100644 index 6eb2ab8..0000000 --- a/ros_packages/mrs_uav_gazebo_simulation/test/takeoff/takeoff_gps_garmin/test.cpp +++ /dev/null @@ -1,62 +0,0 @@ -#include - -#include - -class Tester : public mrs_uav_testing::TestGeneric { - -public: - bool test(); -}; - -bool Tester::test() { - - { - auto [success, message] = spawnGazeboUav(); - - if (!success) { - ROS_ERROR("[%s]: gazebo UAV spawning failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); - return false; - } - } - - { - auto [success, message] = takeoff(); - - if (!success) { - ROS_ERROR("[%s]: takeoff failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); - return false; - } - } - - this->sleep(5.0); - - if (this->isFlyingNormally()) { - return true; - } else { - ROS_ERROR("[%s]: not flying normally", ros::this_node::getName().c_str()); - return false; - } -} - - -TEST(TESTSuite, test) { - - Tester tester; - - bool result = tester.test(); - - if (result) { - GTEST_SUCCEED(); - } else { - GTEST_FAIL(); - } -} - -int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { - - ros::init(argc, argv, "test"); - - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/ros_packages/mrs_uav_gazebo_simulation/test_launch.sh b/ros_packages/mrs_uav_gazebo_simulation/test_launch.sh new file mode 100755 index 0000000..8b7ca45 --- /dev/null +++ b/ros_packages/mrs_uav_gazebo_simulation/test_launch.sh @@ -0,0 +1,2 @@ +#!/usr/bin/bash +roslaunch mrs_uav_gazebo_simulation mrs_drone_spawner.launch custom_config:=/home/mrs/devel_workspace/src/mrs_uav_gazebo_simulation/ros_packages/mrs_uav_gazebo_simulation/config/spawner_custom_config.yaml diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone/session.yml b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone/session.yml index 04ccd98..255c322 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone/session.yml +++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone/session.yml @@ -16,7 +16,7 @@ windows: layout: tiled panes: - waitForRos; roslaunch mrs_uav_gazebo_simulation simulation.launch world_name:=grass_plane gui:=true - - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 $UAV_TYPE --enable-rangefinder" + - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 --$UAV_TYPE --enable-rangefinder" - waitForControl; gz camera -c gzclient_camera -f $UAV_NAME; history -s gz camera -c gzclient_camera -f $UAV_NAME - status: layout: tiled 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 index 67ddffc..41e9d96 100644 --- 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 @@ -16,7 +16,7 @@ windows: layout: tiled panes: - waitForRos; roslaunch mrs_uav_gazebo_simulation simulation.launch world_name:=forest gui:=true - - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 $UAV_TYPE --enable-rangefinder --enable-ouster --ouster-model OS0-128 --use-gpu-ray --horizontal-samples 128 --rotation-freq 10" + - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 --$UAV_TYPE --enable-rangefinder --enable-ouster model:=OS0-128 use_gpu:=True horizontal_samples:=128 update_rate:=10" - waitForControl; gz camera -c gzclient_camera -f $UAV_NAME; history -s gz camera -c gzclient_camera -f $UAV_NAME - status: layout: tiled diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_realsense/session.yml b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_realsense/session.yml index 00e3a3a..4602e37 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_realsense/session.yml +++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_realsense/session.yml @@ -16,7 +16,7 @@ windows: layout: tiled panes: - waitForRos; roslaunch mrs_uav_gazebo_simulation simulation.launch world_name:=mrs_city gui:=true - - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 $UAV_TYPE --enable-rangefinder --enable-realsense-front" + - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 --$UAV_TYPE --enable-rangefinder --enable-realsense-front" - waitForControl; gz camera -c gzclient_camera -f $UAV_NAME; history -s gz camera -c gzclient_camera -f $UAV_NAME - status: layout: tiled diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_vio/session.yml b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_vio/session.yml index e4053bf..ba26d21 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_vio/session.yml +++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/one_drone_vio/session.yml @@ -16,7 +16,7 @@ windows: layout: tiled panes: - waitForRos; roslaunch mrs_uav_gazebo_simulation simulation.launch world_name:=mrs_city gui:=true - - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 $UAV_TYPE --enable-rangefinder --enable-vio" + - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 --$UAV_TYPE --enable-rangefinder --enable-vio" - waitForControl; gz camera -c gzclient_camera -f $UAV_NAME; history -s gz camera -c gzclient_camera -f $UAV_NAME - status: layout: tiled diff --git a/ros_packages/mrs_uav_gazebo_simulation/tmux/three_drones/session.yml b/ros_packages/mrs_uav_gazebo_simulation/tmux/three_drones/session.yml index 05d4564..0dcf85a 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/tmux/three_drones/session.yml +++ b/ros_packages/mrs_uav_gazebo_simulation/tmux/three_drones/session.yml @@ -16,7 +16,7 @@ windows: layout: tiled panes: - waitForRos; roslaunch mrs_uav_gazebo_simulation simulation.launch world_name:=grass_plane gui:=true - - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 2 3 $UAV_TYPE --enable-rangefinder" + - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 2 3 --$UAV_TYPE --enable-rangefinder" - export UAV_NAME=uav1; waitForControl; gz camera -c gzclient_camera -f $UAV_NAME; history -s gz camera -c gzclient_camera -f $UAV_NAME - status: layout: tiled