Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

parametrize spawner params for lidars #3

Merged
merged 4 commits into from
Nov 3, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@ 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 number of horizontal samples for LiDARs', [f450, f550, t650, x500, naki]]
rotation_freq: [None, 'Override default update rate 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]]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -1501,7 +1501,20 @@ limitations under the License.
<!--}-->

<!-- Macro to add a RPlidar A3{-->
{%- macro rplidar_macro(namespace, parent_link, x, y, z, roll, pitch, yaw) -%}
{%- macro rplidar_macro(namespace, parent_link, horizontal_samples, rotation_freq, x, y, z, roll, pitch, yaw) -%}

{% if horizontal_samples == 'None' %}
{%- set samples = 1600 -%}
{% else %}
{%- set samples = horizontal_samples | int -%}
{% endif %}

{% if rotation_freq == 'None' %}
{%- set update_rate = 20 -%}
{% else %}
{%- set update_rate = rotation_freq | int -%}
{% endif %}

<link name="rplidar_link">
<pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
{{ zero_inertial_macro() }}
Expand Down Expand Up @@ -1537,11 +1550,11 @@ limitations under the License.
</visual>
<sensor name='rplidar_sensor' type='ray'>
<visualize>false</visualize>
<update_rate>20</update_rate>
<update_rate>{{ update_rate }}</update_rate>
<ray>
<scan>
<horizontal>
<samples>{{ 14400/20 }}</samples>
<samples>{{ samples }}</samples>
<resolution>1</resolution>
<min_angle>-3.1241390751</min_angle>
<max_angle>3.1241390751</max_angle>
Expand Down Expand Up @@ -1580,7 +1593,7 @@ limitations under the License.
<!--}-->

