value2buttonColor.cpp
Go to the documentation of this file.
00001 /*
00002  * value2buttonColor Node
00003  * by Christian Holl (www.rad-lab.net)
00004  * License: BSD
00005  *
00006  * Have Fun! :-)
00007  */
00008 
00116 #include <ros/ros.h>
00117 #include <string>
00118 #include <vector>
00119 #include <set>
00120 #include <limits>
00121 #include <iostream>
00122 #include <sstream>
00123 #include <std_msgs/Float32.h>
00124 #include <std_msgs/Float64.h>
00125 #include <std_msgs/Int8.h>
00126 #include <std_msgs/Int16.h>
00127 #include <std_msgs/Int32.h>
00128 #include <std_msgs/Int64.h>
00129 #include <std_msgs/UInt8.h>
00130 #include <std_msgs/UInt16.h>
00131 #include <std_msgs/UInt32.h>
00132 #include <std_msgs/UInt64.h>
00133 #include <std_msgs/Bool.h>
00134 #include <boost/algorithm/string.hpp>
00135 #include <sensor_msgs/Joy.h>
00136 #include <x52_joyext/x52_led_color.h>
00137 
00138 enum
00139 {
00140         INPUT_FLOAT32 = 0,
00141         INPUT_FLOAT64,
00142         INPUT_INT8,
00143         INPUT_INT16,
00144         INPUT_INT32,
00145         INPUT_INT64,
00146         INPUT_UINT8,
00147         INPUT_UINT16,
00148         INPUT_UINT32,
00149         INPUT_UINT64,
00150         INPUT_BOOL,
00151         INPUT_JOY,
00152 };
00153 
00154 template <typename T>
00155 class PublishLED
00156 {
00157         enum
00158         {
00159                 OFF='O',
00160                 GREEN='G',
00161                 YELLOW='Y',
00162                 RED='R',
00163         };
00164 
00165 private:
00166         class PairSortMod : public std::pair<unsigned int,T>
00167         {
00168         public:
00169             bool operator<(const PairSortMod& other) const
00170             {
00171                 return this->second < other.second;
00172             }
00173 
00174         };
00175         std::set< PairSortMod > ranges;
00176         ros::NodeHandle *n;
00177         ros::Publisher pub;
00178         int led;
00179         bool axis_or_button;
00180         int axis_button;
00181         ros::Subscriber sub;
00182 
00183 
00184 public:
00185         PublishLED(ros::NodeHandle *n, std::string setup, int led, int axis, bool axis_or_button)
00186         :n(n),
00187          axis_or_button(axis_or_button),
00188          axis_button(axis)
00189         {
00190                 std::vector<std::string> separatedSetup;
00191                 boost::split(separatedSetup, setup, boost::is_any_of("|"));
00192                 PairSortMod pair;
00193                 unsigned int i;
00194                 for (i = 0; i < separatedSetup.size(); ++i)
00195                 {
00196                         if((i+1)%2 )
00197                         {
00198                                 if(separatedSetup[i].size()!=1)
00199                                 {
00200                                         ROS_ERROR("Faulty setup string part, expected 'O','G','Y' or 'R' got %s",separatedSetup[i].c_str());
00201                                         exit(1);
00202                                 }
00203                                 else
00204                                 {
00205                                         switch(separatedSetup[i][0])
00206                                         {
00207                                         case 'O':
00208                                                 pair.first=1;
00209                                                 break;
00210 
00211                                         case 'G':
00212                                                 pair.first=2;
00213                                                 break;
00214 
00215                                         case 'Y':
00216                                                 pair.first=3;
00217                                                 break;
00218 
00219                                         case 'R':
00220                                                 pair.first=4;
00221                                                 break;
00222 
00223                                         default:
00224                                                 ROS_ERROR("Faulty setup string part, expected 'O','G','Y' or 'R' got %s",separatedSetup[i].c_str());
00225                                                 exit(1);
00226                                         }
00227                                 }
00228                         }
00229                         else
00230                         {
00231                                 std::istringstream ss(separatedSetup[i]);
00232                                 T value;
00233                                 ss>>value;
00234                                 //std::cout<<"::"<<value<<"\n";
00235                                 if(ss.bad() || ss.fail() || !ss.eof())
00236                                 {
00237                                         ROS_ERROR("Error in setup string: %s",separatedSetup[i].c_str());
00238                                         exit(1);
00239                                 }
00240                                 pair.second=value;
00241                                 this->ranges.insert(pair);
00242                         }
00243                 }
00244 
00245                 if(!i%2 || i==0)
00246                         ROS_ERROR("Error in setup string, missing led setting for upper end!");
00247                 //Get last one
00248                 pair.second=std::numeric_limits<T>::max();
00249                 this->ranges.insert(pair);
00250 
00251                 x52_joyext::x52_led_color ledmsg;
00252                 if(led<0 || led>(int)ledmsg.color_leds.size())
00253                 {
00254                         ROS_ERROR("LED number out of range!");
00255                         exit(1);
00256                 }
00257                 else
00258                 {
00259                         this->led=led;
00260                 }
00261                 pub=n->advertise< x52_joyext::x52_led_color >("led",1);
00262         }
00263         ~PublishLED()
00264         {}
00265 
00266         void progressValue(T value)
00267         {
00268                 for (typename std::set<PairSortMod>::iterator it = ranges.begin(); it != ranges.end(); ++it)
00269                 {
00270                         if(it->second>value)
00271                         {
00272                                 x52_joyext::x52_led_color msg;
00273                                 msg.color_leds[this->led]=it->first;
00274                                 pub.publish(msg);
00275                                 break;
00276                         }
00277                 }
00278         }
00279 
00280         void JoyCallback(const sensor_msgs::JoyConstPtr &msg)
00281         {
00282                 if(axis_or_button==false)//Axis
00283                 {
00284                         if(axis_button<(int)msg->axes.size() && axis_button>=0)
00285                         {
00286                                 progressValue((T)msg->axes[axis_button]);
00287                         }
00288                         else
00289                         {
00290                                 ROS_ERROR("Axis %i not available for that Joystick!", axis_button);
00291                         }
00292                 }
00293                 else
00294                 {
00295                         if(axis_button<(int)msg->buttons.size() && axis_button>=0)
00296                         {
00297                                 progressValue((T)msg->buttons[axis_button]);
00298                         }
00299                         else
00300                         {
00301                                 ROS_ERROR("Button %i not available for that Joystick!", axis_button);
00302                         }
00303                 }
00304         }
00305 
00306         template <class MSGPTR>
00307         void Callback(const MSGPTR& msg)
00308         {
00309                 progressValue(msg->data);
00310         }
00311 
00312 
00313 
00314         template <class MSG, class MSGPTR>
00315         void start()
00316         {
00317                 sub=n->subscribe<MSG>("in",1000, &PublishLED<T>::Callback< MSGPTR >, this);
00318                 ros::spin();
00319         }
00320 
00321         void startJoy()
00322         {
00323                 sub=n->subscribe<sensor_msgs::Joy>("in",1000, &PublishLED<T>::JoyCallback, this);
00324                 ros::spin();
00325         }
00326 
00327 };
00328 
00329 
00330 
00331 //Off, Green, Yellow, Red
00332 
00333 int main(int argc, char **argv)
00334 {
00335         ros::init(argc, argv, "value2buttonColor");
00336         ros::NodeHandle n("~");
00337 
00338         int type;
00339         int led;
00340         bool axis_or_button;
00341         int axis_button;
00342         std::string setup;
00343 
00344 
00345         n.param<int>("input_type", type, INPUT_FLOAT64);
00346         n.param<int>("joy_axis_button", axis_button, 0);
00347         n.param<bool>("joy_axis_or_button", axis_or_button, 0);
00348         n.param<int>("color_led",led, 0);
00349         ROS_DEBUG("LED -> %i",led);
00350 
00351 
00352         //means OFF till -0.5, GREEN till 0, YELLOW till 0.5, and everything greater is RED
00353         n.param<std::string>("setup_string", setup, "O|-0.5|G|0|Y|0.5|R");
00354 
00355 
00356 
00357 #define casem(CASE,TYPE,TYPEROS)\
00358         case CASE:\
00359         {\
00360                 PublishLED< TYPE > obj(&n,setup,led,axis_button,axis_or_button);\
00361                 obj.start<std_msgs::TYPEROS, std_msgs:: TYPEROS## ConstPtr>();\
00362                 break;\
00363         }\
00364 
00365 
00366 
00367         switch(type)
00368         {
00369         casem(INPUT_FLOAT64, double_t, Float64)
00370         casem(INPUT_FLOAT32, float_t, Float32)
00371         casem(INPUT_INT64, int64_t, Int64)
00372         casem(INPUT_INT32, int32_t, Int32)
00373         casem(INPUT_INT16, int16_t, Int16)
00374         casem(INPUT_INT8, int8_t, Int8)
00375         casem(INPUT_UINT64, uint64_t, UInt64)
00376         casem(INPUT_UINT32, uint32_t, UInt32)
00377         casem(INPUT_UINT16, uint16_t, UInt16)
00378         casem(INPUT_UINT8, uint8_t, UInt8)
00379         casem(INPUT_BOOL, bool, Bool)
00380                 case INPUT_JOY:
00381                 {
00382                         PublishLED< double > obj(&n,setup,led,axis_button, axis_or_button);
00383                         obj.startJoy();
00384                         break;
00385                 }
00386                 default:
00387                         ROS_ERROR("Unkown Input Type!");
00388                         break;
00389         }
00390 }


x52_joyext
Author(s): Christian Holl
autogenerated on Mon Oct 6 2014 09:02:17