updated python wrapper

Author: JonasFrey96

elevation_mapping_cupy/config/parameters.yaml

@@ -40,9 +40,9 @@ max_unsafe_n: 10                                # if the number of cells under s
 overlap_clear_range_xy: 4.0                     # xy range [m] for clearing overlapped area. this defines the valid area for overlap clearance. (used for multi floor setting)
 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: 'enu'                               # 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: 'enu'
+map_frame: 'odom'                               # The map frame where the odometry source uses.
+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/script/elevation_mapping_cupy/elevation_mapping_ros.py

@@ -9,16 +9,21 @@
 import rospy
 import ros_numpy
 from tf.transformations import quaternion_matrix
-from sensor_msgs.msg import PointCloud2
 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")
         self.initalize_elevation_mapping()
-        
+        self.node_name = "elevation_mapping"
         # ROS
         self.initalize_ros()
         self.register_subscribers()
@@ -29,12 +34,15 @@ def initalize_elevation_mapping(self):
         weight_file = os.path.join(self.root, "config/weights.dat") 
         plugin_config_file = os.path.join(self.root, "config/plugin_config.yaml")
         param = Parameter( use_chainer=False, weight_file=weight_file, plugin_config_file=plugin_config_file )
+        param.update()
         self._pointcloud_process_counter = 0
         self._map = ElevationMap(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("elevation_mapping", anonymous=False)
+        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()
@@ -47,37 +55,79 @@ def register_subscribers(self):
 
     def register_publishers(self):
         # TODO publishers
-        print(self.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, data, config):
+    def pointcloud_callback(self, msg, config):
         # convert pcd into numpy array
-        points = ros_numpy.numpify(data)
-        out = np.copy( np.frombuffer(points.tobytes(), np.dtype(np.float32) ))
-        out = out.reshape(out.shape[0],-1)[:,:3]
-        points = points.astype(np.float32)
+        channels = config[0]
+        fusion = config[1]
+        points = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(msg)
 
         # get pose of pointcloud
-        t = rospy.Time(secs=data.header.stamp.secs, nsecs=data.header.stamp.nsecs)
+        t = rospy.Time(secs=msg.header.stamp.secs, nsecs=msg.header.stamp.nsecs)
         try:
-            transform = self._tf_buffer.lookup_transform(self.map_frame, data.header.frame_id, t, rospy.Duration(1.0))
+            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])
+        R = quaternion_matrix([q.w, q.x, q.y, q.z])[:3,:3]
 
         # process pointcloud
-        self._map.input(points, 0, R, t, 0, 0)
-        self._map.update_normal(self._map.elevation_map[0])
+        self._map.input(points, ['x','y','z']+channels, R, t, 0, 0)
         self._pointcloud_process_counter += 1
+        print(self._pointcloud_process_counter)
 
     def update_pose(self, t):
         # get pose of base
@@ -90,9 +140,12 @@ def update_pose(self, t):
         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])
+        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()
 
@@ -104,9 +157,9 @@ def get_ros_params(self):
         # 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")