playpen_server.py

Go to the documentation of this file.

#!/usr/bin/env python
#
# Copyright (c) 2009, Georgia Tech Research Corporation
# 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 Georgia Tech Research Corporation 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 GEORGIA TECH RESEARCH CORPORATION ''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 GEORGIA TECH 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.
#

#Author: Marc Killpack


import roslib; roslib.load_manifest('pr2_playpen')
import rospy
from pr2_playpen.srv import *
from robotis.lib_robotis import *


class Play:

    def __init__(self):
        dyn = USB2Dynamixel_Device('/dev/ttyUSB0')
        self.playpen = Robotis_Servo(dyn, 31)
        self.conveyor = Robotis_Servo(dyn,32)
        self.conveyor.init_cont_turn()
        rospy.init_node('playpen_server')
        s_play = rospy.Service('playpen', Playpen, self.move_playpen)
        s_conv = rospy.Service('conveyor', Conveyor, self.move_conveyor)    
        rospy.spin()

    def move_conveyor(self, data):
        delt_time = abs(data.distance)/2.39/0.685/0.0483*2/0.75
        if data.distance > 0:
            self.conveyor.set_angvel(-5)
        else:
            self.conveyor.set_angvel(5)
        rospy.sleep(delt_time)
        self.conveyor.set_angvel(0)
        print "move conveyor"
        return ConveyorResponse(1)

    def move_playpen(self, data):
        if data.command == 0:
            self.playpen.move_angle(0.28)
            print "closing playpen"
        elif data.command == 1:
            self.playpen.move_angle(1.90)
            print "opening playpen...releasing object"
        return PlaypenResponse(1)


if __name__== "__main__":
    go = Play()