From bcbe63c4cc194e149a656c779a19bdc8ab0ae078 Mon Sep 17 00:00:00 2001
From: Matthew McGill <mmcgill@cse.unsw.edu.au>
Date: Mon, 15 Sep 2014 14:27:05 +1000
Subject: [PATCH] Now publishing the path history.

---
 .../include/crosbot_fastslam/module.hpp        | 18 +++++++++++++++++-
 .../include/crosbot_fastslam/particle.hpp      |  4 ++++
 crosbot_fastslam/src/fastslam.cpp              |  3 ++-
 crosbot_map/include/crosbot_map/map.hpp        |  3 ++-
 4 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/crosbot_fastslam/include/crosbot_fastslam/module.hpp b/crosbot_fastslam/include/crosbot_fastslam/module.hpp
index 833af1e..675aeda 100644
--- a/crosbot_fastslam/include/crosbot_fastslam/module.hpp
+++ b/crosbot_fastslam/include/crosbot_fastslam/module.hpp
@@ -120,7 +120,23 @@ public:
 			}
 		}
 
-		// TODO: publish history
+		// publish history
+		if (historyPub.getNumSubscribers() > 0) {
+			nav_msgs::PathPtr rosPath(new nav_msgs::Path());
+			rosPath->header.frame_id = mapFrame;
+			rosPath->header.stamp = newMean->getLatestUpdate()->timestamp.toROS();
+
+			PathPtr path = newMean->getPath();
+			rosPath->poses.resize(path->path.size());
+			for (size_t i = 0; i < path->path.size(); ++i) {
+				geometry_msgs::PoseStamped& pose = rosPath->poses[i];
+				pose.pose = path->path[i].toROS();
+				pose.header.frame_id = mapFrame;
+				pose.header.stamp = (i < path->timestamps.size())?(path->timestamps[i].toROS()):ros::Time(0);
+			}
+
+			historyPub.publish(rosPath);
+		}
 	}
 
 	void tagAdded(MapPtr map, TagPtr tag) {
diff --git a/crosbot_fastslam/include/crosbot_fastslam/particle.hpp b/crosbot_fastslam/include/crosbot_fastslam/particle.hpp
index 05b7274..a1fa2d2 100644
--- a/crosbot_fastslam/include/crosbot_fastslam/particle.hpp
+++ b/crosbot_fastslam/include/crosbot_fastslam/particle.hpp
@@ -61,6 +61,10 @@ public:
 
 		for (size_t i = 0; i < history.size(); i++) {
 			rval->path.push_back(history[i].pose);
+			if (history[i].cloud != NULL)
+				rval->timestamps.push_back(history[i].cloud->timestamp);
+			else
+				rval->timestamps.push_back(Time(0,0));
 		}
 
 		if (rval->path.size() == 0 || rval->path[rval->path.size()-1] != pose)
diff --git a/crosbot_fastslam/src/fastslam.cpp b/crosbot_fastslam/src/fastslam.cpp
index f4d8874..47c56dd 100644
--- a/crosbot_fastslam/src/fastslam.cpp
+++ b/crosbot_fastslam/src/fastslam.cpp
@@ -468,7 +468,8 @@ void FastSLAMMap::moveParticles(Pose relativeMotion, MapCloudPtr cloud, bool cal
 
 	for (size_t i = 0; i < jobs.size(); i++) {
 		delete jobs[i];
-	} jobs.clear();
+	}
+	jobs.clear();
 }
 
 void FastSLAMMap::updateParticles(MapCloudPtr cloud) {
diff --git a/crosbot_map/include/crosbot_map/map.hpp b/crosbot_map/include/crosbot_map/map.hpp
index dc69d4b..b0fa5c3 100644
--- a/crosbot_map/include/crosbot_map/map.hpp
+++ b/crosbot_map/include/crosbot_map/map.hpp
@@ -20,9 +20,10 @@ namespace crosbot {
 class Path : public HandledObject {
 public:
 	std::vector<Pose> path;
+	std::vector<Time> timestamps;
 
 	Path() {}
-	Path(Pose pose) {
+	Path(Pose pose, Time timstamp = Time(0,0)) {
 		path.push_back(pose);
 	}
 };
-- 
GitLab