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

@@ -54,7 +54,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 =
@@ -74,20 +74,25 @@ class MapPublisher : public rclcpp::Node {
         std::clamp(this->declare_parameter("max_map_height", 1024, max_map_descriptor),
                    max_map_descriptor_int_range.from_value, max_map_descriptor_int_range.to_value);
 
-    RCLCPP_INFO_STREAM(get_logger(), "file_path " << file_path);
-    RCLCPP_INFO_STREAM(get_logger(), "color_path " << color_path);
+    RCLCPP_INFO_STREAM(get_logger(), "gdal_dataset_path: '" << gdal_dataset_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));
-  }
+  void timer_callback() {}
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr original_map_pub_;
   std::shared_ptr<GridMapGeo> map_;