Work in progress for inputting the channel and fusion from the topic.…

… Improved error handling. Added feature for adding layer online instead of initializing from config.

elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics.yaml

@@ -1,10 +1,17 @@
 #### Plugins ########
 plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/setups/turtle_bot/plugin_config.yaml'
 
+pointcloud_channel_fusion:
+  rgb: 'pointcloud_color'
+  default: 'pointcloud_average'
+  sheep: 'pointcloud_average'
+  sofa: 'pointcloud_class_average'
+  person: 'pointcloud_class_average'
+
 #### Subscribers ########
 subscribers:
   front_cam:
-    channels: ['rgb', 'grass','tree',"person" ]
+    channels: ['rgb', 'sheep','sofa',"person" ]
     fusion: [ 'color','class_average','class_average','class_average' ]
     topic_name: '/elevation_mapping/pointcloud_semantic'
     semantic_segmentation: True
@@ -22,7 +29,7 @@ subscribers:
 #### Publishers ########
 publishers:
   elevation_map_raw:
-    layers: ['elevation', 'traversability', 'variance','rgb','sem_fil']
+    layers: ['elevation', 'traversability', 'variance','rgb','sem_fil', 'person', 'sheep', 'sofa']
     basic_layers: ['elevation']
     fps: 5.0
   elevation_map_filter:

elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp

@@ -74,8 +74,9 @@ class ElevationMappingNode {
   void readParameters();
   void setupMapPublishers();
   void pointcloudCallback(const sensor_msgs::PointCloud2& cloud, const std::string& key);
-  void imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg,
-                     const std::string& key);
+  void inputImage(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg,
+                  const std::vector<std::string>& channels, const std::vector<std::string>& fusion_methods);
+  void imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg);
   void publishAsPointCloud(const grid_map::GridMap& map) const;
   bool getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response);
   bool checkSafety(elevation_map_msgs::CheckSafety::Request& request, elevation_map_msgs::CheckSafety::Response& response);

elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp

