carmenwrapper.cpp
Go to the documentation of this file.
00001 /*****************************************************************
00002  *
00003  * This file is part of the GMAPPING project
00004  *
00005  * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
00006  * Cyrill Stachniss, and Wolfram Burgard
00007  *
00008  * This software is licensed under the "Creative Commons 
00009  * License (Attribution-NonCommercial-ShareAlike 2.0)" 
00010  * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
00011  * and Wolfram Burgard.
00012  * 
00013  * Further information on this license can be found at:
00014  * http://creativecommons.org/licenses/by-nc-sa/2.0/
00015  * 
00016  * GMAPPING is distributed in the hope that it will be useful,
00017  * but WITHOUT ANY WARRANTY; without even the implied 
00018  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
00019  * PURPOSE.  
00020  *
00021  *****************************************************************/
00022 
00023 
00024 #include "carmenwrapper.h"
00025 
00026 using namespace GMapping;
00027 using namespace std;
00028 
00029 //static vars for the carmenwrapper
00030 SensorMap CarmenWrapper::m_sensorMap;
00031 deque<RangeReading> CarmenWrapper::m_rangeDeque;
00032 pthread_mutex_t CarmenWrapper::m_mutex;  
00033 sem_t CarmenWrapper::m_dequeSem;  
00034 pthread_mutex_t CarmenWrapper::m_lock;  
00035 pthread_t CarmenWrapper::m_readingThread;
00036 RangeSensor* CarmenWrapper::m_frontLaser=0;
00037 RangeSensor* CarmenWrapper::m_rearLaser=0;
00038 bool CarmenWrapper::m_threadRunning=false;
00039 OrientedPoint CarmenWrapper::m_truepos;
00040 bool CarmenWrapper::stopped=true;
00041 
00042 
00043 void CarmenWrapper::initializeIPC(const char* name) {
00044   carmen_ipc_initialize(1,(char **)&name);
00045 }
00046 
00047 
00048 
00049 
00050 int CarmenWrapper::registerLocalizationMessages(){
00051   lock();
00052   IPC_RETURN_TYPE err;
00053 
00054   /* register globalpos message */
00055   err = IPC_defineMsg(CARMEN_LOCALIZE_GLOBALPOS_NAME, IPC_VARIABLE_LENGTH, 
00056                       CARMEN_LOCALIZE_GLOBALPOS_FMT);
00057   carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_GLOBALPOS_NAME);
00058 
00059   /* register robot particle message */
00060   err = IPC_defineMsg(CARMEN_LOCALIZE_PARTICLE_NAME, IPC_VARIABLE_LENGTH, 
00061                       CARMEN_LOCALIZE_PARTICLE_FMT);
00062   carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_PARTICLE_NAME);
00063 
00064 /*
00065   carmen_localize_subscribe_initialize_placename_message(NULL, 
00066                                                          (carmen_handler_t)
00067                                                          carmen_localize_initialize_placename_handler,
00068                                                          CARMEN_SUBSCRIBE_LATEST);
00069 
00070   // register map request message 
00071   err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_QUERY_NAME, IPC_VARIABLE_LENGTH,
00072                       CARMEN_LOCALIZE_MAP_QUERY_FMT);
00073   carmen_test_ipc_exit(err, "Could not define", 
00074                        CARMEN_LOCALIZE_MAP_QUERY_NAME);
00075 
00076   err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_NAME, IPC_VARIABLE_LENGTH,
00077                       CARMEN_LOCALIZE_MAP_FMT);
00078   carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_MAP_NAME);
00079 
00080   // subscribe to map request message 
00081   err = IPC_subscribe(CARMEN_LOCALIZE_MAP_QUERY_NAME, map_query_handler, NULL);
00082   carmen_test_ipc(err, "Could not subscribe", CARMEN_LOCALIZE_MAP_QUERY_NAME);
00083   IPC_setMsgQueueLength(CARMEN_LOCALIZE_MAP_QUERY_NAME, 1);
00084 
00085 
00086   // register globalpos request message 
00087   err = IPC_defineMsg(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, 
00088                       IPC_VARIABLE_LENGTH,
00089                       CARMEN_DEFAULT_MESSAGE_FMT);
00090   carmen_test_ipc_exit(err, "Could not define", 
00091                        CARMEN_LOCALIZE_MAP_QUERY_NAME);
00092 
00093   // subscribe to globalpos request message 
00094   err = IPC_subscribe(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, 
00095                       globalpos_query_handler, NULL);
00096   carmen_test_ipc(err, "Could not subscribe", 
00097                   CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME);
00098   IPC_setMsgQueueLength(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, 1);
00099 */
00100   unlock();
00101   return 0;
00102 }
00103 
00104 bool CarmenWrapper::start(const char* name){
00105         if (m_threadRunning)
00106                 return false;
00107         carmen_robot_subscribe_frontlaser_message(NULL, (carmen_handler_t)robot_frontlaser_handler, CARMEN_SUBSCRIBE_LATEST);
00108         carmen_robot_subscribe_rearlaser_message(NULL, (carmen_handler_t)robot_rearlaser_handler, CARMEN_SUBSCRIBE_LATEST);
00109         carmen_simulator_subscribe_truepos_message(NULL,(carmen_handler_t) simulator_truepos_handler, CARMEN_SUBSCRIBE_LATEST);
00110 
00111         IPC_RETURN_TYPE err;
00112 
00113         err = IPC_subscribe(CARMEN_NAVIGATOR_GO_NAME, navigator_go_handler,  NULL);
00114         carmen_test_ipc_exit(err, "Could not subscribe", 
00115                              CARMEN_NAVIGATOR_GO_NAME);
00116         IPC_setMsgQueueLength(CARMEN_NAVIGATOR_GO_NAME, 1);
00117 
00118         err = IPC_subscribe(CARMEN_NAVIGATOR_STOP_NAME, navigator_stop_handler,  NULL);
00119         carmen_test_ipc_exit(err, "Could not subscribe", 
00120                              CARMEN_NAVIGATOR_STOP_NAME);
00121         IPC_setMsgQueueLength(CARMEN_NAVIGATOR_STOP_NAME, 1);
00122 
00123 
00124 
00125         signal(SIGINT, shutdown_module);
00126         pthread_mutex_init(&m_mutex, 0);
00127         pthread_mutex_init(&m_lock, 0);
00128         sem_init(&m_dequeSem, 0, 0);
00129         m_threadRunning=true;
00130         pthread_create (&m_readingThread,0,m_reading_function,0);
00131         return true; 
00132 }
00133 
00134 void CarmenWrapper::lock(){
00135         //cerr <<"LOCK" << endl;
00136         pthread_mutex_lock(&m_lock);
00137 }
00138 
00139 void CarmenWrapper::unlock(){
00140         //cerr <<"UNLOCK" << endl;
00141         pthread_mutex_unlock(&m_lock);
00142 }
00143 
00144 
00145 bool CarmenWrapper::sensorMapComputed(){
00146         pthread_mutex_lock(&m_mutex);
00147         bool smok=m_frontLaser;
00148         pthread_mutex_unlock(&m_mutex);
00149         return smok;
00150 }
00151 
00152 const SensorMap& CarmenWrapper::sensorMap(){
00153         return m_sensorMap;
00154 }
00155  
00156 bool CarmenWrapper::isRunning(){
00157         return m_threadRunning;
00158 }
00159 
00160 bool CarmenWrapper::isStopped(){
00161         return stopped;
00162 }
00163 
00164 int CarmenWrapper::queueLength(){
00165         int ql=0;
00166         pthread_mutex_lock(&m_mutex);
00167         ql=m_rangeDeque.size();
00168         pthread_mutex_unlock(&m_mutex);
00169         return ql;
00170 }
00171 
00172 OrientedPoint CarmenWrapper::getTruePos(){
00173         return m_truepos;
00174 }
00175 
00176 bool CarmenWrapper::getReading(RangeReading& reading){
00177         bool present=false;
00178         sem_wait(&m_dequeSem);
00179         pthread_mutex_lock(&m_mutex);
00180         if (!m_rangeDeque.empty()){
00181 //              cerr << __PRETTY_FUNCTION__ << ": queue size=" <<m_rangeDeque.size() << endl;
00182                 reading=m_rangeDeque.front();
00183                 m_rangeDeque.pop_front();
00184                 present=true;
00185         }
00186         int sval;
00187         sem_getvalue(&m_dequeSem,&sval);
00188 //      cerr << "fetch. elements= "<< m_rangeDeque.size() << " sval=" << sval <<endl;
00189         pthread_mutex_unlock(&m_mutex);
00190         return present;
00191 }
00192 
00193 void CarmenWrapper::addReading(RangeReading& reading){
00194         pthread_mutex_lock(&m_mutex);
00195         m_rangeDeque.push_back(reading);
00196         pthread_mutex_unlock(&m_mutex);
00197         sem_post(&m_dequeSem);
00198         int sval;
00199         sem_getvalue(&m_dequeSem,&sval);
00200 //      cerr << "post. elements= "<< m_rangeDeque.size() << " sval=" << sval <<endl;
00201 }
00202 
00203 
00204 //RangeSensor::RangeSensor(std::string name, unsigned int beams_num, unsigned int res, const OrientedPoint& position, double span, double maxrange): 
00205 
00206 void CarmenWrapper::robot_frontlaser_handler(carmen_robot_laser_message* frontlaser) {
00207 /*      if (! m_rangeSensor){
00208                 double res=0;
00209                 if (frontlaser->num_readings==180 || frontlaser->num_readings==181)
00210                         res=M_PI/180;
00211                 if (frontlaser->num_readings==360 || frontlaser->num_readings==361)
00212                         res=M_PI/360;
00213                 assert(res>0);
00214                 m_rangeSensor=new RangeSensor("FLASER",frontlaser->num_readings, res, OrientedPoint(0,0,0), 0, 89.9);
00215                 m_sensorMap.insert(make_pair(string("FLASER"), m_rangeSensor));
00216                 
00217                 cout << __PRETTY_FUNCTION__ 
00218                      << ": FrontLaser configured." 
00219                      << " Readings " << m_rangeSensor->beams().size() 
00220                      << " Resolution " << res << endl;
00221         }
00222         
00223         RangeReading reading(m_rangeSensor, frontlaser->timestamp);
00224         reading.resize(m_rangeSensor->beams().size());
00225         for (unsigned int i=0; i< (unsigned int)frontlaser->num_readings; i++){
00226                 reading[i]=(double)frontlaser->range[i];
00227         }
00228         reading.setPose(OrientedPoint(frontlaser->x, frontlaser->y, frontlaser->theta));
00229 */      
00230         RangeReading reading=carmen2reading(*frontlaser);
00231         addReading(reading);
00232 }
00233 
00234 void CarmenWrapper::robot_rearlaser_handler(carmen_robot_laser_message* rearlaser) {
00235 /*      if (! m_rangeSensor){
00236                 double res=0;
00237                 if (frontlaser->num_readings==180 || frontlaser->num_readings==181)
00238                         res=M_PI/180;
00239                 if (frontlaser->num_readings==360 || frontlaser->num_readings==361)
00240                         res=M_PI/360;
00241                 assert(res>0);
00242                 m_rangeSensor=new RangeSensor("FLASER",frontlaser->num_readings, res, OrientedPoint(0,0,0), 0, 89.9);
00243                 m_sensorMap.insert(make_pair(string("FLASER"), m_rangeSensor));
00244                 
00245                 cout << __PRETTY_FUNCTION__ 
00246                      << ": FrontLaser configured." 
00247                      << " Readings " << m_rangeSensor->beams().size() 
00248                      << " Resolution " << res << endl;
00249         }
00250         
00251         RangeReading reading(m_rangeSensor, frontlaser->timestamp);
00252         reading.resize(m_rangeSensor->beams().size());
00253         for (unsigned int i=0; i< (unsigned int)frontlaser->num_readings; i++){
00254                 reading[i]=(double)frontlaser->range[i];
00255         }
00256         reading.setPose(OrientedPoint(frontlaser->x, frontlaser->y, frontlaser->theta));
00257 */      
00258         RangeReading reading=carmen2reading(*rearlaser);
00259         addReading(reading);
00260 }
00261 
00262 
00263 
00264 
00265 void CarmenWrapper:: navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) {
00266   carmen_navigator_go_message msg;
00267   FORMATTER_PTR formatter;
00268   IPC_RETURN_TYPE err;
00269 
00270   formatter = IPC_msgInstanceFormatter(msgRef);
00271   err = IPC_unmarshallData(formatter, callData, &msg,
00272                            sizeof(carmen_navigator_go_message));  
00273   IPC_freeByteArray(callData);
00274 
00275   carmen_test_ipc_return
00276     (err, "Could not unmarshall", IPC_msgInstanceName(msgRef));
00277   cerr<<"go"<<endl;
00278   stopped=false;
00279 }
00280 
00281 
00282 void CarmenWrapper:: navigator_stop_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) {
00283   carmen_navigator_stop_message msg;
00284   FORMATTER_PTR formatter;
00285   IPC_RETURN_TYPE err;
00286 
00287   formatter = IPC_msgInstanceFormatter(msgRef);
00288   err = IPC_unmarshallData(formatter, callData, &msg,
00289                            sizeof(carmen_navigator_stop_message));  
00290   IPC_freeByteArray(callData);
00291 
00292   carmen_test_ipc_return
00293     (err, "Could not unmarshall", IPC_msgInstanceName(msgRef));
00294   cerr<<"stop"<<endl;
00295   stopped=true;
00296 }
00297 
00298 
00299 
00300 void CarmenWrapper::simulator_truepos_handler(carmen_simulator_truepos_message* truepos){
00301         m_truepos.x=truepos->truepose.x;
00302         m_truepos.y=truepos->truepose.y;
00303         m_truepos.theta=truepos->truepose.theta;
00304 }
00305 
00306 RangeReading CarmenWrapper::carmen2reading(const carmen_robot_laser_message& msg){
00307         //either front laser or rear laser
00308         double dth=msg.laser_pose.theta-msg.robot_pose.theta;
00309         dth=atan2(sin(dth), cos(dth));
00310 
00311         if (msg.laser_pose.theta==msg.robot_pose.theta && !m_frontLaser){
00312                 double res=0;
00313                 res = msg.config.angular_resolution;
00314 //              if (msg.num_readings==180 || msg.num_readings==181)
00315 //                      res=M_PI/180;
00316 //              if (msg.num_readings==360 || msg.num_readings==361)
00317 //                      res=M_PI/360;
00318                 assert(res>0);
00319                 string sensorName="FLASER";
00320                 OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta);
00321                 OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta);
00322                 OrientedPoint dp=absoluteDifference(lpose, rpose);
00323                 m_frontLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0, 
00324                                               msg.config.maximum_range);
00325                 m_frontLaser->updateBeamsLookup();
00326                 m_sensorMap.insert(make_pair(sensorName, m_frontLaser));
00327                 
00328                 cout << __PRETTY_FUNCTION__ 
00329                      << ": " << sensorName <<" configured." 
00330                      << " Readings " << m_frontLaser->beams().size() 
00331                      << " Resolution " << res << endl;
00332         }
00333         if (msg.laser_pose.theta!=msg.robot_pose.theta && !m_rearLaser){
00334                 double res=0;
00335                 res = msg.config.angular_resolution;
00336 //              if (msg.num_readings==180 || msg.num_readings==181)
00337 //                      res=M_PI/180;
00338 //              if (msg.num_readings==360 || msg.num_readings==361)
00339 //                      res=M_PI/360;
00340                 assert(res>0);
00341                 OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta);
00342                 OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta);
00343                 OrientedPoint dp=absoluteDifference(lpose, rpose);
00344                 string sensorName="RLASER";
00345                 m_rearLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0, 
00346                                               msg.config.maximum_range);
00347                 m_rearLaser->updateBeamsLookup();
00348                 m_sensorMap.insert(make_pair(sensorName, m_rearLaser));
00349                 
00350                 cout << __PRETTY_FUNCTION__ 
00351                      << ": " << sensorName <<" configured." 
00352                      << " Readings " << m_rearLaser->beams().size() 
00353                      << " Resolution " << res << endl;
00354         }       
00355 
00356         const RangeSensor * rs=(msg.laser_pose.theta==msg.robot_pose.theta)?m_frontLaser:m_rearLaser;
00357         RangeReading reading(rs, msg.timestamp);
00358         reading.resize(rs->beams().size());
00359         for (unsigned int i=0; i< (unsigned int)msg.num_readings; i++){
00360                 reading[i]=(double)msg.range[i];
00361         }
00362         reading.setPose(OrientedPoint(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta));
00363         return reading;
00364 }
00365 
00366 void CarmenWrapper::publish_globalpos(carmen_localize_summary_p summary)
00367 {
00368   lock();
00369   static carmen_localize_globalpos_message globalpos;
00370   IPC_RETURN_TYPE err;
00371   
00372   globalpos.timestamp = carmen_get_time();
00373   globalpos.host = carmen_get_host();
00374   globalpos.globalpos = summary->mean;
00375   globalpos.globalpos_std = summary->std;
00376   globalpos.globalpos_xy_cov = summary->xy_cov;
00377   globalpos.odometrypos = summary->odometry_pos;
00378   globalpos.converged = summary->converged;
00379   err = IPC_publishData(CARMEN_LOCALIZE_GLOBALPOS_NAME, &globalpos);
00380   carmen_test_ipc_exit(err, "Could not publish", 
00381                        CARMEN_LOCALIZE_GLOBALPOS_NAME);  
00382   unlock();
00383 }
00384 
00385 /* publish a particle message */
00386 
00387 void CarmenWrapper::publish_particles(carmen_localize_particle_filter_p filter, 
00388                        carmen_localize_summary_p summary)
00389 {
00390   lock();
00391   static carmen_localize_particle_message pmsg;
00392   IPC_RETURN_TYPE err;
00393 
00394   pmsg.timestamp = carmen_get_time();
00395   pmsg.host = carmen_get_host();
00396   pmsg.globalpos = summary->mean;
00397   pmsg.globalpos_std = summary->mean;
00398   pmsg.num_particles = filter->param->num_particles;
00399   pmsg.particles = (carmen_localize_particle_ipc_p)filter->particles;
00400   err = IPC_publishData(CARMEN_LOCALIZE_PARTICLE_NAME, &pmsg);
00401   carmen_test_ipc_exit(err, "Could not publish", 
00402                        CARMEN_LOCALIZE_PARTICLE_NAME);  
00403   fprintf(stderr, "P");
00404   unlock();
00405 }
00406 
00407 
00408 
00409 
00410 
00411 void * CarmenWrapper::m_reading_function(void*){
00412         while (true) {
00413                 lock();
00414                 IPC_listen(100);
00415                 unlock();
00416                 usleep(20000);
00417         }    
00418         return 0;
00419 }
00420 
00421 void CarmenWrapper::shutdown_module(int sig){
00422   if(sig == SIGINT) {
00423       carmen_ipc_disconnect();
00424 
00425       fprintf(stderr, "\nDisconnecting (shutdown_module(%d) called).\n",sig);
00426       exit(0);
00427   }
00428 }
00429 /*
00430 typedef struct {
00431   int num_readings;
00432   float *range;
00433   char *tooclose;
00434   double x, y, theta;//position of the laser on the robot
00435   double odom_x, odom_y, odom_theta; //position of the center of the robot
00436   double tv, rv;
00437   double forward_safety_dist, side_safety_dist;
00438   double turn_axis;
00439   double timestamp;
00440   char host[10];
00441 } carmen_robot_laser_message;
00442 */
00443 
00444 carmen_robot_laser_message CarmenWrapper::reading2carmen(const RangeReading& reading){
00445         carmen_robot_laser_message frontlaser;
00446         frontlaser.num_readings=reading.size();
00447         frontlaser.range = new float[frontlaser.num_readings];
00448         frontlaser.tooclose=0;
00449         frontlaser.laser_pose.x=frontlaser.robot_pose.x=reading.getPose().x;
00450         frontlaser.laser_pose.y=frontlaser.robot_pose.y=reading.getPose().y;
00451         frontlaser.laser_pose.theta=frontlaser.robot_pose.theta=reading.getPose().theta;
00452         frontlaser.tv=frontlaser.rv=0;
00453         frontlaser.forward_safety_dist=frontlaser.side_safety_dist=0;
00454         frontlaser.turn_axis=0;
00455         frontlaser.timestamp=reading.getTime();
00456         for (unsigned int i=0; i< reading.size(); i++){
00457                 frontlaser.range[i]=(float)reading[i];
00458         }
00459         return frontlaser;
00460 }
00461 
00462 carmen_point_t CarmenWrapper::point2carmen (const OrientedPoint& p){
00463         return (carmen_point_t){p.x,p.y,p.theta};
00464 }
00465 
00466 OrientedPoint CarmenWrapper::carmen2point (const carmen_point_t& p){
00467         return OrientedPoint(p.x, p.y, p.theta);
00468 }
00469 
00470 
00471 /*
00472 int main (int argc, char** argv) {
00473         CarmenWrapper::start(argc, argv);
00474         while(1){
00475                 sleep(2);
00476                 RangeReading reading(0,0);
00477                 while(CarmenWrapper::getReading(reading)){
00478                         cout << "FLASER " <<  reading.size();
00479                         for (int i=0; i<reading.size(); i++)
00480                                 cout << " " << reading[i];
00481                         cout << reading.getPose().x << " "
00482                              << reading.getPose().y << " "
00483                              << reading.getPose().theta << " 0 cazzo 0" << endl;
00484                 }
00485                 cout << endl;
00486         }
00487         return 1;
00488 }
00489 */
00490 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Fri Aug 28 2015 11:56:21