Use data from IR sensor to build a costmap

edit

Hi,

I'm working on Turtlebot2. I will use cover for my robot, so I moved Kinect to the front of the robot to make a smaller holes in cover. But now I have a blind area in front of the robot, between 0 and 45 cm. I decided to mount some IR sensors. I made transformation for (yet) only one sensor

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "irring_tf");
  ros::NodeHandle n;

  ros::Rate r(100);

  tf::TransformBroadcaster broadcaster;

  while(n.ok()){
    broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.135, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_irring"));
    r.sleep();
  }
}

next I receive data from sensor and publish it as a LaserScan message (I create two points from one sensor, because I will use this code for more sensors, in this case it is no matter)

    ros::Time scan_time = ros::Time::now(); 
    sensor_msgs::LaserScan scan;
    scan.header.stamp = scan_time;
    scan.header.frame_id = "base_irring";
    scan.angle_min = -0.05;     //rad
    scan.angle_max = 0.05;      //rad
    scan.angle_increment = 0.10 / num_readings;     //rad
    scan.time_increment = (1 / laser_frequency) / (num_readings);   //sec
    scan.range_min = 0.20;       //meters
    scan.range_max = 1.15;     //meters

    scan.ranges.resize(num_readings);
    scan.intensities.resize(num_readings);
    for(unsigned int i = 0; i < num_readings; ++i){
      scan.ranges[i] = atof(buf) / 100.0;   //data from arduino
//      scan.intensities[i] = 1; //data from arduino
    }

    scan_pub.publish(scan);

It works, I can visualize data in correct place using Rviz (white line is data from Kinect, red dots data are taken from IR sensor) https://www.youtube.com/watch?v=woMNOnIB2bo&list=UUclHPTMTlaW_M55gYhp_ylQ

Next I want to use this data to build costmap, so I add to costmap_common_params.yaml my irring_scan, changed map_type from voxel to costmap (because I want to clear obstacles detected by IR sensors by Kinect, I want to use 2d map)

max_obstacle_height: 0.60  # assume something like an arm is mounted on top of the robot

# Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation)
# robot_radius: 0.20  # distance a circular robot should be clear of the obstacle (kobuki: 0.18)
footprint: [[-0.25, 0.25], [0.25, 0.25], [0.25, -0.25], [-0.25, -0.25]]  # if the robot is not circular

map_type: costmap

obstacle_layer:
  enabled:              true
  max_obstacle_height:  0.6
  origin_z:             0.0
  z_resolution:         0.2
  z_voxels:             2
  unknown_threshold:    15
  mark_threshold:       0
  combination_method:   1
  track_unknown_space:  true    #true needed for disabling global path planning through unknown space
  obstacle_range: 2.5
  raytrace_range: 3.0
  origin_z: 0.0
  z_resolution: 0.2
  z_voxels: 2
  publish_voxel_map: false
  observation_sources:  scan bump irring_scan
  scan:
    data_type: LaserScan
    topic: scan
    marking: true
    clearing: true
    min_obstacle_height: 0.25
    max_obstacle_height: 0.35
  irring_scan:
    data_type: LaserScan
    sensor_frame: base_link
    topic: irring_scan
    marking: true
    clearing: true
    min_obstacle_height: 0.25
    max_obstacle_height: 0.35
  bump:
    data_type: PointCloud2
    topic: mobile_base/sensors/bumper_pointcloud
    marking: true
    clearing: false
    min_obstacle_height: 0.0
    max_obstacle_height: 0.15
  # for debugging only, let's you see the entire voxel grid

#cost_scaling_factor and inflation_radius were ...