<!-- Macro to add a Velodyne Lidar {-->
{%- 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, rotation_freq, lasers, max_range, noise, vfov_angle, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%}

{# The real ouster is transforming lidar data from lidar_frame to sensor_frame directly for user. #}
{# For simplicity, we are placing sensor_frame to the same place as the lidar_frame is. #}
Expand Down Expand Up @@ -1612,6 +1625,18 @@ limitations under the License.
{%- set sensor_type = "ray" -%}
{% endif %}

{% if horizontal_samples == 'None' %}
{%- set samples = 3600 -%}
{% else %}
{%- set samples = horizontal_samples | int -%}
{% endif %}

{% if rotation_freq == 'None' %}
{%- set update_rate = 20 -%}
{% else %}
{%- set update_rate = rotation_freq | int -%}
{% endif %}

<link name="velodyne_link">
<pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
{{ zero_inertial_macro() }}
Expand Down Expand Up @@ -1664,11 +1689,11 @@ limitations under the License.
<pose>{{ lidar_x }} {{ lidar_y }} {{ lidar_z }} {{ lidar_roll }} {{ lidar_pitch }} {{ lidar_yaw }}</pose>
{# <pose>0 0 0 0 0 0</pose> #}
<visualize>false</visualize>
<update_rate>{{ rot_freq }}</update_rate>
<update_rate>{{ update_rate }}</update_rate>
<ray>
<scan>
<horizontal>
<samples>{{ 291200/(rot_freq*lasers) }}</samples>
<samples>{{ samples }}</samples>
<resolution>1</resolution>
<min_angle>{{ -rad180 }}</min_angle>
<max_angle>{{ rad180 }}</max_angle>
Expand Down Expand Up @@ -1722,7 +1747,7 @@ limitations under the License.
<!-- Macro to add an Ouster Lidar {-->

<!-- Macro to add an Generic Ouster Lidar {-->
{%- 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, rotation_freq, lasers, max_range, noise, vfov_angle, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%}

{# The real ouster is transforming lidar data from lidar_frame to sensor_frame directly for user. #}
{# For simplicity, we are placing sensor_frame to the same place as the lidar_frame is. #}
Expand Down Expand Up @@ -1765,6 +1790,18 @@ limitations under the License.
{%- set sensor_type = "ray" -%}
{% endif %}

{% if horizontal_samples == 'None' %}
{%- set samples = 2048 -%}
{% else %}
{%- set samples = horizontal_samples | int -%}
{% endif %}

{% if rotation_freq == 'None' %}
{%- set update_rate = 10 -%}
{% else %}
{%- set update_rate = rotation_freq | int -%}
{% endif %}

<link name="{{ sensor_link }}">
<pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
{{ zero_inertial_macro() }}
Expand Down Expand Up @@ -1798,7 +1835,7 @@ limitations under the License.
</script>
</material>
</visual>

{# IMU #}
<sensor name="{{ sensor_name }}_imu_sensor" type="imu">
<pose>{{ imu_x }} {{ imu_y }} {{ imu_z }} {{ imu_roll }} {{ imu_pitch }} {{ imu_yaw }}</pose>
Expand Down Expand Up @@ -1829,17 +1866,17 @@ limitations under the License.
<!-- <headingGaussianNoise>0.005</headingGaussianNoise> -->
<!-- </plugin> -->
</sensor>

{# LIDAR #}
<sensor name="{{ sensor_name }}_lidar_sensor" type="{{ sensor_type }}">
<pose>{{ lidar_x }} {{ lidar_y }} {{ lidar_z }} {{ lidar_roll }} {{ lidar_pitch }} {{ lidar_yaw }}</pose>
{# <pose>0 0 0 0 0 0</pose> #}
<visualize>false</visualize>
<update_rate>{{ rot_freq }}</update_rate>
<update_rate>{{ update_rate }}</update_rate>
<ray>
<scan>
<horizontal>
<samples>{{ 20480/rot_freq }}</samples>
<samples>{{ samples }}</samples>
<resolution>1</resolution>
<min_angle>0</min_angle>
<max_angle>{{ 2*pi }}</max_angle>
Expand Down Expand Up @@ -1902,7 +1939,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, rotation_freq, noise, enable_gpu_ray, horizontal_samples, x, y, z, roll, pitch, yaw) -%}

<!-- Outster type selection {-->
{# default: OS1-16 Generation 1 - is specified in the config file #}
Expand Down Expand Up @@ -1993,12 +2030,13 @@ limitations under the License.
namespace = namespace,
parent_link = parent_link,
sensor_name = sensor_name,
rot_freq = rot_freq,
rotation_freq = rotation_freq,
lasers = lasers,
vfov_angle = vfov_angle,
max_range = range,
noise = noise,
enable_gpu_ray = enable_gpu_ray,
horizontal_samples = horizontal_samples,
x = x,
y = y,
z = z,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -857,6 +857,8 @@
{{ components.rplidar_macro(
namespace = namespace,
parent_link = root,
horizontal_samples = horizontal_samples,
rotation_freq = rotation_freq,
x = 0.0,
y = 0.0,
z = 0.1,
Expand All @@ -876,12 +878,13 @@
namespace = namespace,
parent_link = root,
sensor_name = "velodyne",
rot_freq = 20,
rotation_freq = rotation_freq,
lasers = 16,
vfov_angle = 30,
max_range = 100,
noise = 0.01,
enable_gpu_ray = use_gpu_ray,
horizontal_samples = horizontal_samples,
x = 0.0,
y = 0.0,
z = 0.066,
Expand All @@ -901,9 +904,10 @@
parent_link = root,
sensor_name = "os",
ouster_model = ouster_model,
rot_freq = 10,
rotation_freq = rotation_freq,
noise = 0.03,
enable_gpu_ray = use_gpu_ray,
horizontal_samples = horizontal_samples,
x = 0.0,
y = 0.0,
z = 0.066,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -1006,6 +1006,8 @@
{{ components.rplidar_macro(
namespace = namespace,
parent_link = root,
horizontal_samples = horizontal_samples,
rotation_freq = rotation_freq,
x = 0.0,
y = 0.0,
z = 0.1,
Expand All @@ -1025,12 +1027,13 @@
namespace = namespace,
parent_link = root,
sensor_name = "velodyne",
rot_freq = 20,
rotation_freq = rotation_freq,
lasers = 16,
vfov_angle = 30,
max_range = 100,
noise = 0.01,
enable_gpu_ray = use_gpu_ray,
horizontal_samples = horizontal_samples,
x = 0.0,
y = 0.0,
z = 0.066,
Expand All @@ -1050,9 +1053,10 @@
parent_link = root,
sensor_name = "os",
ouster_model = ouster_model,
rot_freq = 10,
rotation_freq = rotation_freq,
noise = 0.03,
enable_gpu_ray = use_gpu_ray,
horizontal_samples = horizontal_samples,
x = 0.0,
y = 0.0,
z = 0.066,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -782,6 +782,8 @@
{{ components.rplidar_macro(
namespace = namespace,
parent_link = root,
horizontal_samples = horizontal_samples,
rotation_freq = rotation_freq,
x = 0.0,
y = 0.0,
z = 0.09,
Expand All @@ -801,9 +803,10 @@
parent_link = root,
sensor_name = "os",
ouster_model = ouster_model,
rot_freq = 10,
rotation_freq = rotation_freq,
noise = 0.03,
enable_gpu_ray = use_gpu_ray,
horizontal_samples = horizontal_samples,
x = 0.0,
y = 0.0,
z = 0.0611,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -213,7 +213,7 @@
yaw = 0)
}}
<!--}-->

<!-- pixhawk {-->
{{ components.visual_mesh_macro(
name = "pixhawk",
Expand Down Expand Up @@ -392,7 +392,7 @@
}}
<!--}-->
{% endif %}

{% if enable_rplidar or fire_challenge_blanket %}
<!-- RPLidar mount {-->
{{ components.visual_mesh_macro(
Expand All @@ -409,7 +409,7 @@
}}
<!--}-->
{% endif %}

{% if enable_realsense_front %}
<!-- Realsense mount {-->
{{ components.visual_mesh_macro(
Expand All @@ -426,7 +426,7 @@
}}
<!--}-->
{% endif %}

{% if enable_ouster or enable_velodyne %}
<!-- Ouster mount {-->
{{ components.visual_mesh_macro(
Expand All @@ -443,7 +443,7 @@
}}
<!--}-->
{% endif %}

<!--}-->
</link>

Expand Down Expand Up @@ -760,6 +760,8 @@
{{ components.rplidar_macro(
namespace = namespace,
parent_link = root,
horizontal_samples = horizontal_samples,
rotation_freq = rotation_freq,
x = 0.0,
y = 0.0,
z = 0.107,
Expand All @@ -779,12 +781,13 @@
namespace = namespace,
parent_link = root,
sensor_name = "velodyne",
rot_freq = 20,
rotation_freq = rotation_freq,
lasers = 16,
vfov_angle = 30,
max_range = 100,
noise = 0.01,
enable_gpu_ray = use_gpu_ray,
horizontal_samples = horizontal_samples,
x = 0.0,
y = 0.0,
z = 0.073,
Expand All @@ -804,9 +807,10 @@
parent_link = root,
sensor_name = "os",
ouster_model = ouster_model,
rot_freq = 10,
rotation_freq = rotation_freq,
noise = 0.03,
enable_gpu_ray = use_gpu_ray,
horizontal_samples = horizontal_samples,
x = 0.0,
y = 0.0,
z = 0.073,
Expand All @@ -817,7 +821,7 @@
<!--}-->
{% endif %}
{# <!--}--> #}

<!-- ========================= camera sensors ========================= -->

<!-- Bluefox camera placements{-->
Expand Down Expand Up @@ -1269,10 +1273,10 @@
yaw=-2.5665787)
}}
<!--}-->

<!--}-->
{% endif %}

{% if fire_challenge_blanket %}
<!-- fire_challenge_blanket {-->

Expand All @@ -1289,7 +1293,7 @@
yaw = 0)
}}
<!--}-->

<!-- rplidar {-->
{{ components.rplidar_macro(
namespace = namespace,
Expand Down Expand Up @@ -1344,7 +1348,7 @@
yaw = 0)
}}
<!--}-->

<!--}-->
{% endif %}
{# <!--}--> #}
Expand Down
Loading