1
$\begingroup$

The earlier issue is fixed but now the map is updating rightly in the rviz. The scanned data is moving along the robot as it moves. This is my main function.

import math
import rclpy
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import OccupancyGrid, Odometry
from geometry_msgs.msg import TransformStamped
import tf2_ros
from scipy.spatial.transform import Rotation as R
from tf2_ros import LookupException, ConnectivityException, ExtrapolationException

def bresenhams_line(x0, y0, x1, y1):
    points = []
    dx = abs(x1 - x0)
    dy = -abs(y1 - y0)
    sx = 1 if x0 < x1 else -1
    sy = 1 if y0 < y1 else -1
    err = dx + dy
    while True:
        points.append((x0, y0))
        if x0 == x1 and y0 == y1:
            break
        e2 = 2 * err
        if e2 >= dy:
            err += dy
            x0 += sx
        if e2 <= dx:
            err += dx
            y0 += sy
    return points

class LocalMap:

    def __init__(self, width, height, resolution, origin):
        self.width = width
        self.height = height
        self.resolution = resolution
        self.origin = origin
        self.unknown = -1
        self.free = 0
        self.occupied = 100
        self.map = [self.unknown] * (width * height)
        self.log_odds = [0.0] * (width * height)
        self.p_free = math.log(0.3 / 0.7)
        self.p_occ = math.log(0.9 / 0.1)
        self.max_log_odds = 100.0
        self.min_log_odds = -100.0

    def adjust_grid_size(self, robot_x, robot_y):
        margin = 1  # meters
        margin_cells = int(margin / self.resolution)

        origin_x = int((robot_x - self.origin[0]) / self.resolution)
        origin_y = int((robot_y - self.origin[1]) / self.resolution)

        if (origin_x < margin_cells or origin_x > self.width - margin_cells or
            origin_y < margin_cells or origin_y > self.height - margin_cells):
            new_width = self.width + 2 * margin_cells
            new_height = self.height + 2 * margin_cells

            new_map = [self.unknown] * (new_width * new_height)
            new_log_odds = [0.0] * (new_width * new_height)

            for y in range(self.height):
                for x in range(self.width):
                    new_x = x + margin_cells
                    new_y = y + margin_cells
                    new_index = new_x + new_y * new_width
                    old_index = x + y * self.width
                    if 0 <= new_index < len(new_map):
                        new_map[new_index] = self.map[old_index]
                        new_log_odds[new_index] = self.log_odds[old_index]

            self.width = new_width
            self.height = new_height
            self.origin[0] -= margin
            self.origin[1] -= margin
            self.map = new_map
            self.log_odds = new_log_odds

    def update(self, scan_data, angle_min, angle_max, angle_increment, range_min, range_max, pose):
        robot_x, robot_y, robot_yaw = pose
        self.adjust_grid_size(robot_x, robot_y)
        robot_x_cell = int((robot_x - self.origin[0]) / self.resolution)
        robot_y_cell = int((robot_y - self.origin[1]) / self.resolution)

        for i, distance in enumerate(scan_data):
            if range_min <= distance <= range_max:
                angle = angle_min + i * angle_increment + robot_yaw
                end_x = robot_x + distance * math.cos(angle)
                end_y = robot_y + distance * math.sin(angle)
                end_x_cell = int((end_x - self.origin[0]) / self.resolution)
                end_y_cell = int((end_y - self.origin[1]) / self.resolution)
                points = bresenhams_line(robot_x_cell, robot_y_cell, end_x_cell, end_y_cell)
                for j, (x, y) in enumerate(points):
                    if 0 <= x < self.width and 0 <= y < self.height:
                        index = y * self.width + x
                        if j == len(points) - 1:
                            self.log_odds[index] = min(self.log_odds[index] + self.p_occ, self.max_log_odds)
                        else:
                            self.log_odds[index] = max(self.log_odds[index] + self.p_free, self.min_log_odds)
                        self.map[index] = self.occupied if self.log_odds[index] > 0 else self.free

