From 9872f1aa78e17d59cf68fa336a072fef20b772ac Mon Sep 17 00:00:00 2001 From: Jatin Patil <89979346+JatinPatil2003@users.noreply.github.com> Date: Wed, 21 Aug 2024 00:17:51 +0530 Subject: [PATCH] Added laser data from map in nav2_loopback_sim (#4617) * Added laser data from map Signed-off-by: Jatin Patil * Fixed Linting Signed-off-by: Jatin Patil * Fixed Linting Signed-off-by: Jatin Patil * Fixed few requested changes Signed-off-by: Jatin Patil * Fixed linting Signed-off-by: Jatin Patil * Requested changes Signed-off-by: Jatin Patil * Linting Signed-off-by: Jatin Patil * Added parameters and fixed requested changes Signed-off-by: Jatin Patil * linting Signed-off-by: Jatin Patil * Added scan using LineIterator Signed-off-by: Jatin Patil * LineIterator max_distance or range_max Signed-off-by: Jatin Patil * min of max_distance or range_max Signed-off-by: Jatin Patil * final updates working correctly Signed-off-by: Jatin Patil * Fix lint Signed-off-by: Jatin Patil * requested changes Signed-off-by: Jatin Patil * Update nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py Signed-off-by: Steve Macenski * Update nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py Signed-off-by: Steve Macenski --------- Signed-off-by: Jatin Patil Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski --- .../nav2_loopback_sim/loopback_simulator.py | 162 ++++++++++++++++-- nav2_loopback_sim/nav2_loopback_sim/utils.py | 12 ++ 2 files changed, 155 insertions(+), 19 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index dd476aa986..62e27e9743 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/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() diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py index 0ed85689dd..468103e94e 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/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]