range_sensor_layer don’t use clear_threshold to clear master grid

[PSotiriadis]

I use ROS kinetic in order to be able to drive my differential drive robot and I also use range_sensor_layer package (Branch Indigo V0.4.0) in order to be able to use SRF sensors. Recently I noticed that range_sensor_layer, in function RangeSensorLayer::updateCosts() in lines 434-437, updates master grid cells for clearing, only if old_cost is NO_INFORMATION (if current=FREE_SPACE=0 then old_cost can never be smaller than current, because old_cost is an unsighned char and cannot be negative)

void RangeSensorLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!enabled_)
    return;

  unsigned char* master_array = master_grid.getCharMap();
  unsigned int span = master_grid.getSizeInCellsX();
  unsigned char clear = to_cost(clear_threshold_), mark = to_cost(mark_threshold_);

  for (int j = min_j; j < max_j; j++)
  {
    unsigned int it = j * span + min_i;
    for (int i = min_i; i < max_i; i++)
    {
      unsigned char prob = costmap_[it];
      unsigned char current;
      if(prob==costmap_2d::NO_INFORMATION){
        it++;
        continue;
      }
      else if(prob>mark)
        current = costmap_2d::LETHAL_OBSTACLE;
      else if(prob<clear)
        current = costmap_2d::FREE_SPACE;
      else{
        it++;
        continue;
      }

      unsigned char old_cost = master_array[it];

      if (old_cost == NO_INFORMATION || old_cost < current)
        master_array[it] = current;
      it++;
    }
  }

  buffered_readings_ = 0;
  current_ = true;
}

But what happened if master grid old_cost is not NO_INFORMATION(255) and current=FREE_SPACE (0)?? Is that not possible?? Because if it’s possible, we ignore completely this case and that may cause errors. In this case maybe a possible solution is something like this (but it is not tested):

void RangeSensorLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  if (!enabled_)
    return;

  unsigned char* master_array = master_grid.getCharMap();
  unsigned int span = master_grid.getSizeInCellsX();
  unsigned char clear = to_cost(clear_threshold_), mark = to_cost(mark_threshold_);

  for (int j = min_j; j < max_j; j++)
  {
    unsigned int it = j * span + min_i;
    for (int i = min_i; i < max_i; i++)
    {
      unsigned char prob = costmap_[it];
	  unsigned char old_cost = master_array[it];
      unsigned char current;
      if(prob==costmap_2d::NO_INFORMATION){
        it++;
        continue;
      }
      else if(prob>mark){
        current = costmap_2d::LETHAL_OBSTACLE;
	if (old_cost == NO_INFORMATION || old_cost < current){
		master_array[it] = current;
	}
	it++;
      }
      else if(prob<clear){
        current = costmap_2d::FREE_SPACE;
	if (old_cost > current){
		master_array[it] = current;
	}
	it++;
      }
      else{
        it++;
        continue;
      }
    }
  }

  buffered_readings_ = 0;
  current_ = true;
}

In addition, I want to highlight another two points of range_sensor_layer source code.
First, in function RangeSensorLayer::processVariableRangeMsg() in line 231, it is critical programming to compare if two float values are equal.

  if (range_message.range == range_message.max_range && clear_on_max_reading_)
    clear_sensor_cone = true;

A better way to do that (considering float A and float B) is:

if ( fabs( A - B ) < limit_value )

where limit_value indicates the desired decimal position, in which we want if-statement to compare A and B equality .

And second, I want to ask if it is possible to define a parameter, in order to be able to configure how long we want to wait for transform in line 245 (now is used a fix value of 0.1(100msec))

  if(!tf_->waitForTransform(global_frame_, in.header.frame_id, in.header.stamp, ros::Duration(0.1)) )
  {
    ROS_ERROR_THROTTLE(1.0, "Range sensor layer can't transform from %s to %s at %f",
        global_frame_.c_str(), in.header.frame_id.c_str(),
        in.header.stamp.toSec());
    return;
  }

Thanks in advance

[sradmard]

Hi,
I am also looking for clearing the local cost map after my sonar is not pointing at an obstacle anymore. The reason is that I am only using sonar for temporary and close by obstacles, so it should get clears after the robot or the obstacle moves away. However, at the moment I cannot clear an obstacle that the range_sensor_layer has placed on the local cost map at all. I would appreciate any advice on that. (I have already set the "clear_on_max_reading" as true, but it doesn't do anything)

[PSotiriadis]

I think I can understand where your problem is.
In range_sensor_layer.cpp there is the following function to process SRF Scans:

void RangeSensorLayer::processVariableRangeMsg(sensor_msgs::Range& range_message) { 
if (range_message.range < range_message.min_range || range_message.range > range_message.max_range) 
return; 
bool clear_sensor_cone = false; 
if (range_message.range == range_message.max_range && clear_on_max_reading_) 
clear_sensor_cone = true; 
updateCostmap(range_message, clear_sensor_cone); 
}

If in your Hardware interface program you publish in range_message.range values < range_message.min_range or > range_message.max_range you don’t be able to clear (processVariableRangeMsg return without clean). You need to control your SRFmeasurementResult, if it is between range_message.min_range and range_message.max_range. If not you have to make it equal to range_max limit:

// Add this in your Hardware interface program
If(SRFmeasurementResult < range_message.min_range || SRFmeasurementResult > range_message.max_range)
	range_message.range = range_message.max_range;
srf_publisher.publish (range_message);

Only then you are able to clear and only if also clear_on_max_reading_ = true , because only then (range_message.range == range_message.max_range && clear_on_max_reading_) is true and that makes also clear_sensor_cone = true.

[DLu]

A) A PR that implemented clearing would be welcome
B) The comparison to max range should maybe just be >= instead testing exact equality.
C) #48 includes a change that makes the transform tolerance parameteriziable.

