Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00045 #include <ros/ros.h>
00046 #include <ias_table_srvs/ias_table_clusters_service.h>
00047
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;
00060 outdata.open("data/table_memory.dat", std::ios::app);
00061 if( !outdata )
00062 {
00063
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
00085
00086
00087
00088
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 }