diff --git a/config/spawner_params.yaml b/config/spawner_params.yaml index 97fa0dd..d62d079 100644 --- a/config/spawner_params.yaml +++ b/config/spawner_params.yaml @@ -15,3 +15,7 @@ mrs_drone_spawner: firmware_launch_delay: 10.0 # [s] after spawn, wait before launching px4 firmware extra_resource_paths: [] + + tf_static_publisher: + base_link: "fcu" + ignored_sensor_links: ["air_pressure_sensor", "magnetometer_sensor", "navsat_sensor", "imu_sensor", "lidar"] #lidar is reserved for PX4 Garmin diff --git a/config/uav_ros_gz_bridge_config.jinja.yaml b/config/uav_ros_gz_bridge_config.jinja.yaml deleted file mode 100644 index 33e01bc..0000000 --- a/config/uav_ros_gz_bridge_config.jinja.yaml +++ /dev/null @@ -1,8 +0,0 @@ -# gz topic published by Sensors plugin (Camera) -{% for topic in camera_info_topic_list %} -- ros_topic_name: "{{ topic }}" - gz_topic_name: "{{ topic }}" - ros_type_name: "sensor_msgs/msg/CameraInfo" - gz_type_name: "gz.msgs.CameraInfo" - direction: GZ_TO_ROS -{% endfor %} diff --git a/config/uav_ros_gz_bridge_config.yaml.jinja b/config/uav_ros_gz_bridge_config.yaml.jinja new file mode 100644 index 0000000..3d63f50 --- /dev/null +++ b/config/uav_ros_gz_bridge_config.yaml.jinja @@ -0,0 +1,23 @@ +{%- for topic in camera_info_topic_list -%} +- ros_topic_name: "{{ topic }}" + gz_topic_name: "{{ topic }}" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "gz.msgs.CameraInfo" + direction: GZ_TO_ROS +{%- endfor -%} + +{%- for topic in twoD_lidar_topic_list -%} +- ros_topic_name: "{{ topic }}" + gz_topic_name: "{{ topic }}" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "gz.msgs.LaserScan" + direction: GZ_TO_ROS +{%- endfor -%} + +{%- for topic in threeD_lidar_topic_list -%} +- ros_topic_name: "{{ topic }}/points" + gz_topic_name: "{{ topic }}/points" + ros_type_name: "sensor_msgs/msg/PointCloud2" + gz_type_name: "gz.msgs.PointCloudPacked" + direction: GZ_TO_ROS +{%- endfor -%} diff --git a/launch/mrs_drone_spawner.launch.py b/launch/mrs_drone_spawner.launch.py index 80397ac..9d7e498 100755 --- a/launch/mrs_drone_spawner.launch.py +++ b/launch/mrs_drone_spawner.launch.py @@ -69,4 +69,4 @@ def generate_launch_description(): ) ) - return ld + return ld \ No newline at end of file diff --git a/launch/simulation.launch.py b/launch/simulation.launch.py index 6371095..5c425a8 100755 --- a/launch/simulation.launch.py +++ b/launch/simulation.launch.py @@ -147,4 +147,4 @@ def generate_launch_description(): bridge, drone_spawner, ] - ) + ) \ No newline at end of file diff --git a/launch/uav_ros_gz_bridge.launch.py b/launch/uav_ros_gz_bridge.launch.py index d3cd0b5..8e91acb 100755 --- a/launch/uav_ros_gz_bridge.launch.py +++ b/launch/uav_ros_gz_bridge.launch.py @@ -97,5 +97,5 @@ def generate_launch_description(): declare_ros_gz_bridge_config_arg, declare_ros_gz_image_topics_arg, declare_bridge_debug_arg, - namespace_group, + namespace_group ]) diff --git a/models/mrs_robots_description/sdf/component_snippets.sdf.jinja b/models/mrs_robots_description/sdf/component_snippets.sdf.jinja deleted file mode 100644 index a02e6c9..0000000 --- a/models/mrs_robots_description/sdf/component_snippets.sdf.jinja +++ /dev/null @@ -1,3023 +0,0 @@ - - - -{%- 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, use_mrs_plugin, prop_ixx, prop_ixy, prop_ixz, prop_iyy, prop_iyz, prop_izz, spawner_args) -%} - - {{ 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, - 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, - robot_namespace = spawner_args['name']) - }} - - {%- 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, - 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, - robot_namespace = spawner_args['name']) - }} - - {%- 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 = 'lidar' -%} - - - - - - {{ 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, - update_rate = spawner_args[spawner_keyword]['update_rate'], - sensor_link_name = sensor_name ~ "_sensor_link", - parent_link_name = parent_link, - samples = 1, - min_distance = spawner_args[spawner_keyword]['min_range'], - max_distance = spawner_args[spawner_keyword]['max_range'], - resolution = 0.005, - x = x, - y = y, - z = z, - roll = roll, - pitch = pitch, - yaw = yaw) - }} - - - - - {{ sensor_name }}_sensor_link - {{ parent_link }} - - - - - {{ mount if 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 if 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 if 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 if 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 if 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 if 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 if 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 if 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 if 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 if 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 if 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 if 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 if 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, - camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', - camera_topic_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 if 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 if 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 front' -%} - {%- 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, - camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', - camera_topic_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 if 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 if 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 if 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 if 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 if 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 if 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 if 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 if 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 if 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 if 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 if 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, led3, x1, y1, z, 0.0, math.radians(90), 0.0), - (2, led3, x2, y2, z, 0.0, math.radians(90), math.radians(90)), - (3, led1, x2, -y2, z, 0.0, math.radians(90), -math.radians(90)), - (4, led1, x1, -y1, z, 0.0, math.radians(90), 0.0), - (5, led2, -x2, y2, z, 0.0, math.radians(90), math.radians(90)), - (6, led2, -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 = sensor_name + '_link', - update_rate = spawner_args[spawner_keyword]['imu_rate'], - topic_name = '/' + spawner_args['name'] + '/' + sensor_name + '/imu', - frame_name = spawner_args['name'] + '/' + sensor_name + '_imu', - noise_mean = spawner_args[spawner_keyword]['imu_noise'], - x = 0, - y = 0, - z = 0, - roll = -math.radians(90), - pitch = 0, - yaw = -math.radians(90)) - }} - - - - {{ generic.tf_static_macro( - namespace = spawner_args['name'] + '/', - parent_link = sensor_name + "_link", - child_link = sensor_name + "_imu_link") - }} - {# #} - - - {{ mount if 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 = sensor_name, - 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)) - }} - - - - {{ generic.tf_static_macro( - namespace = spawner_args['name'] + '/', - parent_link = sensor_name + "_link", - child_link = sensor_name + "_imu_link") - }} - {# #} - - - {{ mount if 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': 2.0, 'chain_mass': 0.1, 'load_mass': 1.0, 'load_radius': 0.1, 'num_links': 15, '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 -%} - - - - {{ generic.load_pendulum_macro( - parent_link = 'pendulum_chain_' + (spawner_args[spawner_keyword]['num_links'] - 1)| string() + '_link', - link_length = spawner_args[spawner_keyword]['chain_length'] / spawner_args[spawner_keyword]['num_links'], - load_mass = spawner_args[spawner_keyword]['load_mass'], - load_radius = spawner_args[spawner_keyword]['load_radius'] - ) - }} - - - {%- 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 }} - - - - {{ 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 if mount }} - - - - - {%- 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/models/mrs_robots_description/sdf/components/camera/bluefox.sdf.jinja b/models/mrs_robots_description/sdf/components/camera/bluefox.sdf.jinja new file mode 100644 index 0000000..3d86e94 --- /dev/null +++ b/models/mrs_robots_description/sdf/components/camera/bluefox.sdf.jinja @@ -0,0 +1,241 @@ + +{%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} + +{# 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) }} + + {# -- tf from link to sensor -- #} + {%- set x_offset = 0.0 -%} + {%- set y_offset = 0.0 -%} + {%- set z_offset = 0.0 -%} + {%- set roll_offset = 0.0 -%} + {%- set pitch_offset = 0.0 -%} + {%- set yaw_offset = 0.0 -%} + + {%- set roll = 0.0 -%} + {%- set pitch = math.radians(90) -%} + {%- set yaw = 0.0 -%} + + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_mesh_macro( + name = camera_name + "_visual", + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + x = 0, + y = 0, + z = 0, + roll = math.radians(90), + pitch = 0, + yaw = 0) + }} + + + {{ generic.camera_macro( + parent_link = parent_link, + camera_name = camera_name, + camera_frame_name = spawner_args['name'] + '/' + camera_name + '_optical', + camera_topic_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'], + x_offset = x_offset, + y_offset = y_offset, + z_offset = z_offset, + roll_offset = roll_offset, + pitch_offset = pitch_offset, + yaw_offset = yaw_offset) + }} + + + + + {{ parent_link }} + {{ camera_name }}_link + + + + + {{ mount if 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) }} + + + {# -- tf from link to sensor -- #} + {%- set x_offset = 0.0 -%} + {%- set y_offset = 0.0 -%} + {%- set z_offset = 0.0 -%} + {%- set roll_offset = 0.0 -%} + {%- set pitch_offset = 0.0 -%} + {%- set yaw_offset = 0.0 -%} + + {%- set roll = 0.0 -%} + {%- set pitch = math.radians(90) -%} + {%- set yaw = math.radians(180) -%} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_mesh_macro( + name = camera_name + "_visual", + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + x = 0, + y = 0, + z = 0, + roll = math.radians(90), + pitch = 0, + yaw = 0) + }} + + + {{ 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'], + x_offset = x_offset, + y_offset = y_offset, + z_offset = z_offset, + roll_offset = roll_offset, + pitch_offset = pitch_offset, + yaw_offset = yaw_offset) + }} + + + + + {{ parent_link }} + {{ camera_name }}_link + + + + {{ mount if 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) }} + + {# -- tf from link to sensor -- #} + {%- set x_offset = 0.0 -%} + {%- set y_offset = 0.0 -%} + {%- set z_offset = 0.0 -%} + {%- set roll_offset = 0.0 -%} + {%- set pitch_offset = 0.0 -%} + {%- set yaw_offset = 0.0 -%} + + {%- set roll = 0.0 -%} + {%- set pitch = 0.0 -%} + {%- set yaw = 0.0-%} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ generic.zero_inertial_macro() }} + {{ generic.visual_mesh_macro( + name = camera_name + "_visual", + mesh_file = 'model://mrs_robots_description/meshes/sensors/bluefox.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + x = 0, + y = 0, + z = 0, + roll = math.radians(90), + pitch = 0, + yaw = 0) + }} + + + {{ 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'], + x_offset = x_offset, + y_offset = y_offset, + z_offset = z_offset, + roll_offset = roll_offset, + pitch_offset = pitch_offset, + yaw_offset = yaw_offset) + }} + + + + + {{ mount if mount }} + + + + {{ parent_link }} + {{ camera_name }}_link + + + + + {%- endif -%} +{%- endmacro -%} +{# #} diff --git a/models/mrs_robots_description/sdf/components/camera/mobius.sdf.jinja b/models/mrs_robots_description/sdf/components/camera/mobius.sdf.jinja new file mode 100644 index 0000000..ec2d9b4 --- /dev/null +++ b/models/mrs_robots_description/sdf/components/camera/mobius.sdf.jinja @@ -0,0 +1,260 @@ + +{%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} + +{# 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) }} + + {# -- tf from link to sensor -- #} + {%- set x_offset = 0.0 -%} + {%- set y_offset = 0.0 -%} + {%- set z_offset = 0.0 -%} + {%- set roll_offset = 0.0 -%} + {%- set pitch_offset = 0.0 -%} + {%- set yaw_offset = 0.0 -%} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ zero_inertial_macro() }} + {{ visual_mesh_macro( + name = camera_name + "_visual", + mesh_file = 'model://mrs_robots_description/meshes/sensors/mobius.dae', + mesh_scale = '1 1 1', + color = 'DarkGrey', + x = -0.004, + y = 0.0045, + z = 0, + roll = 0, + pitch = 0, + yaw = 0) + }} + + + {{ 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'], + x_offset = x_offset, + y_offset = y_offset, + z_offset = z_offset, + roll_offset = roll_offset, + pitch_offset = pitch_offset, + yaw_offset = yaw_offset) + }} + + + + + {{ parent_link }} + {{ camera_name }}_link + + + + {{ mount if 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 if mount }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ parent_link }} + {{ camera_name }}_link + + + + + {%- 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 if mount }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ parent_link }} + {{ camera_name }}_link + + + + + {%- 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 if mount }} + + + + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} + {{ parent_link }} + {{ camera_name }}_link + + + + + {%- endif -%} +{%- endmacro -%} +{# #} diff --git a/models/mrs_robots_description/sdf/components/component_snippets.sdf.jinja b/models/mrs_robots_description/sdf/components/component_snippets.sdf.jinja new file mode 100644 index 0000000..d1fddaa --- /dev/null +++ b/models/mrs_robots_description/sdf/components/component_snippets.sdf.jinja @@ -0,0 +1,247 @@ + + + +{%- import 'mrs_robots_description/sdf/components/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, use_mrs_plugin, prop_ixx, prop_ixy, prop_ixz, prop_iyy, prop_iyz, prop_izz, spawner_args) -%} + + {{ 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, + 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, + robot_namespace = spawner_args['name']) + }} + + {%- 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, + 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, + robot_namespace = spawner_args['name']) + }} + + {%- endif -%} + + {%- endfor -%} + + +{%- endmacro -%} +{# #} + +{# ======================= rangefinder sensors ====================== #} +{# ==== Garmin ==== #} +{%- import "mrs_robots_description/sdf/components/rangefinder/garmin.sdf.jinja" as garmin_rangefinder -%} + +{%- macro garmin_down_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + {{ garmin_rangefinder.garmin_down_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) }} +{%- endmacro -%} +{%- macro garmin_down_external_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + {{ garmin_rangefinder.garmin_down_external_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args)}} +{%- endmacro -%} +{%- macro garmin_up_external_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + {{ garmin_rangefinder.garmin_up_external_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) }} +{%- endmacro -%} + + +{# ==== Teraranger ==== #} +{# TODO: Not tested in ROS2 #} +{%- import "mrs_robots_description/sdf/components/rangefinder/teraranger.sdf.jinja" as teraranger_rangefinder -%} + +{%- macro teraranger_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + {{ teraranger_rangefinder.teraranger_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) }} +{%- endmacro -%} +{%- macro teraranger_evo_tower_macro(parent_link, visualize, frame_name, parent_frame_name, gaussian_noise, x, y, z, roll, pitch, yaw, spawner_args) -%} + {{ teraranger_rangefinder.teraranger_evo_tower_macro(parent_link, visualize, frame_name, parent_frame_name, gaussian_noise, x, y, z, roll, pitch, yaw, spawner_args) }} +{%- endmacro -%} + + +{# ========================== 2D LIDAR sensors ========================= #} +{# ==== RP lidar ==== #} +{%- import "mrs_robots_description/sdf/components/lidar/rplidar.sdf.jinja" as rplidar_lidar -%} +{%- macro rplidar_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + {{ rplidar_lidar.rplidar_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) }} +{%- endmacro -%} + + +{# ==== Scance Sweep lidar ==== #} +{%- import "mrs_robots_description/sdf/components/lidar/scanse_sweep.sdf.jinja" as scanse_sweep_lidar -%} +{%- macro scanse_sweep_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + {{ scanse_sweep_lidar.scanse_sweep_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) }} +{%- endmacro -%} + + +{# ========================== 3D LIDAR sensors ========================= #} +{# ==== Ouster lidar ==== #} +{%- import "mrs_robots_description/sdf/components/lidar/ouster.sdf.jinja" as ouster_lidar -%} +{%- macro ouster_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + {{ ouster_lidar.ouster_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) }} +{%- endmacro -%} + +{# ==== Velodyne lidar ==== #} +{%- import "mrs_robots_description/sdf/components/lidar/velodyne.sdf.jinja" as velodyne_lidar -%} +{%- macro velodyne_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + {{ velodyne_lidar.velodyne_macro(parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) }} +{%- endmacro -%} + + + +{# ============================= cameras ============================ #} +{# ==== Bluefox camera ==== #} +{%- import "mrs_robots_description/sdf/components/camera/bluefox.sdf.jinja" as bluefox_camera -%} + +{%- macro bluefox_camera_macro(camera_name, parent_link, x, y, z, mount, spawner_args) -%} + {{ bluefox_camera.bluefox_camera_macro(camera_name, parent_link, x, y, z, mount, spawner_args) }} +{%- endmacro -%} +{%- macro bluefox_camera_reverse_macro(camera_name, parent_link, x, y, z, mount, spawner_args) -%} + {{ bluefox_camera.bluefox_camera_reverse_macro(camera_name, parent_link, x, y, z, mount, spawner_args) }} +{%- endmacro -%} +{%- macro bluefox_camera_front_macro(camera_name, parent_link, x, y, z, mount, spawner_args) -%} + {{ bluefox_camera.bluefox_camera_front_macro(camera_name, parent_link, x, y, z, mount, spawner_args) }} +{%- endmacro -%} + +{# ==== Mobius camera ==== #} +{%- import "mrs_robots_description/sdf/components/camera/mobius.sdf.jinja" as mobius_camera -%} + +{%- macro mobius_down_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + {{ mobius_camera.mobius_down_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) }} +{%- endmacro -%} +{%- macro mobius_front_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + {{ mobius_camera.mobius_front_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) }} +{%- endmacro -%} +{%- macro mobius_back_left_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + {{ mobius_camera.mobius_back_left_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) }} +{%- endmacro -%} +{%- macro mobius_back_right_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) -%} + {{ mobius_camera.mobius_back_right_macro(camera_name, parent_link, x, y, z, roll, pitch, yaw, mount, spawner_args) }} +{%- endmacro -%} + + + diff --git a/models/mrs_robots_description/sdf/generic_components.sdf.jinja b/models/mrs_robots_description/sdf/components/generic_components.sdf.jinja similarity index 60% rename from models/mrs_robots_description/sdf/generic_components.sdf.jinja rename to models/mrs_robots_description/sdf/components/generic_components.sdf.jinja index e0bfde9..901298c 100644 --- a/models/mrs_robots_description/sdf/generic_components.sdf.jinja +++ b/models/mrs_robots_description/sdf/components/generic_components.sdf.jinja @@ -686,6 +686,58 @@ limitations under the License. {%- endmacro -%} {# #} + +{# gazebo_lidar_plugin_macro {--> #} +{# --- sensor is always defined with respect to the sensor_link frame #} +{%- macro gazebo_lidar_plugin_macro( + lidar_name, lidar_frame_name, lidar_topic_name, + sensor_x, sensor_y, sensor_z, sensor_roll, sensor_pitch, sensor_yaw, + update_rate, + horiz_num_samples, horiz_resolution, horiz_min_angle, horiz_max_angle, + vertic_num_samples, vertic_resolution, vertic_min_angle, vertic_max_angle, + range_min, range_max, range_resolution, + noise_mean, noise_std + ) -%} + + + {{ sensor_x }} {{ sensor_y }} {{ sensor_z }} {{ sensor_roll }} {{ sensor_pitch }} {{ sensor_yaw }} + {{ lidar_topic_name }} + {{ update_rate }} + {{ lidar_frame_name }} + + + + {{horiz_num_samples | int }} + {{horiz_resolution}} + {{horiz_min_angle}} + {{horiz_max_angle}} + + + {{vertic_num_samples | int }} + {{vertic_resolution}} + {{vertic_min_angle}} + {{vertic_max_angle}} + + + + {{range_min}} + {{range_max}} + {{range_resolution}} + + + gaussian + {{noise_mean}} + {{noise_std}} + + + 1 + true + +{%- endmacro -%} +{# #} + + + {# rangefinder_sensor_macro {--> #} {%- macro rangefinder_sensor_macro(name, update_rate, sensor_link_name, parent_link_name, samples, min_distance, max_distance, resolution, x, y, z, roll, pitch, yaw) -%} @@ -718,26 +770,17 @@ limitations under the License. {%- endmacro -%} {# #} + + {# ========================== camera sensors ========================= #} {# camera_macro {--> #} -{%- macro camera_macro(parent_link, camera_name, camera_frame_name, camera_topic_name, frame_rate, 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) - }} +{%- macro camera_macro(parent_link, camera_name, camera_frame_name, camera_topic_name, frame_rate, + horizontal_fov, image_width, image_height, min_distance, max_distance, + noise_mean, noise_stddev, + x_offset, y_offset, z_offset, roll_offset, pitch_offset, yaw_offset) -%} + {{ x_offset }} {{ y_offset }} {{ z_offset }} {{ roll_offset }} {{ pitch_offset }} {{ yaw_offset }} true true {{ camera_topic_name }}/image_raw @@ -754,22 +797,16 @@ limitations under the License. {{ min_distance }} {{ max_distance }} + gaussian - {{ noise_mean }} {{ noise_stddev }} - - - - {{ parent_link }} - {{ camera_name }}_link - {%- endmacro -%} {# #} @@ -794,7 +831,7 @@ limitations under the License. gaussian {{ noise_mean }} {{ noise_stddev }} @@ -833,476 +870,4 @@ limitations under the License. {%- 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 -%} -{# #} - - + \ No newline at end of file diff --git a/models/mrs_robots_description/sdf/components/lidar/ouster.sdf.jinja b/models/mrs_robots_description/sdf/components/lidar/ouster.sdf.jinja new file mode 100644 index 0000000..2a8ae87 --- /dev/null +++ b/models/mrs_robots_description/sdf/components/lidar/ouster.sdf.jinja @@ -0,0 +1,201 @@ + +{%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} + +{# ouster_macro {--> #} +{%- macro ouster_macro(parent_link, 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} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {# model selection {--> #} + {%- set sensor_name = 'ouster' -%} + {%- 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 {--> #} + {# -- 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 -%} + + {# #} + + + + + {{ 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 + + + 0.035 + 0.038 + + + + + + + + + + {{ generic.gazebo_lidar_plugin_macro( + lidar_name = sensor_name, + lidar_frame_name = spawner_args['name'] + '/' + sensor_name + '_link', + lidar_topic_name = spawner_args['name'] + '/' + sensor_name, + sensor_x = lidar_x, sensor_y = lidar_y, sensor_z = lidar_z, sensor_roll = lidar_roll, sensor_pitch =lidar_pitch, sensor_yaw =lidar_yaw, + update_rate = spawner_args[spawner_keyword]['update_rate'], + horiz_num_samples = spawner_args[spawner_keyword]['horizontal_samples'], + horiz_resolution = 1, + horiz_min_angle = 0, + horiz_max_angle =2*math.pi, + vertic_num_samples = lasers, + vertic_resolution = 1, + vertic_min_angle = -vfov_angle/2*math.radians(180)/180.0 , + vertic_max_angle = vfov_angle/2*math.radians(180)/180.0, + range_min = 0.1, + range_max = range, + range_resolution = 0.03, + noise_mean = 0.0, + noise_std= spawner_args[spawner_keyword]['noise'] + )}} + + + + + + {{ parent_link }} + {{ sensor_name }}_link + + + + + {{ mount if mount }} + + + {%- endif -%} +{%- endmacro -%} +{# #} + + diff --git a/models/mrs_robots_description/sdf/components/lidar/rplidar.sdf.jinja b/models/mrs_robots_description/sdf/components/lidar/rplidar.sdf.jinja new file mode 100644 index 0000000..2b514e8 --- /dev/null +++ b/models/mrs_robots_description/sdf/components/lidar/rplidar.sdf.jinja @@ -0,0 +1,96 @@ + +{%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} + +{# 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) }} + + {# -- tf from link to sensor -- #} + {%- set lidar_x = 0.0 -%} + {%- set lidar_y = 0.0 -%} + {%- set lidar_z = 0.0 -%} + {%- set lidar_roll = 0.0 -%} + {%- set lidar_pitch = 0.0 -%} + {%- set lidar_yaw = 0.0 -%} + + + + {%- 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 + + + + + + + + + {{ generic.gazebo_lidar_plugin_macro( + lidar_name = sensor_name, + lidar_frame_name = spawner_args['name'] + '/' + sensor_name + '_link', + lidar_topic_name = spawner_args['name'] + '/' + sensor_name + '/scan', + sensor_x = lidar_x, sensor_y = lidar_y, sensor_z = lidar_z, sensor_roll = lidar_roll, sensor_pitch = lidar_pitch, sensor_yaw = lidar_yaw, + update_rate = spawner_args[spawner_keyword]['update_rate'], + horiz_num_samples = spawner_args[spawner_keyword]['horizontal_samples'], + horiz_resolution = 1, + horiz_min_angle = -3.1241390751, + horiz_max_angle = 3.1241390751, + vertic_num_samples = 1, + vertic_resolution = 0.01, + vertic_min_angle = 0, + vertic_max_angle = 0, + range_min = spawner_args[spawner_keyword]['min_range'], + range_max = spawner_args[spawner_keyword]['max_range'], + range_resolution = 0.01, + noise_mean = 0.0, + noise_std= spawner_args[spawner_keyword]["noise"] + )}} + + + + + {{ parent_link }} + {{ sensor_name }}_link + + + + {{ mount if mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} diff --git a/models/mrs_robots_description/sdf/components/lidar/scanse_sweep.sdf.jinja b/models/mrs_robots_description/sdf/components/lidar/scanse_sweep.sdf.jinja new file mode 100644 index 0000000..6b7c56c --- /dev/null +++ b/models/mrs_robots_description/sdf/components/lidar/scanse_sweep.sdf.jinja @@ -0,0 +1,98 @@ + +{%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} + +{# 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) }} + + {# -- tf from link to sensor -- #} + {%- set lidar_x = 0.0 -%} + {%- set lidar_y = 0.0 -%} + {%- set lidar_z = 0.0 -%} + {%- set lidar_roll = 0.0 -%} + {%- set lidar_pitch = 0.0 -%} + {%- set lidar_yaw = 0.0 -%} + + {%- set sensor_name = 'scanse_sweep' -%} + + + + {{ 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 + + + + + + + + + {{ generic.gazebo_lidar_plugin_macro( + lidar_name = sensor_name, + lidar_frame_name = spawner_args['name'] + '/' + sensor_name + '_link', + lidar_topic_name = spawner_args['name'] + '/' + sensor_name + '/scan', + sensor_x = lidar_x, sensor_y = lidar_y, sensor_z = lidar_z, sensor_roll = lidar_roll, sensor_pitch = lidar_pitch, sensor_yaw = lidar_yaw, + update_rate = spawner_args[spawner_keyword]['update_rate'], + horiz_num_samples = spawner_args[spawner_keyword]['horizontal_samples'], + horiz_resolution = 1, + horiz_min_angle = 0, + horiz_max_angle = 6.283185, + vertic_num_samples = 1, + vertic_resolution = 0.01, + vertic_min_angle = 0, + vertic_max_angle = 0, + range_min = spawner_args[spawner_keyword]['min_range'], + range_max = spawner_args[spawner_keyword]['max_range'], + range_resolution = 0.01, + noise_mean = 0.0, + noise_std= spawner_args[spawner_keyword]["noise"] + )}} + + + + + {{ parent_link }} + {{ sensor_name }}_link + + + + {{ mount if mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} + diff --git a/models/mrs_robots_description/sdf/components/lidar/velodyne.sdf.jinja b/models/mrs_robots_description/sdf/components/lidar/velodyne.sdf.jinja new file mode 100644 index 0000000..b9eb931 --- /dev/null +++ b/models/mrs_robots_description/sdf/components/lidar/velodyne.sdf.jinja @@ -0,0 +1,124 @@ + +{%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} + +{# velodyne_macro {--> #} +{%- macro velodyne_macro(parent_link, 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} -%} + + {%- if spawner_keyword in spawner_args.keys() -%} + {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }} + + {# setup local variables {--> #} + {# -- tf from sensor to lidar -- #} + {%- 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 -%} + + {# 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. #} + + {# #} + + {%- set sensor_name = 'velodyne' -%} + + + + + {{ 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 + + + + + + + + + + {{ generic.gazebo_lidar_plugin_macro( + lidar_name = sensor_name, + lidar_frame_name = spawner_args['name'] + '/' + sensor_name + '_link', + lidar_topic_name = spawner_args['name'] + '/' + sensor_name, + sensor_x = lidar_x, sensor_y = lidar_y, sensor_z = lidar_z, sensor_roll = lidar_roll, sensor_pitch =lidar_pitch, sensor_yaw =lidar_yaw, + update_rate = spawner_args[spawner_keyword]['update_rate'], + horiz_num_samples = spawner_args[spawner_keyword]['horizontal_samples'], + horiz_resolution = 1, + horiz_min_angle = -math.radians(180), + horiz_max_angle = math.radians(180), + vertic_num_samples = spawner_args[spawner_keyword]['lasers'], + vertic_resolution = 1, + vertic_min_angle = -spawner_args[spawner_keyword]['vfov']/2*math.radians(180)/180.0 , + vertic_max_angle = spawner_args[spawner_keyword]['vfov']/2*math.radians(180)/180.0, + range_min = 0.1, + range_max = spawner_args[spawner_keyword]['range'], + range_resolution = 0.03, + noise_mean = 0.0, + noise_std= spawner_args[spawner_keyword]['noise'] + )}} + + + + + {{ parent_link }} + {{ sensor_name }}_link + + + + + + {{ mount if mount }} + + + + {%- endif -%} +{%- endmacro -%} +{# #} diff --git a/models/mrs_robots_description/sdf/components/rangefinder/garmin.sdf.jinja b/models/mrs_robots_description/sdf/components/rangefinder/garmin.sdf.jinja new file mode 100644 index 0000000..05604ff --- /dev/null +++ b/models/mrs_robots_description/sdf/components/rangefinder/garmin.sdf.jinja @@ -0,0 +1,218 @@ + +{%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} + +{# 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 = 'lidar' -%} + + + + + + {{ 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, + update_rate = spawner_args[spawner_keyword]['update_rate'], + sensor_link_name = sensor_name ~ "_sensor_link", + parent_link_name = parent_link, + samples = 1, + min_distance = spawner_args[spawner_keyword]['min_range'], + max_distance = spawner_args[spawner_keyword]['max_range'], + resolution = 0.005, + x = x, + y = y, + z = z, + roll = roll, + pitch = pitch, + yaw = yaw) + }} + + + + + {{ sensor_name }}_sensor_link + {{ parent_link }} + + + + + {{ mount if 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 if 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 if mount }} + + + + + {%- endif -%} +{%- endmacro -%} +{# #} diff --git a/models/mrs_robots_description/sdf/components/rangefinder/teraranger.sdf.jinja b/models/mrs_robots_description/sdf/components/rangefinder/teraranger.sdf.jinja new file mode 100644 index 0000000..4d882bf --- /dev/null +++ b/models/mrs_robots_description/sdf/components/rangefinder/teraranger.sdf.jinja @@ -0,0 +1,129 @@ + +{%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} + + +{# 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 if 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 -%} +{# #} \ No newline at end of file diff --git a/models/mrs_robots_description/sdf/f330.sdf.jinja b/models/mrs_robots_description/sdf/drones/f330.sdf.jinja similarity index 98% rename from models/mrs_robots_description/sdf/f330.sdf.jinja rename to models/mrs_robots_description/sdf/drones/f330.sdf.jinja index 4519053..0ad327a 100644 --- a/models/mrs_robots_description/sdf/f330.sdf.jinja +++ b/models/mrs_robots_description/sdf/drones/f330.sdf.jinja @@ -1,8 +1,8 @@ - {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} - {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} + {%- import 'mrs_robots_description/sdf/components/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} diff --git a/models/mrs_robots_description/sdf/f450.sdf.jinja b/models/mrs_robots_description/sdf/drones/f450.sdf.jinja similarity index 89% rename from models/mrs_robots_description/sdf/f450.sdf.jinja rename to models/mrs_robots_description/sdf/drones/f450.sdf.jinja index 8166032..5bb6480 100644 --- a/models/mrs_robots_description/sdf/f450.sdf.jinja +++ b/models/mrs_robots_description/sdf/drones/f450.sdf.jinja @@ -1,8 +1,8 @@ - {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} - {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} + {%- import 'mrs_robots_description/sdf/components/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} @@ -377,7 +377,7 @@ mesh_file = garmin_mount_mesh, mesh_scale = mesh_scale_milimeters, color = 'DarkGrey', - x = -0.055, + x = -0.062, y = 0, z = -(arm_height + 0.016), roll = math.radians(90), @@ -517,7 +517,7 @@ {# #} - + {{ generic.multirotor_physics_macro( mass = mass, @@ -590,24 +590,6 @@ - {# ================================================================== #} - {# || optional sensor definitions || #} - {# ================================================================== #} - - {# Garmin down {--> #} - {{ components.garmin_down_macro( - parent_link = root, - x = -0.057, - y = 0, - z = -0.069, - roll = 0, - pitch = math.radians(90), - yaw = 0, - mount = garmin_down_mount, - spawner_args = spawner_args) - }} - {# #} - {# Propellers {--> #} {%- set prop_list = [ { @@ -676,5 +658,113 @@ }} {# #} + {# ================================================================== #} + {# || optional sensor definitions || #} + {# ================================================================== #} + + {# ======================= rangefinder sensors ====================== #} + + {# Garmin down {--> #} + {{ components.garmin_down_macro( + parent_link = root, + x = -0.062, + y = 0, + z = -0.059, + roll = 0, + pitch = math.radians(90), + yaw = 0, + mount = garmin_down_mount, + spawner_args = spawner_args) + }} + {# #} + + {# ========================== LIDAR sensors ========================= #} + + {# Scanse Sweep {--> #} + {%- 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, + mount = none, + spawner_args = spawner_args) + -%} + {{ scanse_sweep }} + {# #} + + {# Rplidar {--> #} + {%- set rplidar = components.rplidar_macro( + parent_link = root, + x = 0.0, + y = 0.0, + z = 0.1, + roll = 0, + pitch = 0, + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ rplidar }} + {# #} + + {# Velodyne {--> #} + {%- set velodyne = components.velodyne_macro( + parent_link = root, + x = 0.0, + y = 0.0, + z = 0.066, + roll = 0, + pitch = 0, + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ velodyne }} + {# #} + + {# Ouster {--> #} + {%- set ouster = components.ouster_macro( + parent_link = root, + x = 0.0, + y = 0.0, + z = 0.066, + roll = 0, + pitch = 0, + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ ouster }} + {# #} + + {# ====================== 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/models/mrs_robots_description/sdf/f550.sdf.jinja b/models/mrs_robots_description/sdf/drones/f550.sdf.jinja similarity index 92% rename from models/mrs_robots_description/sdf/f550.sdf.jinja rename to models/mrs_robots_description/sdf/drones/f550.sdf.jinja index f23907e..0496ad0 100644 --- a/models/mrs_robots_description/sdf/f550.sdf.jinja +++ b/models/mrs_robots_description/sdf/drones/f550.sdf.jinja @@ -1,8 +1,8 @@ - {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} - {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} + {%- import 'mrs_robots_description/sdf/components/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} @@ -781,6 +781,8 @@ {# || optional sensor definitions || #} {# ================================================================== #} + {# ======================= rangefinder sensors ====================== #} + {# Garmin down {--> #} {{ components.garmin_down_macro( parent_link = root, @@ -795,5 +797,85 @@ }} {# #} + {# ========================== LIDAR sensors ========================= #} + + {# Scanse Sweep {--> #} + {%- 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, + mount = none, + spawner_args = spawner_args) + -%} + {{ scanse_sweep }} + {# #} + + {# Rplidar {--> #} + {%- set rplidar = components.rplidar_macro( + parent_link = root, + x = 0.0, + y = 0.0, + z = 0.1, + roll = 0, + pitch = 0, + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ rplidar }} + {# #} + + {# Velodyne {--> #} + {%- set velodyne = components.velodyne_macro( + parent_link = root, + x = 0.0, + y = 0.0, + z = 0.066, + roll = 0, + pitch = 0, + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ velodyne }} + {# #} + + {# Ouster {--> #} + {%- set ouster = components.ouster_macro( + parent_link = root, + x = 0.0, + y = 0.0, + z = 0.066, + roll = 0, + pitch = 0, + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ ouster }} + {# #} + + {# ====================== 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 -%} + {# #} + diff --git a/models/mrs_robots_description/sdf/m690.sdf.jinja b/models/mrs_robots_description/sdf/drones/m690.sdf.jinja similarity index 94% rename from models/mrs_robots_description/sdf/m690.sdf.jinja rename to models/mrs_robots_description/sdf/drones/m690.sdf.jinja index d606885..0d556b6 100644 --- a/models/mrs_robots_description/sdf/m690.sdf.jinja +++ b/models/mrs_robots_description/sdf/drones/m690.sdf.jinja @@ -1,8 +1,8 @@ - {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} - {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} + {%- import 'mrs_robots_description/sdf/components/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} @@ -365,6 +365,8 @@ {# || optional sensor definitions || #} {# ================================================================== #} + {# ======================= rangefinder sensors ====================== #} + {# Garmin down {--> #} {{ components.garmin_down_macro( parent_link = root, @@ -380,5 +382,21 @@ {# #} + {# ========================== LIDAR sensors ========================= #} + + {# Ouster {--> #} + {{ components.ouster_macro( + parent_link = root, + x = 0.0, + y = 0.0, + z = 0.107, + roll = 0, + pitch = 0, + yaw = 0, + mount = none, + spawner_args = spawner_args) + }} + {# #} + diff --git a/models/mrs_robots_description/sdf/naki.sdf.jinja b/models/mrs_robots_description/sdf/drones/naki.sdf.jinja similarity index 99% rename from models/mrs_robots_description/sdf/naki.sdf.jinja rename to models/mrs_robots_description/sdf/drones/naki.sdf.jinja index 7d93391..5e5b491 100644 --- a/models/mrs_robots_description/sdf/naki.sdf.jinja +++ b/models/mrs_robots_description/sdf/drones/naki.sdf.jinja @@ -1,8 +1,8 @@ - {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} - {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} + {%- import 'mrs_robots_description/sdf/components/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} diff --git a/models/mrs_robots_description/sdf/robofly.sdf.jinja b/models/mrs_robots_description/sdf/drones/robofly.sdf.jinja similarity index 97% rename from models/mrs_robots_description/sdf/robofly.sdf.jinja rename to models/mrs_robots_description/sdf/drones/robofly.sdf.jinja index 1003f29..b759e1f 100644 --- a/models/mrs_robots_description/sdf/robofly.sdf.jinja +++ b/models/mrs_robots_description/sdf/drones/robofly.sdf.jinja @@ -1,8 +1,8 @@ - {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} - {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} + {%- import 'mrs_robots_description/sdf/components/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} diff --git a/models/mrs_robots_description/sdf/t650.sdf.jinja b/models/mrs_robots_description/sdf/drones/t650.sdf.jinja similarity index 89% rename from models/mrs_robots_description/sdf/t650.sdf.jinja rename to models/mrs_robots_description/sdf/drones/t650.sdf.jinja index 355b56b..390a2c1 100644 --- a/models/mrs_robots_description/sdf/t650.sdf.jinja +++ b/models/mrs_robots_description/sdf/drones/t650.sdf.jinja @@ -1,8 +1,8 @@ - {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} - {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} + {%- import 'mrs_robots_description/sdf/components/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} @@ -579,6 +579,8 @@ {# || optional sensor definitions || #} {# ================================================================== #} + {# ======================= rangefinder sensors ====================== #} + {# Garmin down {--> #} {%- set garmin_down = components.garmin_down_macro( parent_link = root, @@ -594,5 +596,83 @@ {{ garmin_down }} {# #} + {# ========================== LIDAR sensors ========================= #} + + {# Scanse Sweep {--> #} + {{ components.scanse_sweep_macro( + parent_link = root, + x = 0.0, + y = 0.0, + z = 0.12, + roll = 0, + pitch = 0, + yaw = 0, + mount = none, + spawner_args = spawner_args) + }} + {# #} + + {# Rplidar {--> #} + {{ components.rplidar_macro( + parent_link = root, + x = 0.0, + y = 0.0, + z = 0.107, + roll = 0, + pitch = 0, + yaw = 0, + mount = rplidar_mount, + spawner_args = spawner_args) + }} + {# #} + + {# Velodyne {--> #} + {%- set velodyne = components.velodyne_macro( + parent_link = root, + x = 0.0, + y = 0.0, + z = 0.073, + roll = 0, + pitch = 0, + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ velodyne }} + {# #} + + {# Ouster {--> #} + {%- set ouster = components.ouster_macro( + parent_link = root, + x = 0.0, + y = 0.0, + z = 0.073, + roll = 0, + pitch = 0, + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ ouster }} + {# #} + + {# ====================== conditional components ==================== #} + + {# 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 -%} + {# #} + + {# 3D lidar mount {--> #} + {%- if velodyne | replace('\s', '') | length != 0 or ouster | replace('\s', '') | length != 0 -%} + + {{ lidar_mount_3d }} + + {%- endif -%} + {# #} + diff --git a/models/mrs_robots_description/sdf/x500.sdf.jinja b/models/mrs_robots_description/sdf/drones/x500.sdf.jinja similarity index 93% rename from models/mrs_robots_description/sdf/x500.sdf.jinja rename to models/mrs_robots_description/sdf/drones/x500.sdf.jinja index 3a92e08..372cff0 100644 --- a/models/mrs_robots_description/sdf/x500.sdf.jinja +++ b/models/mrs_robots_description/sdf/drones/x500.sdf.jinja @@ -1,8 +1,8 @@ - {%- import 'mrs_robots_description/sdf/component_snippets.sdf.jinja' as components -%} - {%- import 'mrs_robots_description/sdf/generic_components.sdf.jinja' as generic -%} + {%- import 'mrs_robots_description/sdf/components/component_snippets.sdf.jinja' as components -%} + {%- import 'mrs_robots_description/sdf/components/generic_components.sdf.jinja' as generic -%} {# ================================================================== #} {# || parameters definition || #} @@ -706,6 +706,8 @@ {# || optional sensor definitions || #} {# ================================================================== #} + {# ======================= rangefinder sensors ====================== #} + {# Garmin down {--> #} {{ components.garmin_down_macro( parent_link = root, @@ -720,6 +722,38 @@ }} {# #} + {# ========================== LIDAR sensors ========================= #} + + {# Ouster {--> #} + {%- set ouster_macro = components.ouster_macro( + parent_link = root, + x = 0.0, + y = 0.0, + z = 0.103, + roll = 0.0, + pitch = 0.0, + yaw = 0.0, + mount = none, + spawner_args = spawner_args) + -%} + {{ ouster_macro }} + {# #} + + {# Rplidar {--> #} + {%- set rplidar = components.rplidar_macro( + parent_link = root, + x = 0.0, + y = 0.0, + z = 0.136, + roll = 0, + pitch = 0, + yaw = 0, + mount = none, + spawner_args = spawner_args) + -%} + {{ rplidar }} + {# #} + {# ========================== camera sensors ======================== #} {# Bluefox camera placements {--> #} @@ -765,5 +799,26 @@ {# #} + {# ====================== conditional components ==================== #} + + {# lidar mount / gps antenna visuals {--> #} + {%- if rplidar | replace('\s', '') | length != 0 or ouster | replace('\s', '') | length != 0 or livox | 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/mrs_uav_gazebo_simulator/mrs_drone_spawner.py b/mrs_uav_gazebo_simulator/mrs_drone_spawner.py index d06cd79..4460371 100755 --- a/mrs_uav_gazebo_simulator/mrs_drone_spawner.py +++ b/mrs_uav_gazebo_simulator/mrs_drone_spawner.py @@ -22,6 +22,7 @@ from ament_index_python.packages import get_package_share_directory from mrs_uav_gazebo_simulator.utils.component_wrapper import ComponentWrapper from mrs_uav_gazebo_simulator.utils.template_wrapper import TemplateWrapper +from mrs_uav_gazebo_simulator.utils.sdf_to_tf_publisher import SdfTfPublisher # ROS 2 Imports from launch import LaunchDescription, LaunchService @@ -131,6 +132,10 @@ def __init__(self): self.declare_parameter('extra_resource_paths', [""]) + self.declare_parameter('tf_static_publisher.base_link', "base_link") + self.declare_parameter('tf_static_publisher.ignored_sensor_links', ["air_pressure_sensor", "magnetometer_sensor", "navsat_sensor", "imu_sensor"]) + + # Get all parameters try: @@ -144,6 +149,8 @@ def __init__(self): self.firmware_launch_delay = float(self.get_parameter('firmware_launch_delay').value) + self.tf_base_link = self.get_parameter('tf_static_publisher.base_link').value + self.tf_ignored_sensor_links = self.get_parameter('tf_static_publisher.ignored_sensor_links').value except rclpy.exceptions.ParameterNotDeclaredException as e: self.get_logger().error(f'Could not load required param. {e}') @@ -180,7 +187,7 @@ def __init__(self): gazebo_simulator_path = get_package_share_directory('mrs_uav_gazebo_simulator') self.uav_ros_gz_bridge_launch_path = os.path.join(gazebo_simulator_path, 'launch', 'uav_ros_gz_bridge.launch.py') self.uav_ros_gz_bridge_config_path = os.path.join(gazebo_simulator_path, 'config') - self.uav_ros_gz_bridge_config_template_name = 'uav_ros_gz_bridge_config.jinja.yaml' + self.uav_ros_gz_bridge_config_template_name = 'uav_ros_gz_bridge_config.yaml.jinja' px4_api_path = get_package_share_directory('mrs_uav_px4_api') self.mavros_launch_path = os.path.join(px4_api_path, 'launch', 'mavros.launch') self.mavros_px4_config_path = os.path.join(px4_api_path, 'config') @@ -216,6 +223,9 @@ def __init__(self): self.gazebo_delete_future = None self.gazebo_spawn_request_start_time = None + # SdfToTf Publisher + self.sdf_to_tf_publisher = SdfTfPublisher(self, self.tf_base_link, self.tf_ignored_sensor_links) + self.is_initialized = True self.get_logger().info('Initialized') @@ -319,6 +329,8 @@ def spawn_gazebo_model(self, robot_params): self.get_logger().error('Template did not render, spawn failed.') return + self.sdf_to_tf_publisher.generate_tf_publishers(sdf_content) + filename = f'mrs_drone_spawner_{name}.sdf' filepath = os.path.join(self.tempfile_folder, filename) @@ -349,10 +361,6 @@ def spawn_gazebo_model(self, robot_params): def launch_uav_ros_gz_bridge(self, uav_name, ros_gz_bridge_config, sensor_topics): self.get_logger().info(f'Launching ros_gz_bridge for {uav_name}') - if len(sensor_topics['image_topics']) < 1: - self.get_logger().info(f'No image publisher attached, not creating ros_gz_bridge for image topics') - return - launch_arguments = { 'namespace': uav_name, 'ros_gz_bridge_config': str(ros_gz_bridge_config), @@ -398,7 +406,8 @@ def service_response_callback_spawn_gazebo_model(self, future, robot_params): ros_gz_bridge_config, sensor_topics = self.generate_uav_ros_gz_config(robot_params) try: - ros_gz_bridge_process = self.launch_uav_ros_gz_bridge(robot_params['name'], ros_gz_bridge_config, sensor_topics) + if ros_gz_bridge_config != "": + ros_gz_bridge_process = self.launch_uav_ros_gz_bridge(robot_params['name'], ros_gz_bridge_config, sensor_topics) mavros_process = self.launch_mavros(robot_params) firmware_process = self.launch_px4_firmware(robot_params) @@ -1264,9 +1273,10 @@ def generate_mavros_px4_config(self, uav_name): # #{ get_attached_sensors(self, robot_params) def get_attached_sensors(self, robot_params): - attached_sensors = { - 'cameras': [] + 'cameras': [], + '2dlidar': [], + '3dlidar': [] } # not using try-catch, it's already done during the sdf's generation @@ -1274,17 +1284,63 @@ def get_attached_sensors(self, robot_params): sensor_blocks = xmldoc.getElementsByTagName('sensor') for sensor in sensor_blocks: - if sensor.getAttribute('type') == 'camera': - camera = {} - topic = sensor.getElementsByTagName('topic') - if topic: - camera['image_topic'] = '/' + topic[0].firstChild.data - camera['camera_info_topic'] = camera['image_topic'].replace('image_raw', 'camera_info') - attached_sensors['cameras'].append(camera) + sensor_type = sensor.getAttribute('type') + if sensor_type == 'camera': + self.get_attached_camera(attached_sensors, sensor) + elif sensor_type == 'gpu_lidar': + self.get_attached_lidar(attached_sensors, sensor) return attached_sensors # #} + # #{ get_attached_camera(self, attached_sensors, camera_sensor) + def get_attached_camera(self, attached_sensors, camera_sensor) -> None: + camera = {} + topic = camera_sensor.getElementsByTagName('topic') + if topic: + camera['image_topic'] = '/' + topic[0].firstChild.data + camera['camera_info_topic'] = camera['image_topic'].replace('image_raw', 'camera_info') + attached_sensors['cameras'].append(camera) + return + # #} + + # #{ get_attached_lidar(self, attached_sensors, lidar_sensor) + def get_attached_lidar(self, attached_sensors, lidar_sensor) -> None: + + # NOTE: PX4 requires its own bridge with the Garmin rangefinder, so we do not set it up. + # The Garmin rangefinder link is named 'lidar_sensor_link' in the garmin.sdf.jinja template. + # Do not rename the Garmin link or the rangefinder plugin, as PX4 may fail to detect it otherwise. + if lidar_sensor.getAttribute('name') == "lidar_sensor_link": # Garmin rangefinder + return + + lidar = {} + topic = lidar_sensor.getElementsByTagName('topic') + if not topic: + return + lidar['laserscan_topic'] = '/' + topic[0].firstChild.data + + # Differentiate between 1D/2D and 3D LiDARs, since they use different msg type. + # This can be determined by checking the number of vertical samples. + vertical_samples = self.get_number_of_vertical_samples(lidar_sensor) + if vertical_samples == 1: # 2D lidar + attached_sensors['2dlidar'].append(lidar) + elif vertical_samples > 1: # 3D lidar + attached_sensors['3dlidar'].append(lidar) + else: # Incorrect lidar + self.get_logger().error(f"The lidar {lidar_sensor.getAttribute('name')} cannot be loaded. Check if the number of vertical samples is correct.") + return + # #} + + # #{ get_number_of_vertical_samples(self, lidar_sensor) + def get_number_of_vertical_samples(self, lidar_sensor): + vertical_elements = lidar_sensor.getElementsByTagName('vertical') + samples_elements = vertical_elements[0].getElementsByTagName('samples') + vertical_samples = int(samples_elements[0].firstChild.nodeValue.strip()) + + return vertical_samples + # #} + + # #{ generate_uav_ros_gz_config(self, uav_name) def generate_uav_ros_gz_config(self, robot_params): uav_name = robot_params['name'] @@ -1299,15 +1355,34 @@ def generate_uav_ros_gz_config(self, robot_params): sensor_topics = {} + # Camera camera_info_topic_list = [] image_topic_list = [] for camera in attached_sensors['cameras']: camera_info_topic_list.append(camera['camera_info_topic']) image_topic_list.append(camera['image_topic']) + # 1D/2D Lidar + twoD_lidar_topic_list = [] + for lidar in attached_sensors["2dlidar"]: + twoD_lidar_topic_list.append(lidar["laserscan_topic"]) + + + # 3D Lidar + threeD_lidar_topic_list = [] + for lidar in attached_sensors["3dlidar"]: + threeD_lidar_topic_list.append(lidar["laserscan_topic"]) + + + if len(camera_info_topic_list) == 0 and len(twoD_lidar_topic_list)==0 and len(threeD_lidar_topic_list)==0: + self.get_logger().info(f"There are no additional sensors. Skipping launching ros_gz_bridge for {uav_name}") + return "", [] + rendered_template = template.render( - camera_info_topic_list = camera_info_topic_list + camera_info_topic_list = camera_info_topic_list, + twoD_lidar_topic_list = twoD_lidar_topic_list, + threeD_lidar_topic_list = threeD_lidar_topic_list, ) filename = f'ros_gz_bridge_config_{uav_name}.yaml' @@ -1322,6 +1397,11 @@ def generate_uav_ros_gz_config(self, robot_params): return filepath, sensor_topics # #} + # #{ generate_uav_ros_gz_config(self, uav_name) + + # #} + + def main(args=None): rclpy.init(args=args) atexit.register(exit_handler) diff --git a/mrs_uav_gazebo_simulator/utils/sdf_to_tf_publisher.py b/mrs_uav_gazebo_simulator/utils/sdf_to_tf_publisher.py new file mode 100644 index 0000000..26f9b3b --- /dev/null +++ b/mrs_uav_gazebo_simulator/utils/sdf_to_tf_publisher.py @@ -0,0 +1,118 @@ +import xml.etree.ElementTree as ET +import numpy as np +from scipy.spatial.transform import Rotation as R +from tf2_ros import StaticTransformBroadcaster +from geometry_msgs.msg import TransformStamped, Transform + +class SingletonMeta(type): + _instances = {} + + def __call__(cls, *args, **kwargs): + if cls not in cls._instances: + instance = super().__call__(*args, **kwargs) + cls._instances[cls] = instance + return cls._instances[cls] + + +class SdfTfPublisher(metaclass=SingletonMeta): + def __init__(self, ros_node, base_link, ignored_sensors): + self._model_name = "" + self._ignored_sensors = ignored_sensors + self._base_link = base_link + if self._base_link is None: + raise RuntimeError(f"[Sdf2Tf_Publisher] base_link (parent link) is not defined in the config file, cannot create tf publisher") + self._ros_node = ros_node + + def generate_tf_publishers(self, sdf_xml): + root_xml = ET.fromstring(sdf_xml) + model_xml = root_xml.find(".//model") + self._model_name = model_xml.attrib["name"] + + sensor_links_to_poses = self._detect_sensors(model_xml) + sensors_tf = self._detect_sensors_transformations(sensor_links_to_poses) + self._generate_static_tf_broadcasters(sensors_tf) + + def _detect_sensors_transformations(self, sensor_joints): + sensors_Tf = {} + for link_name, poses in sensor_joints.items(): + pose_str = poses["link_pose"] + sensor_pose_str = poses["sensor_pose"] + + # Detect joint pose + if pose_str is None or (pose_str == ""): + self._ros_node.get_logger().info(f"[Sdf2Tf_Publisher] Link {link_name} has no pose, cannot create its tf publisher") + continue + link_pose_rpy = self._str_to_pose(pose_str) + + # Detect sensor offset (optional) + if sensor_pose_str is None or (sensor_pose_str == ""): + self._ros_node.get_logger().info(f"[Sdf2Tf_Publisher] Link {link_name} has no pose specified in its sensor plugin") + sensor_pose_offset = np.zeros(6) + else: + sensor_pose_offset = self._str_to_pose(sensor_pose_str) + + sensors_Tf[link_name] = self._add_pose_with_offset(link_pose_rpy, sensor_pose_offset) + + return sensors_Tf + + def _add_pose_with_offset(self, link_pose, sensor_offset): + T_W_Link = np.eye(4) + T_W_Link[:3, 3] = link_pose[:3] + T_W_Link[:3, :3] = R.from_euler("xyz", link_pose[3:]).as_matrix() + + T_Link_Sensor = np.eye(4) + T_Link_Sensor[:3, 3] = sensor_offset[:3] + T_Link_Sensor[:3, :3] = R.from_euler("xyz", sensor_offset[3:]).as_matrix() + + T_W_Sensor = T_W_Link@T_Link_Sensor + return T_W_Sensor + + def _str_to_pose(self, pose_str): + parts = pose_str.split() + if len(parts) != 6: + raise ValueError(f"[Sdf2Tf_Publisher] Expected 6 elements in pose string, got {len(parts)}: {pose_str}") + + x, y, z, roll, pitch, yaw = map(float, parts) + return np.array([x, y, z, roll, pitch, yaw]) + + def _detect_sensors(self, model_xml): + sensor_links_to_poses = {} + for link in model_xml.findall('.//link'): + for sensor in link.findall('.//sensor'): + if sensor.attrib["name"] not in self._ignored_sensors: + sensor_links_to_poses[link.attrib["name"]] = { + "link_pose" : link.findtext('pose'), + "sensor_pose" : sensor.findtext('pose') + } + return sensor_links_to_poses + + def _generate_static_tf_broadcasters(self, sensors_tf): + broadcaster = StaticTransformBroadcaster(self._ros_node) + time_now = self._ros_node.get_clock().now().to_msg() + + transforms = [] + for link_name, T_W_Sensor in sensors_tf.items(): + t = TransformStamped() + t.header.stamp = time_now + t.header.frame_id = self._model_name + "/" + self._base_link + t.child_frame_id = self._model_name + "/" + link_name + + t.transform = self._get_sensor_pose(T_W_Sensor) + transforms.append(t) + + broadcaster.sendTransform(transforms) + self._ros_node.get_logger().info(f"[Sdf2Tf_Publisher] Published {len(transforms)} static transforms relative to {self._base_link}") + + def _get_sensor_pose(self, T_W_Sensor: np.ndarray) -> Transform: + pose = Transform() + pose.translation.x = T_W_Sensor[0, 3] + pose.translation.y = T_W_Sensor[1, 3] + pose.translation.z = T_W_Sensor[2, 3] + + quat = R.from_matrix(T_W_Sensor[:3, :3]).as_quat() + pose.rotation.x = quat[0] + pose.rotation.y = quat[1] + pose.rotation.z = quat[2] + pose.rotation.w = quat[3] + + return pose diff --git a/tmux/one_drone/session.yml b/tmux/one_drone/session.yml index 5d0689c..55008d0 100644 --- a/tmux/one_drone/session.yml +++ b/tmux/one_drone/session.yml @@ -23,7 +23,7 @@ windows: panes: - ros2 launch mrs_uav_gazebo_simulator simulation.launch.py gz_headless:=false gz_verbose:=true - | - waitForGazebo; sleep 5; ros2 service call /mrs_drone_spawner/spawn mrs_msgs/srv/String "{value: '1 --$UAV_TYPE --enable-rangefinder --enable-bluefox-camera'}" + waitForGazebo; sleep 5; ros2 service call /mrs_drone_spawner/spawn mrs_msgs/srv/String "{value: '1 --$UAV_TYPE --enable-rangefinder'}" - px4_api: layout: tiled panes: