Hello, I’ve written the service server and client to find and follow wall, and it functions correctly most of the times, although, only sometimes, when I try launch the launch file that launches both the nodes for server and client, I get this error: -
(ray is assigned to the laser ranges, as in the server code)
Following are the programs for the server, client and the launch file: -
1. Server
#! /usr/bin/env python
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from wall_follower.srv import FindWall, FindWallResponse
# Defining variables
max_range = 3.5 # Max range of the laser.
min_range = 0.12 # Min range of the laser.
max_angle = 3.142 # Max angle of the laser scanner
min_angle = -3.142 # Min angle of the laser scanner
angle_increment = 0.008 # Angle increment
speed = [0.08, 0.12] # Speed options: slow and fast.
def servCallback(req):
fw_service_response = FindWallResponse() # Defining response object.
rospy.loginfo("/find_wall service called")
min1 = ray[0]
val = findWall(min1)
result = mainBehaviour(val)
fw_service_response.wallfound = result
if fw_service_response.wallfound:
rospy.loginfo("Behaviour achieved")
else:
rospy.loginfo("Behaviour failed!")
return fw_service_response
def posCallback(msg):
global f, b, l, r, ray, f_r, min1
ray = msg.ranges
f = msg.ranges[len(msg.ranges)//2] # Front readings
r = msg.ranges[len(msg.ranges)//4] # Right readings
l = msg.ranges[len(msg.ranges)*3//4] # Left readings
b = msg.ranges[len(msg.ranges) - 1] # Back readings
f_r = ray[len(ray)*3//8] #Front-Right readings (ray number 270)
def moveForward(spd):
vel.linear.x = spd
vel.angular.z = 0
def moveBackwards(spd):
vel.linear.x = -spd
vel.angular.z = 0
def turnCW():
vel.linear.x = 0.1
vel.angular.z = -0.4
def turnCCW():
vel.linear.x = 0.1
vel.angular.z = 0.4
def rotateLeft():
vel.linear.x = 0
vel.angular.z = 0.4
def rotateRight():
vel.linear.x = 0
vel.angular.z = -0.4
def stopRobot():
vel.linear.x = 0
vel.angular.z = 0
# This function will find the wall, by finding the lowest ray value.
def findWall(min1):
# find the minimum ray dist index
for i in range(len(ray)-1):
if ray[i] < min1:
min1 = ray[i]
# time.sleep(0.5)
return min1
# This function will move the robot's front closer to the wall.
def alignFront(val):
aligned = False # Is the robot aligned? -> NO
while not (val+0.05) >= f >= (val-0.05): # while the front is not almost equal to the
# minimum ray distance, rotate the robot to right.
rotateRight()
pub.publish(vel)
rate.sleep()
if (val+0.05) >= f >= (val-0.05):
aligned = True # Is the robot aligned? -> YES
break
stopRobot()
pub.publish(vel)
rate.sleep()
return aligned
# This function willl align the robot's front right to face the wall.
def alignFrontRight(val):
aligned = False
while not (val+0.05) >= f_r >= (val-0.05):
print("f_r: ", f_r)
rotateLeft()
pub.publish(vel)
rate.sleep()
if (val+0.05) >= f_r >= (val-0.05):
aligned = True
break
if (val+0.05) >= f_r >= (val-0.05):
print("f_r in range")
else:
print("NOT in range")
print("Out of loop", f_r)
stopRobot()
pub.publish(vel)
rate.sleep()
return aligned
# This function executes the main expected behavior for the robot
# ie, once the wall is found, it will align its front move closer,
# then align its front-right(ray 270) to the wall, and return True if aligned correctly.
def mainBehaviour(val):
align_obj = alignFront(val)
while align_obj:
if f > 0.3:
moveForward(speed[0])
elif f <= 0.3:
stopRobot()
break
pub.publish(vel)
rate.sleep()
align_f_r_obj = alignFrontRight(val)
return align_f_r_obj
if __name__ == '__main__':
try:
vel = Twist()
rospy.init_node('wall_follow')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
sub = rospy.Subscriber('/scan', LaserScan, posCallback)
fin_wall_service = rospy.Service('/find_wall', FindWall, servCallback)
rospy.loginfo("Service online.")
rate = rospy.Rate(20)
rospy.spin()
except rospy.ROSInterruptException():
rospy.loginfo("Node Terminated")
2. Client
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from wall_follower.srv import FindWall, FindWallRequest
# Defining variables
max_range = 3.5 # Max range of the laser.
min_range = 0.12 # Min range of the laser.
max_angle = 3.142 # Max angle of the laser scanner
min_angle = -3.142 # Min angle of the laser scanner
angle_increment = 0.008 # Angle increment
speed = [0.08, 0.12] # Speeds
def servClient():
rospy.wait_for_service('/find_wall')
find_wall_client = rospy.ServiceProxy('/find_wall', FindWall)
find_wall_request_obj = FindWallRequest()
result = find_wall_client(find_wall_request_obj)
print(result)
def posCallback(msg):
global f, b, l, r, ray # , f_r, b_r, f_l, b_l
ray = msg.ranges
f = msg.ranges[len(msg.ranges)//2] # Front readings
r = msg.ranges[len(msg.ranges)//4] # Right readings
l = msg.ranges[len(msg.ranges)*3//4] # Left readings
b = msg.ranges[len(msg.ranges) - 1] # Back readings
wallFollow()
def moveForward(spd):
vel.linear.x = spd
vel.angular.z = 0
def moveBackwards(spd):
vel.linear.x = -spd
vel.angular.z = 0
def turnCW():
vel.linear.x = 0.1
vel.angular.z = -0.4
def turnCCW():
vel.linear.x = 0.1
vel.angular.z = 0.4
def rotateLeft():
vel.linear.x = 0
vel.angular.z = 0.4
def rotateRight():
vel.linear.x = 0
vel.angular.z = -0.4
def stopRobot():
vel.linear.x = 0
vel.angular.z = 0
# This function makes the robot follow the wall, keeping it to the right side.
def wallFollow():
if max_range > f > 0.8: # Follow the wall fast, if it's farther than 0.8m
moveForward(speed[1])
if 0.3 > r > 0.2: # Keep following fast if the right is correctly aligned.
moveForward(speed[1])
elif max_range > r > 0.3: # If wall at right is farther than 0.3,
turnCW() # the robot must move closer towards it.
elif 0.2 < r < max_range: # If wall at right is closer than 0.2,
turnCCW() # the robot must move away.
elif f < 0.8: # If the wall is closer than 0.8 meters, move slow!
moveForward(speed[0])
turnCCW() # Turn CCW to avoid collision with front wall
if f <= 0.3: # If wall at front is less than 0.3,
# the robot must back up!
moveBackwards(speed[0]) # Go reverse
pub.publish(vel)
rate.sleep()
if __name__ == '__main__':
try:
vel = Twist()
rospy.init_node('wall_follow_client')
servClient()
pub = rospy.Publisher('/cmd_vel', Twist, queue_size= 1)
sub = rospy.Subscriber('/scan', LaserScan, posCallback)
rate = rospy.Rate(20)
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo("Node Terminated")
3. Launch file
<launch>
<!-- this file launches two node, one for the server and the second for the client -->
<node pkg = 'wall_follower'
type = 'find_wall_service_server.py'
name = 'find_wall_server_node'
output = 'screen'>
</node>
<node pkg = 'wall_follower'
type = 'wall_follow_client.py'
name = 'wall_follower_client_node'
output = 'screen'>
</node>
</launch>
Even just trying and launching the launch file again solves the problem too! Sometimes changing the orientation a little, using teleop_keyboard and then trying to launch the launch file also solves the problem.
Can anyone please give me an insight as to why the code runs most times and sometimes it doesn’t? Am I missing something?
Thanks for the time!