mfd_writer.cpp
Go to the documentation of this file.
00001 /*
00002  * Mfd Writer Node
00003  * by Christian Holl (www.rad-lab.net)
00004  * License: BSD
00005  *
00006  * Have Fun! :-)
00007  */
00008 
00143 #include <ros/ros.h>
00144 #include <string>
00145 #include <vector>
00146 #include <set>
00147 #include <limits>
00148 #include <iostream>
00149 #include <sstream>
00150 #include <std_msgs/Float32.h>
00151 #include <std_msgs/Float64.h>
00152 #include <std_msgs/Int8.h>
00153 #include <std_msgs/Int16.h>
00154 #include <std_msgs/Int32.h>
00155 #include <std_msgs/Int64.h>
00156 #include <std_msgs/UInt8.h>
00157 #include <std_msgs/UInt16.h>
00158 #include <std_msgs/UInt32.h>
00159 #include <std_msgs/UInt64.h>
00160 
00161 
00162 
00163 #include <std_msgs/Bool.h>
00164 #include <boost/algorithm/string.hpp>
00165 #include <sensor_msgs/Joy.h>
00166 #include <x52_joyext/x52_mfd.h>
00167 
00168 enum
00169 {
00170         INPUT_FLOAT32 = 0,
00171         INPUT_FLOAT64,
00172         INPUT_INT8,
00173         INPUT_INT16,
00174         INPUT_INT32,
00175         INPUT_INT64,
00176         INPUT_UINT8,
00177         INPUT_UINT16,
00178         INPUT_UINT32,
00179         INPUT_UINT64,
00180         INPUT_BOOL,
00181         INPUT_JOY,
00182 };
00183 
00184 template<typename T>
00185 class PublishMFD
00186 {
00187         enum
00188         {
00189                 LEFT, CENTER, RIGHT,
00190         };
00191 
00192 private:
00193         class PairSortMod: public std::pair<std::string, T>
00194         {
00195         public:
00196                 bool operator<(const PairSortMod& other) const
00197                 {
00198                         return this->second < other.second;
00199                 }
00200 
00201         };
00202         std::set<PairSortMod> ranges;
00203         ros::NodeHandle *n;     
00204         ros::Publisher pub; 
00205         ros::Subscriber sub; 
00206 
00207         int line;       
00208         int pos;        
00209         int field_length; 
00210         int align; 
00211         std::string positive_oversize; 
00212         std::string negative_oversize; 
00213         bool axis_or_button; 
00214         int axis_button; 
00215         bool stringprint; 
00216         std::string stringprint_setup; 
00217         int char_as_int; 
00218 
00219 public:
00220         PublishMFD(ros::NodeHandle *n) :
00221                         n(n)
00222         {
00223                 //Get the parameters
00224                 n->param<int>("line", line, 0);
00225                 n->param<int>("pos", pos, 0);
00226                 n->param<int>("field_length", field_length, 16);
00227                 n->param<int>("align", align, 0);
00228                 n->param<std::string>("positive_oversize", positive_oversize, "#");
00229                 n->param<std::string>("negative_oversize", negative_oversize, "#");
00230                 n->param<bool>("axis_or_button", axis_or_button, 0);
00231                 n->param<int>("axis_button", axis_button, 0);
00232                 n->param<bool>("stringprint", stringprint, 0);
00233                 n->param<std::string>("stringprint_setup", stringprint_setup, "");
00234                 n->param<int>("char_as_int", char_as_int, 0);
00235 
00236                 if(char_as_int!=0)
00237                         ROS_WARN("%s: char_as_int = %i (assuming 0 -> interpred as char)",ros::this_node::getName().c_str(), char_as_int);
00238 
00239 
00240                 if (field_length + pos > 16)
00241                 {
00242                         ROS_ERROR("%s: Field does not fit into display, will be cut!",
00243                                         ros::this_node::getName().c_str());
00244                         field_length -= (field_length + pos) - 16;
00245                 }
00246 
00247                 if (field_length == 0)
00248                 {
00249                         ROS_ERROR("%s: Field length is zero!",
00250                                         ros::this_node::getName().c_str());
00251                 }
00252 
00253 
00254                 if (stringprint)
00255                 {
00256                         std::vector<std::string> separatedSetup;
00257                         boost::split(separatedSetup, stringprint_setup,
00258                                         boost::is_any_of("|"));
00259                         ROS_INFO("%s",stringprint_setup.c_str());
00260                         PairSortMod pair;
00261                         unsigned int i;
00262 
00263                         for (i = 0; i < separatedSetup.size(); ++i)
00264                         {
00265                                 if ((i + 1) % 2)
00266                                 {
00267                                         if (separatedSetup[i].size() > field_length)
00268                                         {
00269                                                 ROS_ERROR("%s: String: %s too big for field!",
00270                                                                 ros::this_node::getName().c_str(),
00271                                                                 separatedSetup[i].c_str());
00272                                                 separatedSetup[i].erase(field_length,
00273                                                                 separatedSetup[i].size() - field_length);
00274                                         }
00275                                         pair.first = separatedSetup[i];
00276                                         ROS_INFO("%s",separatedSetup[i].c_str());
00277                                 }
00278                                 else
00279                                 {
00280                                         std::istringstream ss(separatedSetup[i]);
00281                                         T value;
00282                                         ss>>value;
00283                                         if(ss.bad() || ss.fail() || !ss.eof())
00284                                         {
00285                                                 ROS_ERROR("Error in setup string: %s",separatedSetup[i].c_str());
00286                                                 exit(1);
00287                                         }
00288                                         pair.second=value;
00289                                         this->ranges.insert(pair);
00290                                 }
00291                         }
00292 
00293                         if (!i % 2 || i == 0)
00294                                 ROS_ERROR(
00295                                                 "Error in setup string, missing string for upper end!");
00296                         //Get last one
00297                         pair.second = std::numeric_limits<T>::max();
00298                         this->ranges.insert(pair);
00299 
00300                 }
00301 
00302                 pub = n->advertise<x52_joyext::x52_mfd>("mfd_text", 1,1);
00303                 x52_joyext::x52_mfd init_msg;
00304                 init_msg.clearDisplay=false;
00305                 init_msg.line=line;
00306                 init_msg.pos=pos;
00307                 for (int i = 0; i < field_length; ++i) {
00308                 init_msg.data+="-";
00309                 }
00310                 pub.publish(init_msg);
00311         }
00312         ~PublishMFD()
00313         {
00314         }
00315 
00316 
00317         template <typename V>
00318         std::string get_value_string(V value)
00319         {
00320 
00321                 x52_joyext::x52_mfd msg;
00322                 std::ostringstream ss;
00323                 std::string value_string;
00324                 ss<<value;
00325 
00326 
00327                 return ss.str();
00328 
00329         }
00330 
00331         std::string get_value_string(char value)
00332         {
00333                 x52_joyext::x52_mfd msg;
00334                 std::ostringstream ss;
00335                 std::string value_string;
00336 
00337                 //std::cout<<"::"<<value<<"\n";
00338                 {
00339                         ROS_ERROR("%s: Error in creating (char) value string",ros::this_node::getName().c_str());
00340 
00341                         ss.clear();
00342 
00343                         for(int c = 0; c < field_length; ++c)
00344                         {
00345                                 ss<<'E';
00346                         }
00347                 }
00348 
00349                 if(char_as_int==1)
00350                 {
00351                         if(value!=0)
00352                         ss<<(signed int)value;
00353                 }
00354                 else if(char_as_int==2)
00355                 {
00356                         ss<<(unsigned int)value;
00357                 }
00358                 else
00359                 {
00360                         ss<<value;
00361                 }
00362 
00363                 return ss.str();
00364         }
00365 
00366 
00367 
00368 
00369 
00370         template <typename V>
00371         void progressValue(V value)
00372         {
00373                 x52_joyext::x52_mfd msg;
00374                 msg.clearDisplay = false;
00375                 if (stringprint)
00376                 {
00377                         //Get the right value
00378                         for (typename std::set<PairSortMod>::iterator it = ranges.begin();
00379                                         it != ranges.end(); ++it)
00380                         {
00381                                 if (it->second > value)
00382                                 {
00383                                         msg.pos = pos;
00384                                         msg.line = line;
00385                                         msg.data = it->first;
00386                                         break;
00387                                 }
00388                         }
00389                 }
00390                 else
00391                 {
00392                         msg.clearDisplay = false;
00393                         msg.pos = pos;
00394                         msg.line = line;
00395                         msg.data=get_value_string(value);
00396 
00397                 }
00398 
00399 
00400 
00401 
00402                 if (msg.data.length() < field_length)
00403                 {
00404                         switch (align)
00405                         {
00406                         default:
00407                         case LEFT: //filling up the field to overwrite chars not needed
00408                                 for (int s = 0; s < (msg.data.length() - field_length); ++s)
00409                                 {
00410                                         msg.data.push_back(' ');
00411                                 }
00412                                 break;
00413 
00414                         case CENTER: //shift string the size difference devided by two to the right
00415                                 for (int s = 0; s < ((msg.data.length() - field_length) / 2); ++s)
00416                                 {
00417                                         if(s%2)
00418                                         msg.data.insert(0, " ");
00419                                         else
00420                                         msg.data.push_back(' ');
00421                                 }
00422                                 ;
00423                                 break;
00424 
00425                         case RIGHT: //shift string the size difference to the right
00426                                 for (int s = 0; s < (msg.data.length() - field_length); ++s)
00427                                 {
00428                                         msg.data.insert(0, " ");
00429                                 }
00430                                 ;
00431                                 break;
00432                         }
00433                 }
00434                 else if(msg.data.length()>field_length)
00435                 {
00436                         //Decimal point position
00437                         int dec_pos=msg.data.find('.');
00438 
00439                         std::cout<<msg.data<<std::endl;
00440                         if(dec_pos >= 0 && dec_pos==field_length-1) //decimal is the last what fits into field
00441                         {
00442                                 msg.data.erase(dec_pos,field_length-dec_pos);//erase everything after dec_pos including dec_pos
00443                         }
00444                         else if(dec_pos >= 0 && dec_pos<field_length-1)//decimal point is not the last what fits into field
00445                         {
00446                                 msg.data.erase(field_length-1,msg.data.length()-field_length);//cut off everything whats longer than field
00447                         }
00448                         else //The string is over size for that field!
00449                         {
00450                                 if(value>0)
00451                                 {
00452                                         msg.data=positive_oversize;
00453                                 }
00454                                 else
00455                                 {
00456                                         msg.data=negative_oversize;
00457                                 }
00458                         }
00459                 }
00460                 pub.publish(msg);
00461         }
00462 
00463         void JoyCallback(const sensor_msgs::JoyConstPtr &msg)
00464         {
00465                 if (axis_or_button == false) //Axis
00466                 {
00467                         if (axis_button < (int) msg->axes.size() && axis_button >= 0)
00468                         {
00469                                 progressValue<double>(msg->axes[axis_button]);
00470                         }
00471                         else
00472                         {
00473                                 ROS_ERROR("Axis %i not available for that Joystick!",
00474                                                 axis_button);
00475                         }
00476                 }
00477                 else
00478                 {
00479                         if (axis_button < (int) msg->buttons.size() && axis_button >= 0)
00480                         {
00481                                 progressValue<bool>(msg->buttons[axis_button]);
00482                         }
00483                         else
00484                         {
00485                                 ROS_ERROR("Button %i not available for that Joystick!",
00486                                                 axis_button);
00487                         }
00488                 }
00489         }
00490 
00491         template<class MSGPTR>
00492         void Callback(const MSGPTR& msg)
00493         {
00494                 progressValue(msg->data);
00495         }
00496 
00497         template<class MSG, class MSGPTR>
00498         void start()
00499         {
00500                 sub = n->subscribe<MSG>("in", 1000, &PublishMFD<T>::Callback<MSGPTR>,
00501                                 this);
00502                 ros::spin();
00503         }
00504 
00505         void startJoy()
00506         {
00507                 sub = n->subscribe<sensor_msgs::Joy>("in", 1000,
00508                                 &PublishMFD<T>::JoyCallback, this);
00509                 ros::spin();
00510         }
00511 
00512 };
00513 
00520 int main(int argc, char **argv)
00521 {
00522         ros::init(argc, argv, "mfd_writer");
00523         ros::NodeHandle n("~");
00524 
00525 #define casem(CASE,TYPE,TYPEROS)\
00526         case CASE:\
00527         {\
00528                 PublishMFD< TYPE > obj(&n);\
00529                 obj.start<std_msgs::TYPEROS, std_msgs:: TYPEROS## ConstPtr>();\
00530                 break;\
00531         }\
00532 
00533         int type=0;     
00534         n.param<int>("input_type", type, INPUT_FLOAT64);
00535         switch (type)
00536         {
00537         casem(INPUT_FLOAT64, double_t, Float64)
00538         casem(INPUT_FLOAT32, float_t, Float32)
00539         casem(INPUT_INT64, int64_t, Int64)
00540         casem(INPUT_INT32, int32_t, Int32)
00541         casem(INPUT_INT16, int16_t, Int16)
00542         casem(INPUT_INT8, int8_t, Int8)
00543         casem(INPUT_UINT64, uint64_t, UInt64)
00544         casem(INPUT_UINT32, uint32_t, UInt32)
00545         casem(INPUT_UINT16, uint16_t, UInt16)
00546         casem(INPUT_UINT8, uint8_t, UInt8)
00547         casem(INPUT_BOOL, bool, Bool)
00548         case INPUT_JOY:
00549         {
00550                 PublishMFD<double> obj(&n);
00551                 obj.startJoy();
00552                 break;
00553         }
00554         default:
00555                 ROS_ERROR("Unkown Input Type %i!",type);
00556                 break;
00557         }
00558 
00559         return 0;
00560 }


x52_joyext
Author(s): Christian Holl
autogenerated on Mon Mar 2 2015 18:28:36