Added laser data from map in nav2_loopback_sim

nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py

@@ -17,17 +17,24 @@
 
 from geometry_msgs.msg import PoseWithCovarianceStamped, Twist
 from geometry_msgs.msg import Quaternion, TransformStamped, Vector3
+from nav2_simple_commander.line_iterator import LineIterator
 from nav_msgs.msg import Odometry
+from nav_msgs.srv import GetMap
 import rclpy
 from rclpy.duration import Duration
 from rclpy.node import Node
 from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
 from sensor_msgs.msg import LaserScan
-from tf2_ros import TransformBroadcaster
+from tf2_ros import Buffer, TransformBroadcaster, TransformListener
 import tf_transformations
 
-from .utils import addYawToQuat, matrixToTransform, transformStampedToMatrix
-
+from .utils import (
+    addYawToQuat,
+    getMapOccupancy,
+    matrixToTransform,
+    transformStampedToMatrix,
+    worldToMap,
+)
 
 """
 This is a loopback simulator that replaces a physics simulator to create a
@@ -48,6 +55,8 @@ def __init__(self):
         self.initial_pose = None
         self.timer = None
         self.setupTimer = None
+        self.map = None
+        self.mat_base_to_laser = None
 
         self.declare_parameter('update_duration', 0.01)
         self.update_dur = self.get_parameter('update_duration').get_parameter_value().double_value
@@ -64,6 +73,14 @@ def __init__(self):
         self.declare_parameter('scan_frame_id', 'base_scan')
         self.scan_frame_id = self.get_parameter('scan_frame_id').get_parameter_value().string_value
 
+        self.declare_parameter('scan_publish_dur', 0.1)
+        self.scan_publish_dur = self.get_parameter(
+            'scan_publish_dur').get_parameter_value().double_value
+
+        self.declare_parameter('publish_map_odom_tf', True)
+        self.publish_map_odom_tf = self.get_parameter(
+            'publish_map_odom_tf').get_parameter_value().bool_value
+
         self.t_map_to_odom = TransformStamped()
         self.t_map_to_odom.header.frame_id = self.map_frame_id
         self.t_map_to_odom.child_frame_id = self.odom_frame_id
@@ -88,12 +105,31 @@ def __init__(self):
         self.scan_pub = self.create_publisher(LaserScan, 'scan', sensor_qos)
 
         self.setupTimer = self.create_timer(0.1, self.setupTimerCallback)
+
+        self.map_client = self.create_client(GetMap, '/map_server/map')
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        self.getMap()
+
         self.info('Loopback simulator initialized')
 
+    def getBaseToLaserTf(self):
+        try:
+            # Wait for transform and lookup
+            transform = self.tf_buffer.lookup_transform(
+                self.base_frame_id, self.scan_frame_id, rclpy.time.Time())
+            self.mat_base_to_laser = transformStampedToMatrix(transform)
+
+        except Exception as ex:
+            self.get_logger().error('Transform lookup failed: %s' % str(ex))
+
     def setupTimerCallback(self):
         # Publish initial identity odom transform & laser scan to warm up system
         self.tf_broadcaster.sendTransform(self.t_odom_to_base_link)
-        self.publishLaserScan()
+        if self.mat_base_to_laser is None:
+            self.getBaseToLaserTf()
 
     def cmdVelCallback(self, msg):
         self.debug('Received cmd_vel')
@@ -122,6 +158,7 @@ def initialPoseCallback(self, msg):
                 self.setupTimer.destroy()
                 self.setupTimer = None
             self.timer = self.create_timer(self.update_dur, self.timerCallback)
+            self.timer_laser = self.create_timer(self.scan_publish_dur, self.publishLaserScan)
             return
 
         self.initial_pose = msg.pose.pose
@@ -165,29 +202,33 @@ def timerCallback(self):
 
         self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link)
         self.publishOdometry(self.t_odom_to_base_link)
-        self.publishLaserScan()
 
     def publishLaserScan(self):
         # Publish a bogus laser scan for collision monitor
-        scan = LaserScan()
-        # scan.header.stamp = (self.get_clock().now()).to_msg()
-        scan.header.frame_id = self.scan_frame_id
-        scan.angle_min = -math.pi
-        scan.angle_max = math.pi
-        scan.angle_increment = 0.005817705996  # 0.333 degrees
-        scan.time_increment = 0.0
-        scan.scan_time = 0.1
-        scan.range_min = 0.1
-        scan.range_max = 100.0
-        num_samples = int((scan.angle_max - scan.angle_min) / scan.angle_increment)
-        scan.ranges = [scan.range_max - 0.01] * num_samples
-        self.scan_pub.publish(scan)
+        self.scan_msg = LaserScan()
+        self.scan_msg.header.stamp = (self.get_clock().now()).to_msg()
+        self.scan_msg.header.frame_id = self.scan_frame_id
+        self.scan_msg.angle_min = -math.pi
+        self.scan_msg.angle_max = math.pi
+        # 1.5 degrees
+        self.scan_msg.angle_increment = 0.0261799
+        self.scan_msg.time_increment = 0.0
+        self.scan_msg.scan_time = 0.1
+        self.scan_msg.range_min = 0.05
+        self.scan_msg.range_max = 30.0
+        num_samples = int(
+            (self.scan_msg.angle_max - self.scan_msg.angle_min) /
+            self.scan_msg.angle_increment)
+        self.scan_msg.ranges = [0.0] * num_samples
+        self.getLaserScan(num_samples)
+        self.scan_pub.publish(self.scan_msg)
 
     def publishTransforms(self, map_to_odom, odom_to_base_link):
         map_to_odom.header.stamp = \
             (self.get_clock().now() + Duration(seconds=self.update_dur)).to_msg()
         odom_to_base_link.header.stamp = self.get_clock().now().to_msg()
-        self.tf_broadcaster.sendTransform(map_to_odom)
+        if self.publish_map_odom_tf:
+            self.tf_broadcaster.sendTransform(map_to_odom)
         self.tf_broadcaster.sendTransform(odom_to_base_link)
 
     def publishOdometry(self, odom_to_base_link):
@@ -209,6 +250,89 @@ def debug(self, msg):
         self.get_logger().debug(msg)
         return
 
+    def getMap(self):
+        request = GetMap.Request()
+        if self.map_client.wait_for_service(timeout_sec=5.0):
+            # Request to get map
+            future = self.map_client.call_async(request)
+            rclpy.spin_until_future_complete(self, future)
+            if future.result() is not None:
+                self.map = future.result().map
+                self.get_logger().info('Laser scan will be populated using map data')
+            else:
+                self.get_logger().warn(
+                    'Failed to get map, '
+                    'Laser scan will be populated using max range'
+                )
+        else:
+            self.get_logger().warn(
+                'Failed to get map, '
+                'Laser scan will be populated using max range'
+            )
+
+    def getLaserPose(self):
+        mat_map_to_odom = transformStampedToMatrix(self.t_map_to_odom)
+        mat_odom_to_base = transformStampedToMatrix(self.t_odom_to_base_link)
+
+        mat_map_to_laser = tf_transformations.concatenate_matrices(
+            mat_map_to_odom,
+            mat_odom_to_base,
+            self.mat_base_to_laser
+        )
+        transform = matrixToTransform(mat_map_to_laser)
+
+        x = transform.translation.x
+        y = transform.translation.y
+        theta = tf_transformations.euler_from_quaternion([
+            transform.rotation.x,
+            transform.rotation.y,
+            transform.rotation.z,
+            transform.rotation.w
+        ])[2]
+
+        return x, y, theta
+
+    def getLaserScan(self, num_samples):
+        if self.map is None or self.initial_pose is None or self.mat_base_to_laser is None:
+            self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples
+            return
+
+        x0, y0, theta = self.getLaserPose()
+
+        mx0, my0 = worldToMap(x0, y0, self.map)
+
+        if not 0 < mx0 < self.map.info.width or not 0 < my0 < self.map.info.height:
+            # outside map
+            self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples
+            return
+
+        for i in range(num_samples):
+            curr_angle = theta + self.scan_msg.angle_min + i * self.scan_msg.angle_increment
+            x1 = x0 + self.scan_msg.range_max * math.cos(curr_angle)
+            y1 = y0 + self.scan_msg.range_max * math.sin(curr_angle)
+
+            mx1, my1 = worldToMap(x1, y1, self.map)
+
+            line_iterator = LineIterator(mx0, my0, mx1, my1, 0.5)
+
+            while line_iterator.isValid():
+                mx, my = int(line_iterator.getX()), int(line_iterator.getY())
+
+                if not 0 < mx < self.map.info.width or not 0 < my < self.map.info.height:
+                    # if outside map then check next ray
+                    break
+
+                point_cost = getMapOccupancy(mx, my, self.map)
+
+                if point_cost >= 60:
+                    self.scan_msg.ranges[i] = math.sqrt(
+                        (int(line_iterator.getX()) - mx0) ** 2 +
+                        (int(line_iterator.getY()) - my0) ** 2
+                    ) * self.map.info.resolution
+                    break
+
+                line_iterator.advance()
+
 
 def main():
     rclpy.init()

nav2_loopback_sim/nav2_loopback_sim/utils.py

@@ -13,6 +13,8 @@
 # limitations under the License.
 
 
+import math
+
 from geometry_msgs.msg import Quaternion, Transform
 import numpy as np
 import tf_transformations
@@ -63,3 +65,13 @@ def matrixToTransform(matrix):
     transform.rotation.z = quaternion[2]
     transform.rotation.w = quaternion[3]
     return transform
+
+
+def worldToMap(world_x, world_y, map_msg):
+    map_x = int(math.floor((world_x - map_msg.info.origin.position.x) / map_msg.info.resolution))
+    map_y = int(math.floor((world_y - map_msg.info.origin.position.y) / map_msg.info.resolution))
+    return map_x, map_y
+
+
+def getMapOccupancy(x, y, map_msg):
+    return map_msg.data[y * map_msg.info.width + x]