Hi All,
I have been working on ROSject section 2.
I initialize the service and the subscriber/publisher together, but ideally, I should only trigger the publisher and subscriber once the service has been called. But I can’t find how to do that elegantly.
Paraphrasing the post ROSject part II: How to work with services and subscriptions at the same time, and how the service should wait for the end of the sub-node in order to proceed with its message?
Thank you!
I have copied my code here:
#! /usr/bin/env python
import rospy
from service_rosject.srv import FindWall, FindWallResponse
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
state_dict_ = {
0: 'find the wall',
1: 'Forward',
2: 'follow the wall position',
3: 'Stop!',
}
found_the_wall = False
state_ = 0
def service_callback(request):
global found_the_wall
print("My_callback Find wall has been called")
resp = FindWallResponse()
if found_the_wall is True:
resp.wallfound = True
else:
resp.wallfound = False
return resp.wallfound # the service Response class, in this case FindWall
def scan_callback(msg):
global state_
global found_the_wall
print("Scan callback has been called")
minimum = min(msg.ranges)
index = msg.ranges.index(minimum)
if state_ == 0: # find the wall - Rotate to Closest wall to front
if index > 358 and index < 362:
state_ = 1 # Moved facing the wall! Forward
print("Moved facing the wall!")
else:
if index < 360:
move.angular.z = -3.14/18.0 # turn right
else:
move.angular.z = 3.14/18.0 # turn left
pub_.publish(move)
if state_ == 1: # Forward to the wall!
if msg.ranges[360] > 0.3:
move.angular.z = 0
move.linear.x = 0.1
pub_.publish(move)
else:
move.angular.z = 0
move.linear.x = 0.0
print("We are at 30cm from the wall")
state_ = 2 # Last action, rotate
pub_.publish(move)
if state_ == 2: # Rotate to be in position
if index > 168 and index < 175:
move.angular.z = 0 # stop
pub_.publish(move)
print("Done !")
found_the_wall = True
state_ = 3 # Stop!
elif index <= 168:
move.angular.z = -0.1 # turn right
pub_.publish(move)
else:
move.angular.z = 0.1 # turn left
pub_.publish(move)
if state_ == 3: # Stop!
print("Stop !")
print("index: ", index)
print("dist: ", minimum)
rospy.init_node('reading_laser')
# create the Service called my_service with the defined callback
my_service = rospy.Service('/find_wall', FindWall, service_callback)
pub_ = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
sub = rospy.Subscriber('/scan', LaserScan, scan_callback)
move = Twist()
rospy.spin() # maintain the service open.