# I am trying to create map using odometry and display in rviz2. I am not able to create map as my scan topic doesn't have any publisher

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

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
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.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.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.get_logger().info(f'Odometry updated: position={self.current_position[:2]}, orientation={yaw}')

def scan_callback(self, msg):
try:
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)

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.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.get_logger().info(f"Broadcasted transform from {parent} to {child} with pose {pose}")

def publish_static_transforms(self):

t = TransformStamped()
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.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

any suggestions?