diff --git a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml index 10883ed..14bdd93 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml +++ b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml @@ -28,6 +28,7 @@ enable_velodyne: [False, 'Add Velodyne PUCK Lite laser scanner to the vehicle', enable_ouster: [False, 'Add Ouster laser scanner to the vehicle', [f450, f550, t650, x500, eaglemk2, naki, big_dofec]] ouster_model: ['OS1-16', 'Choose the Ouster model. Options: OS0-32, OS0-64, OS0-128, OS1-16, OS1-32G1, OS1-32, OS1-64, OS1-128, OS2-32, OS2-64, OS2-128', [f450, f550, t650, x500, eaglemk2, naki, big_dofec]] use_gpu_ray: [False, 'Laser ray casting will be handled by GPU shaders', [f330, f450, f550, t650, x500, eaglemk2, naki, big_dofec]] +horizontal_samples: [None, 'Override default base number of horizontal samples for LiDARs', [f450, f550, t650, x500, naki]] enable_light: [False, 'Add light to the vehicle', [f450, f550, t650, x500, naki]] enable_servo_camera: [False, 'Add camera on virtual servo to the vehicle', [f450, f550, t650, naki]] enable_water_gun: [False, 'Add water gun for fire fighting', [t650]] diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja index 2b5b28f..7d3b374 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja @@ -1501,7 +1501,12 @@ limitations under the License. -{%- macro rplidar_macro(namespace, parent_link, x, y, z, roll, pitch, yaw) -%} +{%- macro rplidar_macro(namespace, parent_link, horizontal_samples, x, y, z, roll, pitch, yaw) -%} +{% if horizontal_samples == 'None' %} + {%- set samples = 14400 -%} +{% else %} + {%- set samples = horizontal_samples | int -%} +{% endif %} {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} {{ zero_inertial_macro() }} @@ -1541,7 +1546,7 @@ limitations under the License. - {{ 14400/20 }} + {{ samples/20 }} 1 -3.1241390751 3.1241390751 @@ -1580,7 +1585,7 @@ limitations under the License. -{%- macro velodyne_macro(namespace, parent_link, sensor_name, rot_freq, lasers, max_range, noise, vfov_angle, enable_gpu_ray, x, y, z, roll, pitch, yaw) -%} +{%- macro velodyne_macro(namespace, parent_link, sensor_name, rot_freq, lasers, max_range, noise, vfov_angle, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%} {# The real ouster is transforming lidar data from lidar_frame to sensor_frame directly for user. #} {# For simplicity, we are placing sensor_frame to the same place as the lidar_frame is. #} @@ -1612,6 +1617,12 @@ limitations under the License. {%- set sensor_type = "ray" -%} {% endif %} +{% if horizontal_samples == 'None' %} + {%- set samples = 291200 -%} +{% else %} + {%- set samples = horizontal_samples | int -%} +{% endif %} + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} {{ zero_inertial_macro() }} @@ -1668,7 +1679,7 @@ limitations under the License. - {{ 291200/(rot_freq*lasers) }} + {{ samples/(rot_freq*lasers) }} 1 {{ -rad180 }} {{ rad180 }} @@ -1722,7 +1733,7 @@ limitations under the License. -{%- macro generic_ouster_macro(namespace, parent_link, sensor_name, rot_freq, lasers, max_range, noise, vfov_angle, enable_gpu_ray, x, y, z, roll, pitch, yaw) -%} +{%- macro generic_ouster_macro(namespace, parent_link, sensor_name, rot_freq, lasers, max_range, noise, vfov_angle, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%} {# The real ouster is transforming lidar data from lidar_frame to sensor_frame directly for user. #} {# For simplicity, we are placing sensor_frame to the same place as the lidar_frame is. #} @@ -1765,6 +1776,12 @@ limitations under the License. {%- set sensor_type = "ray" -%} {% endif %} +{% if horizontal_samples == 'None' %} + {%- set samples = 20480 -%} +{% else %} + {%- set samples = horizontal_samples | int -%} +{% endif %} + {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }} {{ zero_inertial_macro() }} @@ -1798,7 +1815,7 @@ limitations under the License. - + {# IMU #} {{ imu_x }} {{ imu_y }} {{ imu_z }} {{ imu_roll }} {{ imu_pitch }} {{ imu_yaw }} @@ -1829,7 +1846,7 @@ limitations under the License. - + {# LIDAR #} {{ lidar_x }} {{ lidar_y }} {{ lidar_z }} {{ lidar_roll }} {{ lidar_pitch }} {{ lidar_yaw }} @@ -1839,7 +1856,7 @@ limitations under the License. - {{ 20480/rot_freq }} + {{ samples/rot_freq }} 1 0 {{ 2*pi }} @@ -1902,7 +1919,7 @@ limitations under the License. {%- endmacro -%} -{%- macro ouster_macro(namespace, parent_link, sensor_name, ouster_model, rot_freq, noise, enable_gpu_ray, x, y, z, roll, pitch, yaw) -%} +{%- macro ouster_macro(namespace, parent_link, sensor_name, ouster_model, rot_freq, noise, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%} {# default: OS1-16 Generation 1 - is specified in the config file #} @@ -1999,6 +2016,7 @@ limitations under the License. max_range = range, noise = noise, enable_gpu_ray = enable_gpu_ray, + horizontal_samples = horizontal_samples, x = x, y = y, z = z, diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f450.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f450.sdf.jinja index e596537..4d34495 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f450.sdf.jinja +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f450.sdf.jinja @@ -857,6 +857,7 @@ {{ components.rplidar_macro( namespace = namespace, parent_link = root, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.1, @@ -882,6 +883,7 @@ max_range = 100, noise = 0.01, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.066, @@ -904,6 +906,7 @@ rot_freq = 10, noise = 0.03, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.066, diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f550.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f550.sdf.jinja index abe26c5..83934df 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f550.sdf.jinja +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/f550.sdf.jinja @@ -1006,6 +1006,7 @@ {{ components.rplidar_macro( namespace = namespace, parent_link = root, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.1, @@ -1031,6 +1032,7 @@ max_range = 100, noise = 0.01, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.066, @@ -1053,6 +1055,7 @@ rot_freq = 10, noise = 0.03, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.066, diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/naki.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/naki.sdf.jinja index b82c923..b821cb8 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/naki.sdf.jinja +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/naki.sdf.jinja @@ -782,6 +782,7 @@ {{ components.rplidar_macro( namespace = namespace, parent_link = root, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.09, @@ -804,6 +805,7 @@ rot_freq = 10, noise = 0.03, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.0611, diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/t650.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/t650.sdf.jinja index ba35a1b..9d0ee68 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/t650.sdf.jinja +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/t650.sdf.jinja @@ -213,7 +213,7 @@ yaw = 0) }} - + {{ components.visual_mesh_macro( name = "pixhawk", @@ -392,7 +392,7 @@ }} {% endif %} - + {% if enable_rplidar or fire_challenge_blanket %} {{ components.visual_mesh_macro( @@ -409,7 +409,7 @@ }} {% endif %} - + {% if enable_realsense_front %} {{ components.visual_mesh_macro( @@ -426,7 +426,7 @@ }} {% endif %} - + {% if enable_ouster or enable_velodyne %} {{ components.visual_mesh_macro( @@ -443,7 +443,7 @@ }} {% endif %} - + @@ -760,6 +760,7 @@ {{ components.rplidar_macro( namespace = namespace, parent_link = root, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.107, @@ -785,6 +786,7 @@ max_range = 100, noise = 0.01, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.073, @@ -807,6 +809,7 @@ rot_freq = 10, noise = 0.03, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.073, @@ -817,7 +820,7 @@ {% endif %} {# #} - + @@ -1269,10 +1272,10 @@ yaw=-2.5665787) }} - + {% endif %} - + {% if fire_challenge_blanket %} @@ -1289,7 +1292,7 @@ yaw = 0) }} - + {{ components.rplidar_macro( namespace = namespace, @@ -1344,7 +1347,7 @@ yaw = 0) }} - + {% endif %} {# #} diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/x500.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/x500.sdf.jinja index 94dbe4e..b00afb9 100644 --- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/x500.sdf.jinja +++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/x500.sdf.jinja @@ -832,6 +832,7 @@ {{ components.rplidar_macro( namespace = namespace, parent_link = root, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.136, @@ -854,6 +855,7 @@ rot_freq = 10, noise = 0.03, enable_gpu_ray = use_gpu_ray, + horizontal_samples = horizontal_samples, x = 0.0, y = 0.0, z = 0.107,