Intel RealSense Robot Obstacle Avoidance

obstacle-avoidance.py

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import pyrealsense2 as rs
import numpy as np

class ObstacleAvoidanceRobot(Node):
    def __init__(self):
        super().__init__('obstacle_avoidance_robot')
        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
        self.timer = self.create_timer(0.1, self.timer_callback)
        
        self.pipeline = rs.pipeline()
        self.config = rs.config()
        self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
        self.pipeline.start(self.config)

        self.turning_direction = None  # To keep track of the current turning direction

    def get_depth_frame(self):
        frames = self.pipeline.wait_for_frames()
        depth_frame = frames.get_depth_frame()
        return np.asanyarray(depth_frame.get_data())

    def process_depth_image(self, depth_image):
        height, width = depth_image.shape

        # Define regions of interest
        left_region = depth_image[height//3:2*height//3, :width//3]
        center_region = depth_image[height//3:2*height//3, width//3:2*width//3]
        right_region = depth_image[height//3:2*height//3, 2*width//3:]

        # Calculate average distances for each region
        left_dist = np.mean(left_region)
        center_dist = np.mean(center_region)
        right_dist = np.mean(right_region)

        return left_dist, center_dist, right_dist

    def decide_movement(self, left_dist, center_dist, right_dist):
        if self.turning_direction:
            # Continue turning in the same direction until the path is clear
            if center_dist > 1000:  # More than 1 meter
                self.turning_direction = None
                return "FORWARD"
            else:
                return self.turning_direction
        elif center_dist < 1000:  # Less than 1 meter
            if left_dist > right_dist:
                self.turning_direction = "LEFT"
            else:
                self.turning_direction = "RIGHT"
            return self.turning_direction
        else:
            return "FORWARD"

    def move(self, direction):
        msg = Twist()
        if direction == "FORWARD":
            msg.linear.x = 0.4
            msg.angular.z = 0.0
        elif direction == "LEFT":
            msg.linear.x = 0.0
            msg.angular.z = 0.4
        elif direction == "RIGHT":
            msg.linear.x = 0.0
            msg.angular.z = -0.4
        else:
            msg.linear.x = 0.0
            msg.angular.z = 0.0
        
        self.publisher_.publish(msg)
        self.get_logger().info(f'Moving: {direction}')

        if direction == "FORWARD":
            self.turning_direction = None  # Reset turning direction when moving forward

    def timer_callback(self):
        depth_image = self.get_depth_frame()
        left_dist, center_dist, right_dist = self.process_depth_image(depth_image)
        self.get_logger().info(f'Distances - Left: {left_dist:.2f}, Center: {center_dist:.2f}, Right: {right_dist:.2f}')
        
        direction = self.decide_movement(left_dist, center_dist, right_dist)
        self.move(direction)

    def __del__(self):
        self.pipeline.stop()

def main(args=None):
    rclpy.init(args=args)
    robot = ObstacleAvoidanceRobot()
    try:
        rclpy.spin(robot)
    except KeyboardInterrupt:
        print("Keyboard interrupt received. Stopping robot.")
    finally:
        robot.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()