From 70a7dba5a84e8efac64dce0f14b693d7b478882a Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Tue, 13 Aug 2024 20:59:02 +0530 Subject: [PATCH 01/17] Added laser data from map Signed-off-by: Jatin Patil --- .../nav2_loopback_sim/loopback_simulator.py | 185 ++++++++++++++++-- nav2_loopback_sim/nav2_loopback_sim/utils.py | 14 ++ 2 files changed, 182 insertions(+), 17 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index dd476aa9869..158477a6ccb 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 0ed85689ddf..f0e54982c51 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 From e0313b71dfa32cf52b35da4103720bff5cafcf61 Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Tue, 13 Aug 2024 21:38:11 +0530 Subject: [PATCH 02/17] Fixed Linting Signed-off-by: Jatin Patil --- .../nav2_loopback_sim/loopback_simulator.py | 91 ++++++++++--------- nav2_loopback_sim/nav2_loopback_sim/utils.py | 14 ++- 2 files changed, 60 insertions(+), 45 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 158477a6ccb..f15b88115f9 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -19,25 +19,23 @@ 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 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, - get_world2map_coordinates, - get_map2world_coordinates, - get_map_occupancy + addYawToQuat, + get_map2world_coordinates, + get_map_occupancy, + get_world2map_coordinates, + matrixToTransform, + transformStampedToMatrix, + ) - """ This is a loopback simulator that replaces a physics simulator to create a frictionless, inertialess, and collisionless simulation environment. It @@ -98,7 +96,7 @@ 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() @@ -192,12 +190,15 @@ def publishLaserScan(self): 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 + # 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 = 100.0 - num_samples = int((self.scan_msg.angle_max - self.scan_msg.angle_min) / self.scan_msg.angle_increment) + 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) @@ -228,7 +229,7 @@ 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): @@ -237,7 +238,8 @@ def get_map(self): 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}') + self.get_logger().info( + f'Received map data, {self.map.info.width}X{self.map.info.height}') else: self.get_logger().error('Failed to call map service') else: @@ -247,11 +249,11 @@ 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( @@ -287,67 +289,73 @@ 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 - + direction = [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: + + 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) + intersect_x = x + (direction[0] * delta_x) + intersect_y = y + (direction[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], 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: + if direction[i] > 0.0: step[i] = 1 - elif dir[i] < 0.0: + elif direction[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]) + tMax[i] = (voxel_border[i] - origin[i]) / direction[i] + # tDelta - distance along ray + # so that vertical/horizontal component equals one voxel + tDelta[i] = self.map.info.resolution / abs(direction[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: + 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 - + 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") @@ -361,6 +369,7 @@ def find_map_range(self, x, y, theta): return current_range + def main(): rclpy.init() loopback_simulator = LoopbackSimulator() diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py index f0e54982c51..ec44509bb83 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -13,9 +13,10 @@ # limitations under the License. +import math + from geometry_msgs.msg import Quaternion, Transform import numpy as np -import math import tf_transformations @@ -65,15 +66,20 @@ def matrixToTransform(matrix): 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 + 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 + return map_msg.data[y * map_msg.info.width + x] From 7db9b1f1b4d567606839d3622681ed728ad2e910 Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Tue, 13 Aug 2024 21:45:51 +0530 Subject: [PATCH 03/17] Fixed Linting Signed-off-by: Jatin Patil --- .../nav2_loopback_sim/loopback_simulator.py | 16 +++++++--------- nav2_loopback_sim/nav2_loopback_sim/utils.py | 4 ++-- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index f15b88115f9..cf34b3f003c 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -34,8 +34,8 @@ get_world2map_coordinates, matrixToTransform, transformStampedToMatrix, - ) + """ This is a loopback simulator that replaces a physics simulator to create a frictionless, inertialess, and collisionless simulation environment. It @@ -197,7 +197,7 @@ def publishLaserScan(self): 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_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() @@ -293,8 +293,8 @@ def find_map_range(self, x, y, theta): 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): + 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) @@ -329,7 +329,7 @@ def find_map_range(self, x, y, theta): # tMax - voxel boundary crossing tMax[i] = (voxel_border[i] - origin[i]) / direction[i] - # tDelta - distance along ray + # tDelta - distance along ray # so that vertical/horizontal component equals one voxel tDelta[i] = self.map.info.resolution / abs(direction[i]) else: @@ -346,9 +346,8 @@ def find_map_range(self, x, y, theta): 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") + if (current[0] < 0 or current[0] >= self.map.info.width or + current[1] < 0 or current[1] >= self.map.info.height): return self.scan_msg.range_max # Determine current range @@ -358,7 +357,6 @@ def find_map_range(self, x, y, theta): # 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) diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py index ec44509bb83..e2a513d1663 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -74,9 +74,9 @@ def get_world2map_coordinates(world_x, world_y, map_msg): def get_map2world_coordinates(map_x, map_y, map_msg): - world_x = ((map_x * map_msg.info.resolution) + map_msg.info.origin.position.x + + 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 + + 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 From ea2857d42a887e4a0e6ba670d0162a1d48fabad5 Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Wed, 14 Aug 2024 20:56:40 +0530 Subject: [PATCH 04/17] Fixed few requested changes Signed-off-by: Jatin Patil --- .../nav2_loopback_sim/loopback_simulator.py | 84 ++++++++----------- nav2_loopback_sim/nav2_loopback_sim/utils.py | 19 +++-- 2 files changed, 49 insertions(+), 54 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index cf34b3f003c..42eb2132924 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -29,11 +29,11 @@ from .utils import ( addYawToQuat, - get_map2world_coordinates, - get_map_occupancy, - get_world2map_coordinates, + getMapOccupancy, + mapToWorld, matrixToTransform, transformStampedToMatrix, + worldToMap, ) """ @@ -102,10 +102,10 @@ def __init__(self): self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) - self.get_map() - self.info('Loopback simulator initialized') + self.getMap() + def setupTimerCallback(self): # Publish initial identity odom transform & laser scan to warm up system self.tf_broadcaster.sendTransform(self.t_odom_to_base_link) @@ -200,8 +200,8 @@ def publishLaserScan(self): (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) + x, y, theta = self.getLaserPose() + self.getLaserScan(num_samples, x, y, theta) self.scan_pub.publish(self.scan_msg) def publishTransforms(self, map_to_odom, odom_to_base_link): @@ -230,34 +230,31 @@ def debug(self, msg): self.get_logger().debug(msg) return - def get_map(self): + def getMap(self): request = GetMap.Request() - if self.map_client.wait_for_service(timeout_sec=1.0): + 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( - f'Received map data, {self.map.info.width}X{self.map.info.height}') + self.get_logger().info(f'Laser scan will be populated') else: - self.get_logger().error('Failed to call map service') + self.get_logger().warn('Failed to get map') + self.get_logger().info(f'Laser scan will be populated using max range') else: - self.get_logger().error('Map service not available') + self.get_logger().warn('Failed to get map') + self.get_logger().info(f'Laser scan will be populated using max range') - def get_laser_pose(self): + def getLaserPose(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)) + 'map', self.scan_frame_id, now, rclpy.duration.Duration(seconds=0.1)) # Extract pose information x = transform.transform.translation.x @@ -276,42 +273,35 @@ def get_laser_pose(self): 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): + def getLaserScan(self, num_samples, x, y, theta): + if self.map is None or self.initial_pose is None: + self.scan_msg.ranges = [self.scan_msg.range_max - 0.01] * num_samples + return + + mx, my = worldToMap(x, y, self.map) + + if not mx and not my: + self.scan_msg.ranges = [self.scan_msg.range_max - 0.01] * num_samples + return + 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) + curr_angle = theta + self.scan_msg.angle_min + i * self.scan_msg.angle_increment + self.scan_msg.ranges[i] = self.findMapRange(mx, my, x, y, curr_angle) - def find_map_range(self, x, y, theta): + def findMapRange(self, mx, my, x, y, theta): # Using "A Faster Voxel Traversal Algorithm for Ray Tracing" by Amanatides & Woo # ======== Initialization Phase ======== origin = [x, y] # u direction = [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 + (direction[0] * delta_x) - intersect_y = y + (direction[1] * delta_y) - start_x, start_y = get_world2map_coordinates(intersect_x, intersect_y, self.map) - - current = [start_x, start_y] # X, Y + current = [mx, my] # 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( + voxel_border[0], voxel_border[1] = mapToWorld( 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 @@ -348,22 +338,22 @@ def find_map_range(self, x, y, theta): # 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): - return self.scan_msg.range_max + return self.scan_msg.range_max - 0.01 # Determine current range current_range = math.sqrt( - (current[0] - start_x) ** 2 + (current[1] - start_y) ** 2 + (current[0] - mx) ** 2 + (current[1] - my) ** 2 ) * self.map.info.resolution # Are we at max range? if current_range > self.scan_msg.range_max: - return self.scan_msg.range_max + return self.scan_msg.range_max - 0.01 else: - occ = get_map_occupancy(current[0], current[1], self.map) + occ = getMapOccupancy(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 0.0 return current_range diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py index e2a513d1663..fc08f4fbf35 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -67,19 +67,24 @@ def matrixToTransform(matrix): return transform -def get_world2map_coordinates(world_x, world_y, map_msg): +def worldToMap(world_x, world_y, map_msg): + # Check if world coordinates are out of bounds + if (world_x < map_msg.info.origin.position.x or world_y < map_msg.info.origin.position.y): + return None, None # Coordinates are out of bounds + 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)) + + if not map_x < map_msg.info.width or not map_y < map_msg.info.height: + return None, None # Coordinates are out of bounds 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) +def mapToWorld(map_x, map_y, map_msg): + world_x = map_msg.info.origin.position.x + ((map_x + 0.5) * map_msg.info.resolution) + world_y = map_msg.info.origin.position.y + ((map_y + 0.5) * map_msg.info.resolution) return world_x, world_y -def get_map_occupancy(x, y, map_msg): +def getMapOccupancy(x, y, map_msg): return map_msg.data[y * map_msg.info.width + x] From 98f1a869c7ff139a56a7d3f43c0f64669cf07d9c Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Wed, 14 Aug 2024 20:59:41 +0530 Subject: [PATCH 05/17] Fixed linting Signed-off-by: Jatin Patil --- .../nav2_loopback_sim/loopback_simulator.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 42eb2132924..2f138c310bc 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -238,13 +238,13 @@ def getMap(self): rclpy.spin_until_future_complete(self, future) if future.result() is not None: self.map = future.result().map - self.get_logger().info(f'Laser scan will be populated') + self.get_logger().info('Laser scan will be populated') else: self.get_logger().warn('Failed to get map') - self.get_logger().info(f'Laser scan will be populated using max range') + self.get_logger().info('Laser scan will be populated using max range') else: self.get_logger().warn('Failed to get map') - self.get_logger().info(f'Laser scan will be populated using max range') + self.get_logger().info('Laser scan will be populated using max range') def getLaserPose(self): try: @@ -277,13 +277,13 @@ def getLaserScan(self, num_samples, x, y, theta): if self.map is None or self.initial_pose is None: self.scan_msg.ranges = [self.scan_msg.range_max - 0.01] * num_samples return - + mx, my = worldToMap(x, y, self.map) if not mx and not my: self.scan_msg.ranges = [self.scan_msg.range_max - 0.01] * num_samples return - + for i in range(int(num_samples)): curr_angle = theta + self.scan_msg.angle_min + i * self.scan_msg.angle_increment self.scan_msg.ranges[i] = self.findMapRange(mx, my, x, y, curr_angle) From 149faf7519be9d425060b2ca85b04b25f99db8d8 Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Wed, 14 Aug 2024 23:15:42 +0530 Subject: [PATCH 06/17] Requested changes Signed-off-by: Jatin Patil --- .../nav2_loopback_sim/loopback_simulator.py | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 2f138c310bc..23727a9adcb 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -102,10 +102,10 @@ def __init__(self): self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) - self.info('Loopback simulator initialized') - self.getMap() + self.info('Loopback simulator initialized') + def setupTimerCallback(self): # Publish initial identity odom transform & laser scan to warm up system self.tf_broadcaster.sendTransform(self.t_odom_to_base_link) @@ -186,7 +186,7 @@ def timerCallback(self): def publishLaserScan(self): # Publish a bogus laser scan for collision monitor self.scan_msg = LaserScan() - self.scan_msg.header.stamp = (self.get_clock().now()).to_msg() + # 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 @@ -238,19 +238,19 @@ def getMap(self): 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') + self.get_logger().info('Laser scan will be populated using map data') else: - self.get_logger().warn('Failed to get map') - self.get_logger().info('Laser scan will be populated using max range') + 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') - self.get_logger().info('Laser scan will be populated using max range') + self.get_logger().warn('Failed to get map, ' + 'Laser scan will be populated using max range') def getLaserPose(self): - try: - if self.initial_pose is None: - return 0.0, 0.0, 0.0 + if self.initial_pose is None: + return 0.0, 0.0, 0.0 + try: # Wait for transform and lookup now = rclpy.time.Time() transform = self.tf_buffer.lookup_transform( @@ -275,13 +275,13 @@ def getLaserPose(self): def getLaserScan(self, num_samples, x, y, theta): if self.map is None or self.initial_pose is None: - self.scan_msg.ranges = [self.scan_msg.range_max - 0.01] * num_samples + self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples return mx, my = worldToMap(x, y, self.map) if not mx and not my: - self.scan_msg.ranges = [self.scan_msg.range_max - 0.01] * num_samples + self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples return for i in range(int(num_samples)): @@ -338,7 +338,7 @@ def findMapRange(self, mx, my, x, y, theta): # 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): - return self.scan_msg.range_max - 0.01 + return self.scan_msg.range_max - 0.1 # Determine current range current_range = math.sqrt( @@ -347,7 +347,7 @@ def findMapRange(self, mx, my, x, y, theta): # Are we at max range? if current_range > self.scan_msg.range_max: - return self.scan_msg.range_max - 0.01 + return self.scan_msg.range_max - 0.1 else: occ = getMapOccupancy(current[0], current[1], self.map) if occ >= 60: # Current cell is occupied From c4010d7b4f6eb93dfcec27d8c7cc61263b9a6d18 Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Wed, 14 Aug 2024 23:20:56 +0530 Subject: [PATCH 07/17] Linting Signed-off-by: Jatin Patil --- .../nav2_loopback_sim/loopback_simulator.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 23727a9adcb..65e2c840c83 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -240,11 +240,15 @@ def getMap(self): 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') + 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') + self.get_logger().warn( + 'Failed to get map, ' + 'Laser scan will be populated using max range' + ) def getLaserPose(self): if self.initial_pose is None: From 0006349a280475a8a898b22b38c4b725191f0312 Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Fri, 16 Aug 2024 17:30:20 +0530 Subject: [PATCH 08/17] Added parameters and fixed requested changes Signed-off-by: Jatin Patil --- .../nav2_loopback_sim/loopback_simulator.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 65e2c840c83..27ffaa731be 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -72,6 +72,12 @@ 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('publish_map_odom_tf', False) + self.publish_map_odom_tf = self.get_parameter('publish_map_odom_tf').get_parameter_value().bool_value + + self.declare_parameter('scan_publish_rate', 10.0) + self.scan_publish_rate = self.get_parameter('scan_publish_rate').get_parameter_value().double_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 @@ -138,7 +144,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) + self.timer_laser = self.create_timer((1/self.scan_publish_rate), self.publishLaserScan) return self.initial_pose = msg.pose.pose @@ -186,7 +192,7 @@ def timerCallback(self): def publishLaserScan(self): # Publish a bogus laser scan for collision monitor self.scan_msg = LaserScan() - # self.scan_msg.header.stamp = (self.get_clock().now()).to_msg() + 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 @@ -208,7 +214,8 @@ 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): @@ -251,9 +258,6 @@ def getMap(self): ) def getLaserPose(self): - if self.initial_pose is None: - return 0.0, 0.0, 0.0 - try: # Wait for transform and lookup now = rclpy.time.Time() From b9fa086f1edac3e9195ed9cde5c04782572f4d0d Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Fri, 16 Aug 2024 17:33:42 +0530 Subject: [PATCH 09/17] linting Signed-off-by: Jatin Patil --- nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 27ffaa731be..94521c2fdd2 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -73,10 +73,12 @@ def __init__(self): self.scan_frame_id = self.get_parameter('scan_frame_id').get_parameter_value().string_value self.declare_parameter('publish_map_odom_tf', False) - self.publish_map_odom_tf = self.get_parameter('publish_map_odom_tf').get_parameter_value().bool_value + self.publish_map_odom_tf = self.get_parameter( + 'publish_map_odom_tf').get_parameter_value().bool_value self.declare_parameter('scan_publish_rate', 10.0) - self.scan_publish_rate = self.get_parameter('scan_publish_rate').get_parameter_value().double_value + self.scan_publish_rate = self.get_parameter( + 'scan_publish_rate').get_parameter_value().double_value self.t_map_to_odom = TransformStamped() self.t_map_to_odom.header.frame_id = self.map_frame_id From 79cac35bf374b616089c7c59a040234ec56222fa Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Sat, 17 Aug 2024 05:06:12 +0530 Subject: [PATCH 10/17] Added scan using LineIterator Signed-off-by: Jatin Patil --- .../nav2_loopback_sim/loopback_simulator.py | 114 +++++++++++++----- 1 file changed, 81 insertions(+), 33 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 94521c2fdd2..54e83cbb6e6 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -27,6 +27,8 @@ from tf2_ros import Buffer, TransformBroadcaster, TransformListener import tf_transformations +# from nav2_simple_commander.line_iterator import LineIterator + from .utils import ( addYawToQuat, getMapOccupancy, @@ -36,6 +38,8 @@ worldToMap, ) +LETHAL_OBSTACLE = 100 + """ This is a loopback simulator that replaces a physics simulator to create a frictionless, inertialess, and collisionless simulation environment. It @@ -56,6 +60,7 @@ def __init__(self): 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 @@ -72,13 +77,13 @@ 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('publish_map_odom_tf', False) + 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.declare_parameter('scan_publish_rate', 10.0) - self.scan_publish_rate = self.get_parameter( - 'scan_publish_rate').get_parameter_value().double_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.t_map_to_odom = TransformStamped() self.t_map_to_odom.header.frame_id = self.map_frame_id @@ -114,10 +119,22 @@ def __init__(self): 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.mat_odom_to_base = transformStampedToMatrix(self.t_odom_to_base_link) + self.getBaseToLaserTf() def cmdVelCallback(self, msg): self.debug('Received cmd_vel') @@ -139,6 +156,7 @@ def initialPoseCallback(self, msg): self.t_odom_to_base_link.transform.translation = Vector3() self.t_odom_to_base_link.transform.rotation = Quaternion() self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link) + self.mat_map_to_odom = transformStampedToMatrix(self.t_map_to_odom) # Start republication timer and velocity processing if self.setupTimer is not None: @@ -146,7 +164,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((1/self.scan_publish_rate), self.publishLaserScan) + self.timer_laser = self.create_timer(self.scan_publish_dur, self.publishLaserScan) return self.initial_pose = msg.pose.pose @@ -190,6 +208,8 @@ 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.mat_map_to_odom = transformStampedToMatrix(self.t_map_to_odom) + self.mat_odom_to_base = transformStampedToMatrix(self.t_odom_to_base_link) def publishLaserScan(self): # Publish a bogus laser scan for collision monitor @@ -208,8 +228,7 @@ def publishLaserScan(self): (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.getLaserPose() - self.getLaserScan(num_samples, x, y, theta) + self.getLaserScan(num_samples) self.scan_pub.publish(self.scan_msg) def publishTransforms(self, map_to_odom, odom_to_base_link): @@ -260,34 +279,31 @@ def getMap(self): ) def getLaserPose(self): - try: - # 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=0.1)) - - # 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 getLaserScan(self, num_samples, x, y, theta): - if self.map is None or self.initial_pose is None: + mat_map_to_laser = tf_transformations.concatenate_matrices( + self.mat_map_to_odom, + self.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 + x, y, theta = self.getLaserPose() + mx, my = worldToMap(x, y, self.map) if not mx and not my: @@ -298,6 +314,38 @@ def getLaserScan(self, num_samples, x, y, theta): curr_angle = theta + self.scan_msg.angle_min + i * self.scan_msg.angle_increment self.scan_msg.ranges[i] = self.findMapRange(mx, my, x, y, curr_angle) + # 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() + + # max_distance = math.sqrt( + # (self.map.info.width * self.map.info.resolution) ** 2 + + # (self.map.info.height * self.map.info.resolution) ** 2) + # for i in range(num_samples): + # curr_angle = theta + self.scan_msg.angle_min + i * self.scan_msg.angle_increment + # x1 = float(x0) + max_distance * math.cos(curr_angle) + # y1 = float(y0) + max_distance * math.sin(curr_angle) + + # line_iterator = LineIterator(float(x0), float(y0), x1, y1, self.map.info.resolution) + + # while line_iterator.isValid(): + # mx, my = worldToMap(line_iterator.getX(), line_iterator.getY(), self.map) + + # if not mx and not my: + # break + + # point_cost = getMapOccupancy(mx, my, self.map) + + # if point_cost >= 60: + # self.scan_msg.ranges[i] = math.sqrt( + # (line_iterator.getX() - x0) ** 2 + (line_iterator.getY() - y0) ** 2) + # break + + # line_iterator.advance() + def findMapRange(self, mx, my, x, y, theta): # Using "A Faster Voxel Traversal Algorithm for Ray Tracing" by Amanatides & Woo # ======== Initialization Phase ======== From d75e3cd8e1eae148873466f89ecb1360cf31ab45 Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Sat, 17 Aug 2024 05:09:25 +0530 Subject: [PATCH 11/17] LineIterator max_distance or range_max Signed-off-by: Jatin Patil --- nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 54e83cbb6e6..c2c4290b685 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -324,6 +324,7 @@ def getLaserScan(self, num_samples): # max_distance = math.sqrt( # (self.map.info.width * self.map.info.resolution) ** 2 + # (self.map.info.height * self.map.info.resolution) ** 2) + # max_distance = max(max_distance, self.scan_msg.range_max) # for i in range(num_samples): # curr_angle = theta + self.scan_msg.angle_min + i * self.scan_msg.angle_increment # x1 = float(x0) + max_distance * math.cos(curr_angle) From 9a4cb0ee59b9077f4631e3172316ec73c70dc304 Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Sat, 17 Aug 2024 16:07:09 +0530 Subject: [PATCH 12/17] min of max_distance or range_max Signed-off-by: Jatin Patil --- nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index c2c4290b685..d1e39822978 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -324,7 +324,7 @@ def getLaserScan(self, num_samples): # max_distance = math.sqrt( # (self.map.info.width * self.map.info.resolution) ** 2 + # (self.map.info.height * self.map.info.resolution) ** 2) - # max_distance = max(max_distance, self.scan_msg.range_max) + # max_distance = min(max_distance, self.scan_msg.range_max) # for i in range(num_samples): # curr_angle = theta + self.scan_msg.angle_min + i * self.scan_msg.angle_increment # x1 = float(x0) + max_distance * math.cos(curr_angle) From 689ee5a261ca3b4f2f83b8e171002bb68c96e101 Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Tue, 20 Aug 2024 15:23:05 +0530 Subject: [PATCH 13/17] final updates working correctly Signed-off-by: Jatin Patil --- .../nav2_loopback_sim/loopback_simulator.py | 158 ++++-------------- nav2_loopback_sim/nav2_loopback_sim/utils.py | 13 -- 2 files changed, 35 insertions(+), 136 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index d1e39822978..95f87eeb838 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -19,6 +19,7 @@ from geometry_msgs.msg import Quaternion, TransformStamped, Vector3 from nav_msgs.msg import Odometry from nav_msgs.srv import GetMap +from nav2_simple_commander.line_iterator import LineIterator import rclpy from rclpy.duration import Duration from rclpy.node import Node @@ -27,19 +28,14 @@ from tf2_ros import Buffer, TransformBroadcaster, TransformListener import tf_transformations -# from nav2_simple_commander.line_iterator import LineIterator - from .utils import ( addYawToQuat, getMapOccupancy, - mapToWorld, matrixToTransform, transformStampedToMatrix, worldToMap, ) -LETHAL_OBSTACLE = 100 - """ This is a loopback simulator that replaces a physics simulator to create a frictionless, inertialess, and collisionless simulation environment. It @@ -77,10 +73,6 @@ 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('publish_map_odom_tf', True) - self.publish_map_odom_tf = self.get_parameter( - 'publish_map_odom_tf').get_parameter_value().bool_value - self.declare_parameter('scan_publish_dur', 0.1) self.scan_publish_dur = self.get_parameter( 'scan_publish_dur').get_parameter_value().double_value @@ -133,7 +125,6 @@ def setupTimerCallback(self): # Publish initial identity odom transform & laser scan to warm up system self.tf_broadcaster.sendTransform(self.t_odom_to_base_link) if self.mat_base_to_laser is None: - self.mat_odom_to_base = transformStampedToMatrix(self.t_odom_to_base_link) self.getBaseToLaserTf() def cmdVelCallback(self, msg): @@ -156,7 +147,6 @@ def initialPoseCallback(self, msg): self.t_odom_to_base_link.transform.translation = Vector3() self.t_odom_to_base_link.transform.rotation = Quaternion() self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link) - self.mat_map_to_odom = transformStampedToMatrix(self.t_map_to_odom) # Start republication timer and velocity processing if self.setupTimer is not None: @@ -208,8 +198,6 @@ 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.mat_map_to_odom = transformStampedToMatrix(self.t_map_to_odom) - self.mat_odom_to_base = transformStampedToMatrix(self.t_odom_to_base_link) def publishLaserScan(self): # Publish a bogus laser scan for collision monitor @@ -235,8 +223,7 @@ 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() - if self.publish_map_odom_tf: - self.tf_broadcaster.sendTransform(map_to_odom) + self.tf_broadcaster.sendTransform(map_to_odom) self.tf_broadcaster.sendTransform(odom_to_base_link) def publishOdometry(self, odom_to_base_link): @@ -279,9 +266,12 @@ def getMap(self): ) 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( - self.mat_map_to_odom, - self.mat_odom_to_base, + mat_map_to_odom, + mat_odom_to_base, self.mat_base_to_laser ) transform = matrixToTransform(mat_map_to_laser) @@ -302,119 +292,41 @@ def getLaserScan(self, num_samples): self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples return - x, y, theta = self.getLaserPose() + x0, y0, theta = self.getLaserPose() - mx, my = worldToMap(x, y, self.map) + mx0, my0 = worldToMap(x0, y0, self.map) - if not mx and not my: + 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(int(num_samples)): + for i in range(num_samples): curr_angle = theta + self.scan_msg.angle_min + i * self.scan_msg.angle_increment - self.scan_msg.ranges[i] = self.findMapRange(mx, my, x, y, curr_angle) - - # 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() - - # max_distance = math.sqrt( - # (self.map.info.width * self.map.info.resolution) ** 2 + - # (self.map.info.height * self.map.info.resolution) ** 2) - # max_distance = min(max_distance, self.scan_msg.range_max) - # for i in range(num_samples): - # curr_angle = theta + self.scan_msg.angle_min + i * self.scan_msg.angle_increment - # x1 = float(x0) + max_distance * math.cos(curr_angle) - # y1 = float(y0) + max_distance * math.sin(curr_angle) - - # line_iterator = LineIterator(float(x0), float(y0), x1, y1, self.map.info.resolution) - - # while line_iterator.isValid(): - # mx, my = worldToMap(line_iterator.getX(), line_iterator.getY(), self.map) - - # if not mx and not my: - # break - - # point_cost = getMapOccupancy(mx, my, self.map) - - # if point_cost >= 60: - # self.scan_msg.ranges[i] = math.sqrt( - # (line_iterator.getX() - x0) ** 2 + (line_iterator.getY() - y0) ** 2) - # break - - # line_iterator.advance() - - def findMapRange(self, mx, my, x, y, theta): - # Using "A Faster Voxel Traversal Algorithm for Ray Tracing" by Amanatides & Woo - # ======== Initialization Phase ======== - origin = [x, y] # u - direction = [math.cos(theta), math.sin(theta)] # v - - current = [mx, my] # 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] = mapToWorld( - current[0], current[1], self.map) - - for i in range(2): # For each dimension (x, y) - # Determine step direction - if direction[i] > 0.0: - step[i] = 1 - elif direction[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]) / direction[i] - # tDelta - distance along ray - # so that vertical/horizontal component equals one voxel - tDelta[i] = self.map.info.resolution / abs(direction[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): - return self.scan_msg.range_max - 0.1 - - # Determine current range - current_range = math.sqrt( - (current[0] - mx) ** 2 + (current[1] - my) ** 2 - ) * self.map.info.resolution - - # Are we at max range? - if current_range > self.scan_msg.range_max: - return self.scan_msg.range_max - 0.1 - else: - occ = getMapOccupancy(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: - return 0.0 + 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 - return current_range + line_iterator.advance() def main(): diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py index fc08f4fbf35..468103e94e8 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -68,23 +68,10 @@ def matrixToTransform(matrix): def worldToMap(world_x, world_y, map_msg): - # Check if world coordinates are out of bounds - if (world_x < map_msg.info.origin.position.x or world_y < map_msg.info.origin.position.y): - return None, None # Coordinates are out of bounds - 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)) - - if not map_x < map_msg.info.width or not map_y < map_msg.info.height: - return None, None # Coordinates are out of bounds return map_x, map_y -def mapToWorld(map_x, map_y, map_msg): - world_x = map_msg.info.origin.position.x + ((map_x + 0.5) * map_msg.info.resolution) - world_y = map_msg.info.origin.position.y + ((map_y + 0.5) * map_msg.info.resolution) - return world_x, world_y - - def getMapOccupancy(x, y, map_msg): return map_msg.data[y * map_msg.info.width + x] From ee5db1d9a5eeb117fca54c3aed898dabbded2324 Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Tue, 20 Aug 2024 15:28:22 +0530 Subject: [PATCH 14/17] Fix lint Signed-off-by: Jatin Patil --- nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 95f87eeb838..6ccab571c50 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -17,9 +17,9 @@ 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 -from nav2_simple_commander.line_iterator import LineIterator import rclpy from rclpy.duration import Duration from rclpy.node import Node From d705fed04fa862ddc4a496fdccc04ef43124042d Mon Sep 17 00:00:00 2001 From: Jatin Patil Date: Tue, 20 Aug 2024 21:40:20 +0530 Subject: [PATCH 15/17] requested changes Signed-off-by: Jatin Patil --- .../nav2_loopback_sim/loopback_simulator.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 6ccab571c50..d43e0df677e 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -77,6 +77,10 @@ def __init__(self): 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 @@ -202,7 +206,7 @@ def timerCallback(self): def publishLaserScan(self): # Publish a bogus laser scan for collision monitor self.scan_msg = LaserScan() - self.scan_msg.header.stamp = (self.get_clock().now()).to_msg() + # 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 @@ -223,7 +227,8 @@ 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): From 46bc106d7389f9199b3d3c791fc375fc3c02ae34 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 20 Aug 2024 11:47:29 -0700 Subject: [PATCH 16/17] Update nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py Signed-off-by: Steve Macenski --- nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index d43e0df677e..29e8e0c716f 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -206,7 +206,7 @@ def timerCallback(self): def publishLaserScan(self): # Publish a bogus laser scan for collision monitor self.scan_msg = LaserScan() - # self.scan_msg.header.stamp = (self.get_clock().now()).to_msg() + 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 From e1e43e1d643d5f69d70be9795ca5658a5879d943 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 20 Aug 2024 11:47:36 -0700 Subject: [PATCH 17/17] Update nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py Signed-off-by: Steve Macenski --- nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 29e8e0c716f..62e27e97433 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -215,7 +215,7 @@ def publishLaserScan(self): 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 + 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)