Finish off parameter re-do, clean up warnings from nvblox core lib.

TODO.md

@@ -10,10 +10,11 @@ Please try to update this in the PR where the appropriate changes are made. :)
   - [x] nvblox_human_node_main (low priority)
 - [x] nvblox_rviz_plugin
 - [ ] check install targets so downstream packages can depend on this
+- [ ] switch to catkin simple?
 
 ## General
-- [ ] Run clang-format with standard Google C++ on everything (should be done after code compiles) or diffs will be hard.
-- [ ] Add a Dockerfile for ROS1
+- [x] Run clang-format with standard Google C++ on everything (should be done after code compiles) or diffs will be hard.
+- [x] Add a Dockerfile for ROS1
 - [ ] Add a CI job
 
 ## Porting Documentation

nvblox_ros/CMakeLists.txt

@@ -19,7 +19,7 @@ cmake_minimum_required(VERSION 3.20)
 include(ExternalProject)
 
 project(nvblox_ros LANGUAGES CXX CUDA)
-add_compile_options(-Wall -Wextra -O3)
+add_compile_options(-Wall -Wextra -Wpedantic -O3)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
@@ -128,7 +128,7 @@ target_include_directories(${PROJECT_NAME}_lib PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>
   ${catkin_INCLUDE_DIRS})
