diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index dd476aa986..158477a6cc 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -18,6 +18,9 @@ from geometry_msgs.msg import PoseWithCovarianceStamped, Twist from geometry_msgs.msg import Quaternion, TransformStamped, Vector3 from nav_msgs.msg import Odometry +from nav_msgs.srv import GetMap +from tf2_ros import TransformListener, Buffer +import tf_transformations import rclpy from rclpy.duration import Duration from rclpy.node import Node @@ -26,8 +29,14 @@ from tf2_ros import TransformBroadcaster import tf_transformations -from .utils import addYawToQuat, matrixToTransform, transformStampedToMatrix - +from .utils import ( + addYawToQuat, + matrixToTransform, + transformStampedToMatrix, + get_world2map_coordinates, + get_map2world_coordinates, + get_map_occupancy +) """ This is a loopback simulator that replaces a physics simulator to create a @@ -48,6 +57,7 @@ def __init__(self): self.initial_pose = None self.timer = None self.setupTimer = None + self.map = None self.declare_parameter('update_duration', 0.01) self.update_dur = self.get_parameter('update_duration').get_parameter_value().double_value @@ -88,6 +98,14 @@ 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.get_map() + self.info('Loopback simulator initialized') def setupTimerCallback(self): @@ -122,6 +140,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(0.1, self.publishLaserScan) return self.initial_pose = msg.pose.pose @@ -165,23 +184,24 @@ 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 + self.scan_msg.angle_increment = 0.0261799 # 1.5 degrees + 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 = 100.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 + x, y, theta = self.get_laser_pose() + self.get_laserscan(num_samples, x, y, theta) + self.scan_pub.publish(self.scan_msg) def publishTransforms(self, map_to_odom, odom_to_base_link): map_to_odom.header.stamp = \ @@ -208,7 +228,138 @@ def info(self, msg): def debug(self, msg): self.get_logger().debug(msg) return - + + def get_map(self): + request = GetMap.Request() + if self.map_client.wait_for_service(timeout_sec=1.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(f'Received map data, {self.map.info.width}X{self.map.info.height}, resolution:={self.map.info.resolution}') + else: + self.get_logger().error('Failed to call map service') + else: + self.get_logger().error('Map service not available') + + def get_laser_pose(self): + try: + if self.initial_pose is None: + return 0.0, 0.0, 0.0 + + if self.map is None: + self.get_map() + return 0.0, 0.0, 0.0 + + # Wait for transform and lookup + now = rclpy.time.Time() + transform = self.tf_buffer.lookup_transform( + 'map', self.scan_frame_id, now, rclpy.duration.Duration(seconds=1.0)) + + # Extract pose information + x = transform.transform.translation.x + y = transform.transform.translation.y + rotation = transform.transform.rotation + theta = tf_transformations.euler_from_quaternion([ + rotation.x, + rotation.y, + rotation.z, + rotation.w + ])[2] + + return x, y, theta + + except Exception as ex: + self.get_logger().error('Transform lookup failed: %s' % str(ex)) + return 0.0, 0.0, 0.0 + + def get_laserscan(self, num_samples, x, y, theta): + for i in range(int(num_samples)): + if self.map is None: + self.scan_msg.ranges = [self.scan_msg.range_max] * num_samples + break + else: + curr_angle = theta + self.scan_msg.angle_min + i * self.scan_msg.angle_increment + self.scan_msg.ranges[i] = self.find_map_range(x, y, curr_angle) + + def find_map_range(self, x, y, theta): + # Using "A Faster Voxel Traversal Algorithm for Ray Tracing" by Amanatides & Woo + # ======== Initialization Phase ======== + origin = [x, y] # u + dir = [math.cos(theta), math.sin(theta)] # v + + start_x, start_y = get_world2map_coordinates(x, y, self.map) + + if start_x < 0 or start_y < 0 or start_x >= self.map.info.width or start_y >= self.map.info.height: + # Outside the map, find entry point + delta_x = abs(self.map.info.origin.position.x - x) + delta_y = abs(self.map.info.origin.position.y - y) + intersect_x = x + (dir[0] * delta_x) + intersect_y = y + (dir[1] * delta_y) + start_x, start_y = get_world2map_coordinates(intersect_x, intersect_y, self.map) + + current = [start_x, start_y] # X, Y + step = [0, 0] # stepX, stepY + tMax = [0.0, 0.0] # tMaxX, tMaxY + tDelta = [0.0, 0.0] # tDeltaX, tDeltaY + + voxel_border = [0.0, 0.0] + voxel_border[0], voxel_border[1] = get_map2world_coordinates(current[0], current[1], self.map) + voxel_border[0] -= 0.5 * self.map.info.resolution # This is the lower left voxel corner + voxel_border[1] -= 0.5 * self.map.info.resolution # This is the lower left voxel corner + + for i in range(2): # For each dimension (x, y) + # Determine step direction + if dir[i] > 0.0: + step[i] = 1 + elif dir[i] < 0.0: + step[i] = -1 + else: + step[i] = 0 + + # Determine tMax, tDelta + if step[i] != 0: + if step[i] == 1: + voxel_border[i] += step[i] * self.map.info.resolution + + # tMax - voxel boundary crossing + tMax[i] = (voxel_border[i] - origin[i]) / dir[i] + # tDelta - distance along ray so that vertical/horizontal component equals one voxel + tDelta[i] = self.map.info.resolution / abs(dir[i]) + else: + tMax[i] = float('inf') + tDelta[i] = float('inf') + + # ======== Incremental Traversal ======== + while True: + # Advance + dim = 0 if tMax[0] < tMax[1] else 1 + + # Advance one voxel + current[dim] += step[dim] + tMax[dim] += tDelta[dim] + + # Check map inbounds + if current[0] < 0 or current[0] >= self.map.info.width or current[1] < 0 or current[1] >= self.map.info.height: + # self.get_logger().info("Here-1") + return self.scan_msg.range_max + + # Determine current range + current_range = math.sqrt((current[0] - start_x) ** 2 + (current[1] - start_y) ** 2) * self.map.info.resolution + + # Are we at max range? + if current_range > self.scan_msg.range_max: + # self.get_logger().info("Here-1") + return self.scan_msg.range_max + else: + occ = get_map_occupancy(current[0], current[1], self.map) + if occ >= 60: # Current cell is occupied + # Is range less than min range + if current_range < self.scan_msg.range_min: + continue + + return current_range def main(): rclpy.init() diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py index 0ed85689dd..f0e54982c5 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -15,6 +15,7 @@ from geometry_msgs.msg import Quaternion, Transform import numpy as np +import math import tf_transformations @@ -63,3 +64,16 @@ def matrixToTransform(matrix): transform.rotation.z = quaternion[2] transform.rotation.w = quaternion[3] return transform + +def get_world2map_coordinates(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 get_map2world_coordinates(map_x, map_y, map_msg): + world_x = (map_x * map_msg.info.resolution) + map_msg.info.origin.position.x + 0.5 * map_msg.info.resolution + world_y = (map_y * map_msg.info.resolution) + map_msg.info.origin.position.y + 0.5 * map_msg.info.resolution + return world_x, world_y + +def get_map_occupancy(x, y, map_msg): + return map_msg.data[y * map_msg.info.width + x] \ No newline at end of file