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

@@ -36,6 +36,10 @@
 
 #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>
 
 #include "grid_map_geo/transform.hpp"

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,17 +64,19 @@ def generate_launch_description():
                 description="Location.",
             ),
             DeclareLaunchArgument(
-                "tif_path",
-                default_value=f'{Path(pkg_grid_map_geo) / "resources" / default_tif_file}',
+                "gdal_dataset_path",
+                default_value=f'{Path(pkg_grid_map_geo) / "resources" / default_gdal_dataset}',
                 description="Full path to the elevation map file.",
             ),
             DeclareLaunchArgument(
-                "tif_color_path",
-                default_value=f'{Path(pkg_grid_map_geo) / "resources" / default_tif_color_file}',
+                "gdal_dataset_color_path",
+                default_value=f'{Path(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" namespace="grid_map_geo" 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

@@ -73,7 +73,7 @@ void GridMapGeo::setMaxMapSizePixels(const int pixels_x, const int pixels_y) {
 
 bool GridMapGeo::Load(const std::string &map_path, const std::string color_map_path) {
   bool loaded = initializeFromGdalDataset(map_path);
-  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;
@@ -84,7 +84,7 @@ bool GridMapGeo::initializeFromGdalDataset(const std::string &path) {
   GDALAllRegister();
   const auto dataset = GDALDatasetUniquePtr(GDALDataset::FromHandle(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;
@@ -175,7 +175,7 @@ bool GridMapGeo::addColorFromGeotiff(const std::string &path) {
   GDALAllRegister();
   const auto dataset = GDALDatasetUniquePtr(GDALDataset::FromHandle(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;
@@ -236,7 +236,7 @@ bool GridMapGeo::addLayerFromGeotiff(const std::string &layer_name, const std::s
   GDALAllRegister();
   const auto dataset = GDALDatasetUniquePtr(GDALDataset::FromHandle(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 frame_id = this->declare_parameter("frame_id", "map");
     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 =
@@ -76,8 +76,8 @@ 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 " << gdal_dataset_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 << "'");
     tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
 
     map_ = std::make_shared<GridMapGeo>(frame_id);