class LidarMappingNode(Node):
    def __init__(self):
        super().__init__('lidar_mapping_node')

        qos_profile_scan = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            history=HistoryPolicy.KEEP_LAST,
            depth=10
        )
        
        qos_profile_odom = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            history=HistoryPolicy.KEEP_LAST,
            depth=10
        )
        
        qos_profile_map = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )

        self.scan_sub = self.create_subscription(
            LaserScan,
            'scan',
            self.scan_callback,
            qos_profile_scan)
        
        self.odom_sub = self.create_subscription(
            Odometry,
            'odom',
            self.odom_callback,
            qos_profile_odom)
        
        self.map_pub = self.create_publisher(OccupancyGrid, 'map', qos_profile_map)
        
        self.map_size_x = 100
        self.map_size_y = 100
        self.map_resolution = 0.1
        self.map_origin = [-self.map_size_x * self.map_resolution / 2, -self.map_size_y * self.map_resolution / 2]
        self.local_map = LocalMap(self.map_size_x, self.map_size_y, self.map_resolution, self.map_origin)
        
        self.current_position = [0.0, 0.0, 0.0]
        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
        self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)

        self.timer = self.create_timer(1.0, self.publish_map)  # Changed to 1 Hz
        self.initialize_map()
        self.publish_static_transforms()

    def initialize_map(self):
        self.occupancy_grid = OccupancyGrid()
        self.occupancy_grid.header.frame_id = 'map'
        self.occupancy_grid.info.resolution = self.map_resolution
        self.occupancy_grid.info.width = self.map_size_x
        self.occupancy_grid.info.height = self.map_size_y
        self.occupancy_grid.info.origin.position.x = self.map_origin[0]
        self.occupancy_grid.info.origin.position.y = self.map_origin[1]
        self.occupancy_grid.info.origin.position.z = 0.0
        self.occupancy_grid.info.origin.orientation.w = 1.0
        self.get_logger().info('Map initialized')

    def odom_callback(self, msg):
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        q = msg.pose.pose.orientation
        r = R.from_quat([q.x, q.y, q.z, q.w])
        yaw = r.as_euler('xyz')[2]
        self.current_position = [x, y, yaw]
        self.handle_robot_pose("odom", "base_link", self.current_position)  # Update transform between odom and base_link
        self.get_logger().info(f'Odometry updated: position={self.current_position[:2]}, orientation={yaw}')

    def scan_callback(self, msg):
        try:
            transform = self.tf_buffer.lookup_transform('map', 'base_link', rclpy.time.Time())
            self.get_logger().info('Transform available')
        except (LookupException, ConnectivityException, ExtrapolationException) as e:
            self.get_logger().warn(f'Transform not available: {e}')
            return

        robot_x = transform.transform.translation.x
        robot_y = transform.transform.translation.y
        robot_yaw = self.get_yaw_from_quaternion(transform.transform.rotation)

        self.local_map.adjust_grid_size(robot_x, robot_y)

        origin_x = int((robot_x - self.local_map.origin[0]) / self.local_map.resolution)
        origin_y = int((robot_y - self.local_map.origin[1]) / self.local_map.resolution)

        self.get_logger().info(f'Robot Position: x={robot_x}, y={robot_y}, yaw={robot_yaw}')
        self.get_logger().info(f'Grid Origin: x={origin_x}, y={origin_y}')

        angle_min = msg.angle_min
        angle_increment = msg.angle_increment
        for i, r in enumerate(msg.ranges):
            if r < msg.range_min or r > msg.range_max:
                continue
            angle = angle_min + i * angle_increment + robot_yaw
            x_end = int(origin_x + (r * math.cos(angle)) / self.local_map.resolution)
            y_end = int(origin_y + (r * math.sin(angle)) / self.local_map.resolution)

            if 0 <= x_end < self.local_map.width and 0 <= y_end < self.local_map.height:
                line_points = bresenhams_line(origin_x, origin_y, x_end, y_end)
                for (x, y) in line_points:
                    if 0 <= x < self.local_map.width and 0 <= y < self.local_map.height:
                        index = x + y * self.local_map.width
                        self.local_map.map[index] = self.local_map.free
                self.local_map.map[x_end + y_end * self.local_map.width] = self.local_map.occupied

        self.update_occupancy_grid()
        self.publish_map()

    def handle_robot_pose(self, parent, child, pose):
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = parent
        t.child_frame_id = child
        t.transform.translation.x = pose[0]
        t.transform.translation.y = pose[1]
        t.transform.translation.z = 0.0
        q = R.from_euler('xyz', [0, 0, pose[2]]).as_quat()
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]
        self.tf_broadcaster.sendTransform(t)
        self.get_logger().info(f"Broadcasted transform from {parent} to {child} with pose {pose}")

    def publish_static_transforms(self):
        static_transform_publisher = tf2_ros.StaticTransformBroadcaster(self)
        
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'map'
        t.child_frame_id = 'odom'
        t.transform.translation.x = 0.0
        t.transform.translation.y = 0.0
        t.transform.translation.z = 0.0
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = 0.0
        t.transform.rotation.w = 1.0
        static_transform_publisher.sendTransform(t)
        self.get_logger().info("Published static transform from map to odom")

    def get_yaw_from_quaternion(self, q):
        r = R.from_quat([q.x, q.y, q.z, q.w])
        return r.as_euler('xyz')[2]

    def update_occupancy_grid(self):
        self.occupancy_grid.info.width = self.local_map.width
        self.occupancy_grid.info.height = self.local_map.height
        self.occupancy_grid.info.origin.position.x = self.local_map.origin[0]
        self.occupancy_grid.info.origin.position.y = self.local_map.origin[1]
        self.occupancy_grid.info.origin.position.z = 0.0
        self.occupancy_grid.info.origin.orientation.w = 1.0
        self.occupancy_grid.data = self.local_map.map

    def publish_map(self):
        self.occupancy_grid.header.stamp = self.get_clock().now().to_msg()
        self.map_pub.publish(self.occupancy_grid)
        self.get_logger().info('Published map')

def main(args=None):
    rclpy.init(args=args)
    lidar_mapping_node = LidarMappingNode()
    rclpy.spin(lidar_mapping_node)
    lidar_mapping_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

and this is the output enter image description here

any suggestions?

$\endgroup$

1 Answer 1

1
$\begingroup$

There is no publisher to the scan topic, so the callback to generate the map is never called. Have you run the node to read the LiDAR data on your robot? Depending on your lidar, launching the ROS node to read the data should probably fix the issue.

This link should help, if you are using an RPLiDAR https://github.com/Slamtec/sllidar_ros2

$\endgroup$
1
  • $\begingroup$ Sriram K , thanks it helped but I am facing another issue now. I have edited the question. Would you mind having a look at it? $\endgroup$
    – Zooz
    Commented May 31 at 0:09

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.