#!/usr/bin/env python3
import rospy
import rosservice
from time import sleep
from unsafe_traversal import get_planner_controller, ViablePlanCheckerService, TraversalActionServer, LaserDistCheckerService
from unsafe_traversal.srv import ChangeTraversalParameters, ChangeTraversalParametersRequest, ChangeTraversalParametersResponse

# init node, only one must exist
rospy.init_node('unsafe_traversal')

# create a new planner
controller = get_planner_controller()

# crash out if we can't find a planner
if controller is None:
    raise Exception("Could not find an appropriate planner.")

ViablePlanCheckerService()
LaserDistCheckerService()

# build service
def set_parameters(request):
    controller.set_unsafe(request.unsafe)
    return ChangeTraversalParametersResponse()


# start service
rospy.Service('/unsafe_traversal/set_unsafe_traversal',
              ChangeTraversalParameters, set_parameters)

# start action server
TraversalActionServer()

# run indefinitely
rospy.spin()

# restore parameters
controller.set_unsafe(False)
