ROSComm.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2009 by Ulrich Friedrich Klank <klank@in.tum.de>
00003  *
00004  * This program is free software; you can redistribute it and/or modify
00005  * it under the terms of the GNU General Public License as published by
00006  * the Free Software Foundation; either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 
00019 #ifndef USE_YARP_COMM
00020 
00021 
00022 
00023 
00024 #include "ROSComm.h"
00025 using namespace vision_msgs;
00026 using namespace vision_srvs;
00027 
00028 #include "BoostUtils.h"
00029 
00030 #ifdef BOOST_THREAD
00031 #include <boost/thread/mutex.hpp>
00032 #include <sstream>
00033 boost::mutex s_mutexAnswer;
00034 #endif
00035 
00036 using namespace cop;
00037 
00038 unsigned long Comm::s_lastCommID = 0;
00039 
00040 /********************************************************************
00041 *     CreatePoseFromMessage                                         */
00042 /********************************************************************
00043 *   @brief Creates a lo from a bottle
00044 *********************************************************************/
00045 std::pair<RelPose*, Probability_1D_t> CreatePoseFromMessage(const apriori_position& call)
00046 {
00047     std::pair<RelPose*, Probability_1D_t> result;
00048     result.second = call.probability;
00049     result.first = RelPoseFactory::FRelPose(call.positionId);
00050     if(result.first == NULL)
00051     {
00052       printf("ROSComm: Major Error in Message: got an undefined pose\n");
00053       throw "Wrong Pose";
00054     }
00055     return result;
00056 }
00057 
00058 std::string AlgorihtmTypeToName(int type)
00059 {
00060   std::string name = "Algorithm";
00061   switch(type)
00062   {
00063     case ALGORITHMTYPE_LOCATE:
00064     name = "Locate";
00065     break;
00066     case ALGORITHMTYPE_TRACK:
00067     name = "Track";
00068     break;
00069     case ALGORITHMTYPE_2OBJS:
00070     name = "Locate2Obj";
00071     break;
00072     case ALGORITHMTYPE_REFINE:
00073     name = "Refine";
00074     break;
00075     case ALGORITHMTYPE_RPOVE:
00076     name = "Prove";
00077     break;
00078     case ALGORITHMTYPE_STOPTRACK:
00079     name = "StopTrack";
00080     break;
00081     case ALGORITHMTYPE_STARTATTEND:
00082     name = "StartAttend";
00083     break;
00084     case ALGORITHMTYPE_STOPATTEND:
00085     name = "StopAttend";
00086     break;
00087     case ALGORITHMTYPE_LOOKUP:
00088     name = "Database LookUp";
00089     break;
00090     case ALGORITHMTYPE_LOOKUPDB:
00091     name = "Database: Subscribe to All";
00092     break;
00093     case ALGORITHMTYPE_LOOKUPALL:
00094     name = "Database: Subscribe to All";
00095     break;
00096   }
00097   return name;
00098 }
00099 
00100 void PutPoseIntoAMessage(cop_answer &answer, SignatureLocations_t new_location)
00101 {
00102   for(unsigned int i = 0; i < new_location.size(); i++)
00103   {
00104     aposteriori_position apo_pose;
00105     apo_pose.objectId = new_location[i].second->m_ID;
00106     apo_pose.probability = new_location[i].first->m_qualityMeasure;
00107     apo_pose.position = new_location[i].first->m_uniqueID;
00108     if(new_location[i].first->m_uniqueID == 0 && new_location[i].second->GetObjectPose() != NULL)
00109     {
00110       apo_pose.probability = new_location[i].second->GetObjectPose()->m_qualityMeasure;
00111       apo_pose.position = new_location[i].second->GetObjectPose()->m_uniqueID;
00112     }
00113     int elems_c = new_location[i].second->CountElems();
00114     for(int j = 0; j < elems_c; j++)
00115     {
00116       Descriptor* descriptor = (Descriptor*)new_location[i].second->GetElement(j, ELEM);
00117       if(descriptor != NULL)
00118       {
00119         vision_msgs::cop_descriptor descr;
00120         if(descriptor->GetClass() != NULL)
00121         {
00122           descr.sem_class = descriptor->GetClass()->GetName();
00123           descr.object_id = descriptor->m_ID;
00124           descr.type      = descriptor->GetNodeName();
00125           descr.quality   = descriptor->GetQuality();
00126           if(descr.type.compare("NamedClass") == 0)
00127             apo_pose.models.insert(apo_pose.models.begin(), descr);
00128           else
00129             apo_pose.models.push_back(descr);
00130         }
00131       }
00132     }
00133     answer.found_poses.push_back(apo_pose);
00134   }
00135 }
00136 
00137 
00138 void PutPoseIntoAMessage(cop_answer &answer, Signature* sig)
00139 {
00140   aposteriori_position apo_pose;
00141   apo_pose.objectId = sig->m_ID;
00142   int elems_c = sig->CountElems();
00143   RelPose* pose = sig->GetObjectPose();
00144   unsigned long max_timestamp = sig->date();
00145   double pose_qual = 0.0;
00146   for(int j = 0; j < elems_c; j++)
00147   {
00148     Descriptor* descriptor = (Descriptor*)sig->GetElement(j, ELEM);
00149     if(descriptor != NULL)
00150     {
00151       vision_msgs::cop_descriptor descr;
00152       if( descriptor->GetClass() != NULL)
00153       {
00154         descr.sem_class = descriptor->GetClass()->GetName();
00155         descr.object_id = descriptor->m_ID;
00156         descr.type      = descriptor->GetNodeName();
00157         descr.quality   = descriptor->GetQuality();
00158         if(descriptor->date() > max_timestamp || pose == NULL)
00159         {
00160           pose = descriptor->GetLastMatchedPose();
00161           if(pose != NULL)
00162           {
00163             max_timestamp = descriptor->date();
00164             pose_qual = pose->m_qualityMeasure;
00165           }
00166         }
00167         apo_pose.models.push_back(descr);
00168       }
00169     }
00170   }
00171   apo_pose.probability = pose_qual;
00172   if(pose != NULL)
00173     apo_pose.position = pose->m_uniqueID;
00174   answer.found_poses.push_back(apo_pose);
00175 }
00176 
00177 void ROSComm::NotifyPoseUpdate(RelPose* pose, bool sendObjectRelation)
00178 {
00179   try
00180   {
00181     cop_answer answer;
00182 
00183     PutPoseIntoAMessage(answer, m_visPrim.GetSignature());
00184     if(pose != NULL)
00185     {
00186       answer.found_poses[0].probability = pose->m_qualityMeasure;
00187       answer.found_poses[0].position = pose->m_uniqueID;
00188       answer.perception_primitive = m_visPrim.GetID();
00189     }
00190     else
00191     {
00192         answer.error = "Object lost";
00193     }
00194     PublishAnswer(answer);
00195   }
00196   catch(...)
00197   {
00198     printf("Problems publishing answer in ROSComm::NotifyPoseUpdate\n");
00199   }
00200 #ifdef _DEBUG
00201   /*printf("Writing a bottle in Pose Update Notification:  %s\n", new_loc_bottle.toString().c_str());*/
00202 #endif /*_DEBUG*/
00203 }
00204 
00205 void ROSComm::NotifyNewObject(Signature* sig, RelPose* pose)
00206 {
00207   try
00208   {
00209     cop_answer answer;
00210     PutPoseIntoAMessage(answer, sig);
00211     if(pose != NULL)
00212     {
00213       answer.found_poses[0].probability = pose->m_qualityMeasure;
00214       answer.found_poses[0].position = pose->m_uniqueID;
00215     }
00216     else
00217     {
00218       answer.found_poses[0].probability = 0.0;
00219       answer.found_poses[0].position = 0;
00220       answer.error = "Object not localized";
00221     }
00222     answer.perception_primitive = m_visPrim.GetID();
00223     PublishAnswer(answer);
00224   }
00225   catch(...)
00226   {
00227     printf("Problems publishing answer in ROSComm::NotifyNewObject\n");
00228   }
00229 #ifdef _DEBUG
00230   /*printf("Writing a bottle in Pose Update Notification:  %s\n", new_loc_bottle.toString().c_str());*/
00231 #endif /*_DEBUG*/
00232 }
00233 
00234 
00235 ROSComm::~ROSComm()
00236 {
00237   /*for(int i = 0; i < m_pose->size(); i++)
00238     RelPoseFactory::FreeRelPose((*m_pose)[i].first);*/
00239   printf("\n\n\nDelete ROS COMM and set Vision Primitive %ld  to terminated\n\n\n", m_visPrim.GetID());
00240   m_visPrim.SetTerminated();
00241   delete m_pose;
00242 }
00243 
00244 void ROSComm::Start()
00245 {
00246 #ifdef BOOST_THREAD
00247   boost::thread( boost::bind(&ROSComm::ProcessCall, this));
00248 #else
00249   ROSComm::ProcessCall(this);
00250 #endif /*BOOST_THREAD*/
00251 }
00252 
00253 void ROSComm::PublishAnswer(cop_answer &answer)
00254 {
00255   int timeout = 0;
00256   if(m_callerid.length() == 0 && m_publisher != NULL)
00257   {
00258     while(m_publisher->getNumSubscribers() < 1 && timeout < 100)
00259     {
00260       if(timeout % 20 == 0)
00261         printf("No subscribers, waiting\n");
00262       Sleeping(10);
00263       timeout++;
00264     }
00265   }
00266   else
00267   {
00268     while(m_publisher != NULL && m_publisher->getNumSubscribers() < 1 && timeout < 100)
00269     {
00270       if(timeout % 20 == 0)
00271         printf("No subscribers, waiting\n");
00272       Sleeping(10);
00273       timeout++;
00274     }
00275     bool right_publisher = false;
00276     while(timeout < 100)
00277     {
00278       if(m_calleridMap.find(m_callerid) == m_calleridMap.end())
00279       {
00280         ROS_WARN("Problems with caller %s\n", m_callerid.c_str());
00281          m_calleridMap[m_callerid] = std::vector<std::string> ();
00282       }
00283       else
00284       {
00285         if(m_publisher == NULL)
00286         {
00287           ROS_ERROR("cop ros comm: Empty publisher, this is unexpected\n");
00288         }
00289         else
00290         {
00291           for(size_t i = 0;  i < m_calleridMap[m_callerid].size(); i++)
00292           {
00293             if(m_calleridMap[m_callerid][i].compare(m_publisher->getTopic()) == 0)
00294             {
00295               right_publisher = true;
00296               break;
00297             }
00298 
00299           }
00300         }
00301         if(right_publisher)
00302           break;
00303       }
00304       printf("Wrong subscribers, waiting for %s\n", m_callerid.c_str());
00305       Sleeping(50);
00306       timeout++;
00307     }
00308   }
00309   if(m_publisher != NULL)
00310   {
00311     BOOST(s_mutexAnswer.lock());
00312     try
00313     {
00314       answer.perception_primitive = m_visPrim.GetID();
00315       m_publisher->publish(answer);
00316     }
00317     catch(...)
00318     {
00319        printf("Problems publishing answer in ROSComm::PublishAnswer\n");
00320     }
00321     BOOST(s_mutexAnswer.unlock());
00322   }
00323 }
00324 
00325 
00326 void ROSComm::ProcessCall()
00327 {
00328 #ifdef _DEBUG
00329     printf("Answer Thread started for object %ld with command %s\n", m_visPrim.GetSignature()->m_ID, AlgorihtmTypeToName(m_actionType).c_str());
00330 #endif
00331     bool bFinished = false;
00332     cop_answer answer;
00333 
00334     switch(m_actionType)
00335     {
00336     case ALGORITHMTYPE_LOOKUPDB:
00337         m_visFinder->m_sigdb->SetNewObjectCallback(this, false);
00338         bFinished = true;
00339         break;
00340   case ALGORITHMTYPE_LOOKUPALL:
00341         m_visFinder->m_sigdb->SetNewObjectCallback(this);
00342         bFinished = false;
00343         break;
00344     case ALGORITHMTYPE_LOOKUP:
00345         PutPoseIntoAMessage(answer, m_visPrim.GetSignature());
00346         PublishAnswer(answer);
00347         break;
00348     case ALGORITHMTYPE_REFINE:
00349         try
00350         {
00351           const SignatureLocations_t &new_location = m_visFinder->m_visLearner->RefineObject(m_pose, m_visPrim, m_numOfObjects);
00352           if(new_location.size() > 0)
00353           {
00354             PutPoseIntoAMessage(answer, new_location);
00355           }
00356           else
00357           {
00358             SignatureLocations_t old_location;
00359             std::pair<RelPose*, Signature*> olddata;
00360             if(m_pose->size() > 0)
00361                 olddata.first = (*m_pose)[0].first;
00362             else
00363                 olddata.first = NULL;
00364             olddata.second = m_visPrim.GetSignature();
00365             old_location.push_back(olddata);
00366             PutPoseIntoAMessage(answer, old_location);
00367             answer.error = "No Refinement Found!";
00368           }
00369           PublishAnswer(answer);
00370         }
00371         catch(const char  *error)
00372         {
00373           printf("Refine failed: %s\n", error);
00374           std::string error_copy = error;
00375           answer.error = error_copy;
00376           PublishAnswer(answer);
00377         }
00378         catch(...)
00379         {
00380           answer.error = "Refine failed";
00381           PublishAnswer(answer);
00382         }
00383         bFinished = true;
00384         break;
00385     case ALGORITHMTYPE_TRACK:
00386        m_visPrim.GetSignature()->SetCommCallBack(this);
00387         if(m_pose->size() > 0)
00388         {
00389           PossibleLocations_t::const_iterator it = m_pose->begin();
00390           m_visFinder->StartTrack(m_visPrim, (*it).first);
00391         }
00392         else
00393           m_visFinder->StartTrack(m_visPrim, NULL);
00394        break;
00395     case ALGORITHMTYPE_STOPTRACK:
00396         m_visPrim.GetSignature()->SetCommCallBack(NULL);
00397         m_visFinder->StopTrack(*m_visPrim.GetSignature());
00398         bFinished = true;
00399         break;
00400     case ALGORITHMTYPE_STARTATTEND:
00401         m_visFinder->m_attentionMan->SetObjectToAttend(&m_visPrim,m_pose, this);
00402         bFinished = false;
00403         break;
00404     case ALGORITHMTYPE_STOPATTEND:
00405         m_visFinder->m_attentionMan->StopAttend(this);
00406         bFinished = true;
00407         break;
00408 
00409     case ALGORITHMTYPE_LOCATE:
00410       try
00411       {
00412         const SignatureLocations_t &new_location = m_visFinder->Locate(m_pose, m_visPrim, m_numOfObjects);
00413         if(m_numOfObjects > 0 && new_location.size() > 0)
00414         {
00415            PutPoseIntoAMessage(answer, new_location);
00416         }
00417         else
00418         {
00419           answer.error = "No Object Found!";
00420         }
00421         PublishAnswer(answer);
00422       }
00423       catch(char const* text)
00424       {
00425         printf("Locate called by ros failed: %s\n", text);
00426         stringstream st;
00427         st << "Locate failed: " << text;
00428         answer.error = st.str();
00429         PublishAnswer(answer);
00430       }
00431       catch(...)
00432       {
00433         answer.error = "Locate failed";
00434         PublishAnswer(answer);
00435       }
00436       bFinished = true;
00437       break;
00438     default:
00439        answer.error = "Unknown command";
00440        PublishAnswer(answer);
00441        bFinished = true;
00442        break;
00443     }
00444 #ifdef _DEBUG
00445     printf("Finished  with action of type \"%s\".\nReturning to listen loop.\n", AlgorihtmTypeToName(m_actionType).c_str());
00446 #endif
00447     /*TODO delete this*/
00448     if(bFinished)
00449       delete this;
00450 
00451 }
00452 
00453 
00454 ROSTopicManager::ROSTopicManager(VisFinder* visFinder, SignatureDB *sigDb) :
00455   m_visFinder(visFinder),
00456   m_sig(sigDb)
00457 {
00458 }
00459 
00460 ROSTopicManager:: ~ROSTopicManager()
00461 {
00462   for(std::map<std::string, ros::Publisher*>::iterator it = m_openTopics.begin(); it != m_openTopics.end(); it++)
00463   {
00464     delete (*it).second;
00465   }
00466 }
00467 
00468 void ROSTopicManager::CloseROSTopic(std::string name)
00469 {
00470 }
00471 
00472 
00473 void ROSTopicManager::Subscriber(const ros::SingleSubscriberPublisher& subs)
00474 {
00475   std::string topic = subs.getTopic();
00476   std::string subscriber = subs.getSubscriberName();
00477   printf("\n\nCop got new subscriber: %s at topic %s\n\n", subscriber.c_str(), topic.c_str());
00478   if(m_subscriberPerTopics.find(subscriber) != m_subscriberPerTopics.end())
00479   {
00480       m_subscriberPerTopics[subscriber].push_back(topic);
00481   }
00482   else
00483   {
00484     std::vector<string> vst;
00485     vst.push_back(topic);
00486     m_subscriberPerTopics[subscriber] = vst;
00487   }
00488 }
00489 
00490 
00491 /*void ROSTopicManager::ListenCallBack(const boost::shared_ptr<const cop_call> &msg)*/
00492 bool ROSTopicManager::ListenCallBack(cop_call::Request& msg, cop_call::Response&  answer)
00493 {
00494 #ifdef _DEBUG
00495   printf("Entering ROSTopicManager::ListenCallBack with find %p sigdb %p\n", m_visFinder, m_sig);
00496   printf("Got Message: noo %ld \n", msg.number_of_objects);
00497 #endif
00498   std::vector<ObjectID_t> class_id;
00499   std::string callerid;
00500   try
00501   {
00502     callerid = (*msg.__connection_header)["callerid"];
00503   }
00504   catch(...)
00505   {
00506     callerid = "";
00507   }
00508   std::string topicname = msg.outputtopic.length() == 0 ? STD_COP_OUTPUT_PORT : msg.outputtopic;
00509   Signature* sig = NULL;
00510   PossibleLocations_t* poses = new PossibleLocations_t();
00511 
00512   for(unsigned int i = 0; i < msg.object_classes.size(); i++)
00513   {
00514     try
00515     {
00516       int class_from_string = m_sig->CheckClass(msg.object_classes[i]);
00517 #ifdef _DEBUG
00518       printf("  Class as string: %s -> %d\n", msg.object_classes[i].c_str(), class_from_string);
00519 #endif
00520       if(class_from_string != -1)
00521         class_id.push_back(class_from_string);
00522       else
00523       {
00524         Class cl;
00525         cl.SetName(msg.object_classes[i]);
00526         cl.m_ID = m_sig->AddClass(cl.GetName(), cl.m_ID);
00527         int class_from_string = m_sig->CheckClass(msg.object_classes[i]);
00528         if(class_from_string != -1)
00529         {
00530           class_id.push_back(class_from_string);
00531           printf("Added Class %s as %ldth element\n", msg.object_classes[i].c_str(), class_id.size());
00532         }
00533 
00534       }
00535     }
00536     catch(...)
00537     {
00538         printf("ROSComm: Problems reading classes\n");
00539         sig = NULL;
00540     }
00541   }
00542   for(unsigned int j = 0; j < msg.object_ids.size(); j++)
00543   {
00544     try
00545     {
00546         int index;
00547         ObjectID_t id = msg.object_ids[j];
00548         if(m_sig->CheckClass(id).length() > 0)
00549         {
00550           class_id.push_back(id);
00551         }
00552         else if(m_sig->Check(id, index))
00553         {
00554           sig = m_sig->GetSignatureByID(id);
00555         }
00556         else
00557         {
00558           printf("Received unknown Element id: %ld\n", id);
00559         }
00560     }
00561     catch(...)
00562     {
00563         printf("ROSComm: Problems reading classes\n");
00564         sig = NULL;
00565     }
00566   }
00567   if(sig == NULL)
00568   {
00569      try
00570      {
00571         sig = m_sig->GetSignature(class_id);
00572         if(sig == NULL)
00573         {
00574 
00575           printf("ROSComm: Could not generate signature for the requested description, trying to download\n");
00576           return false;
00577         }
00578      }
00579      catch(char const* text)
00580      {
00581        printf("Error Creating signature: %s\n", text);
00582        sig = NULL;
00583      }
00584      catch(...)
00585      {
00586        printf("Error Creating signature\n");
00587        sig = NULL;
00588      }
00589   }
00590   else
00591   {
00592     m_sig->CompleteSignature(sig, class_id);
00593   }
00594 #ifdef _DEBUG
00595   if(sig != NULL)
00596   {
00597     printf("Created Signature with all classes (%ld)\n", sig->m_ID);
00598   }
00599 #endif
00600   if(sig == NULL)
00601   {
00602     ROS_INFO("Reaction on cop_call: Creating an empty signature since no further informtion was passed\n");
00603     sig = new Signature();
00604     printf("Created new and empty Signature (%ld)\n", sig->m_ID);
00605     m_sig->AddSignature(sig);
00606   }
00607   for(unsigned int k = 0; k < msg.list_of_poses.size(); k++)
00608   {
00609     try
00610     {
00611       poses->push_back(CreatePoseFromMessage(msg.list_of_poses[k]));
00612     }
00613     catch(char* txt)
00614     {
00615       continue;
00616     }
00617   }
00618 
00619   if(m_openTopics.find(topicname) == m_openTopics.end())
00620   {
00621     ros::NodeHandle n;
00622 
00623     ROS_INFO("Publisher pub = n.advertise<cop_answer>(%s, 1000)\n", topicname.c_str());
00624     ros::Publisher* pub= new ros::Publisher();
00625     *pub = n.advertise<cop_answer>(topicname, 5, boost::bind(&ROSTopicManager::Subscriber, this, _1));
00626 /*     ros::Rate r(1);
00627      r.sleep();
00628     cop_answer answer;
00629      answer.error = "No Object Found!";
00630      pub->publish(answer);
00631      printf("Published?\n");*/
00632      m_openTopics[topicname] = pub;
00633   }
00634   PerceptionPrimitive& vis = m_sig->CreateNewPerceptionPrimitive(sig);
00635   ROSComm* comm = new ROSComm(m_visFinder, poses, vis, m_openTopics[topicname], (int)msg.number_of_objects, (int)msg.action_type, callerid, this->m_subscriberPerTopics);
00636   comm->Start();
00637   answer.perception_primitive = vis.GetID();
00638   return true;
00639 }
00640 
00641 #ifdef WIN32
00642 #include <direct.h>
00643 #define GETCWD(A, B) _getcwd(A, B)
00644 #else
00645 #include <unistd.h>
00646 #define GETCWD(A, B) getcwd(A, B)
00647 #endif
00648 
00649 
00650 bool ROSTopicManager::SaveCallBack(cop_save::Request& msg, cop_save::Response&  answer)
00651 {
00652   try
00653   {
00654     ObjectID_t id = msg.object_id;
00655     char cwd[2048];
00656     char * cwdptr;
00657     std::ostringstream os;
00658     Signature* sig = NULL;
00659     int index;
00660     if(m_sig->CheckClass(id).length() > 0)
00661     {
00662       std::vector<ObjectID_t> class_id;
00663       class_id.push_back(id);
00664       sig = m_sig->GetSignature(class_id);
00665     }
00666     else if(m_sig->Check(id, index))
00667     {
00668       sig = m_sig->GetSignatureByID(id);
00669     }
00670     else
00671     {
00672        ROS_ERROR("Failed to find a specific object (%ld)\n", id);
00673        return false;
00674     }
00675     XMLTag* tag = sig->Save(true);
00676 
00677 
00678     cwdptr = GETCWD(cwd, 2048);
00679 
00680     printf("cwd:  %s / %s\n", cwdptr, cwd);
00681 
00682     os << cwd << "/signature" << tag->date() << ".xml";
00683 
00684     tag->WriteToFile(os.str());
00685 
00686     answer.xmlfilename = os.str();
00687     answer.filenames =  tag->GetSubFilenames();
00688 
00689   }
00690   catch(const char *text)
00691   {
00692      ROS_ERROR("Failed to save an object: %s", text);
00693      return false;
00694 
00695   }
00696   catch(...)
00697   {
00698     ROS_ERROR("Failed to save an object");
00699     return false;
00700   }
00701 
00702   return true;
00703 }
00704 
00705 void ROSTopicManager::NewSignatureCallBack(std_msgs::String::ConstPtr xmlFilename)
00706 {
00707   try
00708   {
00709     XMLTag* signature = XMLTag::ReadFromFile(xmlFilename->data);
00710     std::string global_path = xmlFilename->data.substr(0, xmlFilename->data.find_last_of("/") + 1);
00711     signature->ReplaceSubFilenames(global_path);
00712     if(signature != NULL)
00713     {
00714       if(signature->GetName().compare(XML_NODE_SIGNATURE) == 0)
00715       {
00716         Signature* sig = (Signature*)Elem::ElemFactory(signature);
00717         if(sig != NULL)
00718         {
00719           m_sig->AddSignature(sig);
00720           ROS_INFO("Signature created remotely. num descriptors: %ld\n", sig->CountElems());
00721         }
00722       }
00723       else
00724       {
00725         ROS_ERROR("Unexpected Type incoming\n");
00726       }
00727     }
00728     else
00729     {
00730       ROS_ERROR("Failed ot load %s\n", xmlFilename->data.c_str());
00731     }
00732   }
00733   catch(const char* text)
00734   {
00735     printf("Error loading incoming Signature file: %s\n", text);
00736   }
00737 }
00738 
00739 typedef AlgorithmSelector<std::vector<RelPose*> >  LocateEval;
00740 typedef  AlgorithmSelector<Descriptor*>            RefineEval;
00741 typedef AlgorithmSelector<ImprovedPose >            ProoveEval;
00742 typedef AlgorithmSelector<std::vector<Signature*> >  AttendEval;
00743 
00744 
00745 typedef AlgorithmSelector<std::vector<RelPose*> > LocateSelect;
00746 typedef AlgorithmSelector<std::vector<Descriptor*> > RefineSelect;
00747 typedef AlgorithmSelector<std::vector<ImprovedPose> > ProoveSelect;
00748 typedef AlgorithmSelector<std::vector<Signature*> > AttendSelect;
00749 
00750 template<typename T>
00751 void AddEvals(const std::vector<AlgorithmEval< T > > &locators, vision_srvs::cop_get_methods_list::Response& answer)
00752 {
00753 
00754   /*std::vector< AlgorithmEval<T> >::const_iterator iter */
00755 
00756   for(size_t i = 0; i < locators.size(); i++)
00757   {
00758     vision_msgs::algorithm_evaluation eval;
00759     eval.plugin_name = locators[i].m_algorithm->GetName();
00760     eval.basic_evaluation = locators[i].m_eval;
00761     //printf("Alg %s with length of objects %ld\n", locators[i].m_algorithm->GetName().c_str(), locators[i].m_objectEval.size());
00762     const std::map<ObjectID_t, std::pair<double, int> > map = locators[i].GetMap();
00763     std::map<ObjectID_t, std::pair<double, int> >::const_iterator inner =  map.begin();
00764     for(;inner != map.end(); inner++)
00765     {
00766       vision_msgs::object_algorithm_relation obj;
00767       obj.object_id = (*inner).first;
00768       obj.basic_evaluation = (*inner).second.first;
00769       eval.objects.push_back(obj);
00770     }
00771     answer.method.push_back(eval);
00772   }
00773 }
00774 
00775 
00776 bool ROSTopicManager::ListProgramCallBack(vision_srvs::cop_get_methods_list::Request& msg, vision_srvs::cop_get_methods_list::Response& answer)
00777 {
00778   const LocateEval&  locators   = m_visFinder->GetAlgorithmSelection();
00779   const RefineEval&  refiners   = m_visFinder->m_visLearner->GetRefineAlgorithmSelection();
00780   const ProoveEval&  proovers   = m_visFinder->m_visLearner->GetProoveAlgorithmSelection();
00781   const AttendEval&   attendants = m_visFinder->m_attentionMan->GetAttentionAlgorithmSelection();
00782 
00791   printf("Queried msg.algorithmtype = %ld (msg.algorithmtype & ALGORITHMTYPE_STARTATTEND == %s)\n", msg.algorithmtype, msg.algorithmtype & ALGORITHMTYPE_STARTATTEND ? "true" : "false");
00792   if(msg.algorithmtype == 0 || msg.algorithmtype & ALGORITHMTYPE_TRACK)
00793   {
00794     AddEvals(locators.GetAlgorithmList(), answer);
00795   }
00796   if(msg.algorithmtype == 0 || msg.algorithmtype & ALGORITHMTYPE_REFINE)
00797   {
00798     AddEvals(refiners.GetAlgorithmList(), answer);
00799   }
00800   if(msg.algorithmtype == 0 || msg.algorithmtype & ALGORITHMTYPE_RPOVE)
00801   {
00802     AddEvals(proovers.GetAlgorithmList(), answer);
00803   }
00804   if(msg.algorithmtype == 0 || msg.algorithmtype & ALGORITHMTYPE_STARTATTEND)
00805   {
00806     AddEvals(attendants.GetAlgorithmList(), answer);
00807   }
00808 
00809   return true;
00810 }
00811 
00812 
00813 void ROSTopicManager::FeedbackCallBack(vision_msgs::cop_feedback::ConstPtr feedback)
00814 {
00815   std::vector<ObjectID_t> whitelist;
00816   for(size_t k = 0; k < feedback->eval_whitelist.size(); k++)
00817   {
00818     whitelist.push_back(feedback->eval_whitelist[k]);
00819   }
00820   if( feedback->error.size() > 0)
00821   {
00822     for(size_t i = 0; i < feedback->error.size(); i++)
00823     {
00824       printf("got a feedback:\n Error: %ld (node: %s, description: %s)\n", feedback->error[i].error_id, feedback->error[i].node_name.c_str(), feedback->error[i].error_description.c_str());
00825       if(feedback->error[0].error_id == vision_msgs::system_error::GRASP_FAILED)
00826       {
00827         printf("Negative eval on this error\n");
00828         m_sig->EvaluatePerceptionPrimitive(feedback->perception_primitive, feedback->evaluation,STARTING_WEIGHT, whitelist);
00829       }
00830       else if(feedback->error[0].error_id == vision_msgs::system_error::CONTRADICTING_VISION_RESULTS)
00831       {
00832         printf("Negative eval on this error\n");
00833         m_sig->EvaluatePerceptionPrimitive(feedback->perception_primitive, feedback->evaluation,STARTING_WEIGHT, whitelist);
00834       }
00835     }
00836   }
00837   else
00838   {
00839       printf("Got positive feedback for PP %ld   (%f)\n", feedback->perception_primitive, feedback->evaluation);
00840       m_sig->EvaluatePerceptionPrimitive(feedback->perception_primitive, feedback->evaluation, STARTING_WEIGHT, whitelist);
00841   }
00842 }
00843 
00844 void ROSTopicManager::Listen(std::string name, volatile bool &g_stopall, ros::NodeHandle* node)
00845 {
00849   /*ros::Publisher pub = n.advertise<cop_call>(name, 1000);*/
00850 
00851   ROS_INFO("advertiseService<cop_call> (%s, ...)\n", node->resolveName(name).c_str());
00852   ros::ServiceServer srv = node->advertiseService(node->resolveName(name), &ROSTopicManager::ListenCallBack, this);
00853 
00854   ROS_INFO("advertiseService<cop_save> (%s, ...)\n", node->resolveName("save").c_str());
00855   ros::ServiceServer savesrv = node->advertiseService(node->resolveName("save"), &ROSTopicManager::SaveCallBack, this);
00856 
00857   ROS_INFO("advertiseService<cop_save> (%s, ...)\n", node->resolveName("listmethods").c_str());
00858   ros::ServiceServer listmethods = node->advertiseService(node->resolveName("listmethods"), &ROSTopicManager::ListProgramCallBack, this);
00859 
00860   ROS_INFO("Subscribe to topic %s\n", node->resolveName("new_signatures").c_str());
00861   ros::Subscriber sub_add_sig =
00862         node->subscribe<std_msgs::String>(
00863                  node->resolveName("new_signatures"), 1000,
00864                  boost::bind(&ROSTopicManager::NewSignatureCallBack, this, _1));
00865 
00866   ROS_INFO("Subscribe to topic %s\n", node->resolveName("feedback").c_str());
00867   ros::Subscriber sub_feedback =
00868         node->subscribe<vision_msgs::cop_feedback>(
00869                  node->resolveName("feedback"), 1000,
00870                  boost::bind(&ROSTopicManager::FeedbackCallBack, this, _1));
00871 
00872   ROS_INFO("Publish topic %s\n", node->resolveName("status").c_str());
00873   m_statusPublisher = node->advertise<cop_status>(node->resolveName("status").c_str(), 1);
00874   boost::thread(boost::bind(&ROSTopicManager::StatusThread, this));
00875   ros::Rate r(10);
00876   while (node->ok() && !g_stopall)
00877   {
00878     printf("Call ros spin \n");
00879     try
00880     {
00881       ros::spin();
00882     }
00883     catch(char const * text)
00884     {
00885       printf("Error while spinning: %s\n", text);
00886     }
00887     catch(...)
00888     {
00889       printf("Unknown Error while spinning\n");
00890     }
00891     r.sleep();
00892   }
00893   printf("Returning from ros spinning\n");
00894   return;
00895 }
00896 
00897 bool ROSTopicManager::OpenCommOnROSTopic(std::string st)
00898 {
00899   return true;
00900 }
00901 
00902 extern volatile bool g_stopall;
00903 
00904 void ROSTopicManager::StatusThread()
00905 {
00906   ros::Rate r(10);
00907   r.sleep();
00908   while(!g_stopall)
00909   {
00910     std::vector<std::pair <PerceptionPrimitiveID_t, PerceptionPrimitiveState> >  result = m_sig->GetCurrentRunState();
00911     vision_msgs::cop_status st;
00912     std::vector<std::pair <PerceptionPrimitiveID_t, PerceptionPrimitiveState> >::const_iterator it = result.begin();
00913     for(;it != result.end();it++)
00914     {
00915       vision_msgs::pp_status pp;
00916       pp.perception_primitive = (*it).first;
00917       pp.status  = (*it).second;
00918       st.cop_status.push_back(pp);
00919     }
00920     m_statusPublisher.publish(st);
00921     r.sleep();
00922   }
00923 }
00924 
00925 #endif /*USE_YARP_COMM*/
00926 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


cognitive_perception
Author(s): Ulrich F Klank
autogenerated on Thu May 23 2013 07:38:35