Wall following logic

Hey there,
I’ve been stuck in project part 1 for quite a few days, and I’m feeling quite frustated.
I’m at the point where I might just drop the course and learn the intro to ROS2 in a different place :frowning:
Once I run the code, my robot goes straight for a bit and starts turning to the right until it hits the wall.
If anyone can please let me know what I’m doing wrong and help me it would be greatly appreciated.

This is my wall_following.py file

import rclpy
import numpy as np
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from rclpy.qos import QoSProfile, ReliabilityPolicy

class WallFollowingNode(Node):
    def __init__(self):
        self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
        self.subscriber = self.create_subscription(LaserScan, '/scan', self.laser_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE))
        self.odom_subscriber_ = self.create_subscription(Odometry, '/odom', self.odom_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE))

        self.cmd = Twist()
        self.rotation_z = 0.0

    def laser_callback(self, msg):
        # Get the laser rays
        right_index = self.go_to_closest(msg,np.pi/2) 
        front_index = self.go_to_closest(msg,2*np.pi)
        laser_front = msg.ranges[front_index]
        laser_right = msg.ranges[right_index]
        # Wall-following behavior based on the distance to the wall
        if laser_front < 0.5:
            # If a wall is detected in front, turn fast to the left
            self.cmd.angular.z = 0.5
            self.cmd.linear.x = 0.1
        elif laser_right > 0.3:
            # If the distance to the wall is bigger than 0.3m, approach the wall
            self.cmd.angular.z = -0.1
            self.cmd.linear.x = 0.1
        elif laser_right < 0.2:
            # If the distance to the wall is smaller than 0.2m, move away from the wall
            self.cmd.angular.z = 0.1
            self.cmd.linear.x = 0.1
            # If the distance to the wall is between 0.2m and 0.3m, keep moving forward
            self.cmd.angular.z = 0.0
            self.cmd.linear.x = 0.1

        # Publish the velocity command

    def odom_callback(self, msg):
        # Extract yaw angle from the quaternion
        x = msg.pose.pose.orientation.x
        y = msg.pose.pose.orientation.y
        z = msg.pose.pose.orientation.z
        w = msg.pose.pose.orientation.w

        _, _, yaw = self.euler_from_quaternion([x, y, z, w])

        # We always put the rotation to be bt 0 and 180 degrees
        self.rotation_z = yaw % np.pi
        if yaw < 0:
            self.rotation_z += np.pi

    def euler_from_quaternion(self, quaternion):
        # Function to convert quaternion to euler angles
        x, y, z, w = quaternion
        sinr_cosp = 2 * (w * x + y * z)
        cosr_cosp = 1 - 2 * (x * x + y * y)
        roll = np.arctan2(sinr_cosp, cosr_cosp)
        sinp = 2 * (w * y - z * x)
        pitch = np.arcsin(sinp)
        siny_cosp = 2 * (w * z + x * y)
        cosy_cosp = 1 - 2 * (y * y + z * z)
        yaw = np.arctan2(siny_cosp, cosy_cosp)
        return roll, pitch, yaw
    def go_to_closest(self,msg,direction): 
            # Function to get the index of the laser corresponding to a certain direction 
            actual_angle = direction - self.rotation_z # Since the laser rotates we must subtract the rotation in order to get the angle corresponding to the right 
            # Here we take into account the angle_increment! 
            ranges_length = round((2 * np.pi / msg.angle_increment)) 
            lasers_list = np.arange(0,2 * np.pi, ranges_length) 
            diff_array = np.absolute(lasers_list-actual_angle)
            index = diff_array.argmin()
            return index # We return the index in the msg.ranges[index] corresponding to the direction we want

def main(args=None):
    node = WallFollowingNode()

if __name__ == '__main__':

did you check the laser’s start and end angle?
is it -pi to pi ?
and do explain how you have tried to solve the problem so we can try to help better.

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.