🐛 Fix the laser points projection at the odometry frame

Using tf2_ros package to lookup for transformation between the
odometry and laser frames.

Author: luizcarloscf

turtlebot3_mapper/mapper.py

@@ -8,15 +8,19 @@
 from sensor_msgs.msg import LaserScan
 from nav_msgs.msg import Odometry, OccupancyGrid, MapMetaData
 
+from tf2_ros.buffer import Buffer
+from tf2_ros import LookupException, TransformException 
+from tf2_ros.transform_listener import TransformListener
+
 from turtlebot3_mapper.utils import euler_from_quaternion
 
 class TurtlebotMapper(Node):
 
     def __init__(self,
                  node_name: str = 'turtlebot_mapper',
-                 width: int = 10,
-                 height: int = 10,
-                 resolution: float = 0.025):
+                 width: int = 8,
+                 height: int = 8,
+                 resolution: float = 0.03):
 
         super(TurtlebotMapper, self).__init__(node_name=node_name)
         self.width = width
@@ -33,23 +37,18 @@ def __init__(self,
             callback=self._scan_callback,
             qos_profile=10,
         )
-        self._odom_subscriber = self.create_subscription(
-            msg_type=Odometry,
-            topic="/odom",
-            callback=self._odom_callback,
-            qos_profile=10,
-        )
         self._map_publisher = self.create_publisher(
             msg_type=OccupancyGrid,
             topic="/custom_map",
             qos_profile=10,
         )
         self._update_timer = self.create_timer(
-            timer_period_sec=1,
+            timer_period_sec=0.1,
             callback=self._update_callback,
         )
         self._scan_init = False
-        self._odom_init = False
+        self._tf_buffer = Buffer()
+        self._tf_listener = TransformListener(self._tf_buffer, self)
         self._update = threading.Lock()
         self.get_logger().info(f"Init {node_name}")
 
@@ -58,42 +57,44 @@ def _scan_callback(self, message: LaserScan):
         if not self._update.locked():
             self._scan = message
 
-    def _odom_callback(self, message: Odometry):
-        self._odom_init = True
-        if not self._update.locked():
-            self._odom = message
-
     def _update_callback(self):
-        if self._scan_init and self._odom_init:
+        if self._scan_init:
             if self._update.locked():
                 return
             else:
                 self._update.acquire()
                 self.get_logger().info("updating...")
                 self.update(
                     message_laser=self._scan,
-                    message_odometry=self._odom,
                 )
                 self._map_publisher.publish(self.occupancy_grid)
                 self._update.release()
 
 
-    def update(self, message_laser: LaserScan, message_odometry: Odometry):
-        _, _, theta = euler_from_quaternion(message_odometry.pose.pose.orientation)
-        if theta < 0:
+    def update(self, message_laser: LaserScan):
+        try:
+            tf = self._tf_buffer.lookup_transform('odom',
+                                                 message_laser.header.frame_id,
+                                                 message_laser.header.stamp)
+            q = tf.transform.rotation
+        except (TransformException, LookupException):
+            self.get_logger().info("could not get tf")
+            return
+        _, _, theta = euler_from_quaternion(quaternion=q)
+        if theta < 0.0:
             theta += 2*np.pi
         distances = self.scan_to_distances(message=message_laser)
         xy = self.distances_to_xy(
             distances=distances,
-            x=message_odometry.pose.pose.position.x,
-            y=message_odometry.pose.pose.position.y,
+            x=tf.transform.translation.x,
+            y=tf.transform.translation.y,
             theta=theta,
         )
         xy[:,0] = (xy[:,0] + self.width//2)/self.resolution
         xy[:,1] = (xy[:,1] + self.height//2)/self.resolution
         xy = xy.astype(int)
-        xo = int((message_odometry.pose.pose.position.x + self.width//2)/self.resolution)
-        yo = int((message_odometry.pose.pose.position.y + self.height//2)/self.resolution)
+        xo = int((tf.transform.translation.x + self.width//2)/self.resolution)
+        yo = int((tf.transform.translation.y + self.height//2)/self.resolution)
 
         for i in range(xy.shape[0]):
             points = self.bresenham(
@@ -110,8 +111,8 @@ def update(self, message_laser: LaserScan, message_odometry: Odometry):
                     continue
                 self.grid[y, x] = 0
 
-            x = points[-1,0]
-            y = points[-1,1]
+            x = xy[i,0]
+            y = xy[i,1]
             if distances[i,0] < message_laser.range_max:
                 self.grid[y, x] = 100
                 self.grid[y+1, x] = 100