$search
00001 /**************************************************************** 00002 * 00003 * Copyright (c) 2010 00004 * 00005 * Fraunhofer Institute for Manufacturing Engineering 00006 * and Automation (IPA) 00007 * 00008 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00009 * 00010 * Project name: care-o-bot 00011 * ROS stack name: cob_driver 00012 * ROS package name: cob_light 00013 * Description: Switch robots led color by sending data to 00014 * the led-µC over serial connection. 00015 * 00016 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00017 * 00018 * Author: Benjamin Maidel, email:benjamin.maidel@ipa.fraunhofer.de 00019 * Supervised by: Benjamin Maidel, email:benjamin.maidel@ipa.fraunhofer.de 00020 * 00021 * Date of creation: August 2012 00022 * ToDo: 00023 * 00024 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00025 * 00026 * Redistribution and use in source and binary forms, with or without 00027 * modification, are permitted provided that the following conditions are met: 00028 * 00029 * * Redistributions of source code must retain the above copyright 00030 * notice, this list of conditions and the following disclaimer. 00031 * * Redistributions in binary form must reproduce the above copyright 00032 * notice, this list of conditions and the following disclaimer in the 00033 * documentation and/or other materials provided with the distribution. 00034 * * Neither the name of the Fraunhofer Institute for Manufacturing 00035 * Engineering and Automation (IPA) nor the names of its 00036 * contributors may be used to endorse or promote products derived from 00037 * this software without specific prior written permission. 00038 * 00039 * This program is free software: you can redistribute it and/or modify 00040 * it under the terms of the GNU Lesser General Public License LGPL as 00041 * published by the Free Software Foundation, either version 3 of the 00042 * License, or (at your option) any later version. 00043 * 00044 * This program is distributed in the hope that it will be useful, 00045 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00046 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00047 * GNU Lesser General Public License LGPL for more details. 00048 * 00049 * You should have received a copy of the GNU Lesser General Public 00050 * License LGPL along with this program. 00051 * If not, see <http://www.gnu.org/licenses/>. 00052 * 00053 ****************************************************************/ 00054 00055 // standard includes 00056 #include <stdio.h> 00057 #include <string.h> 00058 #include <sstream> 00059 #include <math.h> 00060 00061 // ros includes 00062 #include <ros/ros.h> 00063 00064 // ros message includes 00065 #include <std_msgs/ColorRGBA.h> 00066 #include <visualization_msgs/Marker.h> 00067 00068 // serial connection includes 00069 #include <unistd.h> 00070 #include <fcntl.h> 00071 #include <errno.h> 00072 #include <termios.h> 00073 #include <time.h> 00074 00075 // if defined and connection to serial port fails 00076 // debug values will be logged 00077 //#define __SIM__ 00078 00079 // define this if you like to turn off the light 00080 // when this node comes up 00081 #define TURN_OFF_LIGHT_ON_STARTUP 00082 00083 class SerialCom 00084 { 00085 public: 00086 // Constructor 00087 SerialCom() : 00088 _fd(-1), m_simulation(false) 00089 { 00090 } 00091 // Destructor 00092 ~SerialCom() 00093 { 00094 closePort(); 00095 } 00096 00097 // Open Serial Port 00098 int openPort(std::string devicestring, int baudrate) 00099 { 00100 if(_fd != -1) return _fd; 00101 00102 ROS_INFO("Open Port on %s",devicestring.c_str()); 00103 _fd = open(devicestring.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); 00104 if(_fd != -1) 00105 { 00106 speed_t baud = getBaudFromInt(baudrate); 00107 fcntl(_fd, F_SETFL, 0); 00108 tcgetattr(_fd, &port_settings); 00109 port_settings.c_cflag &= ~PARENB; 00110 port_settings.c_cflag &= ~CSTOPB; 00111 port_settings.c_cflag &= ~CSIZE; 00112 port_settings.c_cflag = baud | CS8 | CLOCAL | CREAD; 00113 port_settings.c_iflag = IGNPAR; 00114 tcsetattr(_fd, TCSANOW, &port_settings); 00115 ROS_INFO("Serial connection on %s succeeded.", devicestring.c_str()); 00116 } 00117 else 00118 { 00119 ROS_ERROR("Serial connection on %s failed.", devicestring.c_str()); 00120 } 00121 return _fd; 00122 } 00123 00124 // Send Data to Serial Port 00125 int sendData(std::string value) 00126 { 00127 size_t wrote = -1; 00128 if(_fd != -1) 00129 { 00130 wrote = write(_fd, value.c_str(), value.length()); 00131 ROS_DEBUG("Wrote [%s] with %i bytes from %i bytes", value.c_str(), (int)wrote, value.length()); 00132 } 00133 else 00134 ROS_WARN("Can not write to serial port. Port closed! Check if LED board is attached and device is opened correctly."); 00135 00136 return wrote; 00137 } 00138 00139 // Read Data from Serial Port 00140 int readData(std::string &value, size_t nBytes) 00141 { 00142 char buffer[32]; 00143 size_t rec = -1; 00144 rec = read(_fd, buffer, nBytes); 00145 value = std::string(buffer); 00146 return rec; 00147 } 00148 00149 // Check if Serial Port is opened 00150 bool isOpen() 00151 { 00152 return (_fd != -1); 00153 } 00154 00155 // Close Serial Port 00156 void closePort() 00157 { 00158 if(_fd != -1) 00159 close(_fd); 00160 } 00161 00162 private: 00163 // filedescriptor 00164 int _fd; 00165 // serial port settings 00166 struct termios port_settings; 00167 bool m_simulation; 00168 00169 speed_t getBaudFromInt(int baud) 00170 { 00171 speed_t ret; 00172 switch(baud) 00173 { 00174 case 0: ret=B0; break; 00175 case 50: ret=B50; break; 00176 case 75: ret=B75; break; 00177 case 110: ret=B110; break; 00178 case 134: ret=B134; break; 00179 case 150: ret=B150; break; 00180 case 200: ret=B200; break; 00181 case 300: ret=B300; break; 00182 case 1200: ret=B1200; break; 00183 case 1800: ret=B1800; break; 00184 case 2400: ret=B2400; break; 00185 case 4800: ret=B4800; break; 00186 case 9600: ret=B9600; break; 00187 case 19200: ret=B19200; break; 00188 case 38400: ret=B38400; break; 00189 case 57600: ret=B57600; break; 00190 case 115200: ret=B115200; break; 00191 case 230400: ret=B230400; break; 00192 default: 00193 ROS_WARN("Unsupported Baudrate [%i]. Using default [230400]", baud); 00194 ret=B230400; 00195 break; 00196 } 00197 return ret; 00198 } 00199 }; 00200 00201 class LightControl 00202 { 00203 public: 00204 LightControl() : 00205 _invertMask(0) 00206 { 00207 00208 bool invert_output; 00209 00210 //_nh = ros::NodeHandle("~"); 00211 _nh.param<bool>("invert_output", invert_output, false); 00212 _invertMask = (int)invert_output; 00213 00214 _nh.param<std::string>("devicestring",_deviceString,"/dev/ttyLed"); 00215 _nh.param<int>("baudrate",_baudrate,230400); 00216 _nh.param<bool>("pubmarker",_bPubMarker,true); 00217 00218 _sub = _nh.subscribe("command", 1, &LightControl::setRGB, this); 00219 00220 _pubMarker = _nh.advertise<visualization_msgs::Marker>("marker",1); 00221 00222 #ifndef __SIM__ 00223 //open serial port 00224 _serialCom.openPort(_deviceString, _baudrate); 00225 #endif 00226 00227 //if pubmarker is set create timer to pub marker 00228 if(_bPubMarker) 00229 _timerMarker = _nh.createTimer(ros::Duration(0.5), 00230 &LightControl::markerCallback, this); 00231 00232 //initial turn off leds 00233 #ifdef TURN_OFF_LIGHT_ON_STARTUP 00234 _color.a=0; 00235 setRGB(_color); 00236 #endif 00237 } 00238 00239 ~LightControl() 00240 { 00241 } 00242 00243 void setRGB(std_msgs::ColorRGBA color) 00244 { 00245 if(color.r <= 1.0 && color.g <=1.0 && color.b <= 1.0) 00246 { 00247 _color = color; 00248 //calculate rgb spektrum for spezific alpha value, because 00249 //led board is not supporting alpha values 00250 color.r *= color.a; 00251 color.g *= color.a; 00252 color.b *= color.a; 00253 //led board value spektrum is from 0 - 999. 00254 //at r@w's led strip, 0 means fully lighted and 999 light off(_invertMask) 00255 color.r = (fabs(_invertMask-color.r) * 999.0); 00256 color.g = (fabs(_invertMask-color.g) * 999.0); 00257 color.b = (fabs(_invertMask-color.b) * 999.0); 00258 00259 _ssOut.clear(); 00260 _ssOut.str(""); 00261 _ssOut << (int)color.r << " " << (int)color.g << " " << (int)color.b << "\n\r"; 00262 00263 #ifndef __SIM__ 00264 _serialCom.sendData(_ssOut.str()); 00265 #endif 00266 00267 } 00268 else 00269 ROS_ERROR("Unsupported Color format. rgba values range is 0.0 - 1.0"); 00270 } 00271 00272 private: 00273 SerialCom _serialCom; 00274 00275 std::string _deviceString; 00276 int _baudrate; 00277 int _invertMask; 00278 bool _bPubMarker; 00279 00280 std::stringstream _ssOut; 00281 00282 ros::NodeHandle _nh; 00283 ros::Publisher _pubMarker; 00284 ros::Subscriber _sub; 00285 ros::Timer _timerMarker; 00286 00287 std_msgs::ColorRGBA _color; 00288 00289 // creates and publishes an visualization marker 00290 void markerCallback(const ros::TimerEvent &event) 00291 { 00292 visualization_msgs::Marker marker; 00293 marker.header.frame_id = "/base_link"; 00294 marker.header.stamp = ros::Time(); 00295 marker.ns = "color"; 00296 marker.id = 0; 00297 marker.type = visualization_msgs::Marker::SPHERE; 00298 marker.action = visualization_msgs::Marker::ADD; 00299 marker.pose.position.x = 0; 00300 marker.pose.position.y = 0, 00301 marker.pose.position.z = 1.5; 00302 marker.pose.orientation.x = 0.0; 00303 marker.pose.orientation.y = 0.0; 00304 marker.pose.orientation.z = 0.0; 00305 marker.pose.orientation.w = 1.0; 00306 marker.scale.x = 0.1; 00307 marker.scale.y = 0.1; 00308 marker.scale.z = 0.1; 00309 marker.color.a = _color.a; 00310 marker.color.r = _color.r; 00311 marker.color.g = _color.g; 00312 marker.color.b = _color.b; 00313 _pubMarker.publish(marker); 00314 } 00315 }; 00316 00317 int main(int argc, char** argv) 00318 { 00319 // init node 00320 ros::init(argc, argv, "light_controller"); 00321 // create LightControl instance 00322 LightControl lightControl; 00323 00324 ros::spin(); 00325 return 0; 00326 }