prolog_perception: prolog_perception: query_table_memory_write_to

00001 /*
00002  * Copyright (c) 2009 Dejan Pangercic <pangercic -=- 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  * $Id: query_table_memory_write_to_file.cpp 17089 2009-06-15 18:52:12Z pangercic $
00028  *
00029  */
00030 
00045 #include <ros/ros.h>
00046 #include <ias_table_srvs/ias_table_clusters_service.h>
00047 //prolog wrapper
00048 #include <cstdlib>
00049 #include <ctype.h>
00050 #include <vector>
00051 #include <fstream>
00052 #include <ctime>
00053 #include<iomanip>
00054 using std::ofstream;
00055 using namespace ias_table_srvs;
00056 
00057 int record_to_file(std::vector<std::string> data)
00058 {
00059   ofstream outdata; // outdata is like cin    
00060   outdata.open("data/table_memory.dat", std::ios::app); // opens the file
00061   if( !outdata ) 
00062     { 
00063       // file couldn't be opened
00064       ROS_ERROR("Error: file could not be opened");
00065       exit(1);
00066     }
00067   for (unsigned int i =0; i < data.size(); i++)
00068     outdata << data[i] << std::endl;
00069   outdata.close();
00070   return 0;
00071 }
00072 
00073 int main(int argc, char **argv)
00074 {
00075   ros::init(argc, argv, "query_table_memmory_write_to_file");
00076   ias_table_clusters_service call;
00077   ros::NodeHandle n;
00078   std::string table_memory_srvs("/table_memory/table_memory_clusters_service");
00080   ros::service::waitForService(table_memory_srvs);
00081   ros::ServiceClient client = n.serviceClient<ias_table_clusters_service>(table_memory_srvs, true);
00082     
00083   ros::Duration s(10.0);
00084   //  while(n.ok())
00085   //{
00086   //  ROS_DEBUG("Sleeping");
00087   //  s.sleep();
00088   //  ros::spinOnce();
00089       if(!client.call(call))
00090         {
00091           ROS_ERROR("ERROR Calling %s", table_memory_srvs.c_str());
00092         }
00093       else
00094         {
00095           ROS_DEBUG("Called service %s", table_memory_srvs.c_str());
00096         }
00097       std::vector<std::string> data_vector_;
00098       std::ostringstream data_;
00099       for (unsigned int i = 0; i < call.response.prolog_return.size(); i++)
00100         {
00101           std::ostringstream data_;       
00102           ROS_DEBUG("table_id: %ld stamp: %.10lf", call.response.prolog_return[i].table_id,  call.response.prolog_return[i].stamp.toSec());
00103           ROS_DEBUG("table center: %f %f %f", call.response.prolog_return[i].table_center.x, call.response.prolog_return[i].table_center.y,
00104                    call.response.prolog_return[i].table_center.z);
00105           data_ << call.response.prolog_return[i].table_id << " " << 
00106             call.response.prolog_return[i].table_center.x << " " << 
00107             call.response.prolog_return[i].table_center.y << " " << 
00108             call.response.prolog_return[i].table_center.z << " " <<
00109             std::setprecision(20) << 
00110             call.response.prolog_return[i].stamp.toSec() << " " <<  std::setprecision(5);
00111           ROS_DEBUG("cluster center: %f %f %f", call.response.prolog_return[i].cluster_center.x, call.response.prolog_return[i].cluster_center.y,
00112                    call.response.prolog_return[i].cluster_center.z);
00113           data_ << call.response.prolog_return[i].cluster_center.x << " " << 
00114             call.response.prolog_return[i].cluster_center.y << " " << 
00115             call.response.prolog_return[i].cluster_center.z << " ";
00116           for (unsigned int j = 0; j < call.response.prolog_return[i].cluster_colors.size(); j++)
00117             {
00118               ROS_DEBUG("cluster colors %s", call.response.prolog_return[i].cluster_colors[j].c_str());
00119               data_ << call.response.prolog_return[i].cluster_colors[j] << "\n";
00120             }                                   
00121           ROS_DEBUG("Pushing facts %s", data_.str().c_str());
00122           data_vector_.push_back(data_.str());
00123         }
00124       record_to_file(data_vector_);     
00125       // }
00126 }