Final fixes

* Switch to lambda instead of bind for timer callback
* Handle no color by default so SRTM dataset works out of the box
* Improve behavior of nullptr derefernece on publish

Signed-off-by: Ryan Friedman <[email protected]>

Author: Ryanf55

include/grid_map_geo/grid_map_geo.hpp

@@ -39,6 +39,9 @@
 #include <grid_map_core/GridMap.hpp>
 #include <grid_map_core/iterators/GridMapIterator.hpp>
 
+// Color map is optional. If left as this default value, color will not be loaded.
+static const std::string COLOR_MAP_DEFAULT_PATH {""};
+
 
 #include <iostream>
 struct Location {
@@ -188,7 +191,6 @@ class GridMapGeo {
   Location maporigin_;
 
  private:
-
   // Set default map size occupying 4MB RAM assuming 32 bit height precision.
   int max_raster_x_size_ {1024};
   int max_raster_y_size_ {1024};

launch/load_tif.launch.py

@@ -28,15 +28,15 @@ def generate_launch_description():
         ],
     )
 
-    # tif loader node
-    tif_loader = Node(
+    # map publisher node
+    map_publisher = Node(
         package="grid_map_geo",
         namespace="grid_map_geo",
         executable="map_publisher",
-        name="tif_loader",
+        name="map_publisher",
         parameters=[
-            {"tif_path": LaunchConfiguration("tif_path")},
-            {"tif_color_path": LaunchConfiguration("tif_color_path")},
+            {"gdal_dataset_path": LaunchConfiguration("gdal_dataset_path")},
+            {"gdal_dataset_color_path": LaunchConfiguration("gdal_dataset_color_path")},
         ],
         output="screen",
         emulate_tty=True,
@@ -51,8 +51,8 @@ def generate_launch_description():
     )
 
     default_location = "sargans"
-    default_tif_file = "sargans.tif"
-    default_tif_color_file = "sargans_color.tif"
+    default_gdal_dataset = "sargans.tif"
+    default_gdal_color_dataset = "sargans_color.tif"
     return LaunchDescription(
         [
             DeclareLaunchArgument(
@@ -64,21 +64,23 @@ def generate_launch_description():
                 description="Location.",
             ),
             DeclareLaunchArgument(
-                "tif_path",
+                "gdal_dataset_path",
                 default_value=os.path.join(
-                    pkg_grid_map_geo, "resources", default_tif_file
+                    pkg_grid_map_geo, "resources", default_gdal_dataset
                 ),
                 description="Full path to the elevation map file.",
             ),
             DeclareLaunchArgument(
-                "tif_color_path",
+                "gdal_dataset_color_path",
                 default_value=os.path.join(
-                    pkg_grid_map_geo, "resources", default_tif_color_file
+                    pkg_grid_map_geo, "resources", default_gdal_color_dataset
                 ),
                 description="Full path to the elevation texture file.",
             ),
             static_transform_publisher,
-            tif_loader,
+            map_publisher,
             rviz,
         ]
     )
+    
+

launch/load_tif_launch.xml

@@ -4,8 +4,8 @@
     <node pkg="tf2_ros" exec="static_transform_publisher" name="world_map" args="--frame-id world --child-frame-id map"/>
 
     <node pkg="grid_map_geo" exec="map_publisher" name="map_publisher" output="screen">
-        <param name="tif_path" value="$(find-pkg-share grid_map_geo)/resources/sargans.tif"/>
-        <param name="tif_color_path" value="$(find-pkg-share grid_map_geo)/resources/sargans_color.tif"/>
+        <param name="gdal_dataset_path" value="$(find-pkg-share grid_map_geo)/resources/sargans.tif"/>
+        <param name="gdal_dataset_color_path" value="$(find-pkg-share grid_map_geo)/resources/sargans_color.tif"/>
     </node>
 
     <group if="$(var rviz)">

launch/load_vrt_launch.xml

@@ -2,11 +2,11 @@
     <arg name="rviz" default="false"/>
     <node pkg="tf2_ros" exec="static_transform_publisher" name="world_map" args="--frame-id world --child-frame-id map"/>
 
-    <node pkg="grid_map_geo" exec="map_publisher" name="map_publisher" output="screen" launch-prefix="gdb -ex run --args">
+    <node pkg="grid_map_geo" exec="map_publisher" name="map_publisher" output="screen" launch-prefix="gdb -ex run --args" > 
         <param name="gdal_dataset_path" value="$(find-pkg-share grid_map_geo)/resources/ap_srtm1.vrt"/>
     </node>
 
     <group if="$(var rviz)">
-        <node exec="rviz2" name="rviz2" pkg="rviz2"  args="-d $(find-pkg-share grid_map_geo)/launch/config.rviz" />
+        <node exec="rviz2" name="rviz2" pkg="rviz2" args="-d $(find-pkg-share grid_map_geo)/launch/config.rviz" />
     </group>
 </launch>

src/grid_map_geo.cpp

@@ -74,7 +74,7 @@ void GridMapGeo::setMaxMapSizePixels(const int pixels_x, const int pixels_y) {
 
 bool GridMapGeo::Load(const std::string &map_path, bool algin_terrain, const std::string color_map_path) {
   bool loaded = initializeFromGdalDataset(map_path, algin_terrain);
-  if (!color_map_path.empty()) {  // Load color layer if the color path is nonempty
+  if (color_map_path != COLOR_MAP_DEFAULT_PATH) {  // Load color layer if the color path is nonempty
     bool color_loaded = addColorFromGeotiff(color_map_path);
   }
   if (!loaded) return false;
@@ -85,7 +85,7 @@ bool GridMapGeo::initializeFromGdalDataset(const std::string &path, bool align_t
   GDALAllRegister();
   GDALDataset *dataset = (GDALDataset *)GDALOpen(path.c_str(), GA_ReadOnly);
   if (!dataset) {
-    std::cout << "Failed to open" << std::endl;
+    std::cout << __func__ << "Failed to open '" << path << "'" << std::endl;
     return false;
   }
   std::cout << std::endl << "Loading GDAL Dataset for gridmap" << std::endl;
@@ -197,7 +197,7 @@ bool GridMapGeo::addColorFromGeotiff(const std::string &path) {
   GDALAllRegister();
   GDALDataset *dataset = (GDALDataset *)GDALOpen(path.c_str(), GA_ReadOnly);
   if (!dataset) {
-    std::cout << "Failed to open" << std::endl;
+    std::cout << __func__ << "Failed to open '" << path << "'" << std::endl;
     return false;
   }
   std::cout << std::endl << "Loading color layer from GeoTIFF file for gridmap" << std::endl;
@@ -258,7 +258,7 @@ bool GridMapGeo::addLayerFromGeotiff(const std::string &layer_name, const std::s
   GDALAllRegister();
   GDALDataset *dataset = (GDALDataset *)GDALOpen(path.c_str(), GA_ReadOnly);
   if (!dataset) {
-    std::cout << "Failed to open" << std::endl;
+    std::cout << __func__ << "Failed to open '" << path << "'" << std::endl;
     return false;
   }
   std::cout << std::endl << "Loading color layer from GeoTIFF file for gridmap" << std::endl;

src/map_publisher.cpp

@@ -56,7 +56,7 @@ class MapPublisher : public rclcpp::Node {
       original_map_pub_ = this->create_publisher<grid_map_msgs::msg::GridMap>("elevation_map", 1);
 
       const std::string gdal_dataset_path = this->declare_parameter("gdal_dataset_path", ".");
-      const std::string color_path = this->declare_parameter("tif_color_path", ".");
+      const std::string color_path = this->declare_parameter("gdal_dataset_color_path", COLOR_MAP_DEFAULT_PATH);
       rcl_interfaces::msg::ParameterDescriptor max_map_descriptor;
       max_map_descriptor.read_only = true;
       max_map_descriptor.description = "Maximum number of raster pixels able to be loaded. \
@@ -79,17 +79,24 @@ class MapPublisher : public rclcpp::Node {
 
 
       RCLCPP_INFO_STREAM(get_logger(), "gdal_dataset_path: '" << gdal_dataset_path << "'");
-      RCLCPP_INFO_STREAM(get_logger(), "color_path: '" << color_path << "'");
+      RCLCPP_INFO_STREAM(get_logger(), "gdal_dataset_color_path: '" << color_path << "'");
 
       map_ = std::make_shared<GridMapGeo>();
       map_->setMaxMapSizePixels(max_map_width, max_map_height);
       map_->Load(gdal_dataset_path, false, color_path);
-      // timer_ = this->create_wall_timer(5s, std::bind(&MapPublisher::timer_callback, this));
+
+      auto timer_callback = [this]() -> void {
+        auto msg = grid_map::GridMapRosConverter::toMessage(map_->getGridMap());
+        if (msg) {
+          msg->header.stamp = now();
+          original_map_pub_->publish(std::move(msg));
+        }
+      };
+      timer_ = this->create_wall_timer(5s, timer_callback);
     }
   private:
     void timer_callback() {
-      auto msg = grid_map::GridMapRosConverter::toMessage(map_->getGridMap());
-      original_map_pub_->publish(std::move(msg));
+
     }
     rclcpp::TimerBase::SharedPtr timer_;
     rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr original_map_pub_;