rovio_audio.cpp
Go to the documentation of this file.
00001 
00011 #include <arpa/inet.h>
00012 #include <cstdlib>
00013 #include <ros/ros.h>
00014 #include <rovio_av/rovio_audio.h>
00015 #include <rovio_shared/rovio_http.h>
00016 #include <rovio_shared/wav_play.h>
00017 #include <string>
00018 #include <sstream>
00019 #include <sys/socket.h>
00020 
00021 using namespace std;
00022 
00023 audio_controller::audio_controller()
00024 {
00025   // check for all the correct parameters
00026   if (!node.getParam(USER, user))
00027   {
00028     ROS_ERROR("Parameter %s not found.", USER);
00029     exit(-1);
00030   }
00031   if (!node.getParam(PASS, pass))
00032   {
00033     ROS_ERROR("Parameter %s not found.", PASS);
00034     exit(-1);
00035   }
00036   if (!node.getParam(HOST, host))
00037   {
00038     ROS_ERROR("Parameter %s not found.", HOST);
00039     exit(-1);
00040   }
00041 
00042   // add services
00043   wav_play = node.advertiseService("wav_play", &audio_controller::wav_play_callback, this);
00044 
00045   ROS_INFO("Rovio Audio Controller Initialized");
00046 }
00047 
00048 bool audio_controller::wav_play_callback(rovio_shared::wav_play::Request &req, rovio_shared::wav_play::Response &resp)
00049 {
00050   // create a socket to talk to the Rovio
00051   int audio_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
00052   if (audio_socket < 0)
00053   {
00054     ROS_ERROR("Audio socket could not be created.");
00055     return false;
00056   }
00057 
00058   // setup the address
00059   struct sockaddr_in host_addr;
00060   memset(&host_addr, 0, sizeof(host_addr));
00061   host_addr.sin_family = AF_INET;
00062   host_addr.sin_addr.s_addr = inet_addr(host.c_str());
00063   host_addr.sin_port = htons(80);
00064 
00065   // connect the socket
00066   if (connect(audio_socket, (struct sockaddr *)&host_addr, sizeof(host_addr)) < 0)
00067   {
00068     ROS_ERROR("Audio socket could not be connected.");
00069     return false;
00070   }
00071 
00072   // get the file name from the request
00073   string f_name = req.f;
00074 
00075   // try and open the file
00076   FILE *f = fopen(f_name.c_str(), "rb");
00077   if (f == NULL)
00078   {
00079     ROS_ERROR("Could not open file '%s'.", f_name.c_str());
00080     return false;
00081   }
00082 
00083   // find out how big the file is
00084   fseek(f, 0, SEEK_END);
00085   long f_size = ftell(f);
00086   rewind(f);
00087   // create a buffer and read in the entire file
00088   char *buf = (char*)malloc(f_size);
00089   if ((long)fread(buf, 1, f_size, f) != f_size)
00090   {
00091     ROS_ERROR("Error reading file '%s'.", f_name.c_str());
00092     return false;
00093   }
00094 
00095   // build the header
00096   stringstream ss;
00097   ss << "POST /GetAudio.cgi HTTP/1.1\r\n" << "User-Agent: AudioAgent\r\n" << "Host: " << host << "\r\n"
00098       << "Content-Length: " << f_size << "\r\n" << "Cache-Control: no-cache\r\n" << "\r\n";
00099 
00100   // send the sound file
00101   send(audio_socket, ss.str().c_str(), ss.str().size(), 0);
00102   if (send(audio_socket, buf, f_size, 0) != f_size)
00103   {
00104     ROS_ERROR("Could not send entire file.");
00105     return false;
00106   }
00107 
00108   // close the file
00109   fclose(f);
00110   // free the buffer
00111   free(buf);
00112   // close the socket
00113   close(audio_socket);
00114 
00115   return true;
00116 }
00117 
00118 int main(int argc, char **argv)
00119 {
00120   // initialize ROS and the node
00121   ros::init(argc, argv, "rovio_audio");
00122 
00123   // initialize the Rovio controller
00124   audio_controller controller;
00125 
00126   // update at 5 Hz
00127   ros::Rate loop_rate(5);
00128   // continue until a ctrl-c has occurred
00129   while (ros::ok())
00130   {
00131     ros::spinOnce();
00132     loop_rate.sleep();
00133   }
00134 
00135   return EXIT_SUCCESS;
00136 }


rovio_av
Author(s): Russell Toris
autogenerated on Mon Oct 6 2014 07:13:10