def HandleAdjustWallDist(req):
""" Handler for adjusting wall hugging parameters
Responds with wall_dist msg and a bool to verify that the
service command has been accepted
"""
global PATH_CONFIG
# print " wall {}".format(req.cmd.wall)
# print " dist {}\n".format(req.cmd.dist)
resp = wall_dist()
isValid = req.cmd.dist >= 0
if isValid is True and req.cmd.wall != autobot.msg.wall_dist.WALL_FRONT:
""" only accept WALL_LEFT or WALL_RIGHT
Service client can send an invalid wall or distance
query current settings
"""
if req.cmd.wall is not wall_dist.WALL_UNDEF:
PATH_CONFIG.wallToWatch = req.cmd.wall
PATH_CONFIG.desiredTrajectory = req.cmd.dist
else:
isValid = False
resp.wall = PATH_CONFIG.wallToWatch
resp.dist = PATH_CONFIG.desiredTrajectory
return AdjustWallDistResponse(resp, isValid)
def setWallDist(dist, wall):
try:
rospy.wait_for_service('adjustWallDist')
adjustWall = rospy.ServiceProxy('adjustWallDist', AdjustWallDist)
cmd = wall_dist()
cmd.wall = wall
cmd.dist = dist
resp = adjustWall(cmd)
return resp
except rospy.ROSException, e:
print "Service called failed: %s" % e
pass