Turtlebot SLAM

//Code for Video: https://www.youtube.com/watch?v=6FelCZQqIa4

//Launch file
     <node
        name="projectedScan"
        pkg="kinect_fg"
        type="projectedScan"
        respawn="true"
        >
        <remap from="input" to="/camera/depth/points"/>
    </node> <!-- Output: "/obsScan"-->
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Source code for projectedScan.cpp:

#include <ros/ros.h>
#include <boost/foreach.hpp>

#include "pluginlib/class_list_macros.h"
#include "nodelet/nodelet.h"
#include "sensor_msgs/LaserScan.h"
#include <pcl/point_cloud.h>
#include "pcl_ros/point_cloud.h"
#include <pcl/point_types.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <visualization_msgs/Marker.h>
#include <dynamic_reconfigure/server.h>

using namespace std;

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

ros::Publisher pub_;
ros::Subscriber sub_;
ros::Publisher marker_pub;

void callback(const PointCloud::ConstPtr& msg)
{
  sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());

 // output->header = msg->header; //Error here
 output->header = pcl_conversions::fromPCL(msg->header);//use this in Hydro
  output->header.frame_id = "/camera_depth_frame"; //Previous: "/laser"
  output->angle_min = -M_PI / 6 ; //Default: -M_PI/6
  output->angle_max = M_PI / 6 ; //Default: M_PI/6
  output->angle_increment = M_PI / 180.0 /8; //Default: M_PI / 180.0 / 2.0
  output->time_increment = 0.0;
  output->scan_time = 1.0 / 30.0; //Default: 1.0 / 30.0
  output->range_min = 0.3;
  output->range_max = 10.0;

  uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min)/ output->angle_increment);
  output->ranges.assign(ranges_size, output->range_max + 1.0);


  double max_height_ = 0.20;//previous: 5
  double min_height_ = -0.25;//previous: 0
//+++++++++++++++++++++++++++++++++++++++++++++

  BOOST_FOREACH (const pcl::PointXYZ& pt, msg->points)
  {
      float x = pt.x;
      float y = pt.y;
      float z = pt.z;


      if (std::isnan(x) || std::isnan(y) || std::isnan(z)) {
          //ROS_INFO("rejected for nan in point(%f, %f, %f)\n", x, y, z);
          continue;
      }

      if (y > max_height_ || y < min_height_) { //max_height_ min_height_
          //ROS_INFO("rejected for height %f not in range (%f, %f)\n", x, min_height_, max_height_);
          continue;
      }
      double angle = -atan2(x, z);
      if (angle < output->angle_min || angle > output->angle_max) {
         // ROS_INFO("rejected for angle %f not in range (%f, %f)\n",angle, output->angle_min, output->angle_max);
          continue;
      }

      int index = (angle - output->angle_min) / output->angle_increment;
      double range_sq = z * z + x * x;
      if (output->ranges[index] * output->ranges[index] > range_sq)
          output->ranges[index] = sqrt(range_sq);

     // ROS_INFO("Y: %f %f, Z: %f %f", min_y, max_y, min_z, max_z);


  }
  pub_.publish(output);
}

double min_height_, max_height_;
int32_t u_min_, u_max_;
std::string output_frame_id_;
bool dynamic_set;

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

  ros::Subscriber sub = nh.subscribe<PointCloud>("input", 1, callback);
  pub_ = nh.advertise<sensor_msgs::LaserScan> ("/obsScan", 10);
  ros::spin();
}