Merge branch 'feature/grid_map_to_image' into 'master'

[grid_map] Add a grid_map to image node.

GitOrigin-RevId: a689f94420002275b33842859ca0cf1a58e6b461

README.md

@@ -152,6 +152,10 @@ The *grid_map_demos* package contains several demonstration nodes. Use this code
         roslaunch grid_map_demos image_to_gridmap_demo.launch
 
     ![Image to grid map demo result](grid_map_demos/doc/image_to_grid_map_demo_result.png)
+
+* *[grid_map_to_image_demo](grid_map_demos/src/GridmapToImageDemo.cpp)* demonstrates how to save a grid map layer to an image. Start the demonstration with
+
+        rosrun grid_map_demos grid_map_to_image_demo _grid_map_topic:=/grid_map _file:=/home/$USER/Desktop/grid_map_image.png
 
 * *[opencv_demo](grid_map_demos/src/opencv_demo_node.cpp)* demonstrates map manipulations with help of [OpenCV] functions. Start the demonstration with
 

grid_map_demos/CHANGELOG.rst

@@ -2,6 +2,10 @@
 Changelog for package grid_map_demos
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.6.3 (2020-10-19)
+------------------
+* Added GridMapToImageDemo. Saves the elevation layer of a grid_map to a file.
+
 1.6.2 (2019-10-14)
 ------------------
 

grid_map_demos/CMakeLists.txt

@@ -78,6 +78,11 @@ add_executable(image_to_gridmap_demo
   src/ImageToGridmapDemo.cpp
 )
 
