# Sonar sensor measurements not correct

Hi,

If I run Gazebo 5.1.0 from terminal using the sonar_demo.world and it is OK, the range measurements are correct.

However, I am launching Gazebo from a ROS launch file with a SDF which contains the following:

<model name="my_robot">
<pose>0 0 0.126 -3.14159265359 0 1.57079632679</pose>
<static>false</static>
...
<sensor name="sonar" type="sonar">
<pose>0 0 0 0 3.141592 0</pose>
<sonar>
<min>0</min>
<max>2.00</max>
</sonar>
<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>true</visualize>
</sensor>


This model is included in a world which just contains a custom made plugin in order to simulate a motion capture system, ground plane, sun, and quick physics solver. The sensor appears in Gazebo, and I can see the topics. However, the range measurement is always 2 (max), no matter what I place in front of the sensor.

I have tried also to include the code from the sonar example world directly into the world, instead of within a robot model, and it also works. I also tried to put the same piece of code in a new link in the model (outside the main robot link) and it works, but it obviously does not move with the robot.

What am I doing wrong? Thank you!

UPDATE:By the way, I am using ROS Indigo if that matters.

UPDATE2: With the configuration of the main post (sensor sonar in the unique link) there are no collisions at all, even if I place some new obstacles. sonar_demo.world works nicely, collisions are properly detected (the only difference is that the sensor is horizontal, I tried that in my model and nothing).

Creating a new link, and attaching to the body with a joint gives false, constant collisions at 1.61 with the ground plane figure here. This figure shows the robot at 2 different heights giving the same output, not detecting collisions with ground plane (not to mention the problem that the Hokuyo is colliding with something that does not exists).

If a cube is included, collisions work sometimes. If the sensor is closer than 1.61m from the obstacle, collisions are not detected (figure - left, collisions shown are within the cube). However, it I command it to be at 1.86m from the cube (for instance) at least one collision is properly detected and the range measurement is OK (figure - right ). However, as I said, this is only if range > 1.61m.

edit retag close merge delete

How are you reading the sonar value? Does the visualization change in Gazebo when you place an object in front of the sonar?

( 2015-08-20 09:11:23 -0600 )edit

I am using both gz topic -e' an the topic visualization tool. In both cases, I can see the proper values of the ranges min/max, and the timestamp, pose, etc. I am not sure what you mean with the "visualization changes".

( 2015-08-20 09:45:38 -0600 )edit

Creating a new link and a joint (which is supposed to be of type fixed but that gives an error in the parser), and including the sensor in this link works, sonar data is OK. However, then there is a very weird problem that the control I am simulating (which is stable) is not working properly. For instance, I set the setpoin 0,0,1 but it is not able to reach there. If I remove the links, it is. Weird!

( 2015-08-20 09:48:50 -0600 )edit

I got it working only by creating the sonar model in a different file, include it in my robot file, and put them together with a revolute joint. However, the simulation is not nice, the range measurements are not good: sometimes it sticks to 1.61 and it does not go below that, sometimes it does not measure between 1.61 and 1.81, etc. It works specially bad when colliding with the ground plane. @nkoenig is this a known issue?

( 2015-08-20 11:09:03 -0600 )edit

Have you tried running worlds/sonar_demo.world? Also, can you View Contacts? (sonar is using collision detection under the hood)

( 2015-08-20 13:45:13 -0600 )edit

@scpeters I tried that (I said it in the original post), and I am not sure about what with mean with view contacts but I think I could see the topic. Furthermore, as I explained in the previous comments, I was able to make the sensor work with my model but with very weird values sometimes and even modifying the behaviour of my robot sometimes (I think because I had 2 gravity tags).

( 2015-08-20 14:52:52 -0600 )edit

Click the View menu, then Contacts` and it should show blue spheres when the sonar cone touches another object.

( 2015-08-20 15:11:16 -0600 )edit

I'm able to reproduce your problem. Debugging this issue now.

( 2015-08-21 08:58:16 -0600 )edit

Wow, thanks @nkoenig It is kind of relaxing to know that the issue is not just mine :)

( 2015-08-21 09:38:05 -0600 )edit

Sort by » oldest newest most voted

Here is a branch that should resolve this issue. I'll make a pull request shortly.

more

Thank you so much! I cannot test it from source as this would most probably break other stuff, and I am a bit restricted to use Gazebo5. Is Gazebo5 still being updated or now everything is Gazebo6? Could you please link in this post the PR to keep track of it?

( 2015-08-25 06:48:05 -0600 )edit

My plan is to first get this change into gazebo6, and then backport it to gazebo5. At that point we can release a new debian. A rough time frame for all this to happen is probably a couple weeks. The review process can take a bit of time.

( 2015-08-25 10:04:36 -0600 )edit