cob_light.py
Go to the documentation of this file.00001 #!/usr/bin/python 00002 #*************************************************************** 00003 # 00004 # Copyright (c) 2010 00005 # 00006 # Fraunhofer Institute for Manufacturing Engineering 00007 # and Automation (IPA) 00008 # 00009 # +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00010 # 00011 # Project name: care-o-bot 00012 # ROS stack name: cob_driver 00013 # ROS package name: cob_light 00014 # 00015 # +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00016 # 00017 # Author: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de 00018 # Supervised by: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de 00019 # 00020 # Date of creation: June 2010 00021 # ToDo: 00022 # 00023 # +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00024 # 00025 # Redistribution and use in source and binary forms, with or without 00026 # modification, are permitted provided that the following conditions are met: 00027 # 00028 # * Redistributions of source code must retain the above copyright 00029 # notice, this list of conditions and the following disclaimer. 00030 # * Redistributions in binary form must reproduce the above copyright 00031 # notice, this list of conditions and the following disclaimer in the 00032 # documentation and/or other materials provided with the distribution. 00033 # * Neither the name of the Fraunhofer Institute for Manufacturing 00034 # Engineering and Automation (IPA) nor the names of its 00035 # contributors may be used to endorse or promote products derived from 00036 # this software without specific prior written permission. 00037 # 00038 # This program is free software: you can redistribute it and/or modify 00039 # it under the terms of the GNU Lesser General Public License LGPL as 00040 # published by the Free Software Foundation, either version 3 of the 00041 # License, or (at your option) any later version. 00042 # 00043 # This program is distributed in the hope that it will be useful, 00044 # but WITHOUT ANY WARRANTY; without even the implied warranty of 00045 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00046 # GNU Lesser General Public License LGPL for more details. 00047 # 00048 # You should have received a copy of the GNU Lesser General Public 00049 # License LGPL along with this program. 00050 # If not, see <http://www.gnu.org/licenses/>. 00051 # 00052 #**************************************************************** 00053 00054 import roslib; 00055 roslib.load_manifest('cob_light') 00056 import rospy 00057 from cob_light.msg import Light 00058 00059 import serial 00060 import sys 00061 00062 class LightControl: 00063 def __init__(self): 00064 self.ns_global_prefix = "/light_controller" 00065 00066 # get parameter from parameter server 00067 if not rospy.has_param(self.ns_global_prefix + "/devicestring"): 00068 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/devicestring") 00069 sys.exit() 00070 devicestring_param = rospy.get_param(self.ns_global_prefix + "/devicestring") 00071 if not rospy.has_param(self.ns_global_prefix + "/baudrate"): 00072 rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/baudrate") 00073 sys.exit() 00074 baudrate_param = rospy.get_param(self.ns_global_prefix + "/baudrate") 00075 00076 # open serial communication 00077 rospy.loginfo("trying to initializing serial connection") 00078 try: 00079 self.ser = serial.Serial(devicestring_param, baudrate_param) 00080 except serial.serialutil.SerialException: 00081 rospy.logerr("Could not initialize serial connection on %s, aborting...",devicestring_param) 00082 sys.exit() 00083 rospy.loginfo("serial connection initialized successfully") 00084 00085 def setRGB(self, red, green, blue): 00086 #color in rgb color space ranging from 0 to 999 00087 #print "setRGB", red, green, blue 00088 if(red <= 999 and green <= 999 and blue <= 999): 00089 self.ser.write(str(red)+ " " + str(green)+ " " + str(blue)+"\n\r") 00090 00091 def LightCallback(self,light): 00092 rospy.loginfo("Received new color: rgb = [%d, %d, %d]",light.r,light.g,light.b) 00093 print light.name.data 00094 self.setRGB(light.r,light.g,light.b) 00095 00096 if __name__ == '__main__': 00097 rospy.init_node('light_controller') 00098 lc = LightControl() 00099 rospy.Subscriber("command", Light, lc.LightCallback) 00100 rospy.loginfo(rospy.get_name() + " running") 00101 rospy.spin()