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 
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):
super().__init__('wall_following_node')
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
else:
# 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
self.publisher_.publish(self.cmd)
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):
rclpy.init(args=args)
node = WallFollowingNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()