@@ -48,8 +48,8 @@ class ElevationMappingWrapper {
 
   void input(const RowMatrixXd& points, const std::vector<std::string>& channels, const RowMatrixXd& R, const Eigen::VectorXd& t,
              const double positionNoise, const double orientationNoise);
-  void input_image(const std::string& key, const std::vector<ColMatrixXf>& image, const RowMatrixXd& R, const Eigen::VectorXd& t,
-                   const RowMatrixXd& cameraMatrix, int height, int width);
+  void input_image(const std::vector<ColMatrixXf>& multichannel_image, const std::vector<std::string>& channels, const std::vector<std::string>& fusion_methods, const RowMatrixXd& R,
+                   const Eigen::VectorXd& t, const RowMatrixXd& cameraMatrix, int height, int width);
   void move_to(const Eigen::VectorXd& p, const RowMatrixXd& R);
   void clear();
   void update_variance();

elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py

@@ -452,6 +452,7 @@ def input_pointcloud(
         """
         raw_points = cp.asarray(raw_points, dtype=self.data_type)
         additional_channels = channels[3:]
+        print("additional_channels", additional_channels)
         raw_points = raw_points[~cp.isnan(raw_points).any(axis=1)]
         self.update_map_with_kernel(
             raw_points,
@@ -464,8 +465,9 @@ def input_pointcloud(
 
     def input_image(
         self,
-        sub_key: str,
         image: List[cp._core.core.ndarray],
+        channels: List[str],
+        fusion_methods: List[str],
         R: cp._core.core.ndarray,
         t: cp._core.core.ndarray,
         K: cp._core.core.ndarray,
@@ -509,6 +511,9 @@ def input_image(
         self.uv_correspondence *= 0
         self.valid_correspondence[:, :] = False
 
+        print("channels", channels)
+        print("fusion_methods", fusion_methods)
+
         with self.map_lock:
             self.image_to_map_correspondence_kernel(
                 self.elevation_map,
@@ -524,8 +529,10 @@ def input_image(
                 size=int(self.cell_n * self.cell_n),
             )
             self.semantic_map.update_layers_image(
-                sub_key,
+                # sub_key,
                 image,
+                channels,
+                fusion_methods,
                 self.uv_correspondence,
                 self.valid_correspondence,
                 image_height,

elevation_mapping_cupy/script/elevation_mapping_cupy/fusion/fusion_manager.py

@@ -54,11 +54,11 @@ def get_plugin_idx(self, name: str, data_type: str):
         """
         Get a registered fusion plugin
         """
-        name = data_type + "_" + name
+        # name = data_type + "_" + name
         for idx, plugin in enumerate(self.plugins):
             if plugin.name == name:
                 return idx
-        print("Plugin {} is not in the list: {}".format(name, self.plugins))
+        print("[WARNING] Plugin {} is not in the list: {}".format(name, self.plugins))
         return None
 
     def execute_plugin(
@@ -72,8 +72,8 @@ def execute_plugin(
             self.plugins[idx](
                 points_all, R, t, pcl_ids, layer_ids, elevation_map, semantic_map, new_map, elements_to_shift
             )
-        else:
-            raise ValueError("Plugin {} is not registered".format(name))
+        # else:
+        #     raise ValueError("Plugin {} is not registered".format(name))
 
     def execute_image_plugin(
         self,
@@ -104,5 +104,5 @@ def execute_image_plugin(
                 semantic_map,
                 new_map,
             )
-        else:
-            raise ValueError("Plugin {} is not registered".format(name))
+        # else:
+        #     raise ValueError("Plugin {} is not registered".format(name))

elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py

@@ -7,6 +7,7 @@
 import numpy as np
 from simple_parsing.helpers import Serializable
 from dataclasses import field
+from typing import Tuple
 
 
 @dataclass
@@ -33,8 +34,19 @@ class Parameter(Serializable):
             }
         }
     )
-    additional_layers: list = field(default_factory=lambda: ["feat_0"])
-    fusion_algorithms: list = field(default_factory=lambda: ["average"])
+    # additional_layers: list = field(default_factory=lambda: ["feat_0"])
+    # fusion_algorithms: list = field(default_factory=lambda: ["average"])
+    additional_layers: list = field(default_factory=lambda: ["color"])
+    fusion_algorithms: list = field(default_factory=lambda: [
+        "image_color",
+        "image_exponential",
+        "pointcloud_average",
+        "pointcloud_bayesian_inference",
+        "pointcloud_class_average",
+        "pointcloud_class_bayesian",
+        "pointcloud_class_max",
+        "pointcloud_color"])
+    pointcloud_channel_fusion: dict = field(default_factory=lambda: {"rgb": "pointcloud_color", "default": "pointcloud_class_average"})
     data_type: str = np.float32
     average_weight: float = 0.5
 
@@ -126,18 +138,18 @@ def update(self):
         self.cell_n = int(round(self.map_length / self.resolution)) + 2
         self.true_cell_n = round(self.map_length / self.resolution)
         self.true_map_length = self.true_cell_n * self.resolution
-        semantic_layers = []
-        fusion_algorithms = []
-        for subscriber, sub_val in self.subscriber_cfg.items():
-            channels = sub_val["channels"]
-            fusion = sub_val["fusion"]
-            for i in range(len(channels)):
-                name = channels[i]
-                if name not in semantic_layers:
-                    semantic_layers.append(name)
-                    fusion_algorithms.append(fusion[i])
-        self.additional_layers = semantic_layers
-        self.fusion_algorithms = fusion_algorithms
+        # semantic_layers = []
+        # fusion_algorithms = []
+        # for subscriber, sub_val in self.subscriber_cfg.items():
+        #     channels = sub_val["channels"]
+        #     fusion = sub_val["fusion"]
+        #     for i in range(len(channels)):
+        #         name = channels[i]
+        #         if name not in semantic_layers:
+        #             semantic_layers.append(name)
+        #             fusion_algorithms.append(fusion[i])
+        # self.additional_layers = semantic_layers
+        # self.fusion_algorithms = fusion_algorithms
 
 
 if __name__ == "__main__":

elevation_mapping_cupy/script/elevation_mapping_cupy/semantic_map.py

@@ -24,16 +24,31 @@ def __init__(self, param: Parameter):
         self.unique_data = []
         self.elements_to_shift = {}
 
-        for k, config in self.param.subscriber_cfg.items():
-            for f, c in zip(config["fusion"], config["channels"]):
-                if c not in self.layer_names:
-                    self.layer_names.append(c)
-                    self.layer_specs[c] = f
-                else:
-                    assert self.layer_specs[c] == f, "Error: Single layer has multiple fusion algorithms!"
-                if f not in self.unique_fusion:
-                    dt = config["data_type"]
-                    self.unique_fusion.append(dt + "_" + f)
+        # for k, config in self.param.subscriber_cfg.items():
+        #     for f, c in zip(config["fusion"], config["channels"]):
+        #         if c not in self.layer_names:
+        #             self.layer_names.append(c)
+        #             self.layer_specs[c] = f
+        #         else:
+        #             assert self.layer_specs[c] == f, "Error: Single layer has multiple fusion algorithms!"
+        #         if f not in self.unique_fusion:
+        #             dt = config["data_type"]
+        #             self.unique_fusion.append(dt + "_" + f)
+        for channel, fusion in self.param.pointcloud_channel_fusion.items():
+            if channel not in self.layer_names:
+                if channel != "default":
+                    self.layer_names.append(channel)
+                self.layer_specs[channel] = fusion
+            else:
+                assert self.layer_specs[channel] == fusion, "Error: Single layer has multiple fusion algorithms!"
+        #     for f, c in zip(config["fusion"], config["channels"]):
+        #         else:
+        #             assert self.layer_specs[c] == f, "Error: Single layer has multiple fusion algorithms!"
+        #         if f not in self.unique_fusion:
+        #             dt = config["data_type"]
+        #             self.unique_fusion.append(dt + "_" + f)
+        self.layer_names += self.param.additional_layers
+        self.unique_fusion = self.param.fusion_algorithms
 
         self.amount_layer_names = len(self.layer_names)
 
@@ -71,6 +86,43 @@ def initialize_fusion(self):
                 self.elements_to_shift["id_max"] = id_max
             self.fusion_manager.register_plugin(fusion)
 
+    def update_fusion_setting(self):
+        for fusion in self.unique_fusion:
+            if "pointcloud_class_bayesian" == fusion:
+                pcl_ids = self.get_layer_indices("class_bayesian")
+                self.delete_new_layers[pcl_ids] = 0
+            if "pointcloud_class_max" == fusion:
+                pcl_ids = self.get_layer_indices("class_max")
+                self.delete_new_layers[pcl_ids] = 0
+                layer_cnt = self.param.fusion_algorithms.count("class_max")
+                id_max = cp.zeros(
+                    (layer_cnt, self.param.cell_n, self.param.cell_n),
+                    dtype=cp.uint32,
+                )
+                self.elements_to_shift["id_max"] = id_max
+
+    def add_layer(self, name):
+        """
+        Add a new layer to the semantic map.
+
+        Args:
+            name (str): The name of the new layer.
+        """
+        if name not in self.layer_names:
+            self.layer_names.append(name)
+            self.semantic_map = cp.append(
+                self.semantic_map,
+                cp.zeros((1, self.param.cell_n, self.param.cell_n), dtype=self.param.data_type),
+                axis=0,
+            )
+            self.new_map = cp.append(
+                self.new_map,
+                cp.zeros((1, self.param.cell_n, self.param.cell_n), dtype=self.param.data_type),
+                axis=0,
+            )
+            self.delete_new_layers = cp.append(self.delete_new_layers, cp.array([1], dtype=cp.bool8))
+
+
     def pad_value(self, x, shift_value, idx=None, value=0.0):
         """Create a padding of the map along x,y-axis according to amount that has shifted.
 
@@ -120,11 +172,22 @@ def get_fusion_of_pcl(self, channels: List[str]) -> List[str]:
             channels (List[str]):
         """
         fusion_list = []
+        process_channels = []
         for channel in channels:
+            if channel not in self.layer_specs: 
+                if "default" in self.param.pointcloud_channel_fusion:
+                    default_fusion = self.param.pointcloud_channel_fusion["default"]
+                    print(f"[WARNING] Layer {channel} not found in layer_specs. Using {default_fusion} algorithm as default.")
+                    self.layer_specs[channel] = default_fusion
+                    self.update_fusion_setting()
+                else:
+                    print(f"[WARNING] Layer {channel} not found in layer_specs. Skipping.")
+                    continue
             x = self.layer_specs[channel]
             if x not in fusion_list:
                 fusion_list.append(x)
-        return fusion_list
+            process_channels.append(channel)
+        return process_channels, fusion_list
 
     def get_layer_indices(self, fusion_alg):
         """Get the indices of the layers that are used for a specific fusion algorithm.
@@ -177,11 +240,15 @@ def update_layers_pointcloud(self, points_all, channels, R, t, elevation_map):
             t: translation vector
             elevation_map: elevation map object
         """
-        additional_fusion = self.get_fusion_of_pcl(channels)
+        process_channels, additional_fusion = self.get_fusion_of_pcl(channels)
+        for channel in process_channels:
+            if channel not in self.layer_names:
+                print(f"Layer {channel} not found, adding it to the semantic map")
+                self.add_layer(channel)
         self.new_map[self.delete_new_layers] = 0.0
         for fusion in list(set(additional_fusion)):
             # which layers need to be updated with this fusion algorithm
-            pcl_ids, layer_ids = self.get_indices_fusion(channels, fusion)
+            pcl_ids, layer_ids = self.get_indices_fusion(process_channels, fusion)
             # update the layers with the fusion algorithm
             self.fusion_manager.execute_plugin(
                 fusion,
@@ -198,8 +265,10 @@ def update_layers_pointcloud(self, points_all, channels, R, t, elevation_map):
 
     def update_layers_image(
         self,
-        sub_key: str,
+        # sub_key: str,
         image: cp._core.core.ndarray,
+        channels: List[str],
+        fusion_methods: List[str],
         uv_correspondence: cp._core.core.ndarray,
         valid_correspondence: cp._core.core.ndarray,
         image_height: cp._core.core.ndarray,
@@ -216,13 +285,26 @@ def update_layers_image(
             image_width:
         """
 
+        # print("sub_key", sub_key)
+        # print("delete_new_layers", self.delete_new_layers)
+
         # additional_fusion = self.get_fusion_of_pcl(channels)
         self.new_map[self.delete_new_layers] = 0.0
-        config = self.param.subscriber_cfg[sub_key]
+        # config = self.param.subscriber_cfg[sub_key]
 
-        for j, (fusion, channel) in enumerate(zip(config["fusion"], config["channels"])):
+        # print("config", config)
+
+        # for j, (fusion, channel) in enumerate(zip(config["fusion"], config["channels"])):
+        for j, (fusion, channel) in enumerate(zip(fusion_methods, channels)):
+            if channel not in self.layer_names:
+                print(f"Layer {channel} not found, adding it to the semantic map")
+                self.add_layer(channel)
             sem_map_idx = self.get_index(channel)
 
+            if sem_map_idx == -1:
+                print(f"Layer {channel} not found!")
+                return
+
             # which layers need to be updated with this fusion algorithm
             # pcl_ids, layer_ids = self.get_indices_fusion(channels, fusion)
             # update the layers with the fusion algorithm
@@ -266,7 +348,9 @@ def get_map_with_name(self, name):
         Returns:
             cp.array: map
         """
-        if self.layer_specs[name] == "color":
+        print("[get_map_with_name] name", name, self.layer_specs)
+        if name in self.layer_specs and (self.layer_specs[name] == "image_color" or self.layer_specs[name] == "pointcloud_color"):
+            print("this is a color map")
             m = self.get_rgb(name)
             return m
         else:
@@ -321,4 +405,7 @@ def get_index(self, name):
         Returns:
             int: index
         """
-        return self.layer_names.index(name)
+        if name not in self.layer_names:
+            return -1
+        else:
+            return self.layer_names.index(name)