Robotics StackExchange | Archived questions

Adding ultrasonic sensors and range sensor layer

Hello, I need some help with a problem I encountered while adding ultrasonic sensors to a robot (loosely based on Linorobot), already equipped with an RPlidar. Hw/Sw: Raspi3B w/ Ubuntu 16.04.6 LTS, ROS kinetic, a Teensy, 2 Nano.

The robot was working fine with just the lidar, but I need to be able to detect correctly glass and some reflective surfaces, so I'm adding the ultrasonic sensors. The hardware and microcontroller (rosserial) parts seem to be working fine, I suspect it's an error from my part, maybe related to namespaces or transform frames... or maybe I'm missing something gargantuan. I checked and re-checked against online tutorials, examples and other questions similar to this one, but I couldn't identify the culprit.

After executing the launch files I get the standard messages (same as before trying to setup the ultrasonic sensors), plus:

[ INFO] [1631195261.554945536]: global_costmap/sonar_layer: ALL as input_sensor_type given
[ INFO] [1631195261.596176257]: RangeSensorLayer: subscribed to topic /ultrasound_front

and I guess that's good.

Unfortunately from that moment onward I get (with increasingly high figures, of course):

[ WARN] [1631195265.533631740]: No range readings received for 4.02 seconds, while expected at least every 2.00 seconds.

here's a sensor message (from "rostopic echo /ultrasound_front"):

----
header: 
  seq: 1124
  stamp: 
    secs: 1631192726
    nsecs: 301432058
  frame_id: "sonar_front"
radiation_type: 0
field_of_view: 0.259999990463
min_range: 0.0
max_range: 100.0
range: 52.0
----

So, the topic is published and the massages should be ok...

My costmapcommonparams.yaml:

map_type: costmap

transform_tolerance: 1

footprint: [[-0.25, -0.25], [-0.25, 0.25], [0.25, 0.25], [0.25, -0.25]]

inflation_layer:
  inflation_radius: 0.28
  cost_scaling_factor: 3

obstacle_layer:
  obstacle_range: 2.5
  raytrace_range: 3.0
  observation_sources: scan
  observation_persistence: 0.0
  scan:
    data_type: LaserScan
    topic: scan
    marking: true
    clearing: true

sonar_layer:
  frame: sonar_front
  topics: ["/ultrasound_front"]
  no_readings_timeout: 2.0
  clear_on_max_reading: true
  clear_threshold: 0.2
  mark_threshold: 0.80

My globalcostmapparams.yaml:

global_costmap:
  global_frame: /map
  robot_base_frame: /base_footprint
  update_frequency: 1
  publish_frequency: 0.5
  static_map: true
  transform_tolerance: 1
  plugins:
    - {name: static_layer,    type: "costmap_2d::StaticLayer"}
    - {name: sonar_layer,   type: "range_sensor_layer::RangeSensorLayer"}
    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

My localcostmapparams.yaml:

