fix dict populating

Author: Gibbon OPC

elevation_mapping_cupy/elevation_mapping_cupy/plugins/plugin_manager.py

@@ -132,7 +132,7 @@ def init(self, plugin_params: List[PluginParams], extra_params: List[Dict]):
         self.plugin_names = self.get_plugin_names()
 
     def load_plugin_settings(self, file_path: str):
-        print("Start loading plugins...")
+        print("Start loading plugins...", flush=True)
         cfg = YAML().load(open(file_path, "r"))
         plugin_params = []
         extra_params = []
@@ -148,7 +148,7 @@ def load_plugin_settings(self, file_path: str):
                 )
                 extra_params.append(v["extra_params"])
         self.init(plugin_params, extra_params)
-        print("Loaded plugins are ", *self.plugin_names)
+        print("Loaded plugins are ", *self.plugin_names, flush=True)
 
     def get_layer_names(self):
         names = []

elevation_mapping_cupy/elevation_mapping_cupy/semantic_map.py

@@ -156,14 +156,14 @@ def get_fusion(
                     if "default" in channel_fusions:
                         default_fusion = channel_fusions["default"]
                         print(
-                            f"[WARNING] Layer {channel} not found in layer_specs. Using {default_fusion} algorithm as default."
+                            f"[WARNING] Layer {channel} not found in layer_specs. Using {default_fusion} algorithm as default.", flush=True
                         )
                         layer_specs[channel] = default_fusion
                         self.update_fusion_setting()
                     # If there's no default fusion algorithm, we skip this channel
                     else:
                         print(
-                            f"[WARNING] Layer {channel} not found in layer_specs ({layer_specs}) and no default fusion is configured. Skipping."
+                            f"[WARNING] Layer {channel} not found in layer_specs ({layer_specs}) and no default fusion is configured. Skipping.", flush=True
                         )
                         continue
                 else:
@@ -192,7 +192,7 @@ def get_layer_indices(self, fusion_alg, layer_specs):
         """
         layer_indices = cp.array([], dtype=cp.int32)
         for it, (key, val) in enumerate(layer_specs.items()):
-            if key in val == fusion_alg:
+            if val == fusion_alg:
                 layer_indices = cp.append(layer_indices, it).astype(cp.int32)
         return layer_indices
 
@@ -236,7 +236,7 @@ def update_layers_pointcloud(self, points_all, channels, R, t, elevation_map):
         # If channels has a new layer that is not in the semantic map, add it
         for channel in process_channels:
             if channel not in self.layer_names:
-                print(f"Layer {channel} not found, adding it to the semantic map")
+                print(f"Layer {channel} not found, adding it to the semantic map", flush=True)
                 self.add_layer(channel)
 
         # Resetting new_map for the layers that are to be deleted
@@ -286,7 +286,7 @@ def update_layers_image(
         self.new_map[self.delete_new_layers] = 0.0
         for j, (fusion, channel) in enumerate(zip(fusion_methods, process_channels)):
             if channel not in self.layer_names:
-                print(f"Layer {channel} not found, adding it to the semantic map")
+                print(f"Layer {channel} not found, adding it to the semantic map", flush=True)
                 self.add_layer(channel)
             sem_map_idx = self.get_index(channel)
 

elevation_mapping_cupy/src/elevation_mapping_ros.cpp

@@ -109,6 +109,10 @@ ElevationMappingNode::ElevationMappingNode(const rclcpp::NodeOptions& options)
     }
   }
   subscriber_keys.assign(keys_set.begin(), keys_set.end());
+  if (subscriber_keys.empty()) {
+    RCLCPP_FATAL(this->get_logger(), "There aren't any subscribers to be configured, the elevation mapping cannot be configured. Exit");
+    throw std::runtime_error("No subscribers configured");
+  }
 
   for (const auto& key : subscriber_keys) {
     std::string prefix = "subscribers." + key;
@@ -217,6 +221,10 @@ ElevationMappingNode::ElevationMappingNode(const rclcpp::NodeOptions& options)
       }
     }
   }
+  if (publisher_keys.empty()) {
+    RCLCPP_FATAL(this->get_logger(), "There aren't any publishers to be configured, the elevation mapping cannot be configured. Exit");
+    throw std::runtime_error("No publishers configured");
+  }
 
   for (const auto& key : publisher_keys) {
     std::string prefix = "publishers." + key;

elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp

@@ -123,8 +123,10 @@ void ElevationMappingWrapper::setParameters(rclcpp::Node* node) {
       if (name.find("pointcloud_channel_fusions.") == 0) {
           std::string key = name.substr(27); // length of prefix
           std::string val = node->get_parameter(name).as_string();
+          pointcloud_channel_fusion_dict[key.c_str()] = val;
       }
   }
+  RCLCPP_INFO_STREAM(node->get_logger(), "pointcloud_channel_fusions_dict: " << py::str(pointcloud_channel_fusion_dict));
   param_.attr("pointcloud_channel_fusions") = pointcloud_channel_fusion_dict;
 
   // Image channel fusion
@@ -137,6 +139,7 @@ void ElevationMappingWrapper::setParameters(rclcpp::Node* node) {
           image_channel_fusion_dict[key.c_str()] = val;
       }
   }
+  RCLCPP_INFO_STREAM(node->get_logger(), "image_channel_fusions_dict: " << py::str(image_channel_fusion_dict));
   param_.attr("image_channel_fusions") = image_channel_fusion_dict;