darwin_teleop_alg_node.cpp
Go to the documentation of this file.
00001 #include "darwin_teleop_alg_node.h"
00002 
00003 #include <wiimote/State.h>
00004 
00005 DarwinTeleopAlgNode::DarwinTeleopAlgNode(void) :
00006   algorithm_base::IriBaseAlgorithm<DarwinTeleopAlgorithm>()
00007 {
00008   //init class attributes if necessary
00009   //this->loop_rate_ = 2;//in [Hz]
00010 
00011   // [init publishers]
00012   this->execute_action_publisher_ = this->public_node_handle_.advertise<std_msgs::Int32>("execute_action", 100);
00013   
00014   // [init subscribers]
00015   this->joy_subscriber_ = this->public_node_handle_.subscribe("joy", 100, &DarwinTeleopAlgNode::joy_callback, this);
00016   
00017   // [init services]
00018   
00019   // [init clients]
00020   
00021   // [init action servers]
00022   
00023   // [init action clients]
00024 
00025   this->button_1_action=-1;
00026   this->button_2_action=-1;
00027   this->button_B_action=-1;
00028   this->button_plus_action=-1;
00029   this->button_minus_action=-1;
00030   this->button_home_action=-1; 
00031 }
00032 
00033 DarwinTeleopAlgNode::~DarwinTeleopAlgNode(void)
00034 {
00035   // [free dynamic memory]
00036 }
00037 
00038 void DarwinTeleopAlgNode::mainNodeThread(void)
00039 {
00040   // [fill msg structures]
00041   //this->Int32_msg_.data = my_var;
00042   
00043   // [fill srv structure and make request to the server]
00044   
00045   // [fill action structure and make request to the action server]
00046 
00047   // [publish messages]
00048 }
00049 
00050 /*  [subscriber callbacks] */
00051 void DarwinTeleopAlgNode::joy_callback(const sensor_msgs::Joy::ConstPtr& msg) 
00052 { 
00053   std::vector<unsigned int> buttons_index;
00054   bool buttons_mov = this->alg_.check_pressed_button_callback(msg->buttons, buttons_index);
00055 
00056   // BUTTONS
00057   if (buttons_mov)
00058   {
00059     if( !this->alg_.compareIndexVectors(buttons_index, prev_buttons_) )
00060       for(unsigned int i=0; i<buttons_index.size(); i++)
00061       {
00062         switch(buttons_index[i])
00063         {
00064           case wiimote::State::MSG_BTN_1:
00065             ROS_DEBUG("Button 1!");
00066             if(this->button_1_action!=-1)
00067             {
00068               this->Int32_msg_.data=this->button_1_action;
00069               this->execute_action_publisher_.publish(this->Int32_msg_);
00070             }
00071             break;
00072           case wiimote::State::MSG_BTN_2:
00073             ROS_DEBUG("Button 2!");
00074             if(this->button_2_action!=-1)
00075             {
00076               this->Int32_msg_.data=this->button_2_action;
00077               this->execute_action_publisher_.publish(this->Int32_msg_);
00078             }
00079             break;
00080           case wiimote::State::MSG_BTN_B:
00081             ROS_DEBUG("Button B!");
00082             if(this->button_B_action!=-1)
00083             {
00084               this->Int32_msg_.data=this->button_B_action;
00085               this->execute_action_publisher_.publish(this->Int32_msg_);
00086             }
00087             break;
00088           case wiimote::State::MSG_BTN_PLUS:
00089             ROS_DEBUG("Button (+)!");
00090             if(this->button_plus_action!=-1)
00091             {
00092               this->Int32_msg_.data=this->button_plus_action;
00093               this->execute_action_publisher_.publish(this->Int32_msg_);
00094             }
00095             break;
00096           case wiimote::State::MSG_BTN_MINUS:
00097             ROS_DEBUG("Button (-)!");
00098             if(this->button_minus_action!=-1)
00099             {
00100               this->Int32_msg_.data=this->button_minus_action;
00101               this->execute_action_publisher_.publish(this->Int32_msg_);
00102             }
00103             break;
00104           case wiimote::State::MSG_BTN_HOME:
00105             ROS_DEBUG("Button Home!");
00106             if(this->button_home_action!=-1)
00107             {
00108               this->Int32_msg_.data=this->button_home_action;
00109               this->execute_action_publisher_.publish(this->Int32_msg_);
00110             }
00111             break;
00112         }
00113       }
00114 
00115     prev_buttons_ = buttons_index;
00116 
00117   }
00118   else
00119     prev_buttons_.clear();
00120 }
00121 
00122 /*  [service callbacks] */
00123 
00124 /*  [action callbacks] */
00125 
00126 /*  [action requests] */
00127 
00128 void DarwinTeleopAlgNode::node_config_update(Config &config, uint32_t level)
00129 {
00130   this->alg_.lock();
00131   this->button_1_action=config.button_1_action;
00132   this->button_2_action=config.button_2_action;
00133   this->button_B_action=config.button_B_action;
00134   this->button_plus_action=config.button_plus_action;
00135   this->button_minus_action=config.button_minus_action;
00136   this->button_home_action=config.button_home_action;
00137   this->alg_.unlock();
00138 }
00139 
00140 void DarwinTeleopAlgNode::addNodeDiagnostics(void)
00141 {
00142 }
00143 
00144 /* main function */
00145 int main(int argc,char *argv[])
00146 {
00147   return algorithm_base::main<DarwinTeleopAlgNode>(argc, argv, "darwin_teleop_alg_node");
00148 }


darwin_teleop
Author(s): Sergi Hernandez Juan
autogenerated on Fri Dec 6 2013 20:22:28