cob_light: light_action_test.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00004 #
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #
00009 #   http://www.apache.org/licenses/LICENSE-2.0
00010 #
00011 # Unless required by applicable law or agreed to in writing, software
00012 # distributed under the License is distributed on an "AS IS" BASIS,
00013 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014 # See the License for the specific language governing permissions and
00015 # limitations under the License.
00016 
00017 
00018 import rospy
00019 import actionlib
00020 import random
00021 
00022 from cob_light.msg import *
00023 
00024 from cob_light.srv import SetLightMode
00025 from cob_light.srv import SetLightModeRequest
00026 from cob_light.srv import SetLightModeResponse
00027 
00028 from std_msgs.msg import ColorRGBA
00029 
00030 class ActionTestScript(object):
00031     def __init__(self):
00032         self.client = actionlib.SimpleActionClient("/light_torso/set_light", SetLightModeAction)
00033         # trying to connect to server
00034         rospy.logdebug("waiting for /light_torso/set_light action server to start")
00035         if not self.client.wait_for_server(rospy.Duration(5)):
00036             # error: server did not respond
00037             rospy.logerr("/light_torso/set_light action server not ready within timeout, aborting...")
00038         else:
00039             rospy.logdebug("/light_torso/set_light action server ready")
00040 
00041         self.executeMode(0)
00042 
00043     def executeMode(self, event):
00044         rospy.loginfo("setting action mode")
00045 
00046         mode = self.getLightMode(random.randint(0,14))
00047         goal = SetLightModeGoal()
00048         goal.mode = mode.mode
00049 
00050         self.client.send_goal(goal)
00051         self.client.wait_for_result()
00052         res = self.client.get_result()
00053 
00054         duration = random.uniform(0.0, 0.2)
00055         self.timer = rospy.Timer(rospy.Duration(duration), self.executeMode, True)
00056 
00057     def getLightMode(self, mode):
00058         req = SetLightModeRequest()
00059         req.mode.priority = 4
00060 
00061         if mode == 0:
00062             req.mode.mode=LightModes.BREATH
00063             req.mode.frequency = 0.3
00064             req.mode.pulses = 2
00065             req.mode.timeout = 0
00066             req.mode.colors.append(ColorRGBA(1.0,1.0,1.0,1.0))
00067 
00068         elif mode == 1:
00069             req.mode.mode=LightModes.FLASH
00070             req.mode.frequency = 2
00071             req.mode.pulses = 3
00072             req.mode.timeout = 0
00073             req.mode.colors.append(ColorRGBA(1.0,1.0,1.0,1.0))
00074 
00075         elif mode == 2:
00076             req.mode.mode=LightModes.BREATH
00077             req.mode.frequency = 0.4
00078             req.mode.pulses = 0
00079             req.mode.timeout = 0
00080             req.mode.colors.append(ColorRGBA(170.0/255.0, 250.0/255.0, 70.0/255.0, 1.0))
00081 
00082         elif mode == 3:
00083             req.mode.mode=LightModes.BREATH
00084             req.mode.frequency = 0.3
00085             req.mode.pulses = 0
00086             req.mode.timeout = 0
00087             req.mode.colors.append(ColorRGBA(70.0/255.0, 140.0/255.0, 250.0/255.0, 1.0))
00088 
00089         elif mode == 4:
00090             req.mode.mode=LightModes.BREATH
00091             req.mode.frequency = 0.3
00092             req.mode.pulses = 0
00093             req.mode.timeout = 0
00094             req.mode.colors.append(ColorRGBA(240.0/255.0, 250.0/255.0, 70.0/255.0, 1.0))
00095 
00096         elif mode == 5:
00097             req.mode.mode=LightModes.BREATH
00098             req.mode.frequency = 0.3
00099             req.mode.pulses = 0
00100             req.mode.timeout = 0
00101             req.mode.colors.append(ColorRGBA(70.0/255.0, 240.0/255.0, 250.0/255.0, 1.0))
00102 
00103         elif mode == 6: #static yellow black - barrier tape
00104             yellow = ColorRGBA(70.0/255.0, 240.0/255.0, 250.0/255.0, 1.0)
00105             black = ColorRGBA()
00106             req.mode.mode=LightModes.BREATH
00107             req.mode.frequency = 0.3
00108             req.mode.pulses = 0
00109             req.mode.timeout = 0
00110             #58 leds
00111             num_segs = 6
00112             iterations = int(num_segs/2)
00113             seg_div = int(58/num_segs)
00114             for i in range(0, iterations):
00115                 for j in range (0, seg_div):
00116                     req.mode.colors.append(yellow)
00117                 for j in range (0, seg_div):
00118                     req.mode.colors.append(black)
00119 
00120         elif mode == 7:
00121             req.mode.mode=LightModes.BREATH
00122             req.mode.frequency = 0.3
00123             req.mode.pulses = 0
00124             req.mode.timeout = 0
00125             req.mode.colors.append(ColorRGBA(250.0/255.0, 70.0/255.0, 70.0/255.0, 1.0))
00126 
00127         elif mode == 8:
00128             req.mode.mode=LightModes.BREATH
00129             req.mode.frequency = 0.3
00130             req.mode.pulses = 0
00131             req.mode.timeout = 0
00132             req.mode.colors.append(ColorRGBA(230.0/255.0, 250.0/255.0, 250.0/255.0, 1.0))
00133 
00134         elif mode == 9:
00135             req.mode.mode=LightModes.FADE_COLOR
00136             req.mode.frequency = 0.5
00137             req.mode.pulses = 0
00138             req.mode.timeout = 0
00139             req.mode.colors.append(ColorRGBA(1,1,1,1))
00140 
00141         elif mode == 10:
00142             req.mode.mode=LightModes.FLASH
00143             req.mode.frequency = 4
00144             req.mode.pulses = 0
00145             req.mode.timeout = 0
00146             req.mode.colors.append(ColorRGBA(230.0/255.0, 250.0/255.0, 250.0/255.0, 1.0))
00147 
00148         elif mode == 11:
00149             req.mode.mode=LightModes.STATIC
00150             req.mode.frequency = 1
00151             req.mode.pulses = 0
00152             req.mode.timeout = 0
00153             req.mode.colors.append(ColorRGBA(150.0/255.0, 120.0/255.0, 255.0/255.0, 1.0))
00154 
00155         elif mode == 12:
00156             req.mode.mode=LightModes.STATIC
00157             req.mode.frequency = 1
00158             req.mode.pulses = 0
00159             req.mode.timeout = 0
00160             req.mode.colors.append(ColorRGBA(255.0/255.0, 95.0/255.0, 1.0/255.0, 1.0))
00161 
00162         elif mode == 13: #breath fast in red (error)
00163             req.mode.mode=LightModes.BREATH
00164             req.mode.frequency = 0.8
00165             req.mode.pulses = 0
00166             req.mode.timeout = 0
00167             req.mode.colors.append(ColorRGBA(1.0, 0.0, 0.0, 1.0))
00168 
00169         elif mode == 14: #breath fast in red (error)
00170             req.mode.mode=LightModes.XMAS
00171             req.mode.frequency = 0.8
00172             req.mode.pulses = 0
00173             req.mode.timeout = 0
00174             req.mode.colors.append(ColorRGBA(1.0, 0.0, 0.0, 1.0))
00175 
00176         return req
00177 
00178 
00179 if __name__ == '__main__':
00180   rospy.init_node('light_action_test')
00181   ActionTestScript()
00182   rospy.spin()