constraint_app
/home/jbrooksh/drc/software/perception/constraint_app/src/matlab_mex/ConstraintApp.cpp
Go to the documentation of this file.
00001 #include "ConstraintApp.h"
00002 #include <stdio.h>
00003 #include <inttypes.h>
00004 #include <sys/select.h>
00005 typedef int SOCKET;
00006 
00007 //NB: use tail -f cam.log to view output from the ConstraintApp_RB_UKF::main thread
00008 ConstraintApp::ConstraintApp() : m_stopThreads(false), m_log("/tmp/cam.log", std::ios::trunc|std::ios::binary) 
00009 {
00010   m_lcm = new lcm::LCM();
00011 
00012   m_lcm->subscribe("AFFORDANCE_DETECTIONS", &ConstraintApp::AffordanceTrackCollectionHandlerAux, this);
00013   m_lcm->subscribe("CAM_AFFORDANCE_FIT", &ConstraintApp::AffordanceFitHandlerAux, this);
00014   /*
00015   drc_affordance_track_collection_t_subscribe(m_lcm, "AFFORDANCE_TRACK_COLLECTION",
00016                                               AffordanceTrackCollectionHandlerAux, this);
00017   drc_affordance_t_subscribe(m_lcm, "AFFORDANCE_FIT",
00018                              AffordanceFitHandlerAux, this);
00019   */
00020   
00021   m_mainThread = new boost::thread(boost::bind(&ConstraintApp::main, this));
00022 }
00023 
00024 ConstraintApp::~ConstraintApp() 
00025 {
00026   stopThreads();
00027   delete m_mainThread;
00028 
00029   //lcm_destroy(m_lcm);
00030   delete m_lcm;
00031 
00032   m_log << "ConstraintApp::~ConstraintApp" << std::endl;
00033 }
00034 
00035 void ConstraintApp::main() {
00036   while ( !shouldStop() ) {
00037     {
00038       boost::mutex::scoped_lock lock(m_lcmMutex);
00039       lcm_handle_timeout(m_lcm->getUnderlyingLCM(), 500);
00040     }
00041   }
00042 }
00043 
00044 int ConstraintApp::lcm_handle_timeout(lcm_t* lcm, int ms) {
00045   // setup the LCM file descriptor for waiting.
00046   SOCKET lcm_fd = lcm_get_fileno(lcm);
00047   fd_set fds;
00048   FD_ZERO(&fds);
00049   FD_SET(lcm_fd, &fds);
00050 
00051   // wait a limited amount of time for an incoming message
00052   struct timeval timeout = {
00053     ms / 1000,           // seconds
00054     (ms % 1000) * 1000   // microseconds
00055   };
00056   int status = select(lcm_fd + 1, &fds, 0, 0, &timeout);
00057   if(status > 0 && FD_ISSET(lcm_fd, &fds)) {
00058     lcm_handle(lcm);
00059     return 1;
00060   }
00061   
00062   // no messages
00063   return 0;
00064 }
00065 
00066 bool ConstraintApp::shouldStop() 
00067 {
00068   bool ret;
00069   {
00070     boost::mutex::scoped_lock lock(m_stopThreadsMutex);
00071     ret = m_stopThreads;
00072   }
00073   return ret;
00074 }
00075 
00076 void ConstraintApp::stopThreads() 
00077 {
00078   {
00079     boost::mutex::scoped_lock lock(m_stopThreadsMutex);
00080     m_stopThreads = true;
00081   }
00082   m_mainThread->join();
00083 }
00084 
00085 
00086 KDL::Frame ConstraintApp::VectorToFrame(const std::vector<double>& state)
00087 {
00088   KDL::Frame res;
00089   
00090   assert(state.size() == 6);
00091   /*
00092   if ( state.size() != 6 ) {
00093     m_log << "ERROR: improper number of elements while setting state" << std::endl;
00094     return KDL::Frame();
00095   }
00096   */
00097 
00098   res.p[0] = state[0];
00099   res.p[1] = state[1];
00100   res.p[2] = state[2];
00101 
00102   res.M = KDL::Rotation::RPY(state[3], state[4], state[5]);
00103 
00104   return res;
00105 }
00106 
00107 std::vector<double> ConstraintApp::FrameToVector(const KDL::Frame& frame)
00108 {
00109   std::vector<double> state;
00110   state.reserve(6);
00111 
00112   state.push_back(frame.p[0]);
00113   state.push_back(frame.p[1]);
00114   state.push_back(frame.p[2]);
00115   double r,p,w;
00116   frame.M.GetRPY(r,p,w);
00117   state.push_back(r);
00118   state.push_back(p);
00119   state.push_back(w);
00120 
00121   return state;
00122 }
00123 
00124 ConstraintApp::OptionalKDLFrame ConstraintApp::GetFrameFromParams(const drc::affordance_t *msg)
00125 {
00126   std::string names[] = { "x", "y", "z", "roll", "pitch", "yaw" };
00127   double xyzrpw[6];
00128   int found = 0;
00129 
00130   for ( int i = 0; i < msg->nparams; i++ ) {
00131     for ( int j = 0; j < 6; j++ ) {
00132       if ( msg->param_names[i] == names[j] ) {
00133         xyzrpw[j] = msg->params[i];
00134         found++;
00135       }
00136     }
00137   }
00138 
00139   if ( found != 6 ) {
00140     return OptionalKDLFrame();
00141   }
00142 
00143   KDL::Frame ret;
00144   ret.p[0] = xyzrpw[0];
00145   ret.p[1] = xyzrpw[1];
00146   ret.p[2] = xyzrpw[2];
00147   ret.M = KDL::Rotation::RPY(xyzrpw[3], xyzrpw[4], xyzrpw[5]);
00148   return OptionalKDLFrame(ret);
00149 }
 All Classes Files Functions Variables Typedefs