diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja index 6239b30..b8c2eb5 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja @@ -1927,7 +1927,7 @@ limitations under the License. - {{ tilt_update_rate }} + {{ tilt_update_rate }} {{ max_pitch_rate }} {{ max_pitch }} {{ min_pitch }} 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/f450.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f450.sdf.jinja index 866d510..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 @@ -1049,13 +1049,13 @@ offset_roll_link_roll = 0.0, offset_roll_link_yaw = 0.0, offset_roll_link_pitch = 0.0, - tilt_update_rate = 5.0, + tilt_update_rate = 30.0, min_pitch = -1.57, max_pitch = 1.57, min_roll = -0.6, max_roll = 0.6, - max_pitch_rate = 0.1, - max_roll_rate = 0.1, + max_pitch_rate = 0.3, + max_roll_rate = 0.3, camera_update_rate = 30, horizontal_fov = 1.92, img_width = 1920, 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 2c67141..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 @@ -1169,13 +1169,13 @@ offset_roll_link_roll = 0.0, offset_roll_link_yaw = 0.0, offset_roll_link_pitch = 0.0, - tilt_update_rate = 5.0, + tilt_update_rate = 30.0, min_pitch = -1.57, max_pitch = 1.57, min_roll = -0.6, max_roll = 0.6, - max_pitch_rate = 0.1, - max_roll_rate = 0.1, + max_pitch_rate = 0.3, + max_roll_rate = 0.3, camera_update_rate = 30, horizontal_fov = 1.92, img_width = 1920, 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 178e84e..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 @@ -497,7 +497,7 @@ offset_roll_link_roll = 0.0, offset_roll_link_yaw = 0.0, offset_roll_link_pitch = 0.0, - tilt_update_rate = 5.0, + tilt_update_rate = 30.0, min_pitch = -1.57, max_pitch = 1.57, min_roll = -0.6, 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 2a23fa5..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 @@ -647,7 +647,7 @@ offset_roll_link_roll = 0.0, offset_roll_link_yaw = 0.0, offset_roll_link_pitch = 0.0, - tilt_update_rate = 5.0, + tilt_update_rate = 30.0, min_pitch = -1.046, max_pitch = 1.57, min_roll = -0.6, 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 932a6c1..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 @@ -893,13 +893,13 @@ offset_roll_link_roll = 0.0, offset_roll_link_yaw = 0.0, offset_roll_link_pitch = 0.0, - tilt_update_rate = 5.0, + tilt_update_rate = 30.0, min_pitch = -1.57, max_pitch = 1.57, min_roll = -0.6, max_roll = 0.6, - max_pitch_rate = 0.1, - max_roll_rate = 0.1, + max_pitch_rate = 0.3, + max_roll_rate = 0.3, camera_update_rate = 30, horizontal_fov = 1.92, img_width = 1920,