ROSjloComm.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 /*Only this or Yarp*/
00020 
00021 
00022 #include <vision_srvs/srvjlo.h>
00023 #include "ROSjloComm.h"
00024 
00025 #include <boost/thread/mutex.hpp>
00026 boost::mutex s_serviceCall;
00027 boost::mutex s_notDeletedList;
00028 
00029 
00030 using namespace vision_srvs;
00031 using namespace vision_msgs;
00032 
00033 #define JLO_IDQUERY "idquery"
00034 #define JLO_FRAMEQUERY "framequery"
00035 #define JLO_NAMEQUERY "namequery"
00036 #define JLO_DELETE "del"
00037 #define JLO_UPDATE "update"
00038 
00039 std::set<unsigned long> not_deleted_jlos;
00040 
00041 using namespace cop;
00042 
00043 void PutPoseIntoAMessage (partial_lo& plo, RelPose* pose)
00044 {
00045   Matrix m = pose->GetMatrix(0);
00046   int width = 4;
00047   for(int r = 0; r < width; r++)
00048   {
00049     for(int c = 0; c < width; c++)
00050     {
00051         plo.pose[r * width + c] = m.element(r,c);
00052     }
00053   }
00054   m = pose->GetCovarianceMatrix();
00055   width = 6;
00056   for(int r = 0; r < width; r++)
00057   {
00058     for(int c = 0; c < width; c++)
00059     {
00060         plo.cov[r * width + c] = m.element(r,c);
00061     }
00062   }
00063   plo.id = pose->m_uniqueID;
00064   plo.parent_id = pose->m_parentID;
00065   plo.type = 0;
00066 }
00067 
00068 RelPose* GetPoseFromMessage(partial_lo& msg, jlo::LazyLocatedObjectLoader* loader)
00069 {
00070   Matrix m(4,4);
00071   m << msg.pose[ 0] << msg.pose[ 1] << msg.pose[ 2] << msg.pose[ 3]
00072     << msg.pose[ 4] << msg.pose[ 5] << msg.pose[ 6] << msg.pose[ 7]
00073     << msg.pose[ 8] << msg.pose[ 9] << msg.pose[10] << msg.pose[11]
00074     << msg.pose[12] << msg.pose[13] << msg.pose[14] << msg.pose[15];
00075   Matrix cov(6,6);
00076   cov << msg.cov[ 0] << msg.cov[ 1] << msg.cov[ 2] << msg.cov[ 3] << msg.cov[ 4] << msg.cov[ 5]
00077       << msg.cov[ 6] << msg.cov[ 7] << msg.cov[ 8] << msg.cov[ 9] << msg.cov[10] << msg.cov[11]
00078       << msg.cov[12] << msg.cov[13] << msg.cov[14] << msg.cov[15] << msg.cov[16] << msg.cov[17]
00079       << msg.cov[18] << msg.cov[19] << msg.cov[20] << msg.cov[21] << msg.cov[22] << msg.cov[23]
00080       << msg.cov[24] << msg.cov[25] << msg.cov[25] << msg.cov[26] << msg.cov[27] << msg.cov[29]
00081       << msg.cov[30] << msg.cov[31] << msg.cov[32] << msg.cov[33] << msg.cov[34] << msg.cov[35];
00082   return new RelPose(loader,msg.id, msg.parent_id, m, cov, msg.name);
00083 }
00084 
00085 extern volatile bool g_stopall;
00086 
00087 ROSjloComm::ROSjloComm(std::string nodeName) :
00088   m_service(nodeName)
00089 {
00090   ros::NodeHandle node;
00091   while(!g_stopall)
00092   {
00093     if(ros::service::waitForService(m_service, 1000))
00094       break;
00095     else
00096     {
00097       printf("Waiting for Jlo to startup at %s\n", m_service.c_str());
00098       g_stopall = g_stopall || !node.ok();
00099       ros::Rate rate(0.5);
00100       rate.sleep();
00101     }
00102   }
00103   m_client = node.serviceClient<srvjlo>(this->m_service, true);
00104   printf("Jlo at %s\n", m_service.c_str());
00105 
00106 }
00107 
00108 ROSjloComm::~ROSjloComm()
00109 {
00110   boost::mutex::scoped_lock lock(s_notDeletedList);
00111   
00112   std::set<unsigned long>::iterator iter = not_deleted_jlos.begin();
00113   for(;iter != not_deleted_jlos.end(); iter++)
00114   {
00115     if(m_client.isValid())
00116       FreePose((*iter));
00117   }
00118 }
00119 
00120 bool ROSjloComm::GetJloServiceClient(srvjlo &msg)
00121 {
00122   ros::NodeHandle node;
00123   bool rv;
00124   while(!m_client.isValid())
00125   {
00126     if(g_stopall)
00127       throw "Interupted while waiting for jlo";
00128     ros::service::waitForService(m_service, 0);
00129     m_client = node.serviceClient<srvjlo>(this->m_service, true);
00130     if(!m_client.isValid())
00131     {
00132       ros::Rate rate(0.5);
00133       rate.sleep();
00134     }
00135   }
00136   {
00137     boost::mutex::scoped_lock lock(s_serviceCall);
00138     rv = m_client.call(msg);
00139   }
00140   {
00141     boost::mutex::scoped_lock lock(s_notDeletedList);
00142     if(msg.request.command.compare(JLO_DELETE) == 0)
00143     {
00144       std::set<unsigned long>::iterator iter = not_deleted_jlos.find(msg.request.query.id);
00145       if(iter != not_deleted_jlos.end())
00146         not_deleted_jlos.erase(iter);
00147     }
00148     else
00149     {
00150       std::set<unsigned long>::iterator iter = not_deleted_jlos.find(msg.request.query.id);
00151       if(iter == not_deleted_jlos.end())
00152         not_deleted_jlos.insert(msg.response.answer.id);
00153     }
00154   }
00155   /*if(not_deleted_jlos.size() % 100 == 0)
00156     ROS_WARN("cop knows %ld poses", not_deleted_jlos.size());*/
00157 
00158   return rv;
00159 }
00160 
00161 void ROSjloComm::NotifyPoseUpdate(RelPose* pose, bool sendObjectRelation)
00162 {
00163   srvjlo msg;
00164   msg.request.command = JLO_UPDATE;
00165   PutPoseIntoAMessage(msg.request.query, pose);
00166   if (!GetJloServiceClient(msg))
00167   {
00168     printf("Notify Pose Update: Error in ROSjloComm: Update of pose information not possible !\n");
00169   }
00170   else if (msg.response.error.length() > 0)
00171   {
00172     printf("Error from jlo: %s!\n", msg.response.error.c_str());
00173   }
00174   return;
00175 }
00176 
00177 RelPose* ROSjloComm::CreateNewPose(RelPose* pose, Matrix* mat, Matrix* cov)
00178 {
00179   srvjlo msg;
00180   msg.request.command = JLO_UPDATE;
00181   msg.request.query.id = 0;
00182   if(pose != NULL)
00183     msg.request.query.parent_id = pose->m_uniqueID;
00184   else
00185     msg.request.query.parent_id = ID_WORLD;
00186 #ifdef _DEBUG
00187 /*   printf("Parent ID before query: %d\n", (int )  msg.request.query.parent_id);*/
00188 #endif
00189   int width = 4;
00190   for(int r = 0; r < width; r++)
00191   {
00192     for(int c = 0; c < width; c++)
00193     {
00194       msg.request.query.pose[r * width + c] = mat->element(r,c);
00195     }
00196   }
00197   width = 6;
00198   for(int r = 0; r < width; r++)
00199   {
00200     for(int c = 0; c < width; c++)
00201     {
00202         msg.request.query.cov[r * width + c] = cov->element(r,c);
00203     }
00204   }
00205   if (!GetJloServiceClient(msg))
00206   {
00207     printf("Create New Pose: Error in ROSjloComm: Update of pose information not possible!\n");
00208     printf("Debug out of srvjl::request :\n");
00209     printf("command:  %s\n", msg.request.command.c_str());
00210     printf("query.id %d query.parent_id %d type %d\n", (int)msg.request.query.id, (int)msg.request.query.parent_id, msg.request.query.type);
00211     printf("query.pose: \n%f %f %f %f \n %f %f %f %f \n%f %f %f %f\n%f %f %f %f\n",  msg.request.query.pose[0] ,
00212     msg.request.query.pose[1] ,
00213     msg.request.query.pose[2] ,
00214     msg.request.query.pose[3] ,
00215     msg.request.query.pose[4] ,
00216     msg.request.query.pose[5] ,
00217     msg.request.query.pose[6],
00218     msg.request.query.pose[7] ,
00219     msg.request.query.pose[8] ,
00220     msg.request.query.pose[9] ,
00221     msg.request.query.pose[10] ,
00222     msg.request.query.pose[11] ,
00223     msg.request.query.pose[12] ,
00224     msg.request.query.pose[13] ,
00225     msg.request.query.pose[14] ,
00226     msg.request.query.pose[15] );
00227     return NULL;
00228   }
00229   else if (msg.response.error.length() > 0)
00230   {
00231     printf("Message from jlo: %s!\n", msg.response.error.c_str());
00232   }
00233   return GetPoseFromMessage(msg.response.answer, this);
00234 }
00235 
00236 
00237 RelPose* ROSjloComm::CreateNewPose(LocatedObjectID_t parent, Matrix* mat, Matrix* cov)
00238 {
00239   srvjlo msg;
00240   msg.request.command = JLO_UPDATE;
00241   msg.request.query.id = 0;
00242   if (parent == 0)
00243   {
00244     ROS_WARN("ROSjloComm::CreateNewPose was called with an invalid parent ID");
00245     return NULL;
00246   }
00247   msg.request.query.parent_id = parent;
00248 #ifdef _DEBUG
00249 /*   printf("Parent ID before query: %d\n", (int )  msg.request.query.parent_id);*/
00250 #endif
00251   int width = 4;
00252   for(int r = 0; r < width; r++)
00253   {
00254     for(int c = 0; c < width; c++)
00255     {
00256       msg.request.query.pose[r * width + c] = mat->element(r,c);
00257     }
00258   }
00259   width = 6;
00260   for(int r = 0; r < width; r++)
00261   {
00262     for(int c = 0; c < width; c++)
00263     {
00264         msg.request.query.cov[r * width + c] = cov->element(r,c);
00265     }
00266   }
00267   if (!GetJloServiceClient(msg))
00268   {
00269     ROS_WARN("Create New Pose: Error in ROSjloComm: Update of pose information not possible!\n");
00270     printf("command:  %s\n", msg.request.command.c_str());
00271     printf("query.id %d query.parent_id %d type %d\n", (int)msg.request.query.id, (int)msg.request.query.parent_id, msg.request.query.type);
00272     printf("query.pose: \n%f %f %f %f \n %f %f %f %f \n%f %f %f %f\n%f %f %f %f\n",  msg.request.query.pose[0] ,
00273     msg.request.query.pose[1] ,
00274     msg.request.query.pose[2] ,
00275     msg.request.query.pose[3] ,
00276     msg.request.query.pose[4] ,
00277     msg.request.query.pose[5] ,
00278     msg.request.query.pose[6],
00279     msg.request.query.pose[7] ,
00280     msg.request.query.pose[8] ,
00281     msg.request.query.pose[9] ,
00282     msg.request.query.pose[10] ,
00283     msg.request.query.pose[11] ,
00284     msg.request.query.pose[12] ,
00285     msg.request.query.pose[13] ,
00286     msg.request.query.pose[14] ,
00287     msg.request.query.pose[15] );
00288     return NULL;
00289   }
00290   else if (msg.response.error.length() > 0)
00291   {
00292     printf("Message from jlo: %s!\n", msg.response.error.c_str());
00293   }
00294   return GetPoseFromMessage(msg.response.answer, this);
00295 }
00296 
00297 
00298 
00299 RelPose* ROSjloComm::UpdatePose(RelPose* pose, LocatedObjectID_t parentID, Matrix* mat, Matrix* cov)
00300 {
00301   srvjlo msg;
00302   msg.request.command = JLO_UPDATE;
00303   msg.request.query.id = pose->m_uniqueID;
00304   if(pose != NULL)
00305     msg.request.query.parent_id = parentID;
00306   else
00307     msg.request.query.parent_id = ID_WORLD;
00308 #ifdef _DEBUG
00309 /*   printf("Parent ID before query: %d\n", (int )  msg.request.query.parent_id);*/
00310 #endif
00311   int width = 4;
00312   for(int r = 0; r < width; r++)
00313   {
00314     for(int c = 0; c < width; c++)
00315     {
00316       msg.request.query.pose[r * width + c] = mat->element(r,c);
00317     }
00318   }
00319   width = 6;
00320   for(int r = 0; r < width; r++)
00321   {
00322     for(int c = 0; c < width; c++)
00323     {
00324         msg.request.query.cov[r * width + c] = cov->element(r,c);
00325     }
00326   }
00327   if (!GetJloServiceClient(msg))
00328   {
00329     printf("Create New Pose: Error in ROSjloComm: Update of pose information not possible!\n");
00330     printf("Debug out of srvjl::request :\n");
00331     printf("command:  %s\n", msg.request.command.c_str());
00332     printf("query.id %d query.parent_id %d type %d\n", (int)msg.request.query.id, (int)msg.request.query.parent_id, msg.request.query.type);
00333     printf("query.pose: \n%f %f %f %f \n %f %f %f %f \n%f %f %f %f\n%f %f %f %f\n",  msg.request.query.pose[0] ,
00334     msg.request.query.pose[1] ,
00335     msg.request.query.pose[2] ,
00336     msg.request.query.pose[3] ,
00337     msg.request.query.pose[4] ,
00338     msg.request.query.pose[5] ,
00339     msg.request.query.pose[6],
00340     msg.request.query.pose[7] ,
00341     msg.request.query.pose[8] ,
00342     msg.request.query.pose[9] ,
00343     msg.request.query.pose[10] ,
00344     msg.request.query.pose[11] ,
00345     msg.request.query.pose[12] ,
00346     msg.request.query.pose[13] ,
00347     msg.request.query.pose[14] ,
00348     msg.request.query.pose[15] );
00349     return NULL;
00350   }
00351   else if (msg.response.error.length() > 0)
00352   {
00353     printf("Message from jlo: %s!\n", msg.response.error.c_str());
00354   }
00355   return GetPoseFromMessage(msg.response.answer, this);
00356 }
00357 
00358 RelPose* ROSjloComm::GetPose(LocatedObjectID_t poseId)
00359 {
00360    srvjlo msg;
00361    msg.request.command = JLO_IDQUERY;
00362    msg.request.query.id = poseId;
00363   if (!GetJloServiceClient(msg))
00364   {
00365     printf("Error in ROSjloComm: Reading of pose information not possible (task: %s with %ld)!\n", JLO_IDQUERY, poseId);
00366     return NULL;
00367   }
00368   else if (msg.response.error.length() > 0)
00369   {
00370     printf("Error from jlo: %s!\n", msg.response.error.c_str());
00371     return NULL;
00372   }
00373   else
00374     return GetPoseFromMessage(msg.response.answer, this);
00375 }
00376 
00377 
00378 RelPose* ROSjloComm::GetPose(const std::string poseId, bool wait)
00379 {
00380   srvjlo msg;
00381   msg.request.command = JLO_NAMEQUERY;
00382   msg.request.query.name = poseId;
00383   RelPose* returnval = NULL;
00384   while(wait)
00385   {
00386     if (!GetJloServiceClient(msg))
00387     {
00388       printf("Error in ROSjloComm: Reading of pose information not possible (task: %s with %s)!\n", JLO_IDQUERY, poseId.c_str());
00389       return returnval; /*Error in service, break always*/
00390     }
00391     else if (msg.response.error.length() > 0)
00392     {
00393       printf("Error from jlo: %s!\n", msg.response.error.c_str());
00394       ROS_INFO("Cop reports: an Error happened in NameQuery for: %s!\n", poseId.c_str());
00395     }
00396     else
00397       returnval = GetPoseFromMessage(msg.response.answer, this);
00398     if(returnval == NULL)
00399     {
00400       if(g_stopall)
00401         throw "ROSjloComm::ROSjloComm: Error happened in NameQuery";
00402       ros::Rate rate(0.5);
00403       rate.sleep();
00404     }
00405     else
00406      break;
00407   }
00408   return returnval;
00409 }
00410 
00411 jlo::LocatedObject* ROSjloComm::GetParent(const jlo::LocatedObject& child)
00412 {
00413   if(child.m_parentID == 0)
00414     return NULL;
00415   return GetPose(child.m_parentID);
00416 }
00417 
00418 RelPose* ROSjloComm::GetPoseRelative(LocatedObjectID_t poseId, LocatedObjectID_t parentPoseId)
00419 {
00420   srvjlo msg;
00421   msg.request.command  = JLO_FRAMEQUERY;
00422   msg.request.query.id = poseId;
00423   msg.request.query.parent_id = parentPoseId;
00424   if (!GetJloServiceClient(msg))
00425   {
00426     printf("Error in ROSjloComm: Reading of pose information not possible!\n");
00427     return NULL;
00428   }
00429   else if (msg.response.error.length() > 0)
00430   {
00431     printf("Error from jlo: %s!\n", msg.response.error.c_str());
00432     return NULL;
00433   }
00434   return GetPoseFromMessage(msg.response.answer, this);
00435 }
00436 
00437 /***
00438 *  TODO  Makre this async
00439 **/
00440 bool ROSjloComm::FreePose(LocatedObjectID_t poseId)
00441 {
00442   srvjlo msg;
00443   if(poseId < 112)
00444     return true;
00445   msg.request.command = JLO_DELETE;
00446   msg.request.query.type = 0;
00447   msg.request.query.id = poseId;
00448   if(poseId == ID_WORLD)
00449     return true;
00450   if (!GetJloServiceClient(msg))
00451   {
00452     printf("Error in ROSjloComm: Deleting of pose information not possible!\n");
00453   }
00454   else if (msg.response.error.length() > 0)
00455   {
00456     if(msg.response.error.compare("Object deleted.") != 0)
00457     {
00458       return false;
00459     }
00460   }
00461   return true;
00462 }
00463 #endif /*USE_YARP_COMM*/


cognitive_perception
Author(s): Ulrich F Klank
autogenerated on Mon Oct 6 2014 10:48:45