Robotics StackExchange | Archived questions

Multiple Ray Sensors - Interference?

Hi there,

I added multiple single ray sensors to my robot using the appended xacro. I then instantiated three of them, one in the middle, one to the right and one to the left, I appended the code for that too.

The problem is: somehow the left and the right sensor are affected by the one in the middle. If the one in the middle is detecting something, than also the others show the same values. Furthermore: the sensor to the left and the one to the right do have a really strange behaviour, let me show that to you:

image description C:\fakepath\Screen Captureselect-area20200522223754.gif

It looks like those three sensors somehoe interfere internally - but why?

<link name="${link_name}">

  <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${mass}" />
      <xacro:cylinder_inertia m="0.01" r="${radius}" h="${height}" />
  </inertial>

  <visual>
      <origin xyz="0 0 0" rpy="${rpy}"/>
      <geometry>
        <cylinder radius="${radius}" length="${height}"/>
      </geometry>
      <material name="sensor_red" />
  </visual>

  <collision>
      <origin xyz="0 0 0" rpy="${rpy}" />
      <geometry>
        <cylinder radius="${radius}" length="${height}"/>
      </geometry>
  </collision>

</link>

<joint name="joint_${link_name}" type="fixed">
  <origin xyz="${xyz}" rpy="0 0 0"/>
  <parent link="${parent_name}"/>
  <child link="${link_name}"/>
</joint>

<gazebo reference="${link_name}">

  <material>Gazebo/Orange</material>

  <sensor type="ray" name="${link_name}_sensor">

    <update_rate>30</update_rate>
    <visualize>true</visualize>
    <topic>/sensors/${link_name}</topic>

    <plugin name="${link_name}_controller" filename="libgazebo_ros_laser.so">
      <topicName>/sensors/${link_name}</topicName>
      <frameName>${link_name}</frameName>
    </plugin>

    <ray>

      <scan>
        <horizontal>
          <samples>1</samples>
          <resolution>1</resolution>
          <min_angle>${min_angle*(pi/180)}</min_angle>
          <max_angle>${max_angle*(pi/180)}</max_angle>
        </horizontal>
      </scan>

      <!-- Laser Range Finder Bricklet (Tinkerforge) -->
      <range>
        <min>0.1</min> <!-- 0.05 -->
        <max>40.0</max>
        <resolution>0.01</resolution>
      </range>

      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.01</stddev>
      </noise>

    </ray>

  </sensor>
</gazebo>

/xacro:macro


Instantiation:

  <!-- Laser Sensors -->
  <xacro:distance_sensor
      link_name="sensor_laser"
      parent_name="chassis"
      mass="0.01" radius="0.015" height="0.1"
      xyz="0.25 0 0" rpy="0.00 1.5708 0.00"
      min_angle="0" max_angle="0"
      />

  <!-- US Sensor Left -->
  <xacro:distance_sensor
      link_name="sensor_us_l"
      parent_name="chassis"
      mass="0.01" radius="0.015" height="0.1"
      xyz="0.25 0.05 0" rpy="-1.0472 1.5708 0.00"
      min_angle="60" max_angle="60"
      />

  <!-- US Sensor Right -->
  <xacro:distance_sensor
      link_name="sensor_us_r"
      parent_name="chassis"
      mass="0.01" radius="0.015" height="0.1"
      xyz="0.25 -0.05 0" rpy="1.0472 1.5708 0.00"
      min_angle="-60" max_angle="-60"
      />

Asked by flodo on 2020-05-22 15:47:43 UTC

Comments

Answers

image description image description image description image description image description image description image description

As you can see, there is a bug in Gazebo which is related to Resolution / Samples ratio. I fixed it by using two samples.

furthermore, if you specify min_angle = max_angle, then it is alsways measuring to 0 degrees, no matter which angle is specified.

Asked by flodo on 2020-05-26 17:06:44 UTC

Comments