srmainwaring / asv_wave_sim

This package contains plugins that support the simulation of waves and surface vessels in Gazebo.
GNU General Public License v3.0
114 stars 31 forks source link

Determining wave height using ray sensor in Gazebo #64

Closed ConnorDTaylor closed 1 year ago

ConnorDTaylor commented 1 year ago

This questions was asked to Rhys via email, here is the conversation history up to this point for anyone interested.

My initial email: I am trying to use your ASV wave simulator and I have some issues with the collision of the waves. I am trying to use sonar sensors to measure the wave height in Gazebo, but the waves being generated are purely visual and have no collision (which you know).

So, I am wondering: is there any way to apply collision to the visual wavefield? Or is there a way to apply the Gerster wave .vert and .frag files to some sort of collision mesh so I can have my sensors detect the water? I am working on Ubuntu 18.04 with ROS Melodic.

Any help you can provide would be greatly appreciated.

The first response from Rhys: The physical aspects of the waves are handled by the buoyancy and hydrodynamics physics plugins that are attached to the model. If the waves had a collision as well then the objects would sit on top of it (like a terrain), and there would be a significant performance impact as the collision algorithm would need to examine all the wave triangles.

The waves do return depth information when sensed by a depth camera, so perhaps another approach would be to look into how your sonar sensor is getting a return. Is it just the standard sensor available with Gazebo or something custom?

Second email to Rhys from me: I figured giving the waves' collision would cause the simulation to be too computationally intense (especially for my hardware I think), though my solution for this would be to make the wave area as small as possible.

As for my sensor, I am using the sonar sensor from the Hector Quadrotor package. I believe this is simply a ray that looks for something to collide with to return a height.

To solve this problem I was considering trying to adjust the sensor to detect points based on the Gerstner waves being generated by the Wavefield script. However, I am confused about what element of this file defines the calculated wavefield at each time step, could you provide any insight into this?

I am looking to pull the z components of all of the points in the wavefield so that I can use these as conditions for sensor detection.

Answer to the second email from Rhys: I think the approach you’re looking for may be similar to the way a GPU LIDAR sensor is implemented in Gazebo. I think this does record a return from the waves (even for the earlier version using the Gerstner waves scripts).

There is a simple wave example included in the more recent Gazebo distributions. I’ll take a look at whether that example results in a return from the waves for a start and from that we may come up with a way for you to estimate wave heights from a range sensor.

A method of last resort would be to use the same ray intersection calculation used for the wave height in the hydrodynamics plugin (but tracing the ray at a given angle from the sensor location). That would be less efficient than a GPU based approach using the scene depth buffer though.

The rest of this conversation will be had here so anyone can access the information. Thanks for the help Rhys!

srmainwaring commented 1 year ago

Hi @ConnorDTaylor, the gpu_lidar sensor registers returns from the wave surface when using the latest version of the wave plugin with Gazebo Garden.

In the example below I've added a lidar sensor to the waves.sdf example using the sdf

    <model name="gpu_lidar">
      <static>1</static>
      <pose>0 0 1 0 0 0</pose>
      <link name="base_link">
          <visual name="visual">
            <geometry>
              <box>
                <size>1 1 1</size>
              </box>
            </geometry>
          </visual>
          <sensor name='gpu_lidar' type='gpu_lidar'>
            <topic>lidar</topic>
            <update_rate>10</update_rate>
            <lidar>
              <scan>
                <horizontal>
                  <samples>60</samples>
                  <resolution>1</resolution>
                  <min_angle>-3.14159265</min_angle>
                  <max_angle>3.14159265</max_angle>
                </horizontal>
                <vertical>
                  <samples>8</samples>
                  <resolution>1</resolution>
                  <min_angle>-0.261799</min_angle>
                  <max_angle>0.261799</max_angle>
                </vertical>
              </scan>
              <range>
                <min>0.08</min>
                <max>10.0</max>
                <resolution>0.01</resolution>
              </range>
            </lidar>
            <alwaysOn>1</alwaysOn>
            <visualize>true</visualize>
          </sensor>
        </link>
    </model>

waves_lidar

The waves are displayed as wireframe for clarity and only the rays that hit are shown. There isn't a currently a sonar sensor in gz-sensors, but you may be able to adapt the Lidar sensor output for your requirements.

I don't have a Gazebo classic environment set up at present, but it may be worth checking the gpu_lidar in that environment to see if you get similar behaviour.

ConnorDTaylor commented 1 year ago

After spending a bit of time with this, I can't seem to find a "gpu_lidar" sensor in Gazebo 9. I found a "gpu_ray" but it does not seem to detect the waves, instead it seems to detect the mesh under the waves once again. I found someone else saying online that the gpu_ray is supposed to detect visual shapes as well, which I think the waves are so maybe I'm doing something wrong. Any further advice?

ConnorDTaylor commented 1 year ago