local_costmap:
  global_frame: /odom
  robot_base_frame: /base_footprint
  update_frequency: 1
  publish_frequency: 5.0
  static_map: false
  rolling_window: true
  width: 3
  height: 3
  resolution: 0.02
  transform_tolerance: 1
  observation_persistence: 0.0

  plugins:
    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
    - {name: sonar_layer, type: "range_sensor_layer::RangeSensorLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

after getting the messages, "rostopic hz /ultrasound_front" gives: (thanks to @Miura for the suggestion!)

subscribed to [/ultrasound_front]
average rate: 3.494
    min: 0.267s max: 0.305s std dev: 0.01919s window: 3
average rate: 3.384
    min: 0.250s max: 0.353s std dev: 0.03533s window: 6
average rate: 3.362
    min: 0.250s max: 0.353s std dev: 0.02813s window: 9
average rate: 3.352
    min: 0.250s max: 0.353s std dev: 0.02625s window: 13
average rate: 3.349
    min: 0.250s max: 0.353s std dev: 0.02447s window: 16
average rate: 3.344
    min: 0.250s max: 0.353s std dev: 0.02547s window: 20
average rate: 3.341
    min: 0.250s max: 0.353s std dev: 0.02368s window: 23
average rate: 3.256
    min: 0.250s max: 0.490s std dev: 0.04349s window: 26
average rate: 3.336
    min: 0.110s max: 0.490s std dev: 0.05406s window: 30
average rate: 3.335
    min: 0.110s max: 0.490s std dev: 0.05176s window: 33

and so on. Publishing interval in the MCU code is 250ms.

EDIT
"maxrange:1.0" in "rostopic echo /ultrasoundfront" has been corrected (was an error in the original MCU code), the behaviour doesn't change. I modified the output above to to reflect the current version.

"rostopic info /ultrasound_front", after the massages started, gives:

Type: sensor_msgs/Range

Publishers: 
 * /rosserial_NANO_sensors (http://192.168.2.54:34525/)

Subscribers: 
 * /move_base (http://192.168.2.54:40149/)

This is my barebone URDF:

    <?xml version="1.0"?>
    <robot name="linorobot">

    <link name="base_link">
      <visual>
        <geometry>
          <box size="0.50 0.33 0.09"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0.0 0.00 0.085"/>
        <material name="blue">
          <color rgba="0 0 .8 1"/>
        </material>
      </visual>
    </link>

    <link name="perception_deck">
      <visual>
        <geometry>
          <box size="0.18 0.33 0.08"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0.0 0.0 0.17"/>
        <material name="blue">
          <color rgba="0 0 .8 1"/>
        </material>
      </visual>
    </link>


    <link name="wheel_left_front">
      <visual>
        <geometry>
          <cylinder length="0.03" radius="0.06"/>
        </geometry>
        <origin rpy="1.57 0 0" xyz="0.163 0.222 0.03"/>
        <material name="black">
          <color rgba="0 0 0 1"/>
        </material>
      </visual>
    </link>

    <link name="wheel_right_front">
      <visual>
        <geometry>
          <cylinder length="0.03" radius="0.06"/>
        </geometry>
        <origin rpy="1.57 0 0" xyz="0.163 -0.222 0.03"/>
        <material name="black">
          <color rgba="0 0 0 1"/>
        </material>
      </visual>
    </link>

    <link name="wheel_left_rear">
      <visual>
        <geometry>
          <cylinder length="0.03" radius="0.06"/>
        </geometry>
        <origin rpy="1.57 0 0" xyz="-0.163 0.222 0.03"/>
        <material name="black">
          <color rgba="0 0 0 1"/>
        </material>
      </visual>
    </link>

    <link name="wheel_right_rear">
      <visual>
        <geometry>
          <cylinder length="0.03" radius="0.06"/>
        </geometry>
        <origin rpy="1.57 0 0" xyz="-0.163 -0.222 0.03"/>
        <material name="black">
          <color rgba="0 0 0 1"/>
        </material>
      </visual>
    </link>

    <link name="laser">
      <visual>
        <geometry>
          <cylinder length="0.065" radius="0.035"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0.0 0.0 0.2825"/>
        <material name="black"/>
      </visual>
    </link>

    <link name="chassis">
      <visual>
        <geometry>
          <box size="0.5 0.5 0.8"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
        <material name="silver">
          <color rgba="192 192 192 0.6"/>
        </material>
      </visual>
    </link>

    <link name="sonar_front">
      <visual>
       </geometry>
        <origin rpy="1.5708 0.2618 0" xyz="-0.21 0.0 0.235"/>
        <material name="silver">
          <color rgba="192 192 192 0.6"/>
        </material>
      </visual>
    </link>

    <link name="sonar_rear">
      <visual>
        <geometry>
          <box size="0.02 0.025 0.07"/>
        </geometry>
        <origin rpy="1.5708 0.2618 3.1416" xyz="0.23 0.0 0.235"/>
        <material name="silver">
          <color rgba="192 192 192 0.6"/>
        </material>
      </visual>
    </link>

    <link name="sonar_left">
      <visual>
        <geometry>
          <box size="0.02 0.025 0.07"/>
        </geometry>
        <origin rpy="1.5708 -0.2618 1.5708" xyz="0.0 0.18 0.235"/>
        <material name="silver">
          <color rgba="192 192 192 0.6"/>
        </material>
      </visual>
    </link>

    <link name="sonar_right">
      <visual>
        <geometry>
          <box size="0.02 0.025 0.07"/>
        </geometry>
        <origin rpy="1.5708 -0.2618 -1.5708" xyz="0.0 -0.19 0.235"/>
        <material name="silver">
          <color rgba="192 192 192 0.6"/>
        </material>
      </visual>
    </link>

    <joint name="base_to_wheel_left_front" type="fixed">
      <parent link="base_link"/>
      <child link="wheel_left_front"/>
      <origin xyz="0 0 0"/>
    </joint>

    <joint name="base_to_wheel_right_front" type="fixed">
      <parent link="base_link"/>
      <child link="wheel_right_front"/>
      <origin xyz="0 0 0"/>
    </joint>

    <joint name="base_to_wheel_left_rear" type="fixed">
      <parent link="base_link"/>
      <child link="wheel_left_rear"/>
      <origin xyz="0 0 0"/>
    </joint>

    <joint name="base_to_wheel_right_rear" type="fixed">
      <parent link="base_link"/>
      <child link="wheel_right_rear"/>
      <origin xyz="0 0 0"/>
    </joint>

    <joint name="base_to_laser" type="fixed">
      <parent link="base_link"/>
      <child link="laser"/>
      <origin xyz="0.0 0.0 0.0"/>
    </joint>

    <joint name="base_to_left_sonar" type="fixed">
      <parent link="base_link"/>
      <child link="sonar_left"/>
      <origin xyz="0.0 0.0 0.0"/>
    </joint>

    <joint name="base_to_right_sonar" type="fixed">
      <parent link="base_link"/>
      <child link="sonar_right"/>
      <origin xyz="0.0 0.0 0.0"/>
    </joint>

    <joint name="base_to_rear_sonar" type="fixed">
      <parent link="base_link"/>
      <child link="sonar_rear"/>
      <origin xyz="0.0 0.0 0.0"/>
    </joint>

    <joint name="base_to_front_sonar" type="fixed">
      <parent link="base_link"/>
      <child link="sonar_front"/>
      <origin xyz="0.0 0.0 0.0"/>
    </joint>

    <joint name="base_to_perception_deck" type="fixed">
      <parent link="base_link"/>
    <joint name="base_to_laser" type="fixed">
      <parent link="base_link"/>
      <child link="laser"/>
      <origin xyz="0.0 0.0 0.0"/>
    </joint>

    <joint name="base_to_left_sonar" type="fixed">
      <parent link="base_link"/>
      <child link="sonar_left"/>
      <origin xyz="0.0 0.0 0.0"/>
    </joint>

    <joint name="base_to_right_sonar" type="fixed">
      <parent link="base_link"/>
      <child link="sonar_right"/>
      <origin xyz="0.0 0.0 0.0"/>
    </joint>

    <joint name="base_to_rear_sonar" type="fixed">
      <parent link="base_link"/>
      <child link="sonar_rear"/>
      <origin xyz="0.0 0.0 0.0"/>
    </joint>

    <joint name="base_to_front_sonar" type="fixed">
      <parent link="base_link"/>
      <child link="sonar_front"/>
      <origin xyz="0.0 0.0 0.0"/>
    </joint>

    <joint name="base_to_perception_deck" type="fixed">
      <parent link="base_link"/>
      <child link="perception_deck"/>
      <origin xyz="0 0 0.0"/>
    </joint>

    <joint name="base_to_chassis" type="fixed">
      <parent link="base_link"/>
      <child link="chassis"/>
      <origin xyz="0 0 0.44"/>
    </joint>
   </robot>

Asked by LoopingCollider on 2021-09-09 09:25:31 UTC

Comments

Have you tried checking the communication cycle with rostopic hz /ultrasound_front? I think you are getting a warning because the communication cycle is slow.

Also, I find it strange that in rostopic echo /ultrasound_front, range is 52.0 while max_range is 1.0. Normally, the range should be smaller than the max_range. If max_range < range persists, I think there is an issue with the sensor.

Asked by miura on 2021-09-09 17:51:41 UTC

Thank you! Updating the question...

Asked by LoopingCollider on 2021-09-10 04:14:30 UTC

When I refer to the warning displayed in the terminal and the sensor message, the timestamp is way off. If this is the data when they are running at the same time, then the clock may be out of sync. You may need to set one of the clocks.

Also, it may not be the intended behavior, but if you set no_readings_timeout to 0.0 in costmap_common_params.yaml, it may work.

Asked by miura on 2021-09-10 18:17:25 UTC

Thank you once more miura for the help! The timestamp are different because weren't recorded at the same time, I checked the sync and the timestamp secs in the ultrasound_front topic seems to correspond to the timestamp from the launch file.

I tried setting no_readings_timeout to 0.0, as you suggested, I think it's a step forward, or at least could help identify the problem:
I got this an error from the launch file, every second or so: [ERROR] [1631543292.139093839]: Range sensor layer can't transform from odom to sonar_front at 1631543288.530009 OR same error, but ...from map to sonar_front....

I don't get the "no readings" warning anymore, so I'm currently investigating this other error, to see if I can identify the cause (maybe some problem in the tf tree). In any case I will post/update my findings!

Asked by LoopingCollider on 2021-09-13 09:41:03 UTC

Answers

I finally solved some of the problems that emerged after adding the ultrasound sensors. Because of the nature of the errors, and the extremely large amount of different hw/sw configurations possible, I will put here my findings, with some more general info, hoping to help others:

  • Double check the UNIT of MEASURE used in the range fields in the microcontroller code. For example, the library and examples I used and referred to had everything in cm.
    This isn't good for ROS navigation layer, the range/distance numbers passed in the messages should be in meters (min_range, max_range, range).
    HOWEVER the microcontroller code could be passing the data, and using some internal calculations or logic, in centimeters (like here 'https://www.intorobotics.com/how-to-use-sensor_msgs-range-ros-for-multiple-sensors-with-rosserial/', for example), so some changes are probably needed (also regarding the logic behind the clearing of the costmap, but that's a problem for another question).
  • A message rate of 20Hz should be ok, it should not produce missing data messages, sync errors, etc. However please note that it's possible this frequency has to be modified, depending on the hardware involved.
  • The costmap YAML parameter clear_on_max_reading behaviour depends on how the data is presented by your ultrasound sensor (or sensors) MCU code. It's a good idea to try both settings and check which one is more appropriate for your case. You can then modify the MCU code to accomodate for the library logic behind this setting (or the other way around, modifying the libraries).
  • Verify that your RVIZ configuration contains all the necessary information to visualize your ultrasound (range) sensor data (http://wiki.ros.org/rviz/DisplayTypes/Range)
  • The URDF usually gives clear messages if something related to the transforms and related data is not working, once the real problems are solved, it's possible to see the cones and axes in Rviz (IF the unit of measure isn't too small!), so it's easy to correct orientation and position errors.
  • Use check_urdf to verify the validity of the URDF file (http://wiki.ros.org/urdf#Verification), and urdf_to_graphiz to have a visual representation with some more data, that could give some clues on malfunctions or errors (http://wiki.ros.org/urdf#Visualization). Also rqt_graph with enable_statistics set to "true" can give useful clues (http://wiki.ros.org/rqt_graph).

Asked by LoopingCollider on 2021-10-01 09:39:40 UTC

Comments

Hi @LoopingCollider Can you tell us which Ultrasonic sensor did you use ?

Asked by amjack on 2022-05-23 07:23:22 UTC