Improvements for grid map loader.

wpfhtl · Sep 7, 2015 · de882e2 · de882e2
1 parent 0e35f07
commit de882e2
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 5 deletions.
diff --git a/grid_map/src/GridMapRosConverter.cpp b/grid_map/src/GridMapRosConverter.cpp
@@ -409,16 +409,19 @@ bool GridMapRosConverter::loadFromBag(const std::string& pathToBag, const std::s
   bag.open(pathToBag, rosbag::bagmode::Read);
   rosbag::View view(bag, rosbag::TopicQuery(topic));
 
+  bool isDataFound = false;
   for (const auto& messageInstance : view) {
     grid_map_msgs::GridMap::ConstPtr message = messageInstance.instantiate<grid_map_msgs::GridMap>();
     if (message != NULL) {
       fromMessage(*message, gridMap);
+      isDataFound = true;
     } else {
       bag.close();
       ROS_WARN("Unable to load data from ROS bag.");
       return false;
     }
   }
+  if (!isDataFound) ROS_WARN_STREAM("No data under the topic '" << topic << "' was found.");
   bag.close();
   return true;
 }

diff --git a/grid_map_loader/include/grid_map_loader/GridMapLoader.hpp b/grid_map_loader/include/grid_map_loader/GridMapLoader.hpp
@@ -70,7 +70,10 @@ class GridMapLoader
   std::string filePath_;
 
   //! Topic name of the grid map in the ROS bag.
-  std::string topic_;
+  std::string bagTopic_;
+
+  //! Topic name of the grid map to be loaded.
+  std::string publishTopic_;
 
   //! Duration to publish the grid map.
   ros::Duration duration_;

diff --git a/grid_map_loader/src/GridMapLoader.cpp b/grid_map_loader/src/GridMapLoader.cpp
@@ -18,7 +18,7 @@ GridMapLoader::GridMapLoader(ros::NodeHandle nodeHandle)
     : nodeHandle_(nodeHandle)
 {
   readParameters();
-  publisher_ = nodeHandle_.advertise<grid_map_msgs::GridMap>(topic_, 1, true);
+  publisher_ = nodeHandle_.advertise<grid_map_msgs::GridMap>(publishTopic_, 1, true);
   if (!load()) return;
   publish();
   ros::requestShutdown();
@@ -30,7 +30,8 @@ GridMapLoader::~GridMapLoader()
 
 bool GridMapLoader::readParameters()
 {
-  nodeHandle_.param("topic", topic_, std::string("/grid_map"));
+  nodeHandle_.param("bag_topic", bagTopic_, std::string("/grid_map"));
+  nodeHandle_.param("publish_topic", publishTopic_, bagTopic_);
   nodeHandle_.param("file_path", filePath_, std::string());
   double durationInSec;
   nodeHandle_.param("duration", durationInSec, 5.0);
@@ -41,8 +42,7 @@ bool GridMapLoader::readParameters()
 bool GridMapLoader::load()
 {
   ROS_INFO_STREAM("Loading grid map from path " << filePath_ << ".");
-  return GridMapRosConverter::loadFromBag(filePath_, topic_, map_);
-  return true;
+  return GridMapRosConverter::loadFromBag(filePath_, bagTopic_, map_);
 }
 
 void GridMapLoader::publish()