Dear community,
The task requires that first the robot looks for the wall, then it starts to follow it, but when I launch the main launch file, wall following starts even before getting the result of the service.
Could you help me check why this happens? I use self.wall_found flag but it does not work…
Here is my launch file main.launch:
<launch>
<include file="$(find wall_following)/launch/start_find_wall_service_server.launch"/>
<node pkg="wall_following" type="wall_follower.py" name="wall_follower_node" output="screen"/>
</launch>
Here is wall_follower.py:
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from find_wall_service_client import FindWallServiceClient # Import the service client
class WallFollow():
def __init__(self):
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
self.sub = rospy.Subscriber("/scan", LaserScan, self.laser_callback)
self.move_cmd = Twist()
self.num_ray = None
self.front_index = None
self.right_index = None
self.angle_increment = None
self.laser_front_list = []
self.laser_right_list = []
self.front_distance = 0
self.right_distance = 0
self.find_wall_client = FindWallServiceClient() # Initialize the service client
rospy.loginfo("Initialized the service client...")
self.wall_found = False # Initialize the wall_found flag
# Register the shutdown hook
rospy.on_shutdown(self.stop)
# Call the service to find the wall
rospy.loginfo("Call find wall service before starting wall-following behavior")
self.call_find_wall_service()
def call_find_wall_service(self):
# Call the service before starting wall-following behavior
rospy.loginfo("Send request ...")
self.wall_found = self.find_wall_client.send_request()
if self.wall_found:
rospy.loginfo("Wall found, starting wall-following behavior")
else:
rospy.logerr("Failed to find the wall. Shutting down.")
rospy.signal_shutdown("Failed to find wall")
return
def calculate_laser_indices(self, msg):
# get info about laser data
#rospy.loginfo(f"laser angle_min: {msg.angle_min}")
#rospy.loginfo(f"laser angle_max: {msg.angle_max}")
#rospy.loginfo(f"laser beam number: {len(msg.ranges)}")
#rospy.loginfo(f"laser angle increment: {msg.angle_increment}")
# Calculate indices
self.num_ray =len(msg.ranges)
self.front_index = int((0 - msg.angle_min) / msg.angle_increment)
self.right_index = int(self.front_index / 2)
#self.left_index = self.front_index + int((self.num_ray - self.front_index) / 2)
def laser_callback(self, msg):
if self.front_index is None:
self.calculate_laser_indices(msg)
# Use multiple laser readings to make decisions more reliable
self.laser_front_list = msg.ranges[(self.front_index-3):(self.front_index+3)] # Average the readings in front
self.laser_right_list = msg.ranges[(self.right_index-3):(self.right_index+3)] # Average the readings on the right
# Calculate the average distances
self.laser_front = sum(self.laser_front_list) / len(self.laser_front_list)
self.laser_right = sum(self.laser_right_list) / len(self.laser_right_list)
# Log distances for debugging
rospy.loginfo(f"Front: {self.front_distance:.2f}, Right: {self.right_distance:.2f}")
# # Control logic based on laser readings: use the values of the laser to decide the com_vel
# reset the location to desire path
if self.laser_front <= 0.3:
rospy.loginfo("avoid to hit the wall -> back")
self.move_cmd.linear.x = -0.3
self.move_cmd.angular.z = 0
# desire path
elif self.laser_front <= 0.5 and self.laser_right > 0.2:
rospy.loginfo("Closed to the next wall -> turn fast to the left (moving forward at the same time).")
self.move_cmd.linear.x = 0.1
self.move_cmd.angular.z = 1.2
elif self.laser_right > 0.3:
rospy.loginfo("Away from the wall -> turn right")
self.move_cmd.linear.x = 0.1
self.move_cmd.angular.z = -0.2
elif self.laser_right < 0.2:
rospy.loginfo("Too close to the wall -> turn left")
self.move_cmd.linear.x = 0.1
self.move_cmd.angular.z = 0.2
else:
rospy.loginfo("within desired distance -> move forward")
self.move_cmd.linear.x = 0.1
self.move_cmd.angular.z = 0
def move(self):
rate = rospy.Rate(15)
while not rospy.is_shutdown():
if self.wall_found: # Only publish if the wall is found
self.pub.publish(self.move_cmd)
rate.sleep()
def stop(self):
rospy.loginfo("Stopping the robot...")
self.move_cmd.linear.x = 0
self.move_cmd.angular.z = 0
self.pub.publish(self.move_cmd) # Publish the stop command
if __name__ =='__main__':
rospy.init_node("wall_follow_node", anonymous=True)
try:
wall_follow = WallFollow()
wall_follow.move()
except rospy.ROSInterruptException:
wall_follow.stop()
Here is find_wall_service_client.py:
#!/usr/bin/env python
import rospy
from wall_following.srv import FindWall, FindWallRequest
import time
class FindWallServiceClient:
def __init__(self):
self.service_name = '/find_wall'
# Wait for the service client to be running
rospy.wait_for_service(self.service_name)
# Create the connection object to the service
self.service = rospy.ServiceProxy(self.service_name, FindWall)
# Create a MsgRequest object
self.request = FindWallRequest()
def send_request(self):
time.sleep(2) # Add a delay to ensure the service server is ready
# Send the MsgRequest object through the connection object to be executed by the robot
result = self.service(self.request)
# Print the result given by the service called
rospy.loginfo(result)
try:
result = self.service(self.request)
# Print the result given by the service called
rospy.loginfo(f"service call result: wallfound {result.wallfound}")
return result.wallfound
except rospy.ServiceException as e:
rospy.logerr(f"Service call failed: {e}")
return False
if __name__ == '__main__':
rospy.init_node('find_wall_service_client', anonymous=True)
service_client = FindWallServiceClient()
try:
service_client.send_request()
except rospy.ROSInterruptException:
pass
Here is find_wall_service_server.py:
#!/usr/bin/env python
import rospy
from wall_following.srv import FindWall, FindWallResponse
from find_wall_movement import FindWallMove
class FindWallServiceServer:
def __init__(self):
self.service_name = "/find_wall"
self.find_wall = FindWallMove()
self.service = rospy.Service(self.service_name, FindWall, self.callback)
rospy.on_shutdown(self.find_wall.stop_robot)
rospy.loginfo(f"Service {self.service_name} is ready")
def callback(self, request):
rospy.loginfo(f"Service {self.service_name} has been called")
wall_found = self.find_wall.find_wall()
rospy.loginfo(f"Finished ervice {self.service_name}.")
return FindWallResponse(wallfound=wall_found)
def spin(self):
rospy.spin()
if __name__ == '__main__':
rospy.init_node('find_wall_service_server')
service_server = FindWallServiceServer()
try:
service_server.spin()
except rospy.ROSInterruptException:
pass # Normal shutdown, no additional action needed because on_shutdown will handle stop_robot
except Exception as e:
rospy.logerr(f"An unexpected error occurred: {e}")
service_server.find_wall.stop_robot()
Here is find_wall_movement.py:
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
class FindWallMove:
def __init__(self):
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.sub = rospy.Subscriber('/scan', LaserScan, self.laser_callback)
self.rate = rospy.Rate(10)
self.move_cmd = Twist()
self.laser_data = []
self.num_ray = None
self.front_index = None
self.angle_increment = None
def laser_callback(self, msg):
self.laser_data = msg.ranges
if self.front_index is None:
self.calculate_laser_indices(msg)
def calculate_laser_indices(self, msg):
# Determine the indices for the key directions
self.angle_increment = msg.angle_increment
# Calculate indices
self.num_ray =len(msg.ranges)
self.front_index = int((0 - msg.angle_min) / msg.angle_increment)
#self.right_index = int(self.front_index / 2)
#self.left_index = self.front_index + int((self.num_ray - self.front_index) / 2)
def find_wall(self):
rospy.loginfo("Finding the nearest wall...")
# Step 1: Identify the shortest laser ray . Assume the closest obstacle is the wall (only in the simulation/real enviroment in this course).
while not self.laser_data:
self.rate.sleep()
min_distance = min(self.laser_data)
min_index = self.laser_data.index(min_distance)
# Step 2: Rotate the robot until the front of it is facing the wall (front laser ray is the smallest one).
angle_to_rotate = (self.front_index - min_index) * self.angle_increment
rospy.loginfo(f"Rotating by {angle_to_rotate} radians to face the nearest wall")
self.rotate(angle_to_rotate)
# Step 3: Move forward until the front ray is smaller than 30 cm
while self.laser_data[self.front_index] > 0.3:
self.move_cmd.angular.z = 0
self.move_cmd.linear.x = 0.2
self.pub.publish(self.move_cmd)
self.rate.sleep()
# Step 4: Rotate left by 90 degrees to align with the wall
rospy.loginfo("Rotating left by 90 degrees to align with the wall")
self.rotate(1.5708) # Rotate 90 degrees left (π/2 radians)
# Stop the robot after positioning
self.stop_robot()
return True
def rotate(self, angle):
# Rotate by a specific angle
angular_speed = 0.2 # radians per second
duration = abs(angle) / angular_speed
self.move_cmd.linear.x = 0
self.move_cmd.angular.z = (angular_speed if angle > 0 else -angular_speed)
end_time = rospy.Time.now() + rospy.Duration(duration)
while rospy.Time.now() < end_time and not rospy.is_shutdown():
self.pub.publish(self.move_cmd)
self.rate.sleep()
# Stop rotation
self.move_cmd.angular.z = 0
self.pub.publish(self.move_cmd)
def stop_robot(self):
rospy.loginfo("Stopping the robot")
self.move_cmd.linear.x = 0
self.move_cmd.angular.z = 0
self.pub.publish(self.move_cmd)
!