#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of the Willow Garage nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#


## Simple program to send commands to the power board manually

PKG = 'pr2_power_board' # this package name

import roslib; roslib.load_manifest(PKG) 

import sys
import os
import string

import rospy
from pr2_power_board.srv import *

def power_board_client(serial, breaker_number, command, flags):

    # NOTE: you don't have to call rospy.init_node() to make calls against
    # a service. This is because service clients do not have to be
    # nodes.

    # block until the add_two_ints service is available
    rospy.wait_for_service('power_board/control', 5)
    
    try:
        # create a handle to the add_two_ints service
        control = rospy.ServiceProxy('power_board/control', PowerBoardCommand)
        
        print "Requesting %d to %s"%(breaker_number, command)
        
        # simplified style
        resp1 = control(serial, breaker_number, command, flags)
        return resp1.retval
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e
        return 255 

def usage():
    return "%s serial_number breaker_number(0 | 1 | 2) command(start | stop | reset | disable | none) flags"%sys.argv[0]

if __name__ == "__main__":
    
    if len(sys.argv) != 5:
        print usage()
    try:
        serial = int(sys.argv[1])
        breaker_number = string.atoi(sys.argv[2])
        command = sys.argv[3]
        flags = int(sys.argv[4])
    except:
        print "excepted"
        print usage()
        sys.exit(1)
    if command == "force_start":
        power_board_client(serial, breaker_number, "reset", 0)
        sleep(0.5)
        command = "start"

    print "breaker:%d command:%s = response:%d"%(breaker_number, command, power_board_client(serial, breaker_number, command, flags))