I looked into this once again and I realized that the Gerstner waves are being generated on top of a flat mesh using the material script. Is there a way to apply this to the mesh? Or is there a way to adjust the sensors to detect the material of the visual waves?

srmainwaring commented 1 year ago

If you are able to get a newer version of Gazebo installed and running in parallel to your Gazebo9 environment that may help with your investigations into differences in the way the gpu_ray (renamed to gpu_lidar in newer versions) is behaving.

The shader_param example (gz-sim/examples/worlds/shader_param.sdf) uses the same approach to wave generation as the early versions of this package (waves implemented as grid deformations in a vertex shader with wave parameters passed as uniforms https://fuel.ignitionrobotics.org/1.0/openrobotics/models/waves). I added a gpu_lidar to the camera model in that example and captured the results below. The wave amplitude has been doubled from the default model to make the result clearer. The returns are coming from the deformed wave surface, not the underlying mesh.

shader_params_gpu_lidar

There is a discussion in this vrx issue (The ocean reflects the laser beams #29) whether or not this is the correct behaviour for lidar.

ConnorDTaylor commented 1 year ago

Hi Rhys, I tried to get Gazebo Forest running in parallel with Gazebo 9 but I entirely messed up my ROS install. I have now fixed it and was wondering if you have any tips for installing these together without breaking my environment again. If I can get a new version of Gazebo install and working with ROS Melodic I would happily use this to simulate my system.

It is clear to me that the gpu_laser and gpu_lidar are working differently, but I can't figure out why that is the case. You seem to be telling me the lidar is capable of detecting the material shaders whereas the laser can only detect the visual mesh.

ConnorDTaylor commented 1 year ago

Hi Rhys, earlier you mentioned the possibility of doing a ray intersection calculation when you said: "A method of last resort would be to use the same ray intersection calculation used for the wave height in the hydrodynamics plugin (but tracing the ray at a given angle from the sensor location). That would be less efficient than a GPU based approach using the scene depth buffer though."

I am struggling to sort out where wave heights are being calculated and stored in the HydrodynamicsPlugin. Is this information I can pull into ROS and generate the sensor readings from there?

srmainwaring commented 1 year ago

@ConnorDTaylor if you don’t mind how the wave height is determined, and say you just need a ‘ground truth’ type reference, then it would be fairly straightforward to make a plug-in that measures the wave height at the vehicle’s CoM (or some other reference point). We could even get the hydrodynamics plugin to publish a wave height message for the CoM which you could subscribe. The hydrodynamics plugin uses a wave field sampler object to query for wave heights at the object’s collision mesh vertices. This is all on the server side where the physics is calculated. The wave rendering is a different process running on the gui thread. If you could state your requirements clearly that would help me understand what might be the best approach. If your research is to simulate wave height sensing from a floating platform then you’ll need some sort of simulated sensor. On the other hand if you just need a wave height reference for some state model and don’t care how you arrive at it then a more direct approach may be better.

ConnorDTaylor commented 1 year ago

@srmainwaring Okay I understand now, I don't think this would work for my purposes. I am using a quadrotor in flight so by the time the vehicle is on the water I no longer require measurements. I am attempting to use range measurements to sense the wave heights from the air, what form these range measurements come in during simulation isn't exactly my concern at the moment, I just need measurements. At this moment I am more interested in using the sensor readings to inform path planning, control schemes, and state estimation.

I was hoping to use the gpu_ray for this but it doesn't seem to properly detect the waves generated by the Gerstner wave script and it is instead only detecting the visual mesh which is a static plane, so this is leaving me at a loss. I am unsure what I can use to overcome this problem.

srmainwaring commented 1 year ago

A similar approach would work, a wave height service where you query for a height at a number of locations would satisfy requirements. It would not matter that the vehicle was not on the water.

ConnorDTaylor commented 1 year ago

I do not have a lot of experience using Gazebo in this way. What exactly would I be querying to find the stored wave heights? I don’t understand where to get the wave heights directly without using a sensor to sense them.

ConnorDTaylor commented 1 year ago

Hi Rhys I am still hoping to get an answer to this question, as I'm entirely stuck on my problem until I know where this information is. What I may try doing is to use the area of a gazebo range sensor and make a condition that provides the wave distance from the quadrotor in the case of the net distance being less than the max sensor range. Does this sound reasonable? All I need to know is how to bring the wave height data into the sensor script. Thanks again for the help!

srmainwaring commented 1 year ago

@ConnorDTaylor, the hydrodynamics plugin calculates the vertical distance from points on a vehicle mesh to the waves using the WavefieldSampler class. This class maintains a small grid of points sampled from the full wavefield centred on the projection of the vehicle pose. It is set up here:

https://github.com/srmainwaring/asv_wave_sim/blob/8ba52653e179dc442bfa7c06432c89a44ba0dd07/gz-waves/src/systems/hydrodynamics/Hydrodynamics.cc#L591-L606

The physics engine calls the methods:

https://github.com/srmainwaring/asv_wave_sim/blob/8ba52653e179dc442bfa7c06432c89a44ba0dd07/gz-waves/include/gz/waves/WavefieldSampler.hh#L70-L75

to get the depth (height) at the desired points.

You could write your own sensor plugin using these classes and publish the data you require. Another approach that may be less effort is to add a range publisher to the hydrodynamics plugin. As the vehicle is out of the water it will not experience any buoyancy forces, but the heights will still be calculated, adding a hydrodynamics plugin to a (dummy) link on your vehicle would trigger the data to be published.

ConnorDTaylor commented 1 year ago

That is a huge help! Thank you very much. So it should be possible to make multiple dummy links on the vehicle to compute the wave heights at different locations which would act as the sensor readings, right?

srmainwaring commented 1 year ago

That's right, but there will need to be a bit of extra work to obtain the information. If I get a chance I'll see if I can put together an example demonstrating the concept. It'll be on a branch based on master so you'll need a Gazebo Garden environment to run it. I'll probably use an Iris quadcopter and ArduPilot for control, so if you are familiar with these that will help as well (https://github.com/ArduPilot/ardupilot_gazebo/tree/ignition-garden).

ConnorDTaylor commented 1 year ago

I am working on Ubuntu 18.04. Would downloading Gazebo Forest allow me to run it? Or would it be easily transferable at least?

My ultimate plan would likely be to translate the concepts in Gazebo 9 I think.

srmainwaring commented 1 year ago

Unfortunately not. There is a namespace change between Gazebo Forest and Garden (everything named ignition / ign has been moved to gz). Garden is the first version with full support for macOS / Metal which is what I use for development - so that's the reason this project has skipped forward to match. There are nightly builds for Garden on Ubuntu so you should be able to pull in binaries for that rather than have to build from source.

ConnorDTaylor commented 1 year ago

On the Gazebo website, it says that Gazebo Garden is only supported by Ubuntu 20.04 and 22.04. Frustratingly, I believe this means Garden will not work on my system. This is my problem to overcome though. I can try to rework the Gazebo Garden solution in Gazebo 9 as I figure what needs to be done will be the same but it will just look different.

I appreciate your help in this! You're a huge help.

ConnorDTaylor commented 1 year ago

Hi Rhys, have you happened to work out how to do this in Gazebo Garden? I am not sure what you mean when you say some extra work will be required to pull the data. From what I can figure out, it seems like I will need to pass the depth information from the Wavefield plugin to the Hydrodynamics plugin and then output this using the plugin. Is this correct? The specifics on how to do this will take me some time to figure out, so I am sure if it is done in Garden it will help me implement it in Gazebo 9.

srmainwaring commented 1 year ago

Hi Connor, here's a branch containing an initial sketch of the functions you need to intercept to build a wave sensor: https://github.com/srmainwaring/asv_wave_sim/tree/feature/wave-sensor

That's it - you have the mesh vertex and the distance to the wave.

To turn it into a sensor requires capturing a sample at some requested update rate (say 10Hz - 50Hz, not the physics step rate which is 1000Hz) and publishing the sample to gz-transport in a message format of your choice. The range message could probably be adapted.

wave_sensor_experiment_1

ConnorDTaylor commented 1 year ago

Hi Rhys,

I think I have converted what you added to your work to work in Gazebo 9 but I am running into something strange. When I add the gzmsg to the Physics.cc script, it slows my simulation down substantially for some reason (simulation time cuts down to 0.04s).

I am also not sure where how I can access the depth reading in the terminal. Is there a way to rad out gazebo messages to the terminal or will I first need to use the gz-transport to read that info into ROS?

srmainwaring commented 1 year ago

When I add the gzmsg to the Physics.cc script, it slows my simulation down substantially for some reason (simulation time cuts down to 0.04s).

Yes, writing to the logger in a tight physics loop will be bad for performance. To implement a sensor correctly you will want to capture the data you need in a (new) object and reduce the sampling rate of the capture to something sensible. My example using the logger was intended to point you to the sections of code that you'll need to adapt to build your sensor.

ConnorDTaylor commented 1 year ago

Okay that makes sense to me. However I am still unsure how to get the output from this gzmsg to print to the terminal as you have in your example, is there a command for this I am just not able to find? In your example image you seem to be able to see these messages in terminal, but this isn't the case for me. I have noticed a lot of Gazebo's documentation links are broken at the moment, likely due to the shift from ignition/classic back to just gazeo.

srmainwaring commented 1 year ago

gzmsg to print to the terminal

To see messages when running Gazebo you need to run with the verbose flags set:

gz sim -v4 <world.sdf>

To see the full documentation of the command line tools use: gz sim -h.

srmainwaring commented 1 year ago

Closing as no issue in Gazebo Garden and an approach has been suggested for legacy versions.