Introduction

This section discusses the implementation of Robot Localization, which is critical for accurate mapping. Localization ensures that the robot's perceived position and orientation align with its actual movement in the real world. Without proper localization, the robot could misinterpret its traveled distance and orientation, leading to inaccurate maps.

---

Overview

Localization is implemented using the robot_localization ROS 2 package, which utilizes a Kalman Filter to integrate multiple sensor inputs and estimate the robot's position and velocity. While the package is designed to accept data from various sources, this implementation focuses on two primary inputs:

1. Velocity (X-axis): Derived from a custom node that calculates velocity using the Ping2 sonar.
2. Orientation: Provided by an orientation sensor.

---

Attempted Solutions

The following methods were explored but did not yield reliable results for localization:

• Calculating position using the Ping2 sonar.
• Estimating velocity based on acceleration data from the BNO085 orientation sensor.
• Acceleration from BNO085.

Details about these approaches can be found in the Outdated/Failed Concepts section.

---

Current Solution: Distance to Velocity

The current approach calculates velocity using the Ping2 sonar. This method was chosen for its reliability during testing. The robot_localization package combines this velocity data with orientation readings to provide a robust estimate of the robot's position and heading.

Key Features:

• Custom Velocity Node:
  • Subscribes to the /scan/front topic for sonar data.
  • Uses a moving average buffer to smooth distance readings.
  • Publishes velocity data to the odom topic as an Odometry message.
• Integration with Orientation Sensor:
  Combines velocity data with orientation information to track the robot's movement and heading accurately.

---

Challenges

1. Sensor Integration:
   Ensuring that velocity and orientation data are synchronized and compatible with the robot_localization package required precise configuration.
2. Noise Filtering:
   Applying noise thresholds and moving averages was necessary to reduce inaccuracies in distance measurements from the sonar.
3. Turning Detection:
   Preventing false velocity readings during turns required monitoring the robot's turning status.

---

Code Listing

Below is the code for the custom velocity node:

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from std_msgs.msg import Int32

class LaserScanVelocitySubscriber(Node):
    def __init__(self):
        super().__init__('laser_scan_velocity_subscriber')
        
        self.scan_subscriber = self.create_subscription(
            LaserScan,
            '/scan/front',
            self.scan_callback,
            10
        )
        self.turning_subscriber = self.create_subscription(
            Int32,  
            '/turning',
            self.turning_callback,
            10
        )
        
        self.prev_distance = None

        # Publisher for Odometry message
        self.odom_publisher = self.create_publisher(Odometry, 'odom', 10)

        self.noise_threshold = 0.001       # Noise threshold to filter small changes (meters)

        # Moving average buffer and size
        self.distance_buffer = []
        self.buffer_size = 5  # Number of readings for averaging

        self.turn_status = 0

        # Timer callback at a fixed interval
        self.timer_period = 1.2  # Timer period in seconds
        self.create_timer(self.timer_period, self.timer_callback)

    def turning_callback(self, msg):
        self.turn_status = msg.data

    def scan_callback(self, msg):
        latest_distance = msg.ranges[0]

        # Add the latest distance to the buffer
        self.distance_buffer.append(latest_distance)

        # Limit Buffer Size
        if len(self.distance_buffer) > self.buffer_size:
            self.distance_buffer.pop(0)

    def timer_callback(self):
        if len(self.distance_buffer) == 0:
            return

        avg_distance = sum(self.distance_buffer) / len(self.distance_buffer)

        # Calculate velocity if we have a previous distance
        if self.prev_distance is not None:
            # Time difference is constant and equal to self.timer_period
            avg_distance_diff = avg_distance - self.prev_distance
            
            # Apply noise threshold
            if abs(avg_distance_diff) < self.noise_threshold or self.turn_status == 1:
                velocity = 0.0  # No significant movement or turning
            else:
                # Calculate velocity using the average distance
                velocity = avg_distance_diff / self.timer_period

            self.get_logger().info(f'Latest avg distance: {avg_distance:.2f} m, '
                                   f'Velocity: {velocity:.2f} m/s')
            self.publish_odom(velocity)
        
        # Update previous distance
        self.prev_distance = avg_distance

        if self.turn_status == 1:
            self.prev_distance = None

    def publish_odom(self, velocity):
        # Create an Odometry message to publish velocity
        odom_msg = Odometry()
        odom_msg.header.stamp = self.get_clock().now().to_msg()
        odom_msg.header.frame_id = 'odom'
        odom_msg.child_frame_id = 'base_link'
        odom_msg.twist.twist.linear.x = velocity
        
        # Publish the Odometry message
        self.odom_publisher.publish(odom_msg)

def main(args=None):
    rclpy.init(args=args)
    node = LaserScanVelocitySubscriber()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()