1 #include "nodeTemplate.h"
   2 
   3 nodeTemplate::nodeTemplate(wxTextCtrl *TextBox)// ros::Node("nodeTemplate")
   4 {
   5    m_pTextBox = TextBox;
   6    initNode();
   7    m_pNode = new ros::Node("templateNode");
   8 
   9       //put node-specific subscriptions, advertising, and services here
  10 
  11    m_SubscribeMonitorNodeBus = _T("monitor_node_bus");
  12    m_pNode->subscribe(std::string(m_SubscribeMonitorNodeBus.mb_str()),monitorNodeBus, &nodeTemplate::monitorNodeBusCallback, this, 100);
  13 
  14    m_AdvertiseNodeBusOut = _T("node_bus_out");
  15    m_pNode->advertise<wxWidgetsNodeTemplate::ROS_Node_Template>(std::string(m_AdvertiseNodeBusOut.mb_str()), 100);
  16 
  17    m_AdvertiseServiceDataBus = _T("node_service_bus");
  18    m_pNode->advertiseService("node_service_bus", &nodeTemplate::serviceTemplateCallback, this);
  19 
  20    m_pTextBox->AppendText(_T("node_template is running."));
  21 }
  22 
  23 nodeTemplate::~nodeTemplate()
  24 {
  25    m_pNode->shutdown(); // exit cleanly from the master
  26    delete m_pNode;
  27 }
  28 
  29 void nodeTemplate::initNode()
  30 {
  31  	char fileName[20] = "ROS_Node_Template";
  32  	char *pArgumentList[4];
  33 	pArgumentList[0] = fileName;	//argv[0] is the filename.
  34 	pArgumentList[1] = NULL;	// argv[argc] is a null pointer.
  35 	int i = 1;
  36 	ros::init(i,pArgumentList);	// this is typcially called from within a main(int argc, char **argv) routine, so we have to send the correct arguments
  37                                  // to do: pick up the remappings from the command line and use them here
  38 
  39 
  40    // this is an alternative method of calling ros::init, but it does not seem to work
  41    //ros::VP_string test; // compiler says VP_string is not a member of ros, but documentation says it is the following typedef
  42    //std::vector<std::pair<std::string, std::string> > test;
  43 	//test[0].first = "nodeTemplate";
  44 	//test[0].second = "nodeTemplate";
  45 	//ros::init(test);   // init refuses this datatype
  46 }
  47 
  48 
  49 void nodeTemplate::Publish()
  50 {
  51    wxString tempString = _T("\nPublished on ");
  52    tempString << m_AdvertiseNodeBusOut << _T("\n");
  53    m_pTextBox->AppendText(tempString);
  54 
  55    wxString messageOut;
  56    uint8_t Value8;
  57    uint16_t Value16 ;
  58    uint32_t Value32 ;
  59    uint64_t Value64 ;
  60 
  61    getValuesNodeBusOut( &messageOut, &Value8, &Value16, &Value32, &Value64);
  62 
  63    nodeBusOut.Test_msg = std::string(messageOut.mb_str());
  64    nodeBusOut.Test_uint8 = Value8;
  65    nodeBusOut.Test_uint16 = Value16;
  66    nodeBusOut.Test_uint32 = Value32;
  67    nodeBusOut.Test_uint64 = Value64;
  68 
  69    m_pNode->publish(std::string(m_AdvertiseNodeBusOut.mb_str()), nodeBusOut);
  70 
  71 }
  72 
  73 void nodeTemplate::monitorNodeBusCallback()
  74 {
  75    wxString messageOrigin = _T("\n Received on ");
  76    messageOrigin << m_SubscribeMonitorNodeBus;
  77    messageOrigin << _T(": \n DataType = ");
  78    std::string stlstring = monitorNodeBus.__s_getDataType();
  79    wxString dataType(stlstring.c_str(), wxConvUTF8); // where the message is from-- we could use this to identify how to decode it, but it is easier just
  80                            // to set up separate callbacks for each subscription.  This is really useful, however if we want to set the subscription up at runtime
  81    messageOrigin<< dataType;
  82 
  83 
  84    stlstring = monitorNodeBus.Test_msg;
  85    wxString messageIn(stlstring.c_str(), wxConvUTF8);
  86    uint8_t Value8 = monitorNodeBus.Test_uint8;
  87    int16_t Value16 = monitorNodeBus.Test_uint16;
  88    int32_t Value32 = monitorNodeBus.Test_uint32;
  89    int64_t Value64 = monitorNodeBus.Test_uint64;
  90 
  91    setValuesMonitorNodeBus(messageIn, Value8, Value16, Value32, Value64);
  92    messageOrigin << _T("\n Values = ") << m_Value8 <<  _T(", ") << m_Value16 << _T(", ") << m_Value32 <<  _T(", ") << m_Value64 << _T("\n");
  93    m_pTextBox->AppendText(messageOrigin);
  94 }
  95 
  96 
  97 bool nodeTemplate::serviceTemplateCallback(serverNode::nodeTemplateServer::Request  &req,
  98            serverNode::nodeTemplateServer::Response &res )
  99   {
 100     wxString tempString = _T("\nService called: \n Message is ");
 101     int8_t test8;
 102     int16_t test16;
 103     test8 = req.testIn8;
 104     test16 = req.testIn16;
 105     std::string stlstring = req.testMessageIn;
 106     wxString Message(stlstring.c_str(), wxConvUTF8);
 107 
 108     stlstring = "Success";
 109     res.testOut8 = 8;
 110     res.testOut32 = 32;
 111     res.testMessageOut = stlstring;
 112 
 113     tempString << Message;
 114     m_pTextBox->AppendText(tempString);
 115 
 116     return true;
 117   }
 118 
 119 // node specific routines
 120 void nodeTemplate::getValuesNodeBusOut( wxString *pMessageOut, uint8_t *pValue8, uint16_t *pValue16, uint32_t *pValue32, uint64_t *pValue64)
 121 {
 122 
 123    *pMessageOut = _T("\n Hello World");
 124    *pValue8 = 2;
 125    *pValue16 = 3;
 126    *pValue32 = 5;
 127    *pValue64 = 7;
 128 }
 129 
 130 
 131 void nodeTemplate::setValuesMonitorNodeBus( wxString messageIn, uint8_t Value8, uint16_t Value16, uint32_t Value32, uint64_t Value64)
 132 {
 133 
 134    m_MessageIn = messageIn;
 135    m_Value8 = Value8;
 136    m_Value16 = Value16;
 137    m_Value32 = Value32;
 138    m_Value64 = Value64;
 139 }