Skip to content

Commit 781b94d

Browse files
stibipetstibipet
authored
Merge pull request #28 from ctu-mrs/ft_ros2_imu
Updated IMU plugin, enabled standalone IMU sensors Added IMU publisher to ouster Bugfix: corrected pointcloud frame id for all lidars
2 parents 90a30a8 + 80642ad commit 781b94d

File tree

18 files changed

+186
-56
lines changed

18 files changed

+186
-56
lines changed

‎config/uav_ros_gz_bridge_config.yaml.jinja‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,3 +21,11 @@
2121
gz_type_name: "gz.msgs.PointCloudPacked"
2222
direction: GZ_TO_ROS
2323
{% endfor %}
24+
25+
{%- for topic in imu_topic_list -%}
26+
- ros_topic_name: "{{ topic["ros"] }}"
27+
gz_topic_name: "{{ topic["gazebo"] }}"
28+
ros_type_name: "sensor_msgs/msg/Imu"
29+
gz_type_name: "gz.msgs.IMU"
30+
direction: GZ_TO_ROS
31+
{% endfor %}

‎models/mrs_robots_description/sdf/README.md‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -350,8 +350,10 @@ gz topic -i -t topic_name
350350
- if there is `no subscriber` the topic is not being subscribed by `ros_gz_bridge`
351351
Both issues are most likely caused by incorrect topic names. Verify that the topic names in your sensor’s SDF file do not violate the rules described above, and check the autogenerated `ros_gz_bridge` config file located in the `/tmp` directory.
352352

353-
354-
353+
#### Tests
354+
There are unit tests that can partially verify the Jinja syntax within macros.
355+
Be aware that these tests only ensure the Jinja templates can be rendered successfully. They do not check whether the resulting SDF is valid. Additionally, there is a `test_camera_topic_names` test that verifies the naming conventions for all types of camera sensors.
356+
These tests automatically detect the sensor folders, so you should not need to modify them.
355357

356358

357359

‎models/mrs_robots_description/sdf/components/generic_components.sdf.jinja‎

