Ultrasonic sensor with range_sensor_layer not forming cone obstacles

edit

Hi,

I am able to form a costmap using my Lidar data and move_base through the ROS Navigation stack. However when I try to integrate an ultrasonic sensor to the costmap using range_sensor_layer there are 2 problems:

1. The Lidar is no longer creating obstacles on the local costmap
2. The Ultrasonic sensor is only creating obstacles in line of single pixels in front of the robot (by pixels I mean the smallest unit of map resolution, 0.05m in my case)

Below: local costmap created from lidar, without the range_sensor_layer plugin. Everything working fine

Below: Costmap with range_sensor_layer plugin active. I moved an object towards the ultrasonic sensor starting at about 80cm, moving to 30cm (it doesn't clear objects behind others thus why is forms a line). The robot is located at the center cross facing right in both these images

I have tried playing with the parameters discussed in range_sensor_layer but this did not help. Is there some blatant oversight that I'm making? Below are my config files

costmap_common_params.yaml

obstacle_range: 4.0
raytrace_range: 5.0
footprint: [[0.075, 0.235], [0.075,-0.235], [-0.455,-0.235], [-0.455,0.235]]
inflation_radius: 0.55

observation_sources: laser_scan_sensor

laser_scan_sensor: {sensor_frame: /scan, data_type: LaserScan, topic: /scan, marking: true, clearing: true}

sonar:
  topics: ['/ultrasonic']
  clear_threshold: 0.3
  mark_threshold: 0.6
  clear_on_max_reading: true
  no_readings_timeout: 2.0

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

local_costmap_params.yaml

local_costmap:
  global_frame: odom
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 6.0
  height: 6.0
  resolution: 0.05

global_costmap_params.yaml

global_costmap:
  global_frame: /map
  robot_base_frame: base_link
  update_frequency: 5.0
  static_map: false
  rolling_window: true
  height: 10.0
  width: 10.0
  resolution: 0.05

And here is my Ultrasonic message:

EDIT 1

The ultrasonic sensors are now marking obstacles after fixing the different layers and plugins in Costmap2D. However the range_sensor_layer package is still only marking dots, instead of arcs on the costmap. Is it even supposed to mark arcs? Maybe my understanding is incorrect. However there are field of view parameters so I would assume it is supposed to mark an arc obstacles at the detected range.