Merge branch 'feature/4_project_semantic_image' into documentation

Author: JonasFrey96

.gitignore

@@ -1,9 +1,11 @@
 *.pyc
 *.bkp
 *.orig
-.idea
+.idea*
 .pytest_cache
 .run/
 docs/build 
 _build
-
+.idea*
+.vscode*
+*.egg-info

elevation_mapping_cupy/config/parameters.yaml

@@ -41,8 +41,8 @@ overlap_clear_range_xy: 4.0                     # xy range [m] for clearing over
 overlap_clear_range_z: 2.0                      # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting)
 
 map_frame: 'odom'                               # The map frame where the odometry source uses.
-base_frame: 'base_footprint'                              # The robot's base frame. This frame will be a center of the map.
-corrected_map_frame: 'corrected_odom'
+base_frame: 'base'                              # The robot's base frame. This frame will be a center of the map.
+corrected_map_frame: 'odom_corrected'
 
 #### Feature toggles ########
 enable_edge_sharpen: true
@@ -76,14 +76,14 @@ publishers:
     layers: ['elevation', 'traversability', 'variance']
     basic_layers: ['elevation', 'traversability']
     fps: 5.0
-  elevation_map_recordable:
-    layers: ['elevation', 'traversability']
-    basic_layers: ['elevation', 'traversability']
-    fps: 2.0
-  elevation_map_filter:
-    layers: ['min_filter', 'smooth', 'inpaint', 'upper_bound','rgb']
-    basic_layers: ['min_filter']
-    fps: 3.0
+  # elevation_map_recordable:
+  #   layers: ['elevation', 'traversability']
+  #   basic_layers: ['elevation', 'traversability']
+  #   fps: 2.0
+  # elevation_map_filter:
+  #   layers: ['min_filter', 'smooth', 'inpaint', 'upper_bound','rgb']
+  #   basic_layers: ['min_filter']
+  #   fps: 3.0
 
 #### Initializer ########
 initialize_method: 'linear'                                         # Choose one from 'nearest', 'linear', 'cubic'

elevation_mapping_cupy/config/sensor_parameter.yaml

@@ -1,10 +1,9 @@
 subscribers:
-#  sensor_name: #if you have multiple sensors assign different names
-#    channels: ['features','person'] # names of the channels, for sem seg it should be the same name as the
-                                    # pretrained network uses
-#    fusion: ['average','class_average'] # the fusion algorithm type used.
-                                    # Pick between: ['color', 'average','class_average']
-#    topic_name: '/elevation_mapping/pointcloud_semantic' # of the published pointcloud subscribed by the el_map
+  sensor_name:
+     channels: ['feat_0','feat_1']
+     fusion: ['average','average']
+     topic_name: '/elevation_mapping/pointcloud_semantic'
+
   front_cam:
     channels: ['rgb','feat_0','feat_1','grass','tree','fence','person']
     fusion: ['color','average','average','class_average','class_average','class_average','class_average']
@@ -30,7 +29,33 @@ subscribers:
     confidence_topic: "/camera/depth/image_raw"
     confidence_threshold: 10
     feature_extractor: False
+    
+  front_bpearl:
+    channels: []
+    fusion: ['average']
+    topic_name: /robot_self_filter/bpearl_front/point_cloud
+  
+  rear_bpearl:
+    channels: []
+    fusion: ['average']
+    topic_name: /robot_self_filter/bpearl_rear/point_cloud
 
-
-
-
+  front_depth:
+    channels: []
+    fusion: ['average']
+    topic_name: /depth_camera_front/point_cloud_self_filtered
+  
+  rear_depth:
+    channels: []
+    fusion: ['average']
+    topic_name: /depth_camera_rear/point_cloud_self_filtered
+  
+  left_depth:
+    channels: []
+    fusion: ['average']
+    topic_name: /depth_camera_left/point_cloud_self_filtered
+  
+  right_depth:
+    channels: []
+    fusion: ['average']
+    topic_name: /depth_camera_right/point_cloud_self_filtered

elevation_mapping_cupy/script/elevation_mapping_cupy/__init__.py

