refinements.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, U. Klank   klank@in.tum.de
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/ros.h>
00031 #include <cop_client_cpp/cop_client.h>
00032 #include <std_msgs/String.h>
00033 #include <sstream>
00034 
00035 ros::Subscriber sr;
00036 
00037 std::map<long, std::string> mapping;
00038 
00039 void cb_logging(const std_msgs::StringConstPtr& str)
00040 {
00041   long test = atoi((*str).data.c_str());
00042   if(test != 0)
00043   {
00044     mapping[test] = (*str).data;
00045   }
00046 }
00047 
00048 int main(int argc, char* argv[])
00049 {
00050   ros::init( argc, argv, "test_cop");
00051   ros::NodeHandle nh;
00052   CopClient cop(nh);
00053   sr = nh.subscribe("/cop/logging", 1, cb_logging);
00054   
00055 
00056   ros::Rate r(100);
00057   bool refine = false;
00058   bool grasp = true;
00059   bool arm = true;
00060   bool cheap = true;
00061   double xoff = 0.0, yoff = 0.0, zoff = 0.0;
00062 
00063   unsigned long id_base_link = cop.LONameQuery("/base_link");
00064   unsigned long id_startpos = cop.LOFrameQuery(id_base_link, 1);
00065   unsigned long id_cam = cop.LONameQuery("/RightEyeCalc");
00066   if(argc > 1)
00067     refine = true;
00068     
00069   unsigned long id_searchspace = cop.LONameQuery("/openni_rgb_optical_frame");
00070   if(id_searchspace == 0)
00071     id_searchspace = cop.LONameQuery("/narrow_stereo_optical_frame");
00072    std::string object = "Cluster";
00073   while(true)
00074   {
00075     long vision_primitive =  cop.CallCop(object, id_searchspace, 10);
00076 
00077 
00078     size_t num_results_expected = 1;
00079 
00080 
00081     std::vector<vision_msgs::cop_answer> results;
00082     while(nh.ok())
00083     {
00084       if(cop.HasResult(vision_primitive) >= num_results_expected)
00085         break;
00086       ros::spinOnce();
00087       r.sleep();
00088     }
00089     std::vector<vision_msgs::cop_answer> result = cop.GetResult (vision_primitive);
00090     int j = 0; 
00091     int i = 0;
00092     if(result.size() > 0)
00093     {
00094       system("reset");
00095       printf("============================\n      Refine\n============================\n\n");
00096       for(j = 0; j < result[i].found_poses.size(); j++)
00097       {
00098         printf("%d: Refine %s (%ld at %ld)\n", j, result[i].found_poses[j].models[0].sem_class.c_str(),
00099                                                 result[i].found_poses[j].objectId,
00100                                                 result[i].found_poses[j].position);
00101         long vision_primitive =  cop.CallCop("",  result[i].found_poses[j].position, 1, result[i].found_poses[j].objectId, 256);
00102         printf("Wait for refine\n");
00103         while(nh.ok())
00104         {
00105           if(cop.HasResult(vision_primitive) >= num_results_expected)
00106             break;
00107           ros::spinOnce();
00108           r.sleep();
00109         }    
00110         std::vector<vision_msgs::cop_answer> result_tmp = cop.GetResult (vision_primitive);
00111         
00112         result.push_back(result_tmp[0]);
00113       }
00114     }
00115     for(int bla = 1 ; bla < 2 ; bla++)
00116     for (int trials = 0; trials < 20; trials++)
00117     {
00118      system("reset");
00119      printf("============================\n      Redetect \n============================\n\n");
00120      int i = 0;
00121      long vision_primitive_ss =  cop.CallCop(object, id_searchspace, 10);
00122      size_t num_results_expected_ss = 1;
00123 
00124 
00125       std::vector<vision_msgs::cop_answer> results_ss;
00126       while(nh.ok())
00127       {
00128         if(cop.HasResult(vision_primitive_ss) >= num_results_expected_ss)
00129           break;
00130         ros::spinOnce();
00131         r.sleep();
00132       }
00133       results_ss = cop.GetResult (vision_primitive);
00134       std::vector<unsigned long> vec ;
00135       
00136       for(i = 0; i < results_ss[0].found_poses.size(); i++)
00137       {
00138         vec.push_back(results_ss[0].found_poses[i].position);
00139       }
00140         
00141      for(i = 1; i < result.size(); i++)
00142      {
00143        for(j = 0; j < result[i].found_poses.size(); j++)
00144        {
00145          printf("%d: Redetect %s (%ld at %ld)\n", i, result[i].found_poses[j].models[result[i].found_poses[j].models.size() - 1].sem_class.c_str(),
00146                                                  result[i].found_poses[j].objectId,
00147                                                  result[i].found_poses[j].position);
00148                                                
00149        }
00150        int select = i;
00151        
00152        if(result.size() > select && result[select].found_poses.size() > 0)
00153        {
00154          long vision_primitive =  cop.CallCop("", vec, 1, result[select].found_poses[0].objectId, 0);
00155          printf("Wait for relocate\n");
00156          while(nh.ok())
00157          {
00158            if(cop.HasResult(vision_primitive) >= num_results_expected)
00159              break;
00160            ros::spinOnce();
00161            r.sleep();
00162          }    
00163          std::vector<vision_msgs::cop_answer> result_tmp = cop.GetResult (vision_primitive);
00164          char txt[512];
00165          bool succes = false;
00166          double dist = 0.0, dist_cam = 0.0;
00167          if(result_tmp.size() > 0 && result_tmp[0].found_poses.size() > 0)
00168          {
00169            dist = cop.LODistanceQuery(result[select].found_poses[0].position, result_tmp[0].found_poses[0].position);
00170            dist_cam = cop.LODistanceQuery(result[select].found_poses[0].position, id_cam);
00171            
00172            succes = true;
00173          }
00174          if(bla > 0)
00175          {
00176           if(dist < dist_cam*0.15)
00177           {
00178             cop.CopFeedBack(vision_primitive, 1.0, 0);           
00179           }
00180           else
00181           {
00182             cop.CopFeedBack(vision_primitive, 0.1, 2048);           
00183           }
00184          }
00185          std::string text;
00186          if(mapping.find(vision_primitive) != mapping.end())
00187           text = mapping[vision_primitive];
00188          sprintf(txt, "%ld %d %f %f %f %f %f %d %s\n", result[select].found_poses[0].objectId, succes ? 1 : 0, dist, xoff, yoff, zoff, dist_cam, bla, text.c_str());
00189 
00190          FILE *file = fopen("results.txt", "a");
00191          fwrite(txt, sizeof(*txt), strlen(txt), file);
00192          fclose(file);
00193        }
00194       }
00195 /*   if(cheap)
00196         cheap = !cheap;*/
00197       while(true)
00198       {
00199         /* Move */ 
00200         xoff = 0.5 * ((double)rand() / RAND_MAX) - 0.25;
00201         yoff = 0.5 * ((double)rand() / RAND_MAX) - 0.25;
00202         
00203         if(xoff > 0.25)
00204           xoff = 0.25;
00205          if(xoff < -0.25)
00206           xoff = -0.25;  
00207          
00208         
00209         if(yoff > 0.25)
00210             yoff = 0.25; 
00211         if(yoff < -0.25)
00212            yoff = -0.25;        
00213         unsigned long newpos = cop.LOOffset(0, id_base_link, xoff, yoff, zoff);
00214         
00215         printf("GENERATED LO ID %ld\n\n\n", newpos);
00216         double dist = cop.LODistanceQuery(newpos, id_startpos);
00217         if(dist < 0.5)        
00218         {
00219           char cmddrive[256];
00220           sprintf(cmddrive, "rosrun drive_at drive_at.py -l %ld", newpos);
00221           printf(cmddrive);
00222           system(cmddrive);
00223           break;
00224         }
00225         else
00226         {
00227           printf("Its FAAAR away (dist = %f), ignore\n", dist);
00228         }
00229         cop.LOFreeID(newpos);
00230       }
00231          
00232     }
00233 
00234     while(true)
00235     {
00236     system("reset");
00237     printf("============================\n      END\n============================\n\n");
00238 
00239     printf("(0) Home & continue\n(1) END \n(2) Toggle arm\n(3) Toggle refine\n(4) Toggle grasp\n(5) Set search space\n(6) Set search term\n(7)continue w/o move");
00240     
00241      int select2;
00242      int test = scanf("%d", &select2);
00243      printf("Selected %d\n", select2);
00244      if(select2 == 0)
00245      {
00246        char cmddrive[256];
00247        sprintf(cmddrive, "rosrun drive_at drive_at.py -l %ld", id_startpos);
00248        system(cmddrive);
00249        break;
00250      }
00251      else if(select2 == 1)
00252        exit(0);
00253      else if(select2 == 2)
00254         arm = !arm;
00255      else if(select2 == 3)
00256         refine = !refine;
00257      else if(select2 == 4)    
00258         grasp = !grasp;
00259      else if(select2 == 5)
00260      {
00261          char crash[200];
00262          test = scanf("%s", &crash);
00263          if(atoi(crash) != 0)
00264            id_searchspace = atoi(crash);
00265         else
00266         {
00267           unsigned long test = cop.LONameQuery(crash);
00268           if(test != 0)
00269             id_searchspace = test;
00270         }
00271      }
00272      else if(select2 == 6)
00273      {
00274        char crash[200];
00275        test = scanf("%s", &crash);
00276        object = crash;
00277      }
00278      else if (select2 == 7)
00279      {
00280       break;
00281      }
00282    }
00283   }
00284   return 0;
00285 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


cop_client_cpp
Author(s): Ulrich F Klank
autogenerated on Thu May 23 2013 09:34:13