+add_executable(grid_map_to_image_demo
+  src/grid_map_to_image_demo_node.cpp
+  src/GridmapToImageDemo.cpp
+)
+
 add_executable(octomap_to_gridmap_demo
   src/octomap_to_gridmap_demo_node.cpp
   src/OctomapToGridmapDemo.cpp
@@ -134,6 +139,11 @@ target_link_libraries(
   ${catkin_LIBRARIES}
 )
 
+target_link_libraries(
+  grid_map_to_image_demo
+  ${catkin_LIBRARIES}
+)
+
 target_link_libraries(
   octomap_to_gridmap_demo
   ${catkin_LIBRARIES}
@@ -190,6 +200,7 @@ install(
   TARGETS 
     filters_demo
     image_to_gridmap_demo
+    grid_map_to_image_demo
     interpolation_demo
     iterator_benchmark
     iterators_demo
@@ -221,6 +232,7 @@ if(CATKIN_ENABLE_TESTING)
   add_dependencies(${PROJECT_NAME}-test
     filters_demo
     image_to_gridmap_demo
+    grid_map_to_image_demo
     interpolation_demo
     iterator_benchmark
     iterators_demo

grid_map_demos/include/grid_map_demos/GridmapToImageDemo.hpp

@@ -0,0 +1,64 @@
+/*
+ * GridMapToImageDemo.hpp
+ *
+ *  Created on: Oktober 19, 2020
+ *      Author: Magnus Gärtner
+ *	 Institute: ETH Zurich, ANYbotics
+ *
+ */
+
+#pragma once
+
+// ROS
+#include <ros/ros.h>
+
+#include <string>
+
+#include <grid_map_msgs/GridMap.h>
+#include <grid_map_ros/grid_map_ros.hpp>
+
+namespace grid_map_demos {
+
+/*!
+ * Saves an ElevationMapping layer as image.
+ */
+class GridMapToImageDemo {
+ public:
+  /*!
+   * Constructor.
+   * @param nodeHandle the ROS node handle.
+   */
+  GridMapToImageDemo(ros::NodeHandle& nodeHandle);
+
+  /*!
+   * Destructor.
+   */
+  virtual ~GridMapToImageDemo();
+
+ private:
+  /*!
+   * @brief Reads and verifies the ROS parameters.
+   */
+  void readParameters();
+
+  /**
+   * @brief The callback receiving the grid map.
+   * It will convert the elevation layer into a png image and save it to the specified location. Afterwards the node will terminate.
+   * @param msg the recieved grid map to save to a file.
+   */
+  void gridMapCallback(const grid_map_msgs::GridMap& msg);
+
+  //! ROS nodehandle.
+  ros::NodeHandle& nodeHandle_;
+
+  //! GridMap subscriber
+  ros::Subscriber gridMapSubscriber_;
+
+  //! Name of the grid map topic.
+  std::string gridMapTopic_;
+
+  //! Path where to store the image.
+  std::string filePath_;
+};
+
+}  // namespace grid_map_demos

grid_map_demos/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>grid_map_demos</name>
-  <version>1.6.2</version>
+  <version>1.6.3</version>
   <description>Demo nodes to demonstrate the usage of the grid map library.</description>
   <maintainer email="[email protected]">Maximilian Wulf</maintainer>
   <maintainer email="[email protected]">Yoshua Nava</maintainer>

grid_map_demos/src/GridmapToImageDemo.cpp

@@ -0,0 +1,44 @@
+/*
+ * GridMapToImageDemo.cpp
+ *
+ *  Created on: October 19, 2020
+ *      Author: Magnus Gärtner
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_demos/GridmapToImageDemo.hpp"
+
+#include <image_transport/image_transport.h>
+#include <opencv2/imgcodecs.hpp>
+
+namespace grid_map_demos {
+
+GridMapToImageDemo::GridMapToImageDemo(ros::NodeHandle& nodeHandle)
+    : nodeHandle_(nodeHandle)
+{
+  readParameters();
+  gridMapSubscriber_ = nodeHandle_.subscribe(gridMapTopic_, 1, &GridMapToImageDemo::gridMapCallback, this);
+  ROS_ERROR("Subscribed to %s", nodeHandle_.resolveName(gridMapTopic_).c_str());
+}
+
+GridMapToImageDemo::~GridMapToImageDemo()=default;
+
+void GridMapToImageDemo::readParameters()
+{
+  nodeHandle_.param("grid_map_topic", gridMapTopic_, std::string("/grid_map"));
+  nodeHandle_.param("file", filePath_, std::string("~/.ros/grid_map.png"));
+}
+
+void GridMapToImageDemo::gridMapCallback(const grid_map_msgs::GridMap& msg)
+{
+  ROS_INFO("Saving map received from: %s to file %s.", nodeHandle_.resolveName(gridMapTopic_).c_str(), filePath_.c_str());
+  grid_map::GridMap map;
+  cv_bridge::CvImage image;
+  grid_map::GridMapRosConverter::fromMessage(msg, map, {"elevation"});
+  grid_map::GridMapRosConverter::toCvImage(map,"elevation", sensor_msgs::image_encodings::MONO8, image);
+  bool success = cv::imwrite(filePath_.c_str(),image.image, {cv::IMWRITE_PNG_STRATEGY_DEFAULT});
+  ROS_INFO("Success writing image: %s", success?"true":"false");
+  ros::shutdown();
+}
+
+} /* namespace */

grid_map_demos/src/grid_map_to_image_demo_node.cpp

@@ -0,0 +1,26 @@
+/*
+ * grid_map_to_image_demo.cpp
+ *
+ *  Created on: October 19, 2020
+ *      Author: Magnus Gärtner
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#include "grid_map_demos/GridmapToImageDemo.hpp"
+
+#include <ros/ros.h>
+
+/*
+ * Usage:
+ * $ rosrun grid_map_demos grid_map_to_image_demo _grid_map_topic:=/grid_map _file:=/home/$USER/Desktop/grid_map_image.png
+ */
+int main(int argc, char** argv)
+{
+  // Initialize node and publisher.
+  ros::init(argc, argv, "grid_map_to_image_demo");
+  ros::NodeHandle nh("~");
+  grid_map_demos::GridMapToImageDemo gridMapToImageDemo(nh);
+
+  ros::spin();
+  return EXIT_SUCCESS;
+}