Sonar sensor measurements not correct

edit

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>
<link name="main_frame">
...
    <sensor name="sonar" type="sonar">
        <pose>0 0 0 0 3.141592 0</pose>
        <sonar>
            <min>0</min>
            <max>2.00</max>
            <radius>0.3</radius>
        </sonar>
        <always_on>1</always_on>
    <update_rate>30</update_rate>
    <visualize>true</visualize>
    </sensor>
</link>

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.