Skip to content

Commit

Permalink
make sensor definitions consistent
Browse files Browse the repository at this point in the history
  • Loading branch information
stibipet committed Nov 15, 2023
1 parent 7f537d5 commit 0a6b969
Showing 1 changed file with 21 additions and 23 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -844,29 +844,27 @@ limitations under the License.

<!-- Macro to add the gps_plugin {-->
{%- macro gps_macro(gps_name, parent_link, update_rate, gps_noise, gps_xy_random_walk, gps_z_random_walk, gps_xy_noise_density, gps_z_noise_density, gps_vxy_noise_density, gps_vz_noise_density, x, y, z, roll, pitch, yaw) -%}
<model name="{{ gps_name }}">
<link name="link">
<pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
{{ zero_inertial_macro() }}
<sensor name="gps_sensor" type="gps">
<plugin name="gps_plugin" filename="libgazebo_gps_plugin.so">
<robotNamespace/>
<update_rate>{{ update_rate }}</update_rate>
<gpsNoise>{{ gps_noise }}</gpsNoise>
<gpsXYRandomWalk>{{ gps_xy_random_walk }}</gpsXYRandomWalk>
<gpsZRandomWalk>{{ gps_z_random_walk }}</gpsZRandomWalk>
<gpsXYNoiseDensity>{{ gps_xy_noise_density }}</gpsXYNoiseDensity>
<gpsZNoiseDensity>{{ gps_z_noise_density }}</gpsZNoiseDensity>
<gpsVXYNoiseDensity>{{ gps_vxy_noise_density }}</gpsVXYNoiseDensity>
<gpsVZNoiseDensity>{{ gps_vz_noise_density }}</gpsVZNoiseDensity>
<topic>{{ gps_name }}</topic>
</plugin>
</sensor>
</link>
</model>
<link name="{{ gps_name }}_link">

This comment has been minimized.

Copy link
@spurnvoj

spurnvoj Nov 15, 2023

Member

@stibipet I left it as a model, so it is the same as for baro, imu and other important sensors that are already model plugins. The GPS plugin was not transported to the model plugin yet in the sitl version (git commit) we use. Therefore, you cannot have for example multiple GPS running.

This comment has been minimized.

Copy link
@stibipet

stibipet Nov 16, 2023

Author Member

I'm sorry but I do not understand your point.

  • Baro, imu and other sensors are added as links, not models. This is why I suggest this change to make it consistent.
  • You can simulate multiple GPS modules (multiple drones, multiple GPS per drone) without nested models.

This comment has been minimized.

Copy link
@spurnvoj

spurnvoj Nov 16, 2023

Member

There was some problem with why it didn't work with ModelPlugin. Here is the PR that I used as a reference when I was working on it PX4/PX4-SITL_gazebo-classic#905. However, They reverted the PR later because it was slowing the simulation. So you still cannot have multiple sensors that would be actively detected by PX4. They will be there, but PX4 will not use them.

This comment has been minimized.

Copy link
@spurnvoj

spurnvoj Nov 16, 2023

Member

I think the problem is/was in the mavlink_plugin in this part https://github.com/ctu-mrs/px4_sitl_gazebo/blob/firmware_1.13.2-deb/src/gazebo_mavlink_interface.cpp#L447. But maybe I remember wrong.

<pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
{{ zero_inertial_macro() }}
<sensor name="gps_sensor" type="gps">
<plugin name="gps_plugin" filename="libgazebo_gps_plugin.so">
<robotNamespace/>
<update_rate>{{ update_rate }}</update_rate>
<gpsNoise>{{ gps_noise }}</gpsNoise>
<gpsXYRandomWalk>{{ gps_xy_random_walk }}</gpsXYRandomWalk>
<gpsZRandomWalk>{{ gps_z_random_walk }}</gpsZRandomWalk>
<gpsXYNoiseDensity>{{ gps_xy_noise_density }}</gpsXYNoiseDensity>
<gpsZNoiseDensity>{{ gps_z_noise_density }}</gpsZNoiseDensity>
<gpsVXYNoiseDensity>{{ gps_vxy_noise_density }}</gpsVXYNoiseDensity>
<gpsVZNoiseDensity>{{ gps_vz_noise_density }}</gpsVZNoiseDensity>
<topic>{{ gps_name }}</topic>
</plugin>
</sensor>
</link>

<joint name='{{ gps_name }}_joint' type='fixed'>
<child>{{ gps_name }}::link</child>
<child>{{ gps_name }}_link</child>
<parent>{{ parent_link }}</parent>
</joint>

Expand Down Expand Up @@ -1238,7 +1236,7 @@ limitations under the License.

<!-- Macro to add a garmin (connected to the pixhawk) {-->
{%- macro garmin_macro(sensor_name, parent_link, x, y, z, roll, pitch, yaw) -%}
<link name="{{ sensor_name }}::link">
<link name="{{ sensor_name }}_link">
<pose>{{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}</pose>
{{ zero_inertial_macro() }}
<visual name="visual">
Expand Down Expand Up @@ -1290,7 +1288,7 @@ limitations under the License.

<joint name="{{ sensor_name }}_joint" type="fixed">
<parent>{{ parent_link }}</parent>
<child>{{ sensor_name }}::link</child>
<child>{{ sensor_name }}_link</child>
</joint>
{%- endmacro -%}
<!--}-->
Expand Down

0 comments on commit 0a6b969

Please sign in to comment.