[ju-mingyue]

@PSotiriadis
Dear PSotiriadis ， I have a problem when using ultrasound to avoid obstacles. My configuration in the local_costmap is as follows:


The ros version I use is melodic. I have successfully configured range_sensor_layer in it, but now ultrasound cannot add obstacles to my local map, as shown in the following figure:

I use rostopic echo to view the published topic, but I can find the message in the topic.Here is the procedure I used to publish ultrasound topic.
`
#!/usr/bin/env python3
# -- coding: utf-8 --

import rospy
from serial import Serial
import serial.tools.list_ports
from sensor_msgs.msg import Range
from std_msgs.msg import Header
import struct

import math 
import os,sys
import time
import threading


x=serial.Serial('/dev/Ultrasound',9600)
def faSong():   
    while True: 
          
        myinput=b'\xb4\x01'    
        x.write(myinput)    
        time.sleep(0.2)     
        #print(x)
def jieShou():  
    while True: 
        while x.inWaiting()>0:  
            myout=x.read(24) 
            #print (myout)
            datas=''.join(map(lambda x:('/x' if len(hex(x))>=4 else '/x0')+hex(x)[2:],myout))
            #print(datas)
            new_datas=datas[2:].split('/x') 
            need=''.join(new_datas) 
           
            print(need)
            data_1=float(need[4]+need[5]+need[6]+need[7])/1000
            data_2=float(need[16]+need[17]+need[18]+need[19])/1000
            data_3=float(need[28]+need[29]+need[30]+need[31])/1000
            data_4=float(need[40]+need[41]+need[42]+need[43])/1000
           
            print(data_1,data_2,data_3,data_4)
            msg1=Range()
            msg2=Range()
            msg3=Range()
            msg4=Range()

            header1=Header()
            header2=Header()
            header3=Header()
            header4=Header()

            header1.stamp=rospy.Time.now()
            header2.stamp=rospy.Time.now()
            header3.stamp=rospy.Time.now()
            header4.stamp=rospy.Time.now()

            header1.frame_id="/ultrasound1"
            header2.frame_id="/ultrasound2"
            header3.frame_id="/ultrasound3"
            header4.frame_id="/ultrasound4"

            msg1.header=header1
            msg2.header=header2
            msg3.header=header3
            msg4.header=header4

            msg1.field_of_view=1
            msg2.field_of_view=1
            msg3.field_of_view=1
            msg4.field_of_view=1

            msg1.min_range=0.25
            msg2.min_range=0.25
            msg3.min_range=0.25
            msg4.min_range=0.25

            msg1.max_range=1.5
            msg2.max_range=1.5
            msg3.max_range=1.5
            msg4.max_range=1.5

            msg1.range=data_1
            msg2.range=data_2
            msg3.range=data_3
            msg4.range=data_4

            global cmd_pub1,cmd_pub2,cmd_pub3,cmd_pub4
            cmd_pub1.publish(msg1)
            cmd_pub2.publish(msg2)
            cmd_pub3.publish(msg3)
            cmd_pub4.publish(msg4)
if __name__ == '__main__':
    rospy.init_node('ultrasonic')
    global cmd_pub1,cmd_pub2,cmd_pub3,cmd_pub4
    cmd_pub1 = rospy.Publisher('ultrasoundpublisher1',Range,queue_size=1)
    cmd_pub2 = rospy.Publisher('ultrasoundpublisher2',Range,queue_size=1)
    cmd_pub3 = rospy.Publisher('ultrasoundpublisher3',Range,queue_size=1)
    cmd_pub4 = rospy.Publisher('ultrasoundpublisher4',Range,queue_size=1)
    t1=threading.Thread(target=jieShou)
    t2=threading.Thread(target=faSong)
    t2.start()
    t1.start()

`
I don't know where the problem is now, so I really hope to get your help.

[PSotiriadis]

Hi @ju-mingyue,
range_sensor_layer uses a probabilistic model to calculate probabilities of obstacle detection on costmap cells.

To be able to control this, there are mark_threshold and clear_threshold parameters. These are defined in ROS wiki as following:

clear_threshold (double, default: .2)
Cells with a probability lower than the clear_threshold are marked as free space in the master costmap.
mark_threshold (double, default: .8)
Cells with a probability higher than the mark_threshold are marked as lethal obstacles in the master costmap.

As you see, these parameters are probabilities and that means they can take values between 0 and 1.
Default are configured with 0.2 and 0.8 and that means that cells with a probability lower than the 0.2 are marked as free space in the master costmap, and cells with a probability higher than the 0.8 are marked as lethal obstacles in the master costmap.
You use mark_threshold=1.5 and you mark as lethal obstacles cells with probability higher than 1.5 and thats not possible.

I think is better to use the default values and make your experiment again. As i see in RViz your srf topics are published correctly
(I mean that srf cones have different sizes, but only you can check if the measurements are correct). You can change RViz
local costmap visualization from map to costmap to be able to see inflation costs. In addition, you can deactivate laser obastacle layer to be able to see only srf obstacles on local costmap with inflations.

I hope this will hekp you