-target_include_directories(${PROJECT_NAME}_lib BEFORE PRIVATE
+target_include_directories(${PROJECT_NAME}_lib SYSTEM BEFORE PRIVATE
   $<TARGET_PROPERTY:nvblox::nvblox_eigen,INTERFACE_INCLUDE_DIRECTORIES>)
 
 

nvblox_ros/include/nvblox_ros/mapper_initialization.hpp

@@ -22,13 +22,11 @@
 
 #include <ros/ros.h>
 
-
 namespace nvblox {
 
 class Mapper;
 
-bool initializeMapper(const std::string& mapper_name, Mapper* mapper_ptr,
-                      ros::NodeHandle& nh);
+void initializeMapper(Mapper* mapper_ptr, ros::NodeHandle& nh);
 
 }  // namespace nvblox
 

nvblox_ros/include/nvblox_ros/nvblox_node.hpp

@@ -60,7 +60,7 @@ class NvbloxNode {
   virtual ~NvbloxNode();
 
   // Setup. These are called by the constructor.
-  bool getParameters();
+  void getParameters();
   void subscribeToTopics();
   void advertiseTopics();
   void advertiseServices();
@@ -216,28 +216,18 @@ class NvbloxNode {
   ros::Timer clear_outside_radius_timer_;
 
   // ROS & nvblox settings
-
-  // Topic Names
-  std::string depth_image_topic_name_ = "null";
-  std::string depth_image_camera_info_topic_name_ = "null";
-
-  std::string color_image_topic_name_ = "null";
-  std::string color_image_camera_info_topic_name_ = "null";
-
-  std::string pointcloud_topic_name_ = "null";
-
   float voxel_size_ = 0.05f;
-  bool esdf_2d_ = true;
-  bool esdf_distance_slice_ = true;
+  bool esdf_2d_ = false;
+  bool esdf_distance_slice_ = false;
   float esdf_slice_height_ = 1.0f;
   ProjectiveLayerType static_projective_layer_type_ =
       ProjectiveLayerType::kTsdf;
-  bool is_realsense_data_ = true;
+  bool is_realsense_data_ = false;
 
   // Toggle parameters
-  bool use_depth_ = false;
-  bool use_lidar_ = true;
-  bool use_color_ = false;
+  bool use_depth_ = true;
+  bool use_lidar_ = false;
+  bool use_color_ = true;
   bool compute_esdf_ = true;
   bool compute_mesh_ = true;
 
@@ -254,13 +244,13 @@ class NvbloxNode {
   float esdf_2d_max_height_ = 1.0f;
 
   // Slice visualization params
-  std::string slice_visualization_attachment_frame_id_ = "lidar";
+  std::string slice_visualization_attachment_frame_id_ = "base_link";
   float slice_visualization_side_length_ = 10.0f;
 
   // ROS settings & update throttles
   std::string global_frame_ = "map";
   /// Pose frame to use if using transform topics.
-  std::string pose_frame_ = "lidar";
+  std::string pose_frame_ = "base_link";
   float max_depth_update_hz_ = 10.0f;
   float max_color_update_hz_ = 5.0f;
   float max_lidar_update_hz_ = 10.0f;

nvblox_ros/src/lib/mapper_initialization.cpp

@@ -43,12 +43,9 @@ WeightingFunctionType weighting_function_type_from_string(
   }
 }
 
-bool initializeMapper(const std::string& mapper_name, Mapper* mapper_ptr,
-                      ros::NodeHandle& nh) {
+void initializeMapper(Mapper* mapper_ptr, ros::NodeHandle& nh) {
   ROS_INFO_STREAM("Initialize Mapper:");
 
-  bool isInitializationSuccessful = true;
-
   // tsdf or occupancy integrator
   float projective_integrator_max_integration_distance_m = 0.0f;
   if (nh.getParam("projective_integrator_max_integration_distance_m",
@@ -57,10 +54,6 @@ bool initializeMapper(const std::string& mapper_name, Mapper* mapper_ptr,
         projective_integrator_max_integration_distance_m);
     mapper_ptr->occupancy_integrator().max_integration_distance_m(
         projective_integrator_max_integration_distance_m);
-  } else {
-    ROS_ERROR_STREAM(
-        "projective_integrator_max_integration_distance_m not found");
-    isInitializationSuccessful = false;
   }
 
   float lidar_projective_integrator_max_integration_distance_m = 0.0f;
@@ -70,10 +63,6 @@ bool initializeMapper(const std::string& mapper_name, Mapper* mapper_ptr,
         lidar_projective_integrator_max_integration_distance_m);
     mapper_ptr->lidar_occupancy_integrator().max_integration_distance_m(
         lidar_projective_integrator_max_integration_distance_m);
-  } else {
-    ROS_ERROR_STREAM(
-        "lidar_projective_integrator_max_integration_distance_m not found");
-    isInitializationSuccessful = false;
   }
 
   float projective_integrator_truncation_distance_vox = 0.0f;
@@ -87,32 +76,23 @@ bool initializeMapper(const std::string& mapper_name, Mapper* mapper_ptr,
         projective_integrator_truncation_distance_vox);
     mapper_ptr->lidar_occupancy_integrator().truncation_distance_vox(
         projective_integrator_truncation_distance_vox);
-  } else {
-    ROS_ERROR_STREAM("projective_integrator_truncation_distance_vox not found");
-    isInitializationSuccessful = false;
   }
 
   // tsdf and color integrator
   // NOTE(alexmillane): Currently weighting mode does not affect the occupancy
   // integrator.
-  std::string weighting_mode_param = "null";
+  std::string weighting_mode_param = "";
   if (nh.getParam("weighting_mode", weighting_mode_param)) {
     const WeightingFunctionType weight_mode =
         weighting_function_type_from_string(weighting_mode_param);
     mapper_ptr->tsdf_integrator().weighting_function_type(weight_mode);
     mapper_ptr->color_integrator().weighting_function_type(weight_mode);
-  } else {
-    ROS_ERROR_STREAM("weighting_mode not found");
-    isInitializationSuccessful = false;
   }
 
   float tsdf_integrator_max_weight = 0.0f;
   if (nh.getParam("tsdf_integrator_max_weight", tsdf_integrator_max_weight)) {
     mapper_ptr->tsdf_integrator().max_weight(tsdf_integrator_max_weight);
     mapper_ptr->lidar_tsdf_integrator().max_weight(tsdf_integrator_max_weight);
-  } else {
-    ROS_ERROR_STREAM("tsdf_integrator_max_weight not found");
-    isInitializationSuccessful = false;
   }
 
   // occupancy integrator
@@ -123,9 +103,6 @@ bool initializeMapper(const std::string& mapper_name, Mapper* mapper_ptr,
         free_region_occupancy_probability);
     mapper_ptr->lidar_occupancy_integrator().free_region_occupancy_probability(
         free_region_occupancy_probability);
-  } else {
-    ROS_ERROR_STREAM("free_region_occupancy_probability not found");
-    isInitializationSuccessful = false;
   }
 
   float occupied_region_occupancy_probability = 0.0f;
@@ -136,9 +113,6 @@ bool initializeMapper(const std::string& mapper_name, Mapper* mapper_ptr,
     mapper_ptr->lidar_occupancy_integrator()
         .occupied_region_occupancy_probability(
             occupied_region_occupancy_probability);
-  } else {
-    ROS_ERROR_STREAM("occupied_region_occupancy_probability not found");
-    isInitializationSuccessful = false;
   }
 
   float unobserved_region_occupancy_probability = 0.0f;
@@ -149,9 +123,6 @@ bool initializeMapper(const std::string& mapper_name, Mapper* mapper_ptr,
     mapper_ptr->lidar_occupancy_integrator()
         .unobserved_region_occupancy_probability(
             unobserved_region_occupancy_probability);
-  } else {
-    ROS_ERROR_STREAM("unobserved_region_occupancy_probability not found");
-    isInitializationSuccessful = false;
   }
 
   float occupied_region_half_width_m = 0.0f;
@@ -161,46 +132,31 @@ bool initializeMapper(const std::string& mapper_name, Mapper* mapper_ptr,
         occupied_region_half_width_m);
     mapper_ptr->lidar_occupancy_integrator().occupied_region_half_width_m(
         occupied_region_half_width_m);
-  } else {
-    ROS_ERROR_STREAM("occupied_region_half_width_m not found");
-    isInitializationSuccessful = false;
   }
 
   float free_region_decay_probability = 0.0f;
   if (nh.getParam("free_region_decay_probability",
                   free_region_decay_probability)) {
     mapper_ptr->occupancy_decay_integrator().free_region_decay_probability(
         free_region_decay_probability);
-  } else {
-    ROS_ERROR_STREAM("free_region_decay_probability not found");
-    isInitializationSuccessful = false;
   }
 
   float occupied_region_decay_probability = 0.0f;
   if (nh.getParam("occupied_region_decay_probability",
                   occupied_region_decay_probability)) {
     mapper_ptr->occupancy_decay_integrator().occupied_region_decay_probability(
         occupied_region_decay_probability);
-  } else {
-    ROS_ERROR_STREAM("occupied_region_decay_probability not found");
-    isInitializationSuccessful = false;
   }
 
   float mesh_integrator_min_weight = 0.0f;
   if (nh.getParam("mesh_integrator_min_weight", mesh_integrator_min_weight)) {
     mapper_ptr->mesh_integrator().min_weight(mesh_integrator_min_weight);
-  } else {
-    ROS_ERROR_STREAM("mesh_integrator_min_weight not found");
-    isInitializationSuccessful = false;
   }
 
   bool mesh_integrator_weld_vertices = false;
   if (nh.getParam("mesh_integrator_weld_vertices",
                   mesh_integrator_weld_vertices)) {
     mapper_ptr->mesh_integrator().weld_vertices(mesh_integrator_weld_vertices);
-  } else {
-    ROS_ERROR_STREAM("mesh_integrator_weld_vertices not found");
-    isInitializationSuccessful = false;
   }
 
   // color integrator
@@ -209,41 +165,27 @@ bool initializeMapper(const std::string& mapper_name, Mapper* mapper_ptr,
                   color_integrator_max_integration_distance_m)) {
     mapper_ptr->color_integrator().max_integration_distance_m(
         color_integrator_max_integration_distance_m);
-  } else {
-    ROS_ERROR_STREAM("color_integrator_max_integration_distance_m not found");
-    isInitializationSuccessful = false;
   }
 
   // esdf integrator
   float esdf_integrator_min_weight = 0.0f;
   if (nh.getParam("esdf_integrator_min_weight", esdf_integrator_min_weight)) {
     mapper_ptr->esdf_integrator().min_weight(esdf_integrator_min_weight);
-  } else {
-    ROS_ERROR_STREAM("esdf_integrator_min_weight not found");
-    isInitializationSuccessful = false;
   }
 
   float esdf_integrator_max_site_distance_vox = 0.0f;
   if (nh.getParam("esdf_integrator_max_site_distance_vox",
                   esdf_integrator_max_site_distance_vox)) {
     mapper_ptr->esdf_integrator().max_site_distance_vox(
         esdf_integrator_max_site_distance_vox);
-  } else {
-    ROS_ERROR_STREAM("esdf_integrator_max_site_distance_vox not found");
-    isInitializationSuccessful = false;
   }
 
   float esdf_integrator_max_distance_m = 0.0f;
   if (nh.getParam("esdf_integrator_max_distance_m",
                   esdf_integrator_max_distance_m)) {
     mapper_ptr->esdf_integrator().max_distance_m(
         esdf_integrator_max_distance_m);
-  } else {
-    ROS_ERROR_STREAM("esdf_integrator_max_distance_m not found");
-    isInitializationSuccessful = false;
   }
-
-  return isInitializationSuccessful;
 }
 
 }  // namespace nvblox

nvblox_ros/src/lib/nvblox_human_node.cpp

@@ -74,7 +74,7 @@ void NvbloxHumanNode::initializeMultiMapper() {
   // calling initializeMapper() (again) (it its also called in the base
   // constructor, on the now-deleted Mapper).
   mapper_ = multi_mapper_.get()->unmasked_mapper();
-  initializeMapper("mapper", mapper_.get(), nh_);
+  initializeMapper(mapper_.get(), nh_);
   // Set to an invalid depth to ignore human pixels in the unmasked mapper
   // during integration.
   multi_mapper_->setDepthUnmaskedImageInvalidPixel(-1.f);
@@ -84,7 +84,7 @@ void NvbloxHumanNode::initializeMultiMapper() {
   human_mapper_ = multi_mapper_.get()->masked_mapper();
   // Human mapper params have not been declared yet
   // declareMapperParameters(mapper_name, this);
-  initializeMapper(mapper_name, human_mapper_.get(), nh_);
+  initializeMapper(human_mapper_.get(), nh_);
   // Set to a distance bigger than the max. integration distance to not include
   // non human pixels on the human mapper, but clear along the projection.
   // TODO(remosteiner): Think of a better way to do this.