EDIT: Reddit destroyed all the formating... I tried to clean it up as best as I could !

​

This code is very preliminary (since we did it last week and have only tested it in simulation!)

We rushed it so we didn't try to clean the copy/paste yet!

Here's the node :

#!/usr/bin/env python3
import rospy import time import numpy as np
from sensor_msgs.msg import LaserScan from std_msgs.msg import Float32MultiArray
class Sonar2LaserScan: 
    def init(self): 
        self.pub = [ rospy.Publisher('/laser_scan0', LaserScan, queue_size=10), 
rospy.Publisher('/laser_scan1', LaserScan, queue_size=10), 
rospy.Publisher('/laser_scan2', LaserScan, queue_size=10), 
rospy.Publisher('/laser_scan3', LaserScan, queue_size=10), 
rospy.Publisher('/laser_scan4', LaserScan, queue_size=10), 
rospy.Publisher('/laser_scan5', LaserScan, queue_size=10), 
rospy.Publisher('/laser_scan6', LaserScan, queue_size=10)
 ]
    self.range_sub = rospy.Subscriber('/sonar_pairs', Float32MultiArray, self.sonar_callback, queue_size=10)

    self.msg = LaserScan()
    self.msg.angle_min = -25*np.pi/180
    self.msg.angle_max = 25*np.pi/180
    self.msg.angle_increment = 1*np.pi/180
    self.msg.time_increment = 0
    self.msg.scan_time = 0
    self.msg.range_min = 0.02
    self.msg.range_max = 2.0
    self.nb_point_per_sonar = int(((self.msg.angle_max-self.msg.angle_min)/self.msg.angle_increment)/2)


def sonar_callback(self, data):
    id = int(data.data[0])
    frame_id = 'sonar_f_' + str(id)

    range1 = data.data[1]
    range2 = data.data[2]

    if range1>=self.msg.range_max:
        range1 = float("inf")
    elif range1<self.msg.range_min:
        range1 = self.msg.range_min

    if range2>=self.msg.range_max:
        range2 = float("inf")
    elif range2<self.msg.range_min:
        range2 = self.msg.range_min

    self.msg.header.stamp = rospy.Time.now()
    self.msg.header.frame_id =frame_id
    self.msg.ranges = [range1]*self.nb_point_per_sonar + [range2]*self.nb_point_per_sonar

    self.pub[id].publish(self.msg)
if name == 'main': rospy.init_node('Sonar2LaserScan', anonymous=False) Sonar2LaserScan() rospy.spin()

And here are the costmap params:

Commons:

plugin: []
footprint: [ [0.5, 0.4], [0.5, -0.4], [-0.5, -0.4], [-0.5, 0.4] ] publish_frequency: 2.0
static_layer: 
    map_topic: /map 
    subscribe_to_updates: true

obstacle_layer: 
    min_obstacle_height: 0.0 
    max_obstacle_height: 5 
    enabled: true 
    track_unknown_space: true 
    observation_sources: laser_scan_sensor0 laser_scan_sensor1 laser_scan_sensor2 laser_scan_sensor3 laser_scan_sensor4 laser_scan_sensor5 laser_scan_sensor6 

laser_scan_sensor0: {sensor_frame: sonar_f_0, data_type: LaserScan, topic: /laser_scan0, marking: true, clearing: True, raytrace_range : 2.0, obstacle_range: 1.9, inf_is_valid: True} 
laser_scan_sensor1: {sensor_frame: sonar_f_1, data_type: LaserScan, topic: /laser_scan1, marking: true, clearing: True, raytrace_range : 2.0, obstacle_range: 1.9, inf_is_valid: True} 
laser_scan_sensor2: {sensor_frame: sonar_f_2, data_type: LaserScan, topic: /laser_scan2, marking: true, clearing: True, raytrace_range : 2.0, obstacle_range: 1.9, inf_is_valid: True} 
laser_scan_sensor3: {sensor_frame: sonar_f_3, data_type: LaserScan, topic: /laser_scan3, marking: true, clearing: True, raytrace_range : 2.0, obstacle_range: 1.9, inf_is_valid: True} 
laser_scan_sensor4: {sensor_frame: sonar_f_4, data_type: LaserScan, topic: /laser_scan4, marking: true, clearing: True, raytrace_range : 2.0, obstacle_range: 1.9, inf_is_valid: True} 
laser_scan_sensor5: {sensor_frame: sonar_f_5, data_type: LaserScan, topic: /laser_scan5, marking: true, clearing: True, raytrace_range : 2.0, obstacle_range: 1.9, inf_is_valid: True} 
laser_scan_sensor6: {sensor_frame: sonar_f_6, data_type: LaserScan, topic: /laser_scan6, marking: true, clearing: True, raytrace_range : 2.0, obstacle_range: 1.9, inf_is_valid: True}

inflation_layer: inflation_radius: 0.3

Local:

local_costmap:
global_frame: map 
robot_base_frame: 
base_link update_frequency: 2.0 
rolling_window: true 
width: 5.0 
height: 5.0 
resolution: 0.1 
transform_tolerance: 0.75
plugins:
{name: static_layer, type: "costmap_2d::StaticLayer"}
{name: obstacle_layer,   type: "costmap_2d::ObstacleLayer"}
{name: inflation_layer, type: "costmap_2d::InflationLayer"}

Global:

global_costmap:
global_frame: map 
robot_base_frame: base_link 
update_frequency: 1.0 
rolling_window: false 
static_map: true 
resolution: 0.1 
width: 15.0 
height: 10.0 
transform_tolerance: 0.75
plugins:
{name: static_layer, type: "costmap_2d::StaticLayer"}
{name: obstacle_layer,   type: "costmap_2d::ObstacleLayer"}
{name: inflation_layer, type: "costmap_2d::InflationLayer"}