david.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009 Andreas Holzbach <holzbach -=- cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027 */
00028 
00041 //ROS core
00042 #include "ros/ros.h"
00043 
00044 #include <iostream>
00045 #include <stdio.h>
00046 #include <stdlib.h>
00047 #include <string.h>
00048 #include <unistd.h>
00049 #include <fstream>
00050 #include <cstdlib>
00051 
00052 //TCP/IP and Socket
00053 #include <sys/socket.h>
00054 #include <arpa/inet.h>
00055 #include <netinet/in.h>
00056 
00057 //ROS message
00058 #include <perception_srvs/David.h>
00059 
00060 #define BUFFSIZE 32
00061 
00062 using namespace std;
00063 using namespace perception_srvs;
00064 
00065 class DavidService
00066 {
00067 
00068 private:
00069   ros::NodeHandle node_handle_;
00070   ros::ServiceServer service_; 
00071 
00072   //Variables for IP Connection
00073   int sock; //<0 if connection fails
00074   //char* ip; //IP address of server (char*)
00075   struct sockaddr_in server;
00076   char buffer[BUFFSIZE];
00077   unsigned int msglen; //packet message length for TCP connection
00078   std::string david_ip;  //IP address of server (string)
00079   //unsigned int port; //Port number of server (unsigned int)
00080   std::string port_string; //Port number of server (string)
00081   std::string argument; //Argument to be executed by server
00082 
00083 
00084   bool debug_out;
00085 
00087   public:
00088     DavidService(const ros::NodeHandle& node_handle) : node_handle_(node_handle)
00089     {
00090       if ((sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0) 
00091       {
00092         ROS_WARN("Failed to create socket");
00093       }
00094     }
00095 
00098   void init()
00099   {     
00100     debug_out = true;   
00101     node_handle_.param("~ip",david_ip,string("127.0.0.1"));
00102     node_handle_.param("~port",port_string,string("19919"));
00103     service_ = node_handle_.advertiseService("david", &DavidService::david, this);      
00104     ROS_INFO("DAVID service has been initialized");     
00105   }
00106   
00109   void start()
00110   {
00111     char *test = "start\n";
00112     msglen = strlen(test);
00113 
00114     if (send(sock, test, msglen, 0) != msglen) 
00115     {
00116       ROS_INFO("Mismatch in number of sent bytes");
00117     }
00118       if (debug_out)
00119         ROS_INFO("DAVID method: start");
00120   }
00121 
00124   void stop()
00125   {
00126     char *test = "stop\n";
00127     msglen = strlen(test);
00128 
00129     if (send(sock, test, msglen, 0) != msglen) 
00130     {
00131       ROS_INFO("Mismatch in number of sent bytes");
00132     }
00133     if (debug_out)
00134       ROS_INFO("DAVID mehtod: stop");
00135   }
00136 
00139   void grabTexture()
00140   {
00141     char *test = "grabTexture\n";
00142     msglen = strlen(test);
00143 
00144     if (send(sock, test, msglen, 0) != msglen) 
00145     {
00146       ROS_INFO("Mismatch in number of sent bytes");
00147     }
00148     if (debug_out)
00149       ROS_INFO("DAVID mehtod: grabTexture");
00150   }
00151 
00154   void save(std::string arg)
00155   {
00156     //std::string temp = "save";
00157     //temp.append(arg.str);
00158     //temp.append("\n");
00159     std::string temp = arg;
00160     temp.append("\n");
00161     const char *test = temp.c_str();
00162     msglen = strlen(test);
00163                 
00164     //char *test = "save\n";
00165     //msglen = strlen(test);
00166 
00167     if (send(sock, test, msglen, 0) != msglen) 
00168     {
00169       ROS_INFO("Mismatch in number of sent bytes");
00170     }
00171     if (debug_out)
00172       ROS_INFO("DAVID mehtod: save");
00173   }
00174 
00177   void erase()
00178   {
00179     char *test = "erase\n";
00180     msglen = strlen(test);
00181 
00182     if (send(sock, test, msglen, 0) != msglen) 
00183     {
00184       ROS_INFO("Mismatch in number of sent bytes");
00185     }
00186     if (debug_out)
00187       ROS_INFO("DAVID mehtod: erase");
00188   }
00189 
00192   void eraseTexture()
00193   {
00194     char *test = "eraseTexture\n";
00195     msglen = strlen(test);
00196 
00197     if (send(sock, test, msglen, 0) != msglen) 
00198     {
00199       ROS_INFO("Mismatch in number of sent bytes");
00200     }
00201     if (debug_out)
00202       ROS_INFO("DAVID mehtod: eraseTexture");
00203   }
00204 
00207   void connect2David()
00208   {             
00209     ROS_INFO("DAVID: Connecting to DAVID Server...");           
00210     //getIpPortFromFile("davidIP.conf");
00211     //std::string ipInfo = "David IP ";
00212     //cout << david_ip << ":" << port;          
00213     //david_ip = "127.0.0.1";
00214     //port = 19919;
00215     msglen = strlen(david_ip.c_str());
00216     /* Construct the server sockaddr_in structure */
00217     memset(&server, 0, sizeof(server));       /* Clear struct */
00218     server.sin_family = AF_INET;
00219     server.sin_addr.s_addr = inet_addr(david_ip.c_str());
00220     server.sin_port = htons(atoi(port_string.c_str()));
00221     //server.sin_port = htons(atoi("19919"));
00222 
00223     /* Establish connection */
00224     if (connect(sock,(struct sockaddr *) &server,
00225       sizeof(server)) < 0) 
00226     {
00227       ROS_INFO("Failed to connect with server");
00228     }
00229 
00230     ROS_INFO("DAVID: Connected to DAVID Server");
00231   }
00234         void disconnect()
00235         {
00236                 close(sock);
00237         }
00238         
00240         ~DavidService()
00241         {
00242                 close(sock);
00243         }
00244         
00246   /* Function that gets called by other services. The request argument is the 
00247       method to be called on the David server
00248   */
00249         bool david(David::Request &request, David::Response &resp)
00250         {
00251                 argument = request.david_method;
00252                 if (argument.find("connect")!=-1)
00253                         connect2David();
00254                 if (argument.find("start") != -1)
00255                         start();
00256                 if (argument.find("stop") != -1)
00257                         stop();
00258                 if (argument.find("erase") != -1)
00259                 {               
00260                     if (argument.find("eraseTexture") != -1)
00261                     {
00262                         eraseTexture();
00263                     }
00264                     else
00265                     {
00266                         erase();
00267                     }
00268                 }
00269                 if (argument.find("grabTexture") != -1)
00270                         grabTexture();
00271                 if (argument.find("save") != -1)
00272                         save(argument);         
00273                 return true;
00274         }
00275 
00276   /*
00278   void getIpPortFromFile(std::string fileName)
00279   {
00280     ifstream file;      
00281     file.open(fileName.c_str());
00282     if (!file)
00283     {
00284       ROS_INFO("Error getIpPortFromFile: no File");
00285     }
00286     std::string file_ip;
00287     std::string file_port;
00288     std::getline(file, file_ip);
00289     std::getline(file, file_port);
00290     david_ip = file_ip;
00291     port = atoi(file_port.c_str());             
00292   }
00293 */
00294 };
00295 
00296 
00298   int main(int argc, char **argv)
00299   {
00300     //ROS
00301     ros::init(argc, argv, "david");
00302     ros::NodeHandle n;  
00303     DavidService david(n);
00304                 
00305     david.init(); //Connect to Server
00306     ros::spin();
00307     return 0;
00308   }
00309 


david_laserscanner
Author(s): Andreas Holzbach, Dejan Pangercic
autogenerated on Mon Oct 6 2014 08:17:17