00001 /* 00002 * x52_joyext Node 00003 * by Christian Holl (www.rad-lab.net) 00004 * License: BSD 00005 * 00006 * Have Fun! :-) 00007 */ 00008 00009 /* 00010 * Modified by Murilo FM (muhrix@gmail.com) 00011 * 12 Dec 2013 00012 * 00013 */ 00014 00026 #include <ros/ros.h> 00027 00028 #include <std_msgs/UInt8.h> 00029 #include <boost/thread/mutex.hpp> 00030 #include <string> 00031 #include <inttypes.h> 00032 #include "x52_joyext/x52_date.h" 00033 #include "x52_joyext/x52_time.h" 00034 #include "x52_joyext/x52_led_color.h" 00035 #include "x52_joyext/x52_mfd.h" 00036 00037 extern "C" { 00038 #include <x52pro.h> 00039 } 00040 00041 #ifndef X52JOYEXT_HPP_ 00042 #define X52JOYEXT_HPP_ 00043 00044 class X52_JoyExt 00045 { 00046 ros::NodeHandle &nh; 00047 ros::Rate *loop_rate; 00048 00049 00050 bool updateLED[10]; 00051 bool updateLED_b; 00052 bool updateMFD[3]; 00053 bool updateMFD_b; 00054 bool updateDate; 00055 bool updateTime; 00056 bool updateOffset[2]; 00057 bool updateBrightnessMFD; 00058 bool updateBrightnessLED; 00059 00060 00061 uint8_t LED[19]; 00062 std::string mfd_content[3]; 00063 uint8_t Date[3]; 00064 uint8_t Time_24; 00065 uint8_t Time[2]; 00066 uint8_t Offset[2]; 00067 uint8_t Offset_24[2]; 00068 uint8_t Offset_Inv[2]; 00069 uint8_t brightnessMFD; 00070 uint8_t brightnessLED; 00071 00072 00073 ros::Subscriber subleds; 00074 ros::Subscriber submfd_text; 00075 ros::Subscriber subdate; 00076 ros::Subscriber subtime; 00077 ros::Subscriber subbrightnessMFD; 00078 ros::Subscriber subbrightnessLED; 00079 00080 void setLEDs(uint8_t inValue, uint8_t *red, uint8_t *green, bool *update) 00081 { 00082 switch(inValue) 00083 { 00084 case x52_joyext::x52_led_color::NO_STATUS_CHANGE: 00085 *update=false; 00086 break; 00087 case x52_joyext::x52_led_color::OFF: 00088 *red=0; 00089 *green=0; 00090 break; 00091 case x52_joyext::x52_led_color::RED: 00092 *red=1; 00093 *green=0; 00094 break; 00095 case x52_joyext::x52_led_color::GREEN: 00096 *red=0; 00097 *green=1; 00098 break; 00099 case x52_joyext::x52_led_color::YELLOW: 00100 *red=1; 00101 *green=1; 00102 break; 00103 default: 00104 ROS_WARN("WRONG VALUE (%i) FOR LED FOUND! Value must be in the range of 0-4",inValue); 00105 return; 00106 break; 00107 } 00108 *update=true; 00109 } 00110 00111 void cb_leds(const x52_joyext::x52_led_colorConstPtr &msg); 00112 void cb_mfd_text(const x52_joyext::x52_mfdConstPtr &msg); 00113 void cb_date(const x52_joyext::x52_dateConstPtr &msg); 00114 void cb_time(const x52_joyext::x52_timeConstPtr &msg); 00115 void cb_brighnessMFD(const std_msgs::UInt8ConstPtr &msg); 00116 void cb_brighnessLED(const std_msgs::UInt8ConstPtr &msg); 00117 00118 00119 public: 00120 X52_JoyExt(ros::NodeHandle &n); 00121 virtual ~X52_JoyExt(); 00122 00123 void send_to_joystick(); 00124 }; 00125 00126 #endif /* X52JOYEXT_HPP_ */