rtabmap_ros/launch/jfr2018/slam_gmapping.patch

diff --git a/gmapping/src/slam_gmapping.cpp b/gmapping/src/slam_gmapping.cpp
index bd9977c..b4f9382 100644
--- a/gmapping/src/slam_gmapping.cpp
+++ b/gmapping/src/slam_gmapping.cpp
@@ -114,6 +114,7 @@ Initial map dimensions and resolution:
 #include "ros/ros.h"
 #include "ros/console.h"
 #include "nav_msgs/MapMetaData.h"
+#include <nav_msgs/Path.h>
 
 #include "gmapping/sensor/sensor_range/rangesensor.h"
 #include "gmapping/sensor/sensor_odometry/odometrysensor.h"
@@ -258,6 +259,7 @@ void SlamGMapping::startLiveSlam()
   entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
   sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
   sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
+  pathPub_ = node_.advertise<nav_msgs::Path>("mapPath", 1, true);
   ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
   scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
   scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
@@ -273,6 +275,7 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t
   entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
   sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
   sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
+  pathPub_ = node_.advertise<nav_msgs::Path>("map_path", 1, true);
   ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
   
   rosbag::Bag bag;
@@ -410,6 +413,18 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
     return false;
   }
 
+  try
+    {
+	  	tf_.lookupTransform(laser_frame_, base_frame_, scan.header.stamp, scan_to_base_);
+	  	scan_to_base_.getOrigin().m_floats[2] = 0;
+    }
+    catch(tf::TransformException e)
+    {
+      ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
+               e.what());
+      return false;
+    }
+
   // create a point 1m above the laser position and transform it into the laser-frame
   tf::Vector3 v;
   v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
@@ -617,6 +632,8 @@ SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
     ROS_DEBUG("scan processed");
 
     GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
+    GMapping::GridSlamProcessor::TNode * node = gsp_->getParticles()[gsp_->getBestParticleIndex()].node;
+
     ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
     ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
     ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
@@ -699,6 +716,23 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
                                 delta_);
 
   ROS_DEBUG("Trajectory tree:");
+  nav_msgs::Path path;
+  int count = 0;
+  for(GMapping::GridSlamProcessor::TNode* n = best.node;
+        n;
+        n = n->parent)
+    {
+      if(!n->reading)
+      {
+        ROS_DEBUG("Reading is NULL");
+        continue;
+      }
+	  ++count;
+    }
+  path.poses.resize(count);
+  int oi = path.poses.size()-1;
+
+
   for(GMapping::GridSlamProcessor::TNode* n = best.node;
       n;
       n = n->parent)
@@ -712,6 +746,13 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
       ROS_DEBUG("Reading is NULL");
       continue;
     }
+    path.poses[oi].header.frame_id = map_frame_;
+    path.poses[oi].header.stamp = ros::Time(n->reading->getTime());
+
+    tf::Transform tmp = tf::Transform(tf::createQuaternionFromRPY(0, 0, n->pose.theta), tf::Vector3(n->pose.x, n->pose.y, 0))*scan_to_base_;
+    tf::poseTFToMsg(tmp, path.poses[oi].pose);
+    --oi;
+
     matcher.invalidateActiveArea();
     matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
     matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
@@ -764,8 +805,11 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
   map_.map.header.stamp = ros::Time::now();
   map_.map.header.frame_id = tf_.resolve( map_frame_ );
 
+  path.header = map_.map.header;
+
   sst_.publish(map_.map);
   sstm_.publish(map_.map.info);
+  pathPub_.publish(path);
 }
 
 bool 
diff --git a/gmapping/src/slam_gmapping.h b/gmapping/src/slam_gmapping.h
index ae622b9..8d84645 100644
--- a/gmapping/src/slam_gmapping.h
+++ b/gmapping/src/slam_gmapping.h
@@ -53,6 +53,7 @@ class SlamGMapping
     ros::Publisher entropy_publisher_;
     ros::Publisher sst_;
     ros::Publisher sstm_;
+    ros::Publisher pathPub_;
     ros::ServiceServer ss_;
     tf::TransformListener tf_;
     message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
@@ -92,6 +93,8 @@ class SlamGMapping
     std::string map_frame_;
     std::string odom_frame_;
 
+    tf::StampedTransform scan_to_base_;
+
     void updateMap(const sensor_msgs::LaserScan& scan);
     bool getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t);
     bool initMapper(const sensor_msgs::LaserScan& scan);