added service to export trajectory

edward-wq · Apr 18, 2017 · 3a3fbba · 3a3fbba
1 parent ce2ae33
commit 3a3fbba
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 0 deletions.
diff --git a/laser_slam_ros/include/laser_slam_ros/laser_slam_worker.hpp b/laser_slam_ros/include/laser_slam_ros/laser_slam_worker.hpp
@@ -8,6 +8,7 @@
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/point_types.h>
 #include <sensor_msgs/PointCloud2.h>
+#include <std_srvs/Empty.h>
 #include <tf/transform_listener.h>
 
 #include "laser_slam_ros/GetLaserTrackSrv.h"
@@ -66,6 +67,8 @@ class LaserSlamWorker {
 
   void exportTrajectoryHead(laser_slam::Time head_duration_ns, const std::string& filename) const;
 
+  bool exportTrajectoryServiceCall(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
+
  private:
   // Convert a tf::StampedTransform to a laser_slam::Pose.
   laser_slam::Pose tfTransformToPose(const tf::StampedTransform& tf_transform);
@@ -117,6 +120,7 @@ class LaserSlamWorker {
 
   // Services.
   ros::ServiceServer get_laser_track_srv_;
+  ros::ServiceServer export_trajectory_srv_;
 
   tf::TransformListener tf_listener_;
 

diff --git a/laser_slam_ros/src/laser_slam_worker.cpp b/laser_slam_ros/src/laser_slam_worker.cpp
@@ -63,6 +63,9 @@ void LaserSlamWorker::init(
   get_laser_track_srv_ = nh.advertiseService(
       params_.get_laser_track_srv_topic,
       &LaserSlamWorker::getLaserTracksServiceCall, this);
+  export_trajectory_srv_ = nh.advertiseService(
+      "export_trajectory",
+      &LaserSlamWorker::exportTrajectoryServiceCall, this);
 
   voxel_filter_.setLeafSize(params_.voxel_size_m, params_.voxel_size_m,
                             params_.voxel_size_m);
@@ -594,4 +597,11 @@ void LaserSlamWorker::exportTrajectoryHead(laser_slam::Time head_duration_ns,
   LOG(INFO) << "Exported " << i << " trajectory poses to " << filename << ".";
 }
 
+bool LaserSlamWorker::exportTrajectoryServiceCall(std_srvs::Empty::Request& req,
+                                                  std_srvs::Empty::Response& res) {
+  exportTrajectoryHead(laser_track_->getMaxTime(),
+                       "/tmp/online_matcher/trajectory.csv");
+  return true;
+}
+
 } // namespace laser_slam_ros