Make lidar octomap updater capable of resetting octomaps and loading …

…prior ones.

See moveit/moveit_ros#651 as to why

vigir_lidar_octomap_updater/include/vigir_lidar_octomap_updater/lidar_octomap_updater.h

@@ -49,6 +49,9 @@
 #include <filters/filter_chain.h>
 #include <vigir_perception_msgs/FilteredLocalizedLaserScan.h>
 
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <std_srvs/Empty.h>
+
 namespace occupancy_map_monitor
 {
 
@@ -75,8 +78,12 @@ class LidarOctomapUpdater : public OccupancyMapUpdater
 
   bool getShapeTransform(ShapeHandle h, Eigen::Affine3d &transform) const;
   void cloudMsgCallback(const sensor_msgs::LaserScan::ConstPtr &cloud_msg);
+  void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped pose);
+  bool clearOctomap(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
   void stopHelper();
 
+  void resetOctomap(bool load_prior = true);
+
   ros::NodeHandle root_nh_;
   ros::NodeHandle private_nh_;
   boost::shared_ptr<tf::Transformer> tf_;
@@ -90,6 +97,8 @@ class LidarOctomapUpdater : public OccupancyMapUpdater
   std::string filtered_cloud_topic_;
   ros::Publisher filtered_cloud_publisher_;
 
+  std::string p_prior_map_file_;
+
   message_filters::Subscriber<sensor_msgs::LaserScan> *point_cloud_subscriber_;
   tf::MessageFilter<sensor_msgs::LaserScan> *point_cloud_filter_;
 
@@ -113,6 +122,9 @@ class LidarOctomapUpdater : public OccupancyMapUpdater
   ros::Publisher scan_self_filtered_publisher_;
   ros::Publisher filtered_localized_scan_publisher_;
 
+  ros::Subscriber initial_pose_sub_;
+  ros::ServiceServer clear_service_;
+
   //octomap::KeySet free_cells, occupied_cells, model_cells, clip_cells;
 
 };

vigir_lidar_octomap_updater/src/lidar_octomap_updater.cpp

@@ -137,6 +137,22 @@ bool LidarOctomapUpdater::initialize()
   scan_filtered_publisher_ = private_nh_.advertise<sensor_msgs::LaserScan>("scan_filtered", 10, false);
   scan_self_filtered_publisher_ = private_nh_.advertise<sensor_msgs::LaserScan>("scan_self_filtered", 10, false);
   filtered_localized_scan_publisher_ = private_nh_.advertise<vigir_perception_msgs::FilteredLocalizedLaserScan>("scan_filtered_localized", 10, false);
+
+
+  private_nh_.param("prior_octomap_to_load", p_prior_map_file_, std::string(""));
+
+  if (!p_prior_map_file_.empty()){
+    ROS_INFO("Using prior octomap file: %s", p_prior_map_file_.c_str());
+    tree_->lockWrite();
+    tree_->readBinary(p_prior_map_file_);
+    tree_->unlockWrite();
+  }else{
+    ROS_INFO("No prior octomap to use given, not loading any.");
+  }
+
+  initial_pose_sub_ = private_nh_.subscribe("/initialpose", 1, &LidarOctomapUpdater::initialPoseCallback, this);
+  clear_service_ = private_nh_.advertiseService("/clear_octomap", &LidarOctomapUpdater::clearOctomap, this);
+
   return true;
 }
 
@@ -516,4 +532,35 @@ void LidarOctomapUpdater::cloudMsgCallback(const sensor_msgs::LaserScan::ConstPt
   ROS_DEBUG("Processed laser scan in %lf ms. Self filtering took %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0, (self_filter_finished_time - start).toSec() * 1000.0 );
 }
 
+void LidarOctomapUpdater::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped pose)
+{
+  {
+    ROS_INFO("Received intialpose, resetting planning scene octomap");
+
+    this->resetOctomap();
+  }
+}
+
+bool LidarOctomapUpdater::clearOctomap(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
+{
+  this->resetOctomap();
+
+  return true;
+}
+
+void LidarOctomapUpdater::resetOctomap(bool use_prior)
+{
+  if (use_prior && !p_prior_map_file_.empty()){
+    ROS_INFO("Resetting planning scene octomap to prior map");
+    tree_->lockWrite();
+    tree_->readBinary(p_prior_map_file_);
+    tree_->unlockWrite();
+  }else{
+    ROS_INFO("Resetting planning scene octomap to cleared map");
+    tree_->lockWrite();
+    tree_->clear();
+    tree_->unlockWrite();
+  }
+}
+
 }