Lines changed: 80 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -634,7 +634,80 @@ limitations under the License.
634634
{# <!--}--> #}
635635

636636
{# imu_sensor_macro {--> #}
637-
{%- macro imu_sensor_macro(name, update_rate, angular_velocity_noise_mean_x, angular_velocity_noise_std_x, angular_velocity_noise_mean_y, angular_velocity_noise_std_y,angular_velocity_noise_mean_z, angular_velocity_noise_std_z, linear_acceleration_noise_mean_x, linear_acceleration_noise_std_x, linear_acceleration_noise_mean_y, linear_acceleration_noise_std_y, linear_acceleration_noise_mean_z, linear_acceleration_noise_std_z) -%}
637+
{%- macro imu_sensor_macro(name, update_rate,
638+
imu_sdf_topic_name, imu_gz_topic_name, imu_ros_topic_name,
639+
sensor_x, sensor_y, sensor_z, sensor_roll, sensor_pitch, sensor_yaw,
640+
angular_velocity_noise_mean_x, angular_velocity_noise_std_x,
641+
angular_velocity_noise_mean_y, angular_velocity_noise_std_y,
642+
angular_velocity_noise_mean_z, angular_velocity_noise_std_z,
643+
linear_acceleration_noise_mean_x, linear_acceleration_noise_std_x,
644+
linear_acceleration_noise_mean_y, linear_acceleration_noise_std_y,
645+
linear_acceleration_noise_mean_z, linear_acceleration_noise_std_z) -%}
646+
<sensor name="{{ name }}/imu" type="imu">
647+
<pose>{{ sensor_x }} {{ sensor_y }} {{ sensor_z }} {{ sensor_roll }} {{ sensor_pitch }} {{ sensor_yaw }}</pose>
648+
<always_on>1</always_on>
649+
<update_rate>{{ update_rate }}</update_rate>
650+
<topic>{{ imu_sdf_topic_name }}</topic>
651+
<imu_gz_topic>{{ imu_gz_topic_name }}</imu_gz_topic>
652+
<imu_ros_topic>{{ imu_ros_topic_name }}</imu_ros_topic>
653+
<imu>
654+
<angular_velocity>
655+
<!-- 0.05 deg/s converted to rad/s -->
656+
<x>
657+
<noise type="gaussian">
658+
<mean>{{ angular_velocity_noise_mean_x }}</mean>
659+
<stddev>{{ angular_velocity_noise_std_x }}</stddev>
660+
</noise>
661+
</x>
662+
<y>
663+
<noise type="gaussian">
664+
<mean>{{ angular_velocity_noise_mean_y }}</mean>
665+
<stddev>{{ angular_velocity_noise_std_y }}</stddev>
666+
</noise>
667+
</y>
668+
<z>
669+
<noise type="gaussian">
670+
<mean>{{ angular_velocity_noise_mean_z }}</mean>
671+
<stddev>{{ angular_velocity_noise_std_z }}</stddev>
672+
</noise>
673+
</z>
674+
</angular_velocity>
675+
<linear_acceleration>
676+
<!-- X & Y axis: 0.65 mg-rms converted to m/ss -->
677+
<x>
678+
<noise type="gaussian">
679+
<mean>{{ linear_acceleration_noise_mean_x }}</mean>
680+
<stddev>{{ linear_acceleration_noise_std_x }}</stddev>
681+
</noise>
682+
</x>
683+
<y>
684+
<noise type="gaussian">
685+
<mean>{{ linear_acceleration_noise_mean_y }}</mean>
686+
<stddev>{{ linear_acceleration_noise_std_y }}</stddev>
687+
</noise>
688+
</y>
689+
<!-- Z axis: 0.70 mg-rms converted to m/ss-->
690+
<z>
691+
<noise type="gaussian">
692+
<mean>{{ linear_acceleration_noise_mean_z }}</mean>
693+
<stddev>{{ linear_acceleration_noise_std_z }}</stddev>
694+
</noise>
695+
</z>
696+
</linear_acceleration>
697+
</imu>
698+
</sensor>
699+
{%- endmacro -%}
700+
{# <!--}--> #}
701+
702+
703+
{# imu_pixhawk_sensor_macro {--> #}
704+
{%- macro imu_pixhawk_sensor_macro(name, update_rate,
705+
angular_velocity_noise_mean_x, angular_velocity_noise_std_x,
706+
angular_velocity_noise_mean_y, angular_velocity_noise_std_y,
707+
angular_velocity_noise_mean_z, angular_velocity_noise_std_z,
708+
linear_acceleration_noise_mean_x, linear_acceleration_noise_std_x,
709+
linear_acceleration_noise_mean_y, linear_acceleration_noise_std_y,
710+
linear_acceleration_noise_mean_z, linear_acceleration_noise_std_z) -%}
638711
<sensor name="{{ name }}_sensor" type="imu">
639712
<always_on>1</always_on>
640713
<update_rate>{{ update_rate }}</update_rate>
@@ -683,21 +756,22 @@ limitations under the License.
683756
</z>
684757
</linear_acceleration>
685758
</imu>
686-
</sensor>{%- endmacro -%}
759+
</sensor>
760+
{%- endmacro -%}
687761
{# <!--}--> #}
688762

689763
{# gazebo_twod_lidar_plugin_macro {--> #}
690764
{# --- sensor is always defined with respect to the sensor_link frame #}
691765
{%- macro gazebo_twod_lidar_plugin_macro(
692-
lidar_name, lidar_gz_frame_id, lidar_sdf_topic_name, lidar_gz_topic_name, lidar_ros_topic_name,
766+
lidar_gz_frame_id, lidar_sdf_topic_name, lidar_gz_topic_name, lidar_ros_topic_name,
693767
sensor_x, sensor_y, sensor_z, sensor_roll, sensor_pitch, sensor_yaw,
694768
update_rate,
695769
horiz_num_samples, horiz_resolution, horiz_min_angle, horiz_max_angle,
696770
range_min, range_max, range_resolution,
697771
noise_mean, noise_std
698772
) -%}
699773

700-
<sensor name="{{ lidar_name }}/sensor" type='gpu_lidar'>
774+
<sensor name="{{ lidar_gz_frame_id }}" type='gpu_lidar'>
701775
<pose>{{ sensor_x }} {{ sensor_y }} {{ sensor_z }} {{ sensor_roll }} {{ sensor_pitch }} {{ sensor_yaw }}</pose>
702776
<topic>{{ lidar_sdf_topic_name }}</topic>
703777
<update_rate>{{ update_rate }}</update_rate>
@@ -740,7 +814,7 @@ limitations under the License.
740814
{# gazebo_threed_lidar_plugin_macro {--> #}
741815
{# --- sensor is always defined with respect to the sensor_link frame #}
742816
{%- macro gazebo_threed_lidar_plugin_macro(
743-
lidar_name, lidar_gz_frame_id, lidar_sdf_topic_name, lidar_gz_topic_name, lidar_ros_topic_name,
817+
lidar_gz_frame_id, lidar_sdf_topic_name, lidar_gz_topic_name, lidar_ros_topic_name,
744818
sensor_x, sensor_y, sensor_z, sensor_roll, sensor_pitch, sensor_yaw,
745819
update_rate,
746820
horiz_num_samples, horiz_resolution, horiz_min_angle, horiz_max_angle,
@@ -749,7 +823,7 @@ limitations under the License.
749823
noise_mean, noise_std
750824
) -%}
751825

752-
<sensor name="{{ lidar_name }}/sensor" type='gpu_lidar'>
826+
<sensor name="{{ lidar_gz_frame_id }}" type='gpu_lidar'>
753827
<pose>{{ sensor_x }} {{ sensor_y }} {{ sensor_z }} {{ sensor_roll }} {{ sensor_pitch }} {{ sensor_yaw }}</pose>
754828
<update_rate>{{ update_rate }}</update_rate>
755829
<gz_frame_id>{{ lidar_gz_frame_id }}</gz_frame_id>

‎models/mrs_robots_description/sdf/components/lidar/ouster/ouster.sdf.jinja‎

Lines changed: 24 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
{%- set sensor_name = 'ouster' -%}
1616
{%- set ouster_model = spawner_args[spawner_keyword]['model'] -%}
1717
{%- set sensor_link_name = sensor_name -%}
18-
{%- set sensor_gz_frame_id = spawner_args['name'] + '/' + sensor_name + '/sensor' -%}
18+
{%- set lidar_gz_frame_id = spawner_args['name'] + '/' + sensor_name + '/lidar' -%}
1919

2020
{# OS0 {--> #}
2121

@@ -163,8 +163,7 @@
163163

164164
<!-- sensor + plugin {-->
165165
{{ generic.gazebo_threed_lidar_plugin_macro(
166-
lidar_name = sensor_name,
167-
lidar_gz_frame_id = sensor_gz_frame_id,
166+
lidar_gz_frame_id = lidar_gz_frame_id,
168167
lidar_sdf_topic_name = spawner_args['name'] + '/' + sensor_name,
169168
lidar_gz_topic_name = spawner_args['name'] + '/' + sensor_name + '/points',
170169
lidar_ros_topic_name = spawner_args['name'] + '/' + sensor_name + '/points',
@@ -182,8 +181,28 @@
182181
range_max = range,
183182
range_resolution = 0.03,
184183
noise_mean = 0.0,
185-
noise_std= spawner_args[spawner_keyword]['noise']
186-
)}}
184+
noise_std= spawner_args[spawner_keyword]['noise'])
185+
}}
186+
{{ generic.imu_sensor_macro(
187+
name = sensor_name,
188+
update_rate = 250,
189+
imu_sdf_topic_name = spawner_args['name'] + '/' + sensor_name + '/imu',
190+
imu_gz_topic_name = spawner_args['name'] + '/' + sensor_name + '/imu',
191+
imu_ros_topic_name = spawner_args['name'] + '/' + sensor_name + '/imu',
192+
sensor_x = 0.0, sensor_y = 0.0, sensor_z = 0.0, sensor_roll = 0.0, sensor_pitch = 0.0, sensor_yaw = 0.0,
193+
angular_velocity_noise_mean_x = 0.0,
194+
angular_velocity_noise_std_x = 0.0008726646,
195+
angular_velocity_noise_mean_y = 0.0,
196+
angular_velocity_noise_std_y = 0.0008726646,
197+
angular_velocity_noise_mean_z = 0.0,
198+
angular_velocity_noise_std_z = 0.0008726646,
199+
linear_acceleration_noise_mean_x = 0.0,
200+
linear_acceleration_noise_std_x = 0.00637,
201+
linear_acceleration_noise_mean_y = 0.0,
202+
linear_acceleration_noise_std_y = 0.00637,
203+
linear_acceleration_noise_mean_z = 0.0,
204+
linear_acceleration_noise_std_z = 0.00686)
205+
}}
187206
<!--}-->
188207

189208
</link>

‎models/mrs_robots_description/sdf/components/lidar/rplidar/rplidar.sdf.jinja‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515

1616
{%- set sensor_name = 'rplidar' -%}
1717
{%- set sensor_link_name = sensor_name -%}
18-
{%- set sensor_gz_frame_id = spawner_args['name'] + '/' + sensor_name + '/sensor' -%}
18+
{%- set lidar_gz_frame_id = spawner_args['name'] + '/' + sensor_name + '/lidar' -%}
1919

2020
{# -- tf from link to sensor -- #}
2121
{%- set lidar_x = 0.0 -%}
@@ -60,8 +60,7 @@
6060
<!--}-->
6161

6262
{{ generic.gazebo_twod_lidar_plugin_macro(
63-
lidar_name = sensor_name,
64-
lidar_gz_frame_id = sensor_gz_frame_id,
63+
lidar_gz_frame_id = lidar_gz_frame_id,
6564
lidar_sdf_topic_name = spawner_args['name'] + '/' + sensor_name + '/scan',
6665
lidar_gz_topic_name = spawner_args['name'] + '/' + sensor_name + '/scan',
6766
lidar_ros_topic_name = spawner_args['name'] + '/' + sensor_name + '/scan',

‎models/mrs_robots_description/sdf/components/lidar/scanse_sweep/scanse_sweep.sdf.jinja‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@
1414
<!-- scanse sweep {-->
1515
{%- set sensor_name = 'scanse_sweep' -%}
1616
{%- set sensor_link_name = sensor_name -%}
17-
{%- set sensor_gz_frame_id = spawner_args['name'] + '/' + sensor_name + '/sensor' -%}
17+
{%- set lidar_gz_frame_id = spawner_args['name'] + '/' + sensor_name + '/lidar' -%}
1818

1919
{# -- tf from link to sensor -- #}
2020
{%- set lidar_x = 0.0 -%}
@@ -61,8 +61,7 @@
6161
<!--}-->
6262

6363
{{ generic.gazebo_twod_lidar_plugin_macro(
64-
lidar_name = sensor_name,
65-
lidar_gz_frame_id = sensor_gz_frame_id,
64+
lidar_gz_frame_id = lidar_gz_frame_id,
6665
lidar_sdf_topic_name = spawner_args['name'] + '/' + sensor_name + '/scan',
6766
lidar_gz_topic_name = spawner_args['name'] + '/' + sensor_name + '/scan',
6867
lidar_ros_topic_name = spawner_args['name'] + '/' + sensor_name + '/scan',

‎models/mrs_robots_description/sdf/components/lidar/velodyne/velodyne.sdf.jinja‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@
2929
<!-- Velodyne {-->
3030
{%- set sensor_name = 'velodyne' -%}
3131
{%- set sensor_link_name = sensor_name -%}
32-
{%- set sensor_gz_frame_id = spawner_args['name'] + '/' + sensor_name + '/sensor' -%}
32+
{%- set lidar_gz_frame_id = spawner_args['name'] + '/' + sensor_name + '/lidar' -%}
3333

3434
<link name="{{ sensor_link_name }}">
3535
<pose relative_to="{{ parent_link }}">{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
@@ -86,8 +86,7 @@
8686

8787
<!-- sensor + plugin {-->
8888
{{ generic.gazebo_threed_lidar_plugin_macro(
89-
lidar_name = sensor_name,
90-
lidar_gz_frame_id = sensor_gz_frame_id,
89+
lidar_gz_frame_id = lidar_gz_frame_id,
9190
lidar_sdf_topic_name = spawner_args['name'] + '/' + sensor_name,
9291
lidar_gz_topic_name = spawner_args['name'] + '/' + sensor_name + '/points',
9392
lidar_ros_topic_name = spawner_args['name'] + '/' + sensor_name + '/points',

‎models/mrs_robots_description/sdf/drones/f330.sdf.jinja‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -446,7 +446,7 @@
446446

447447
<!-- IMU {-->
448448
<!-- Noise modeled after IIM42653 -->
449-
{{ generic.imu_sensor_macro(
449+
{{ generic.imu_pixhawk_sensor_macro(
450450
name = "imu",
451451
update_rate = 250,
452452
angular_velocity_noise_mean_x = 0.0,

‎models/mrs_robots_description/sdf/drones/f450.sdf.jinja‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -568,7 +568,7 @@
568568

569569
<!-- IMU {-->
570570
<!-- Noise modeled after IIM42653 -->
571-
{{ generic.imu_sensor_macro(
571+
{{ generic.imu_pixhawk_sensor_macro(
572572
name = "imu",
573573
update_rate = 250,
574574
angular_velocity_noise_mean_x = 0.0,

‎models/mrs_robots_description/sdf/drones/f550.sdf.jinja‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -596,7 +596,7 @@
596596

597597
<!-- IMU {-->
598598
<!-- Noise modeled after IIM42653 -->
599-
{{ generic.imu_sensor_macro(
599+
{{ generic.imu_pixhawk_sensor_macro(
600600
name = "imu",
601601
update_rate = 250,
602602
angular_velocity_noise_mean_x = 0.0,

0 commit comments

Comments
 (0)