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 #ifndef CARMENWRAPPER_H 00025 #define CARMENWRAPPER_H 00026 00027 #include <iostream> 00028 #include <deque> 00029 #include <pthread.h> 00030 #include <semaphore.h> 00031 #include <carmen/carmen.h> 00032 #include <carmen/global.h> 00033 #include <sensor/sensor_base/sensor.h> 00034 #include <log/carmenconfiguration.h> 00035 #include <log/sensorstream.h> 00036 #include <log/sensorlog.h> 00037 #include <sensor/sensor_range/rangesensor.h> 00038 #include <sensor/sensor_range/rangereading.h> 00039 00040 namespace GMapping{ 00041 00042 class CarmenWrapper { 00043 public: 00044 static void initializeIPC(const char* name); 00045 static bool start(const char* name); 00046 static bool isRunning(); 00047 static void lock(); 00048 static void unlock(); 00049 static int registerLocalizationMessages(); 00050 00051 static int queueLength(); 00052 static OrientedPoint getTruePos(); 00053 static bool getReading(RangeReading& reading); 00054 static void addReading(RangeReading& reading); 00055 static const SensorMap& sensorMap(); 00056 static bool sensorMapComputed(); 00057 static bool isStopped(); 00058 00059 // conversion function 00060 static carmen_robot_laser_message reading2carmen(const RangeReading& reading); 00061 static RangeReading carmen2reading(const carmen_robot_laser_message& msg); 00062 static carmen_point_t point2carmen (const OrientedPoint& p); 00063 static OrientedPoint carmen2point (const carmen_point_t& p); 00064 00065 00066 // carmen interaction 00067 static void robot_frontlaser_handler(carmen_robot_laser_message* frontlaser); 00068 static void robot_rearlaser_handler(carmen_robot_laser_message* frontlaser); 00069 static void simulator_truepos_handler(carmen_simulator_truepos_message* truepos); 00070 //babsi: 00071 static void navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ; 00072 static void navigator_stop_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ; 00073 00074 //babsi: 00075 static void publish_globalpos(carmen_localize_summary_p summary); 00076 static void publish_particles(carmen_localize_particle_filter_p filter, 00077 carmen_localize_summary_p summary); 00078 00079 static void shutdown_module(int sig); 00080 00081 private: 00082 static std::deque<RangeReading> m_rangeDeque; 00083 static sem_t m_dequeSem; 00084 static pthread_mutex_t m_mutex, m_lock; 00085 static pthread_t m_readingThread; 00086 static void * m_reading_function(void*); 00087 static bool m_threadRunning; 00088 static SensorMap m_sensorMap; 00089 static RangeSensor* m_frontLaser, *m_rearLaser; 00090 static OrientedPoint m_truepos; 00091 static bool stopped; 00092 }; 00093 00094 } //end namespace 00095 00096 00097 00098 #endif 00099 /* 00100 int main (int argc, char** argv) { 00101 00102 CarmenWrapper::init_carmen(argc, argv); 00103 while (true) { 00104 IPC_listenWait(100); 00105 } 00106 return 1; 00107 } 00108 */