@@ -0,0 +1,2 @@
+from .parameter import Parameter
+from .elevation_mapping import ElevationMap

elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py

@@ -9,20 +9,19 @@
 import threading
 import subprocess
 
-from .traversability_filter import get_filter_chainer, get_filter_torch
-from .parameter import Parameter
-from .custom_kernels import add_points_kernel, add_color_kernel, color_average_kernel
-from .custom_kernels import sum_kernel
-from .custom_kernels import error_counting_kernel
-from .custom_kernels import average_map_kernel
-from .custom_kernels import dilation_filter_kernel
-from .custom_kernels import normal_filter_kernel
-from .custom_kernels import polygon_mask_kernel
-from .map_initializer import MapInitializer
-from .plugins.plugin_manager import PluginManager
-from .semantic_map import SemanticMap
-
-from .traversability_polygon import (
+from elevation_mapping_cupy.traversability_filter import get_filter_chainer, get_filter_torch
+from elevation_mapping_cupy.parameter import Parameter
+from elevation_mapping_cupy.custom_kernels import add_points_kernel, add_color_kernel, color_average_kernel
+from elevation_mapping_cupy.custom_kernels import sum_kernel
+from elevation_mapping_cupy.custom_kernels import error_counting_kernel
+from elevation_mapping_cupy.custom_kernels import average_map_kernel
+from elevation_mapping_cupy.custom_kernels import dilation_filter_kernel
+from elevation_mapping_cupy.custom_kernels import normal_filter_kernel
+from elevation_mapping_cupy.custom_kernels import polygon_mask_kernel
+from elevation_mapping_cupy.map_initializer import MapInitializer
+from elevation_mapping_cupy.plugins.plugin_manager import PluginManager
+from elevation_mapping_cupy.semantic_map import SemanticMap
+from elevation_mapping_cupy.traversability_polygon import (
     get_masked_traversability,
     is_traversable,
     calculate_area,

elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping_ros.py

@@ -0,0 +1,210 @@
+from elevation_mapping_cupy import ElevationMap
+from elevation_mapping_cupy import Parameter
+
+# General
+import os 
+import numpy as np
+
+# ROS
+import rospy
+import ros_numpy
+from tf.transformations import quaternion_matrix
+import tf2_ros
+import rospkg
+
+from sensor_msgs.msg import PointCloud2
+from grid_map_msgs.msg import GridMap
+from std_msgs.msg import Float32MultiArray
+from std_msgs.msg import MultiArrayLayout as MAL
+from std_msgs.msg import MultiArrayDimension as MAD
+
+class ElevationMapWrapper():
+    def __init__(self):
+        rospack = rospkg.RosPack()
+        self.root = rospack.get_path("elevation_mapping_cupy")
+        weight_file = os.path.join(self.root, "config/weights.dat")
+        plugin_config_file = os.path.join(self.root, "config/plugin_config.yaml")
+        self.param = Parameter( use_chainer=False, weight_file=weight_file, plugin_config_file=plugin_config_file )
+        self.node_name = "elevation_mapping"
+        # ROS
+        self.initalize_ros()
+        self.register_subscribers()
+        self.initalize_elevation_mapping()
+        self.register_publishers()
+        self.register_timers()
+
+    def initalize_elevation_mapping(self):
+        self.param.update()
+        self._pointcloud_process_counter = 0
+        self._map = ElevationMap(self.param)
+        self._map_data = np.zeros((self._map.cell_n - 2, self._map.cell_n - 2), dtype=np.float32)
+        self._map_q = None
+        self._map_t = None
+        
+    def initalize_ros(self):
+        rospy.init_node(self.node_name, anonymous=False)
+        self._tf_buffer = tf2_ros.Buffer()
+        self._listener = tf2_ros.TransformListener(self._tf_buffer)
+        self.get_ros_params()
+        
+    def register_subscribers(self):
+        pointcloud_subs = {}
+        additional = []
+        fusion = []
+        for key, config in self.subscribers.items():
+            pointcloud_subs[key] = rospy.Subscriber(config["topic_name"], PointCloud2, self.pointcloud_callback, (config["channels"], config["fusion"]))
+            for chan,fus in zip(config["channels"],config["fusion"]):
+                if chan not in additional:
+                    additional.append(chan)
+                    fusion.append(fus)
+        self.param.additional_layers = additional
+        self.param.fusion_algorithms = fusion
+        self.dtype = [
+            ("x", np.float32),
+            ("y", np.float32),
+            ("z", np.float32),
+        ]
+        for chan in config["channels"]:
+            self.dtype.append((chan, np.float32))
+
+    def register_publishers(self):
+        # TODO publishers
+        self._publishers = {}
+        self._publishers_timers = []
+        for k, v in self.publishers.items():
+            print(f"Register Publisher: {k}")
+            self._publishers[k] = rospy.Publisher(f"/{self.node_name}/{k}", GridMap, queue_size=10)
+            self._publishers_timers.append( rospy.Timer(rospy.Duration(1/v["fps"]), lambda x: self.publish_map(x, k)))
+            
+    def publish_map(self, t, k):
+        print(k)
+        if self._map_q is None:
+            return
+        
+        gm = GridMap()
+        gm.info.header.frame_id = self.map_frame
+        gm.info.header.stamp = rospy.Time.now()
+        gm.info.header.seq = 0
+        gm.info.resolution = self._map.resolution
+        gm.info.length_x = self._map.map_length
+        gm.info.length_y = self._map.map_length
+        gm.info.pose.position.x = self._map_t.x
+        gm.info.pose.position.y = self._map_t.y
+        gm.info.pose.position.z = self._map_t.z
+        gm.info.pose.orientation.w = 1 # self._map_q.w
+        gm.info.pose.orientation.x = 0 # self._map_q.x
+        gm.info.pose.orientation.y = 0 # self._map_q.y
+        gm.info.pose.orientation.z = 0 # self._map_q.z
+        gm.layers = self.publishers[k]["layers"]
+        gm.basic_layers = self.publishers[k]["basic_layers"]
+
+        # arr.layout.dim = [int(gm.info.length_x/gm.info.resolution), int(gm.info.length_y/gm.info.resolution)]
+
+        for i, layer in enumerate(gm.layers):
+            self._map.get_map_with_name_ref(layer, self._map_data)
+            # self._map_data = np.nan_to_num(self._map_data, False, 0)
+            data = self._map_data.copy() 
+            arr = Float32MultiArray()
+            arr.layout = MAL()
+            N = self._map_data.shape[0]
+            arr.layout.dim.append( MAD(label="column_index",size=N,stride=int(N*N)))
+            arr.layout.dim.append( MAD(label="row_index",size=N,stride=N))
+            arr.data =  tuple(np.ascontiguousarray(data).reshape(-1))
+            gm.data.append( arr )
+        
+        self._publishers[k].publish( gm )
+    
+    def register_timers(self):
+        self.timer_variance = rospy.Timer(rospy.Duration(1/self.update_variance_fps), self.update_variance)
+        self.timer_pose = rospy.Timer(rospy.Duration(1/self.update_pose_fps), self.update_pose)
+        self.timer_time = rospy.Timer(rospy.Duration(self.time_interval), self.update_time)
+        
+    def pointcloud_callback(self, msg, config):
+        # convert pcd into numpy array
+        channels = ['x','y','z']+config[0]
+        fusion = config[1]
+        points = ros_numpy.numpify(msg)
+        pts = np.empty((points.shape[0],0))
+        for ch in channels:
+            pts = np.append(pts,np.expand_dims(points[ch],1),axis=1)
+
+        # get pose of pointcloud
+        t = rospy.Time(secs=msg.header.stamp.secs, nsecs=msg.header.stamp.nsecs)
+        try:
+            transform = self._tf_buffer.lookup_transform(self.map_frame, msg.header.frame_id, t, rospy.Duration(1.0))
+        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
+            print(e)
+            return
+
+        t = transform.transform.translation
+        t = np.array( [t.x, t.y, t.z] )
+        q = transform.transform.rotation
+        R = quaternion_matrix([q.w, q.x, q.y, q.z])[:3,:3]
+
+        # process pointcloud
+        self._map.input(pts, channels, R, t, 0, 0)
+        self._pointcloud_process_counter += 1
+        print(self._pointcloud_process_counter)
+        
+    def update_pose(self, t):
+        # get pose of base
+        t = rospy.Time.now()
+        try:
+            transform = self._tf_buffer.lookup_transform(self.map_frame, self.base_frame, t, rospy.Duration(1.0))
+        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
+            print(e)
+            return        
+        t = transform.transform.translation
+        trans = np.array( [t.x, t.y, t.z] )
+        q = transform.transform.rotation
+        rot = quaternion_matrix([q.w, q.x, q.y, q.z])[:3,:3]
+        self._map.move_to(trans, rot)
+        
+        self._map_t = t
+        self._map_q = q
+        
+    def update_variance(self, t):
+        self._map.update_variance()
+    
+    def update_time(self, t):
+        self._map.update_time()
+    
+    def get_ros_params(self):
+        # TODO fix this here when later launching with launch-file
+        # This is currently {p} elevation_mapping")
+        para = os.path.join(self.root, "config/parameters.yaml")
+        sens = os.path.join(self.root, "config/sensor_parameter.yaml")
+        os.system(f"rosparam delete /{self.node_name}")
+        os.system(f"rosparam load {para} elevation_mapping")
+        os.system(f"rosparam load {sens} elevation_mapping")
+        self.subscribers = rospy.get_param("~subscribers")
+        self.publishers = rospy.get_param("~publishers")
+        self.initialize_frame_id = rospy.get_param("~initialize_frame_id", "base")
+        self.initialize_tf_offset = rospy.get_param("~initialize_tf_offset", 0.0)
+        self.pose_topic = rospy.get_param("~pose_topic", "pose")
+        self.map_frame = rospy.get_param("~map_frame", "map")
+        self.base_frame = rospy.get_param("~base_frame", "base")
+        self.corrected_map_frame = rospy.get_param("~corrected_map_frame", "corrected_map")
+        self.initialize_method = rospy.get_param("~initialize_method", "cubic")
+        self.position_lowpass_alpha = rospy.get_param("~position_lowpass_alpha", 0.2)
+        self.orientation_lowpass_alpha = rospy.get_param("~orientation_lowpass_alpha", 0.2)
+        self.recordable_fps = rospy.get_param("~recordable_fps", 3.0)
+        self.update_variance_fps = rospy.get_param("~update_variance_fps", 1.0)
+        self.time_interval = rospy.get_param("~time_interval", 0.1)
+        self.update_pose_fps = rospy.get_param("~update_pose_fps", 10.0)
+        self.initialize_tf_grid_size = rospy.get_param("~initialize_tf_grid_size", 0.5)
+        self.map_acquire_fps = rospy.get_param("~map_acquire_fps", 5.0)
+        self.publish_statistics_fps = rospy.get_param("~publish_statistics_fps", 1.0)
+        self.enable_pointcloud_publishing = rospy.get_param("~enable_pointcloud_publishing", False)
+        self.enable_normal_arrow_publishing = rospy.get_param("~enable_normal_arrow_publishing", False)
+        self.enable_drift_corrected_TF_publishing = rospy.get_param("~enable_drift_corrected_TF_publishing", False)
+        self.use_initializer_at_start = rospy.get_param("~use_initializer_at_start", False)
+    
+if __name__ == '__main__':
+    emw = ElevationMapWrapper()
+    
+    while not rospy.is_shutdown():
+        try:
+            rospy.spin()
+        except rospy.ROSInterruptException:
+            print("Error")

elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py

@@ -6,13 +6,13 @@
 import pickle
 import numpy as np
 from simple_parsing.helpers import Serializable
-
+from dataclasses import field
 
 @dataclass
 class Parameter(Serializable):
     resolution: float = 0.04
-    additional_layers: list = ['feat_0'].copy
-    fusion_algorithms: list = ['average'].copy
+    additional_layers: list = field(default_factory=lambda: ["feat_0"]) 
+    fusion_algorithms: list = field(default_factory=lambda: ["average"])
     cell_n: int = None
     data_type: str = np.float32