Gazebo | Ignition | Community
Ask Your Question
0

Two lasers on one robot not working properly

asked 2020-07-15 11:30:04 -0600

teshansj gravatar image

updated 2020-07-15 11:37:01 -0600

I have two lasers (ray sensors) on my robot (modeled in URDF), on front and back. However, when I use both of them, I can see a few problems

  1. they do not publish at the intended frequency. Published frequency seems to be around 10Hz at the beginning and slows down little by little to around 8Hz.
  2. Each laser shows a glitch few times a second where the laser seems to be shifted
  3. When I set the laser to be visualized, gazebo only shows one laser

When I only use one laser, it works fine and publishes at the rate specified in <update_rate> tag.


Here's some images from the robot with ROS move-base with both lasers in use. Green dots in line shows the laser. Black box is the robot

  1. Both lasers on. Only the rear one seems to be working Both lasers on. Only one seems to be working

  2. How gazebo shows only one laser. The other laser is positioned on top left corner of the robot How gazebo shows only one laser

  3. The glitch. The one laser that was working is still showing as well. The shifted laser seems to be the combined laser showing the full 360 view but it is rotated 180 image description


This is the gazebo elements for the lasers. What am I missing here?

<gazebo reference="front_scan">
  <material>Gazebo/FlatBlack</material>
  <sensor name="lds_lfcd_sensor" type="ray">
    <pose>0 0 0 0 0 0</pose>
    <visualize>false</visualize>
    <update_rate>2</update_rate>
    <ray>
      <scan>
        <horizontal>
          <samples>360</samples>
          <resolution>1</resolution>
          <min_angle>-2.25619449019</min_angle>
          <max_angle>2.25619449019</max_angle>
        </horizontal>
      </scan>
      <range>
        <min>0.12</min>
        <max>15.0</max>
        <resolution>0.015</resolution>
      </range>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.01</stddev>
      </noise>
    </ray>
    <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_lds_lfcd_controller">
      <topicName>scan</topicName>
      <frameName>front_scan</frameName>
    </plugin>
  </sensor>
</gazebo>
<gazebo reference="rear_scan">
  <material>Gazebo/FlatBlack</material>
  <sensor name="lds_lfcd_sensor" type="ray">
    <pose>0 0 0 0 0 0</pose>
    <visualize>false</visualize>
    <update_rate>2</update_rate>
    <ray>
      <scan>
        <horizontal>
          <samples>360</samples>
          <resolution>1</resolution>
          <min_angle>-2.25619449019</min_angle>
          <max_angle>2.25619449019</max_angle>
        </horizontal>
      </scan>
      <range>
        <min>0.12</min>
        <max>15.0</max>
        <resolution>0.015</resolution>
      </range>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.01</stddev>
      </noise>
    </ray>
    <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_lds_lfcd_controller">
      <topicName>scan</topicName>
      <frameName>rear_scan</frameName>
    </plugin>
  </sensor>
</gazebo>
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2020-07-15 11:52:11 -0600

updated 2020-07-15 13:04:22 -0600

The problem is <topicName>scan</topicName>. Both lasers publish on the same topic. This creates conflicting information on the laser scans. The glitch you mention probably comes from the localization failing to keep up.

Each laser should publish on its own topic, e.g. scan_front and scan_rear.

Edit:
Actually, no. The frame names should be enough to differentiate them. Not sure what to assume with the given information.

Edit2:
So, I did some experiments. Your problem is probably the sensor names.

  • You are using the same names for the sensors and the plugins. You need to differentiate them. Although gazebo doesn't throw any errors when you set the same sensor name, the output laser scans are wrong.
  • Regarding the publish rate, check the update rate of all the range sensors on your robot. I had problems in the past where I couldn't increase the rate of a range sensor, and it turned out that this was somehow connected to the rate of my laser scanners. (also, in the code you provided, the update rate is 2)
edit flag offensive delete link more

Comments

Yes the sensor names was the problem. Thank you.

With both sensors publishing to /scan, the update rate is 4 now which is correct. However both are publishing at the same time and it seems rviz has trouble visualizing both. Most of the time it shows one sensor (rear one) points. But I don't think there's a way to delay start one sensor. Not much of a problem but if someone knows a work around, please reply

teshansj gravatar imageteshansj ( 2020-07-16 07:11:01 -0600 )edit

That's normal. The laserscan display shows the latest scan, which has the data from one of the sensors. To handle this properly, you should publish to individual topics, as I initially tried to suggest. If your localization cannot handle multiple sources, you can additionally create a node that merges the scans and publishes the result to a new topic, and then have localization subscribe to that.

nlamprian gravatar imagenlamprian ( 2020-07-16 08:23:15 -0600 )edit

Yes the localization cannot handle multiple sources. I am going to write a separate node for merging scans. Thanks

teshansj gravatar imageteshansj ( 2020-07-16 22:34:43 -0600 )edit

If what you had before was working for you, see here

nlamprian gravatar imagenlamprian ( 2020-07-17 07:19:02 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2020-07-15 11:30:04 -0600

Seen: 1,062 times

Last updated: Jul 15 '20